diff --git a/2PieceChoreo.chor b/2PieceChoreo.chor index 479eae5..4db83b7 100644 --- a/2PieceChoreo.chor +++ b/2PieceChoreo.chor @@ -250,7 +250,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "Choreo 2 Piece 2": { "waypoints": [ @@ -261,12 +261,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 29 }, { "x": 3.9220988750457764, "y": 1.2191578149795532, - "heading": 1.5337761150741231, + "heading": 0.02492521296459199, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -275,7 +275,7 @@ { "x": 8.581049919128418, "y": 0.7145513892173767, - "heading": 1.5835345242998375, + "heading": 0.013149053743484958, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -287,415 +287,487 @@ "x": 2.578, "y": 3.434, "heading": 2.456, - "angularVelocity": 1.0219812008109311e-25, - "velocityX": -7.726426952121863e-25, - "velocityY": -8.236726072553901e-26, + "angularVelocity": -2.2243193051899215e-21, + "velocityX": 4.420947543039026e-20, + "velocityY": 1.7808811974639867e-21, "timestamp": 0 }, { - "x": 2.584286650145692, - "y": 3.421951310408596, - "heading": 2.4350954049954208, - "angularVelocity": -0.3464845678617885, - "velocityX": 0.10419849121933693, - "velocityY": -0.1997017883132429, - "timestamp": 0.06033340859474565 - }, - { - "x": 2.5967989346479854, - "y": 3.3977100915163163, - "heading": 2.3938477021537534, - "angularVelocity": -0.6836627301919046, - "velocityX": 0.2073856722788015, - "velocityY": -0.401787657234916, - "timestamp": 0.1206668171894913 - }, - { - "x": 2.615462449887643, - "y": 3.361117228577582, - "heading": 2.332876835959309, - "angularVelocity": -1.0105655823953221, - "velocityX": 0.3093396457179988, - "velocityY": -0.6065107838432474, - "timestamp": 0.18100022578423694 - }, - { - "x": 2.640198059989427, - "y": 3.3119715556696296, - "heading": 2.2531378381258196, - "angularVelocity": -1.3216391994208312, - "velocityX": 0.4099819764524013, - "velocityY": -0.8145681481061332, - "timestamp": 0.2413336343789826 - }, - { - "x": 2.6709458792631127, - "y": 3.2500095590805107, - "heading": 2.1562840802735845, - "angularVelocity": -1.605308901122988, - "velocityX": 0.5096317279240128, - "velocityY": -1.026993137505499, - "timestamp": 0.30166704297372826 - }, - { - "x": 2.7077008608268662, - "y": 3.1749016535376815, - "heading": 2.045096498973365, - "angularVelocity": -1.8428857889839654, - "velocityX": 0.6091978295248277, - "velocityY": -1.2448808594144314, - "timestamp": 0.36200045156847394 - }, - { - "x": 2.7505245327527716, - "y": 3.0862787536737564, - "heading": 1.9239140602650735, - "angularVelocity": -2.0085462023580276, - "velocityX": 0.7097837321532509, - "velocityY": -1.4688860107208206, - "timestamp": 0.4223338601632196 - }, - { - "x": 2.7994482297249395, - "y": 2.9838047667261183, - "heading": 1.7994067052659932, - "angularVelocity": -2.0636552434056146, - "velocityX": 0.8108889935389533, - "velocityY": -1.6984617533537163, - "timestamp": 0.4826672687579653 - }, - { - "x": 2.8540407096263034, - "y": 2.867709288903639, - "heading": 1.6856485603266385, - "angularVelocity": -1.885491763004119, - "velocityX": 0.9048466044419411, - "velocityY": -1.924232038708154, - "timestamp": 0.543000677352711 - }, - { - "x": 2.9119156128261765, - "y": 2.7403356565757657, - "heading": 1.604080673099594, - "angularVelocity": -1.3519522454786057, - "velocityX": 0.95925134262865, - "velocityY": -2.1111625431845082, - "timestamp": 0.6033340859474566 - }, - { - "x": 2.9732896898216743, - "y": 2.6016323913890385, - "heading": 1.553622517502379, - "angularVelocity": -0.8363219776979306, - "velocityX": 1.0172486260098852, - "velocityY": -2.298946278974311, - "timestamp": 0.6636674945422023 - }, - { - "x": 3.038399585143699, - "y": 2.4516460837393157, - "heading": 1.5337765355615411, - "angularVelocity": -0.32893851686951064, - "velocityX": 1.0791681895409253, - "velocityY": -2.4859577992214517, - "timestamp": 0.724000903136948 - }, - { - "x": 3.1118123187513027, - "y": 2.2970091829036123, - "heading": 1.5337764978118669, - "angularVelocity": -6.256844289379484e-7, - "velocityX": 1.2167841220560034, - "velocityY": -2.5630393580841155, - "timestamp": 0.7843343117316937 - }, - { - "x": 3.1852252486361183, - "y": 2.1423723752490966, - "heading": 1.5337764600625183, - "angularVelocity": -6.256790302738932e-7, - "velocityX": 1.216787375265398, - "velocityY": -2.563037813646468, - "timestamp": 0.8446677203264393 - }, - { - "x": 3.2586383526710114, - "y": 1.9877356502714374, - "heading": 1.5337764223131956, - "angularVelocity": -6.256786018158767e-7, - "velocityX": 1.2167902617271742, - "velocityY": -2.5630364433135364, - "timestamp": 0.905001128921185 - }, - { - "x": 3.339262039402322, - "y": 1.8367329293476955, - "heading": 1.5337763853100257, - "angularVelocity": -6.133114449160005e-7, - "velocityX": 1.3363025330269804, - "velocityY": -2.5028044070576922, - "timestamp": 0.9653345375159307 - }, - { - "x": 3.432942751765767, - "y": 1.6934642146806478, - "heading": 1.533776348204764, - "angularVelocity": -6.150035711868799e-7, - "velocityX": 1.5527170525486216, - "velocityY": -2.3746166179565877, - "timestamp": 1.0256679461106764 - }, - { - "x": 3.5389443424843976, - "y": 1.559055647892523, - "heading": 1.533776308684474, - "angularVelocity": -6.550316132036273e-7, - "velocityX": 1.7569302512084846, - "velocityY": -2.2277635213839058, - "timestamp": 1.086001354705422 - }, - { - "x": 3.6564336903688766, - "y": 1.4345636707390452, - "heading": 1.533776263619502, - "angularVelocity": -7.469323051999795e-7, - "velocityX": 1.9473348285962937, - "velocityY": -2.063400362304737, - "timestamp": 1.1463347633001675 - }, - { - "x": 3.7844873612800467, - "y": 1.3209667734788668, - "heading": 1.533776207514778, - "angularVelocity": -9.29911391627009e-7, - "velocityX": 2.1224338868585284, - "velocityY": -1.8828191528709874, - "timestamp": 1.206668171894913 + "x": 2.5808143020901366, + "y": 3.4305511164394358, + "heading": 2.421420914475003, + "angularVelocity": -0.7062096950549269, + "velocityX": 0.05747657552803998, + "velocityY": -0.07043665182601658, + "timestamp": 0.04896433136946255 + }, + { + "x": 2.5865263986145903, + "y": 3.423301716588506, + "heading": 2.353190948627095, + "angularVelocity": -1.3934626275824327, + "velocityX": 0.11665831769034016, + "velocityY": -0.14805470938110385, + "timestamp": 0.0979286627389251 + }, + { + "x": 2.5951072938917, + "y": 3.411803141318997, + "heading": 2.252339287405858, + "angularVelocity": -2.059696485187491, + "velocityX": 0.1752478801857445, + "velocityY": -0.23483574569237411, + "timestamp": 0.14689299410838766 + }, + { + "x": 2.6063817008248584, + "y": 3.395552329101315, + "heading": 2.1199335438440907, + "angularVelocity": -2.704126449980364, + "velocityX": 0.23025754907213325, + "velocityY": -0.3318908226296179, + "timestamp": 0.1958573254778502 + }, + { + "x": 2.6200490589295313, + "y": 3.3739990762642234, + "heading": 1.957267690055127, + "angularVelocity": -3.322129583707805, + "velocityX": 0.2791288622216491, + "velocityY": -0.44018272555292703, + "timestamp": 0.24482165684731275 + }, + { + "x": 2.6358077196258662, + "y": 3.346424151831386, + "heading": 1.7667703729621236, + "angularVelocity": -3.890532388885235, + "velocityX": 0.3218395974292316, + "velocityY": -0.5631635041587543, + "timestamp": 0.2937859882167753 + }, + { + "x": 2.6542369913783963, + "y": 3.311395651079251, + "heading": 1.5565531821810574, + "angularVelocity": -4.293271957394105, + "velocityX": 0.37638156668515743, + "velocityY": -0.7153881156437778, + "timestamp": 0.34275031958623786 + }, + { + "x": 2.677454625002408, + "y": 3.268157202317915, + "heading": 1.3372511935604507, + "angularVelocity": -4.478811054640037, + "velocityX": 0.4741744240049492, + "velocityY": -0.8830601287108768, + "timestamp": 0.3917146509557004 + }, + { + "x": 2.706331728536566, + "y": 3.2188789886399363, + "heading": 1.1357091046403869, + "angularVelocity": -4.116100093337721, + "velocityX": 0.5897579467859876, + "velocityY": -1.0064104277491477, + "timestamp": 0.44067898232516295 + }, + { + "x": 2.7406445911847674, + "y": 3.1628572349258164, + "heading": 0.9501369190074549, + "angularVelocity": -3.7899462821760768, + "velocityX": 0.7007726173006585, + "velocityY": -1.1441339470436553, + "timestamp": 0.4896433136946255 + }, + { + "x": 2.77961503012768, + "y": 3.099262425694046, + "heading": 0.7791196669307836, + "angularVelocity": -3.4926904400318053, + "velocityX": 0.7958944368897106, + "velocityY": -1.2987986857600298, + "timestamp": 0.5386076450640881 + }, + { + "x": 2.821598359020251, + "y": 3.0272467328347097, + "heading": 0.6221812057707179, + "angularVelocity": -3.2051588732189473, + "velocityX": 0.8574267781913745, + "velocityY": -1.4707786432530845, + "timestamp": 0.5875719764335506 + }, + { + "x": 2.8636245721731495, + "y": 2.9463007656727807, + "heading": 0.4788804629728746, + "angularVelocity": -2.926635344339965, + "velocityX": 0.8583026047225649, + "velocityY": -1.6531619016942172, + "timestamp": 0.6365363078030132 + }, + { + "x": 2.9010385562385563, + "y": 2.8582501044070647, + "heading": 0.35102943849270457, + "angularVelocity": -2.6111052863249458, + "velocityX": 0.7641069125006744, + "velocityY": -1.7982612812846699, + "timestamp": 0.6855006391724757 + }, + { + "x": 2.9381959956467134, + "y": 2.7627981012239995, + "heading": 0.24579862310441236, + "angularVelocity": -2.1491320813567003, + "velocityX": 0.7588674933153383, + "velocityY": -1.949419108019954, + "timestamp": 0.7344649705419383 + }, + { + "x": 2.9765902696547557, + "y": 2.659953900667316, + "heading": 0.16360236786662388, + "angularVelocity": -1.678696572359438, + "velocityX": 0.7841274032359897, + "velocityY": -2.1003901754668526, + "timestamp": 0.7834293019114008 + }, + { + "x": 3.017865715538054, + "y": 2.5502233979386646, + "heading": 0.10367998080393974, + "angularVelocity": -1.2237966982646422, + "velocityX": 0.8429696623825639, + "velocityY": -2.241029330119635, + "timestamp": 0.8323936332808634 + }, + { + "x": 3.063560421246788, + "y": 2.434583512531678, + "heading": 0.0641258476586635, + "angularVelocity": -0.8078152409928518, + "velocityX": 0.9332243376096464, + "velocityY": -2.3617168288158923, + "timestamp": 0.8813579646503259 + }, + { + "x": 3.115401381748359, + "y": 2.3142782546228218, + "heading": 0.04299697728371769, + "angularVelocity": -0.4315155498707209, + "velocityX": 1.0587494825651373, + "velocityY": -2.456997870573845, + "timestamp": 0.9303222960197884 + }, + { + "x": 3.173044544543568, + "y": 2.188964351893496, + "heading": 0.04038455549354976, + "angularVelocity": -0.0533535681403628, + "velocityX": 1.1772480330663169, + "velocityY": -2.559289573134488, + "timestamp": 0.979286627389251 + }, + { + "x": 3.232092719253376, + "y": 2.0632166415863358, + "heading": 0.04038346790054724, + "angularVelocity": -0.000022211944329746854, + "velocityX": 1.2059426333065189, + "velocityY": -2.568149238234756, + "timestamp": 1.0282509587587136 + }, + { + "x": 3.2911425700668717, + "y": 1.9374697182380494, + "heading": 0.04038238058608858, + "angularVelocity": -0.000022206255619953, + "velocityX": 1.2059768644246023, + "velocityY": -2.568133166149746, + "timestamp": 1.0772152901281762 + }, + { + "x": 3.3583393467395415, + "y": 1.8158809793568123, + "heading": 0.0403818264403618, + "angularVelocity": -0.000011317334706426395, + "velocityX": 1.3723617742393555, + "velocityY": -2.483210440755192, + "timestamp": 1.1261796214976387 + }, + { + "x": 3.4341046175380407, + "y": 1.6994385677916184, + "heading": 0.040381131756193525, + "angularVelocity": -0.000014187555488713555, + "velocityX": 1.5473563853405372, + "velocityY": -2.3781068444822635, + "timestamp": 1.1751439528671013 + }, + { + "x": 3.5178049582052053, + "y": 1.5890648238390772, + "heading": 0.039385078873634086, + "angularVelocity": -0.020342417729421827, + "velocityX": 1.7094145539434111, + "velocityY": -2.2541662648206686, + "timestamp": 1.2241082842365638 + }, + { + "x": 3.608774844587042, + "y": 1.4855271513306618, + "heading": 0.036685479739839225, + "angularVelocity": -0.05513399362946303, + "velocityX": 1.8578807029022373, + "velocityY": -2.1145529738200697, + "timestamp": 1.2730726156060264 + }, + { + "x": 3.7066552250984595, + "y": 1.3891998671498478, + "heading": 0.032788252293559854, + "angularVelocity": -0.07959319237656216, + "velocityX": 1.9990139306283103, + "velocityY": -1.9672949979443013, + "timestamp": 1.322036946975489 + }, + { + "x": 3.8111775959241823, + "y": 1.300340998111743, + "heading": 0.028541553024262294, + "angularVelocity": -0.0867304658416327, + "velocityX": 2.1346634969251537, + "velocityY": -1.8147673327263893, + "timestamp": 1.3710012783449514 }, { "x": 3.922098875045776, "y": 1.2191578149795532, - "heading": 1.5337761150741231, - "angularVelocity": -0.000001532163640308562, - "velocityX": 2.2808509741270218, - "velocityY": -1.6874391961368493, - "timestamp": 1.2670015804896586 - }, - { - "x": 4.133215899282255, - "y": 1.100654492548557, - "heading": 1.5337761164554422, - "angularVelocity": 1.6187741152567537e-8, - "velocityX": 2.474089843952036, - "velocityY": -1.3887457326638517, - "timestamp": 1.3523327679372603 - }, - { - "x": 4.357502974678636, - "y": 1.0095021810718636, - "heading": 1.533776117552903, - "angularVelocity": 1.2861193573885216e-8, - "velocityX": 2.628430262195824, - "velocityY": -1.0682180127009988, - "timestamp": 1.437663955384862 - }, - { - "x": 4.59143382498898, - "y": 0.9471339698744419, - "heading": 1.5337761185021443, - "angularVelocity": 1.1124200677480839e-8, - "velocityX": 2.7414460914890086, - "velocityY": -0.730895854879782, - "timestamp": 1.5229951428324637 - }, - { - "x": 4.831330550647783, - "y": 0.9145303241420727, - "heading": 1.5337761193835278, - "angularVelocity": 1.0328972095127946e-8, - "velocityX": 2.811360451372056, - "velocityY": -0.38208358171963364, - "timestamp": 1.6083263302800654 - }, - { - "x": 5.0730890918673595, - "y": 0.9016367408729034, - "heading": 1.5337761202500422, - "angularVelocity": 1.0154720062447253e-8, - "velocityX": 2.8331791511518682, - "velocityY": -0.1511004786741872, - "timestamp": 1.6936575177276671 - }, - { - "x": 5.314847643606767, - "y": 0.8887433548552026, - "heading": 1.5337761211165564, - "angularVelocity": 1.0154719461608446e-8, - "velocityX": 2.833179274434229, - "velocityY": -0.1510981670754085, - "timestamp": 1.7789887051752689 - }, - { - "x": 5.556606195346236, - "y": 0.8758499688386339, - "heading": 1.5337761219830706, - "angularVelocity": 1.0154719791329154e-8, - "velocityX": 2.8331792744349373, - "velocityY": -0.15109816706214324, - "timestamp": 1.8643198926228706 - }, - { - "x": 5.798364747085703, - "y": 0.862956582822065, - "heading": 1.533776122849585, - "angularVelocity": 1.0154719990167722e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 1.9496510800704723 - }, - { - "x": 6.040123298825171, - "y": 0.8500631968054962, - "heading": 1.5337761237160994, - "angularVelocity": 1.0154719824488932e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.0349822675180738 - }, - { - "x": 6.281881850564639, - "y": 0.8371698107889274, - "heading": 1.5337761245826136, - "angularVelocity": 1.0154719065955154e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.1203134549656752 - }, - { - "x": 6.523640402304108, - "y": 0.8242764247723586, - "heading": 1.533776125449128, - "angularVelocity": 1.015472011167056e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.2056446424132767 - }, - { - "x": 6.765398954043575, - "y": 0.8113830387557898, - "heading": 1.5337761263156422, - "angularVelocity": 1.0154720167050709e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.1510981670621432, - "timestamp": 2.290975829860878 - }, - { - "x": 7.007157505783043, - "y": 0.798489652739221, - "heading": 1.5337761271821566, - "angularVelocity": 1.0154720142992157e-8, - "velocityX": 2.8331792744349364, - "velocityY": -0.15109816706214316, - "timestamp": 2.3763070173084797 - }, - { - "x": 7.248916057522512, - "y": 0.7855962667226521, - "heading": 1.533776128048671, - "angularVelocity": 1.0154720015095924e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.461638204756081 - }, - { - "x": 7.490674609261979, - "y": 0.7727028807060851, - "heading": 1.5337761289151852, - "angularVelocity": 1.015471927674143e-8, - "velocityX": 2.8331792744349378, - "velocityY": -0.15109816706212442, - "timestamp": 2.5469693922036827 - }, - { - "x": 7.7324331610160755, - "y": 0.7598094949690721, - "heading": 1.5337761297818946, - "angularVelocity": 1.0157005131011138e-8, - "velocityX": 2.833179274606365, - "velocityY": -0.15109816378601534, - "timestamp": 2.632300579651284 - }, - { - "x": 7.944587343206802, - "y": 0.7484949682004244, - "heading": 1.546215447760533, - "angularVelocity": 0.1457769234288085, - "velocityX": 2.486244344378793, - "velocityY": -0.1325954449607942, - "timestamp": 2.7176317670988857 - }, - { - "x": 8.12643379054156, - "y": 0.738796802526807, - "heading": 1.5568779338247372, - "angularVelocity": 0.12495415079922173, - "velocityX": 2.131066644846836, - "velocityY": -0.11365323703684094, - "timestamp": 2.802962954546487 - }, - { - "x": 8.277972498710973, - "y": 0.7307149979607698, - "heading": 1.5657634145409067, - "angularVelocity": 0.10412934569351617, - "velocityX": 1.7758888948131386, - "velocityY": -0.09471102896581571, - "timestamp": 2.8882941419940886 - }, - { - "x": 8.399203466259046, - "y": 0.7242495544154388, - "heading": 1.5728718351258109, - "angularVelocity": 0.08330389858068031, - "velocityX": 1.420711127716517, - "velocityY": -0.07576882191286877, - "timestamp": 2.97362532944169 - }, - { - "x": 8.490126692448452, - "y": 0.719400471808881, - "heading": 1.5782031697100374, - "angularVelocity": 0.06247814830304947, - "velocityX": 1.0655333519791725, - "velocityY": -0.05682661582010127, - "timestamp": 3.0589565168892916 - }, - { - "x": 8.550742176835362, - "y": 0.7161677500844638, - "heading": 1.5817574030517292, - "angularVelocity": 0.04165221940541114, - "velocityX": 0.7103555710405419, - "velocityY": -0.03788441039101151, - "timestamp": 3.144287704336893 + "heading": 0.02492521296459199, + "angularVelocity": -0.07385662090191833, + "velocityX": 2.2653485919093206, + "velocityY": -1.6580065705303566, + "timestamp": 1.419965609714414 + }, + { + "x": 4.133754076857049, + "y": 1.1017463293287075, + "heading": 0.024925206123710186, + "angularVelocity": -8.018916008040494e-8, + "velocityX": 2.481032906181765, + "velocityY": -1.3763033318839852, + "timestamp": 1.505274918302749 + }, + { + "x": 4.358435567651369, + "y": 1.011737381091072, + "heading": 0.024925205866123235, + "angularVelocity": -3.0194471762044385e-9, + "velocityX": 2.6337277198966884, + "velocityY": -1.0550894120123064, + "timestamp": 1.590584226891084 + }, + { + "x": 4.5926126795284254, + "y": 0.9505453575452277, + "heading": 0.024925205642497325, + "angularVelocity": -2.6213541510789247e-9, + "velocityX": 2.745035867153646, + "velocityY": -0.7172959734217197, + "timestamp": 1.6758935354794189 + }, + { + "x": 4.832605528624095, + "y": 0.9191317291658319, + "heading": 0.024925205434181426, + "angularVelocity": -2.44188946073925e-9, + "velocityX": 2.813208230930211, + "velocityY": -0.3682321296375535, + "timestamp": 1.7612028440677538 + }, + { + "x": 5.074285887787392, + "y": 0.9059412894376224, + "heading": 0.024925205229021138, + "angularVelocity": -2.404899218132483e-9, + "velocityX": 2.8329893086994424, + "velocityY": -0.154618996994628, + "timestamp": 1.8465121526560888 + }, + { + "x": 5.315966255946622, + "y": 0.892751014537456, + "heading": 0.024925205023860926, + "angularVelocity": -2.404898293722967e-9, + "velocityX": 2.832989414150284, + "velocityY": -0.15461706487131047, + "timestamp": 1.9318214612444238 + }, + { + "x": 5.55764662410591, + "y": 0.8795607396382881, + "heading": 0.024925204818700676, + "angularVelocity": -2.4048987883106987e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.1546170648602097, + "timestamp": 2.0171307698327587 + }, + { + "x": 5.799326992265192, + "y": 0.8663704647390634, + "heading": 0.024925204613540374, + "angularVelocity": -2.4048993550096436e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020768, + "timestamp": 2.1024400784210937 + }, + { + "x": 6.04100736042448, + "y": 0.8531801898398956, + "heading": 0.02492520440838013, + "angularVelocity": -2.4048986846119357e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020765, + "timestamp": 2.1877493870094287 + }, + { + "x": 6.282687728583764, + "y": 0.8399899149407278, + "heading": 0.024925204203219874, + "angularVelocity": -2.4048988174777923e-9, + "velocityX": 2.832989414150889, + "velocityY": -0.15461706486020768, + "timestamp": 2.2730586955977636 + }, + { + "x": 6.52436809674305, + "y": 0.8267996400415316, + "heading": 0.024925203998059656, + "angularVelocity": -2.4048983926523396e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020763, + "timestamp": 2.3583680041860986 + }, + { + "x": 6.766048464902335, + "y": 0.8136093651422786, + "heading": 0.02492520379289938, + "angularVelocity": -2.4048990449979503e-9, + "velocityX": 2.832989414150889, + "velocityY": -0.15461706486020768, + "timestamp": 2.4436773127744336 + }, + { + "x": 7.007728833061619, + "y": 0.8004190902431392, + "heading": 0.024925203587739076, + "angularVelocity": -2.404899369928152e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020763, + "timestamp": 2.5289866213627685 + }, + { + "x": 7.249409201220906, + "y": 0.7872288153439714, + "heading": 0.024925203382578868, + "angularVelocity": -2.4048982983592985e-9, + "velocityX": 2.832989414150889, + "velocityY": -0.15461706486020768, + "timestamp": 2.6142959299511035 + }, + { + "x": 7.491089569380192, + "y": 0.7740385404447467, + "heading": 0.024925203177418673, + "angularVelocity": -2.4048980880486034e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020768, + "timestamp": 2.6996052385394385 + }, + { + "x": 7.732769937539386, + "y": 0.7608482655471763, + "heading": 0.024925202972228687, + "angularVelocity": -2.405247335081803e-9, + "velocityX": 2.8329894141498406, + "velocityY": -0.15461706484157828, + "timestamp": 2.7849145471277734 + }, + { + "x": 7.944839923818755, + "y": 0.7492740469626908, + "heading": 0.02198121203200045, + "angularVelocity": -0.03450960966563021, + "velocityX": 2.485895030549647, + "velocityY": -0.13567357157148477, + "timestamp": 2.8702238557161084 + }, + { + "x": 8.126614204583387, + "y": 0.7393532878041089, + "heading": 0.019457756648722437, + "angularVelocity": -0.029580070745328552, + "velocityX": 2.1307672488800957, + "velocityY": -0.11629163713479268, + "timestamp": 2.9555331643044433 + }, + { + "x": 8.278092774424247, + "y": 0.7310859883666867, + "heading": 0.017354864228594055, + "angularVelocity": -0.02465021056818093, + "velocityX": 1.775639403805618, + "velocityY": -0.09690969923711115, + "timestamp": 3.0408424728927783 + }, + { + "x": 8.399275631538378, + "y": 0.7244721487488996, + "heading": 0.015672543952851302, + "angularVelocity": -0.019720242768123807, + "velocityX": 1.4205115375967707, + "velocityY": -0.07752776018500543, + "timestamp": 3.1261517814811133 + }, + { + "x": 8.49016277502432, + "y": 0.7195117690000139, + "heading": 0.01441080042435244, + "angularVelocity": -0.014790221013130912, + "velocityX": 1.0653836608209173, + "velocityY": -0.05814582055535578, + "timestamp": 3.2114610900694482 + }, + { + "x": 8.550754204341196, + "y": 0.7162048491495833, + "heading": 0.013569636407469346, + "angularVelocity": -0.009860166854032092, + "velocityX": 0.7102557777048953, + "velocityY": -0.03876388057911283, + "timestamp": 3.296770398657783 }, { "x": 8.581049919128418, "y": 0.7145513892173767, - "heading": 1.5835345242998375, - "angularVelocity": 0.020826163343848954, - "velocityX": 0.35517778668750655, - "velocityY": -0.018942205252676, - "timestamp": 3.2296188917844946 + "heading": 0.013149053743484958, + "angularVelocity": -0.004930091111322124, + "velocityX": 0.3551278903620608, + "velocityY": -0.0193819403719191, + "timestamp": 3.382079707246118 }, { "x": 8.581049919128418, "y": 0.7145513892173767, - "heading": 1.5835345242998375, - "angularVelocity": -1.33415825335105e-27, - "velocityX": -1.0225334762057861e-26, - "velocityY": 2.2811900613103305e-26, - "timestamp": 3.314950079232096 + "heading": 0.013149053743484958, + "angularVelocity": 2.9756992123058065e-23, + "velocityX": -2.4082246897494348e-21, + "velocityY": -1.7635906049154447e-20, + "timestamp": 3.467389015834453 } ], "trajectoryWaypoints": [ @@ -708,25 +780,25 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 29 }, { - "timestamp": 1.2670015804896586, + "timestamp": 1.419965609714414, "isStopPoint": false, "x": 3.9220988750457764, "y": 1.2191578149795532, - "heading": 1.5337761150741231, + "heading": 0.02492521296459199, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 24 }, { - "timestamp": 3.314950079232096, + "timestamp": 3.467389015834453, "isStopPoint": true, "x": 8.581049919128418, "y": 0.7145513892173767, - "heading": 1.5835345242998375, + "heading": 0.013149053743484958, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -759,11 +831,11 @@ { "x": 8.59085464477539, "y": 0.6655267477035522, - "heading": 1.584, + "heading": 0.003579741058468549, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 27 + "controlIntervalCount": 35 }, { "x": 3.9970710277557373, @@ -788,398 +860,470 @@ { "x": 8.59085464477539, "y": 0.6655267477035522, - "heading": 1.584, - "angularVelocity": -1.194765989428476e-25, - "velocityX": 1.0490028843079857e-24, - "velocityY": 4.196119292892119e-25, + "heading": 0.003579741058468549, + "angularVelocity": -3.0917568952683027e-22, + "velocityX": 1.6244082062603867e-21, + "velocityY": -5.90354802461058e-22, "timestamp": 0 }, { - "x": 8.569338797659704, - "y": 0.6716822291946352, - "heading": 1.601438244624625, - "angularVelocity": 0.23422359204075424, - "velocityX": -0.28899233298513016, - "velocityY": 0.08267798833065461, - "timestamp": 0.07445127313046465 - }, - { - "x": 8.526241242230943, - "y": 0.6840212818502882, - "heading": 1.635395030064886, - "angularVelocity": 0.45609408694404635, - "velocityX": -0.5788692874766389, - "velocityY": 0.16573326602529256, - "timestamp": 0.1489025462609293 - }, - { - "x": 8.46148078388417, - "y": 0.702580492867376, - "heading": 1.6846695662056992, - "angularVelocity": 0.6618360448244704, - "velocityX": -0.8698368157289821, - "velocityY": 0.24927996845084846, - "timestamp": 0.22335381939139395 - }, - { - "x": 8.374959318473937, - "y": 0.7274054321169395, - "heading": 1.747704005496551, - "angularVelocity": 0.8466536116903378, - "velocityX": -1.1621220399900463, - "velocityY": 0.3334387473275515, - "timestamp": 0.2978050925218586 - }, - { - "x": 8.266556813740802, - "y": 0.7585470442654593, - "heading": 1.8223949877693462, - "angularVelocity": 1.0032196782170628, - "velocityX": -1.4560194899981764, - "velocityY": 0.41828179477802546, - "timestamp": 0.37225636565232323 - }, - { - "x": 8.13612233451273, - "y": 0.7960531508424938, - "heading": 1.905548533586374, - "angularVelocity": 1.116885478523836, - "velocityX": -1.7519442414303088, - "velocityY": 0.5037671620646551, - "timestamp": 0.44670763878278785 - }, - { - "x": 7.983495243498111, - "y": 0.8399498616480997, - "heading": 1.9911506980893818, - "angularVelocity": 1.1497743544694357, - "velocityX": -2.050026609312157, - "velocityY": 0.5896032258398517, - "timestamp": 0.5211589119132525 - }, - { - "x": 7.809039943700515, - "y": 0.8900668498836193, - "heading": 2.063896942952919, - "angularVelocity": 0.9770987359216822, - "velocityX": -2.343214460441654, - "velocityY": 0.673151527546038, - "timestamp": 0.5956101850437171 - }, - { - "x": 7.618770984995591, - "y": 0.9491126693527949, - "heading": 2.0933181978593507, - "angularVelocity": 0.39517463797933094, - "velocityX": -2.5556172608560517, - "velocityY": 0.7930800507025142, - "timestamp": 0.6700614581741817 - }, - { - "x": 7.415983670640095, - "y": 1.0082473892448494, - "heading": 2.093318235466261, - "angularVelocity": 5.051211193561299e-7, - "velocityX": -2.723758853661807, - "velocityY": 0.794274125956581, - "timestamp": 0.7445127313046463 - }, - { - "x": 7.213196316699656, - "y": 1.0673819733908265, - "heading": 2.0933182730728315, - "angularVelocity": 5.051165512148015e-7, - "velocityX": -2.723759385351071, - "velocityY": 0.7942723026690597, - "timestamp": 0.818964004435111 - }, - { - "x": 7.010408962758315, - "y": 1.1265165575337097, - "heading": 2.0933183106794018, - "angularVelocity": 5.051165509169277e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274991, - "timestamp": 0.8934152775655756 - }, - { - "x": 6.807621608816973, - "y": 1.1856511416765925, - "heading": 2.093318348285972, - "angularVelocity": 5.051165516307808e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274983, - "timestamp": 0.9678665506960402 - }, - { - "x": 6.604834254875632, - "y": 1.2447857258194757, - "heading": 2.0933183858925424, - "angularVelocity": 5.051165517874018e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274995, - "timestamp": 1.042317823826505 - }, - { - "x": 6.40204690093429, - "y": 1.3039203099623589, - "heading": 2.0933184234991127, - "angularVelocity": 5.051165514872098e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274996, - "timestamp": 1.1167690969569697 - }, - { - "x": 6.199259546992949, - "y": 1.3630548941052418, - "heading": 2.093318461105683, - "angularVelocity": 5.051165527045066e-7, - "velocityX": -2.7237593853631896, - "velocityY": 0.7942723026275, - "timestamp": 1.1912203700874344 - }, - { - "x": 5.996472193051607, - "y": 1.422189478248125, - "heading": 2.0933184987122533, - "angularVelocity": 5.05116552107724e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275003, - "timestamp": 1.2656716432178992 - }, - { - "x": 5.793684839110266, - "y": 1.481324062391008, - "heading": 2.0933185363188236, - "angularVelocity": 5.051165528043117e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275005, - "timestamp": 1.340122916348364 - }, - { - "x": 5.590897485168925, - "y": 1.5404586465338912, - "heading": 2.093318573925394, - "angularVelocity": 5.051165529619199e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275014, - "timestamp": 1.4145741894788286 - }, - { - "x": 5.388110131227584, - "y": 1.5995932306767744, - "heading": 2.093318611531964, - "angularVelocity": 5.051165529309538e-7, - "velocityX": -2.7237593853631896, - "velocityY": 0.7942723026275016, - "timestamp": 1.4890254626092934 - }, - { - "x": 5.185322777286242, - "y": 1.6587278148196576, - "heading": 2.0933186491385345, - "angularVelocity": 5.051165538065676e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275019, - "timestamp": 1.563476735739758 - }, - { - "x": 4.982535423344901, - "y": 1.7178623989625408, - "heading": 2.093318686745105, - "angularVelocity": 5.051165537812963e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275026, - "timestamp": 1.6379280088702228 - }, - { - "x": 4.779748069403955, - "y": 1.7769969831067813, - "heading": 2.093318724351675, - "angularVelocity": 5.051165541155275e-7, - "velocityX": -2.7237593853578734, - "velocityY": 0.7942723026457309, - "timestamp": 1.7123792820006876 - }, - { - "x": 4.5769607328234585, - "y": 1.8361316267843304, - "heading": 2.093318761958246, - "angularVelocity": 5.051165582796277e-7, - "velocityX": -2.7237591521792095, - "velocityY": 0.7942731022735431, - "timestamp": 1.7868305551311523 - }, - { - "x": 4.374941076875769, - "y": 1.897837935676309, - "heading": 2.0933187996353646, - "angularVelocity": 5.060641290697099e-7, - "velocityX": -2.7134479701063015, - "velocityY": 0.8288146904331337, - "timestamp": 1.861281828261617 - }, - { - "x": 4.18087007463253, - "y": 1.9812409077974285, - "heading": 2.093318838788321, - "angularVelocity": 5.258869968320627e-7, - "velocityX": -2.6066848031350367, - "velocityY": 1.1202356738073298, - "timestamp": 1.9357331013920818 + "x": 8.581278259975488, + "y": 0.6665691805773499, + "heading": 0.049210209005701194, + "angularVelocity": 0.7412190640469178, + "velocityX": -0.1555583209566295, + "velocityY": 0.01693322803398738, + "timestamp": 0.0615613793014157 + }, + { + "x": 8.562140843080153, + "y": 0.6681624484519715, + "heading": 0.14036624615251878, + "angularVelocity": 1.4807341580262696, + "velocityX": -0.3108672533413248, + "velocityY": 0.02588096453818382, + "timestamp": 0.1231227586028314 + }, + { + "x": 8.53345789975939, + "y": 0.6696003342516827, + "heading": 0.27652291893725567, + "angularVelocity": 2.2117222571977972, + "velocityX": -0.4659243123895012, + "velocityY": 0.023356945799909313, + "timestamp": 0.1846841379042471 + }, + { + "x": 8.494774393681595, + "y": 0.6700913705803325, + "heading": 0.45437781249309583, + "angularVelocity": 2.8890660926395153, + "velocityX": -0.6283729590981868, + "velocityY": 0.007976369831580163, + "timestamp": 0.2462455172056628 + }, + { + "x": 8.445611037120612, + "y": 0.676397849256214, + "heading": 0.6654299357693332, + "angularVelocity": 3.4283202499880296, + "velocityX": -0.7986071319206547, + "velocityY": 0.10244212763661113, + "timestamp": 0.3078068965070785 + }, + { + "x": 8.384281821724839, + "y": 0.6873224721600781, + "heading": 0.905533699151053, + "angularVelocity": 3.9002336547095253, + "velocityX": -0.9962287410016354, + "velocityY": 0.17745903402155838, + "timestamp": 0.3693682758084942 + }, + { + "x": 8.308431678971848, + "y": 0.7019055174308684, + "heading": 1.16123585616331, + "angularVelocity": 4.153613189208983, + "velocityX": -1.2321059666583078, + "velocityY": 0.23688626597186843, + "timestamp": 0.4309296551099099 + }, + { + "x": 8.217894206979377, + "y": 0.7176144971385626, + "heading": 1.398998972446434, + "angularVelocity": 3.8622122990291166, + "velocityX": -1.4706862162587828, + "velocityY": 0.2551758892662216, + "timestamp": 0.4924910344113256 + }, + { + "x": 8.116714631429112, + "y": 0.7341860694138748, + "heading": 1.595116572105417, + "angularVelocity": 3.185724587143436, + "velocityX": -1.6435560199987134, + "velocityY": 0.26918780026312733, + "timestamp": 0.5540524137127413 + }, + { + "x": 8.00449036583329, + "y": 0.7525893238791683, + "heading": 1.7539844890971241, + "angularVelocity": 2.5806425845961125, + "velocityX": -1.8229654187302906, + "velocityY": 0.2989415551459339, + "timestamp": 0.615613793014157 + }, + { + "x": 7.883611717181969, + "y": 0.7794643101857551, + "heading": 1.8813239739827363, + "angularVelocity": 2.0684962931407838, + "velocityX": -1.9635467889612548, + "velocityY": 0.43655594808884984, + "timestamp": 0.6771751723155727 + }, + { + "x": 7.75310035066067, + "y": 0.8143312926954029, + "heading": 1.9786852945004982, + "angularVelocity": 1.5815324741354986, + "velocityX": -2.120020181521479, + "velocityY": 0.5663775390563021, + "timestamp": 0.7387365516169884 + }, + { + "x": 7.6123058950143605, + "y": 0.8564531019763303, + "heading": 2.0464927345736803, + "angularVelocity": 1.1014607021909668, + "velocityX": -2.2870581725752865, + "velocityY": 0.6842245862408529, + "timestamp": 0.8002979309184041 + }, + { + "x": 7.460836017732183, + "y": 0.9050424052451691, + "heading": 2.0846731899989006, + "angularVelocity": 0.6202014291830935, + "velocityX": -2.4604691935271816, + "velocityY": 0.7892822386408368, + "timestamp": 0.8618593102198198 + }, + { + "x": 7.298037440831554, + "y": 0.9578569112638236, + "heading": 2.093276903536618, + "angularVelocity": 0.13975829709064802, + "velocityX": -2.6444920297113224, + "velocityY": 0.8579162230928108, + "timestamp": 0.9234206895212355 + }, + { + "x": 7.131708118089986, + "y": 1.0111620444693228, + "heading": 2.0932771548444187, + "angularVelocity": 0.000004082231488927161, + "velocityX": -2.701845290489505, + "velocityY": 0.8658859468451401, + "timestamp": 0.9849820688226512 + }, + { + "x": 6.965378792094487, + "y": 1.0644671675235107, + "heading": 2.093277406150717, + "angularVelocity": 0.000004082207080043569, + "velocityX": -2.7018453433462057, + "velocityY": 0.8658857819477386, + "timestamp": 1.046543448124067 + }, + { + "x": 6.799049466098943, + "y": 1.1177722905775571, + "heading": 2.0932776574570204, + "angularVelocity": 0.000004082207158569437, + "velocityX": -2.7018453433469287, + "velocityY": 0.8658857819454437, + "timestamp": 1.1081048274254828 + }, + { + "x": 6.632720140103406, + "y": 1.1710774136316247, + "heading": 2.0932779087633286, + "angularVelocity": 0.0000040822072370843174, + "velocityX": -2.7018453433468057, + "velocityY": 0.8658857819457861, + "timestamp": 1.1696662067268986 + }, + { + "x": 6.466390814107878, + "y": 1.2243825366857128, + "heading": 2.0932781600696417, + "angularVelocity": 0.000004082207315896948, + "velocityX": -2.7018453433466827, + "velocityY": 0.8658857819461279, + "timestamp": 1.2312275860283144 + }, + { + "x": 6.300061488112355, + "y": 1.2776876597398226, + "heading": 2.0932784113759593, + "angularVelocity": 0.00000408220739475046, + "velocityX": -2.7018453433465606, + "velocityY": 0.8658857819464707, + "timestamp": 1.2927889653297302 + }, + { + "x": 6.133732162116841, + "y": 1.3309927827939534, + "heading": 2.093278662682282, + "angularVelocity": 0.000004082207473083141, + "velocityX": -2.7018453433464376, + "velocityY": 0.8658857819468134, + "timestamp": 1.354350344631146 + }, + { + "x": 5.967402836121334, + "y": 1.384297905848105, + "heading": 2.09327891398861, + "angularVelocity": 0.0000040822075518289365, + "velocityX": -2.701845343346315, + "velocityY": 0.8658857819471559, + "timestamp": 1.4159117239325618 + }, + { + "x": 5.801073510125836, + "y": 1.4376030289022779, + "heading": 2.093279165294942, + "angularVelocity": 0.000004082207630794512, + "velocityX": -2.701845343346192, + "velocityY": 0.8658857819474983, + "timestamp": 1.4774731032339776 + }, + { + "x": 5.634744184130344, + "y": 1.4909081519564717, + "heading": 2.093279416601279, + "angularVelocity": 0.000004082207709261434, + "velocityX": -2.701845343346069, + "velocityY": 0.8658857819478408, + "timestamp": 1.5390344825353934 + }, + { + "x": 5.468414858134862, + "y": 1.544213275010687, + "heading": 2.093279667907621, + "angularVelocity": 0.00000408220778743359, + "velocityX": -2.701845343345947, + "velocityY": 0.8658857819481832, + "timestamp": 1.6005958618368092 + }, + { + "x": 5.302085532139386, + "y": 1.597518398064923, + "heading": 2.0932799192139675, + "angularVelocity": 0.00000408220786728094, + "velocityX": -2.7018453433458234, + "velocityY": 0.8658857819485258, + "timestamp": 1.662157241138225 + }, + { + "x": 5.135756206143918, + "y": 1.65082352111918, + "heading": 2.0932801705203192, + "angularVelocity": 0.000004082207945695806, + "velocityX": -2.701845343345701, + "velocityY": 0.8658857819488687, + "timestamp": 1.7237186204396409 + }, + { + "x": 4.969426880148456, + "y": 1.7041286441734584, + "heading": 2.093280421826676, + "angularVelocity": 0.000004082208024460346, + "velocityX": -2.701845343345578, + "velocityY": 0.8658857819492114, + "timestamp": 1.7852799997410567 + }, + { + "x": 4.803097554153003, + "y": 1.7574337672277578, + "heading": 2.0932806731330373, + "angularVelocity": 0.000004082208102945811, + "velocityX": -2.7018453433454552, + "velocityY": 0.8658857819495545, + "timestamp": 1.8468413790424725 + }, + { + "x": 4.6367682281581555, + "y": 1.810738890283948, + "heading": 2.0932809244394037, + "angularVelocity": 0.0000040822081820467654, + "velocityX": -2.7018453433355996, + "velocityY": 0.8658857819802678, + "timestamp": 1.9084027583438883 + }, + { + "x": 4.470438939607256, + "y": 1.8640441301771067, + "heading": 2.0932811757459198, + "angularVelocity": 0.000004082210616647047, + "velocityX": -2.701844735097984, + "velocityY": 0.8658876798742076, + "timestamp": 1.969964137645304 + }, + { + "x": 4.306176669660201, + "y": 1.9234140327969524, + "heading": 2.093281442887588, + "angularVelocity": 0.000004339436045796883, + "velocityX": -2.668268187150199, + "velocityY": 0.9644017611944702, + "timestamp": 2.03152551694672 + }, + { + "x": 4.147951507665768, + "y": 1.9973846826411954, + "heading": 2.093281853657193, + "angularVelocity": 0.000006672521138398165, + "velocityX": -2.570201704216772, + "velocityY": 1.2015755768250211, + "timestamp": 2.0930868962481357 }, { "x": 3.9970710277557377, "y": 2.0853443145751953, "heading": 2.0933188819049127, - "angularVelocity": 5.791249741848488e-7, - "velocityX": -2.468715968828528, - "velocityY": 1.3982757097429517, - "timestamp": 2.0101843745225465 - }, - { - "x": 3.8417885319373366, - "y": 2.194746303912347, - "heading": 2.093318907323053, - "angularVelocity": 3.796579913258463e-7, - "velocityX": -2.3193766038751393, - "velocityY": 1.634082535501843, - "timestamp": 2.0771344746308946 - }, - { - "x": 3.6980071143049322, - "y": 2.31887669174007, - "heading": 2.093318931169399, - "angularVelocity": 3.5618088315780296e-7, - "velocityX": -2.147590778799685, - "velocityY": 1.854073222098814, - "timestamp": 2.1440845747392427 - }, - { - "x": 3.5671177985502105, - "y": 2.456533643810284, - "heading": 2.093318954469134, - "angularVelocity": 3.480164309169473e-7, - "velocityX": -1.9550279318910289, - "velocityY": 2.0561127145058484, - "timestamp": 2.211034674847591 - }, - { - "x": 3.440627773307266, - "y": 2.598243639821922, - "heading": 2.093318977734502, - "angularVelocity": 3.4750310659854874e-7, - "velocityX": -1.8893179403502105, - "velocityY": 2.116650995029174, - "timestamp": 2.277984774955939 - }, - { - "x": 3.314137927462374, - "y": 2.7399537959635722, - "heading": 2.093319001000051, - "angularVelocity": 3.475058166249576e-7, - "velocityX": -1.8893152607716497, - "velocityY": 2.116653386810687, - "timestamp": 2.344934875064287 - }, - { - "x": 3.1930707757983563, - "y": 2.8770243686628967, - "heading": 2.114175460284759, - "angularVelocity": 0.3115224510636341, - "velocityX": -1.8083192029300867, - "velocityY": 2.047354260523847, - "timestamp": 2.4118849751726352 - }, - { - "x": 3.082167243806988, - "y": 3.0018001495332065, - "heading": 2.1671051603213085, - "angularVelocity": 0.7905843299844287, - "velocityX": -1.6565103235378043, - "velocityY": 1.8637131336380355, - "timestamp": 2.4788350752809833 - }, - { - "x": 2.9836145948825417, - "y": 3.1125735219481654, - "heading": 2.2223290697280427, - "angularVelocity": 0.8248517824075432, - "velocityX": -1.4720313900196498, - "velocityY": 1.6545661953557886, - "timestamp": 2.5457851753893315 - }, - { - "x": 2.897439709795232, - "y": 3.2093792657888156, - "heading": 2.2747244573880905, - "angularVelocity": 0.7826035745316955, - "velocityX": -1.2871509519455429, - "velocityY": 1.4459387466782787, - "timestamp": 2.6127352754976796 - }, - { - "x": 2.8236207245224336, - "y": 3.2922706416025598, - "heading": 2.3220108807788065, - "angularVelocity": 0.7062935427160024, - "velocityX": -1.1025970857897722, - "velocityY": 1.2381068240315942, - "timestamp": 2.6796853756060277 - }, - { - "x": 2.762136351273778, - "y": 3.3612886907695043, - "heading": 2.3628840100451693, - "angularVelocity": 0.6105013913379634, - "velocityX": -0.9183611846607055, - "velocityY": 1.0308879158544917, - "timestamp": 2.746635475714376 - }, - { - "x": 2.712969884575379, - "y": 3.4164643641886956, - "heading": 2.3965008699236137, - "angularVelocity": 0.5021181420795616, - "velocityX": -0.7343748047998556, - "velocityY": 0.8241313057022762, - "timestamp": 2.813585575822724 - }, - { - "x": 2.676108490863902, - "y": 3.4578214702193035, - "heading": 2.42227370118923, - "angularVelocity": 0.3849558286530869, - "velocityX": -0.5505801134251128, - "velocityY": 0.6177303090462575, - "timestamp": 2.880535675931072 - }, - { - "x": 2.651542090312647, - "y": 3.4853787426726646, - "heading": 2.4397690611795695, - "angularVelocity": 0.2613194000012874, - "velocityX": -0.36693597935624045, - "velocityY": 0.4116091299156203, - "timestamp": 2.94748577603942 + "angularVelocity": 0.0006014850242100183, + "velocityX": -2.450895051770885, + "velocityY": 1.4288119098718277, + "timestamp": 2.1546482755495515 + }, + { + "x": 3.843205910355169, + "y": 2.1966367718038953, + "heading": 2.093318891906276, + "angularVelocity": 1.4942876205203166e-7, + "velocityX": -2.2988740829855514, + "velocityY": 1.662802783874456, + "timestamp": 2.2215789171794507 + }, + { + "x": 3.701017218385049, + "y": 2.3225052008470026, + "heading": 2.0933189013422897, + "angularVelocity": 1.409819682458666e-7, + "velocityX": -2.124418480198765, + "velocityY": 1.8805800449233931, + "timestamp": 2.28850955880935 + }, + { + "x": 3.5718797746002595, + "y": 2.4617316574789556, + "heading": 2.093318910626833, + "angularVelocity": 1.387188756626878e-7, + "velocityX": -1.9294218707609325, + "velocityY": 2.0801601963092122, + "timestamp": 2.355440200439249 + }, + { + "x": 3.4447158253880024, + "y": 2.6027629151415783, + "heading": 2.0933189199075177, + "angularVelocity": 1.3866122154045785e-7, + "velocityX": -1.8999362043385877, + "velocityY": 2.107125439532927, + "timestamp": 2.422370842069148 + }, + { + "x": 3.317551912654746, + "y": 2.7437942056962745, + "heading": 2.0933189291882313, + "angularVelocity": 1.3866165525269637e-7, + "velocityX": -1.8999356593116823, + "velocityY": 2.1071259309681434, + "timestamp": 2.4893014836990472 + }, + { + "x": 3.1958691275778026, + "y": 2.880185747480273, + "heading": 2.114236602845924, + "angularVelocity": 0.3125276128885705, + "velocityX": -1.8180430086088528, + "velocityY": 2.0378041874780046, + "timestamp": 2.5562321253289464 + }, + { + "x": 3.084402838552189, + "y": 3.004327638614834, + "heading": 2.167146744456709, + "angularVelocity": 0.7905219541052365, + "velocityX": -1.6653999769190886, + "velocityY": 1.8547841184762293, + "timestamp": 2.6231627669588455 + }, + { + "x": 2.9853512622212666, + "y": 3.1145382871506517, + "heading": 2.222354458529675, + "angularVelocity": 0.8248496163871107, + "velocityX": -1.4799137423280575, + "velocityY": 1.6466396534077914, + "timestamp": 2.6900934085887447 + }, + { + "x": 2.8987408213697994, + "y": 3.210852203380145, + "heading": 2.2747381958649533, + "angularVelocity": 0.7826570321100548, + "velocityX": -1.294032729140559, + "velocityY": 1.439010801092748, + "timestamp": 2.757024050218644 + }, + { + "x": 2.8245492222018562, + "y": 3.2933223707153383, + "heading": 2.322017090666629, + "angularVelocity": 0.7063863971767974, + "velocityX": -1.1084848039884865, + "velocityY": 1.2321735654533488, + "timestamp": 2.823954691848543 + }, + { + "x": 2.762754840052126, + "y": 3.361989636833625, + "heading": 2.36288597520057, + "angularVelocity": 0.6106154601046585, + "velocityX": -0.9232599694984354, + "velocityY": 1.0259466284215513, + "timestamp": 2.890885333478442 + }, + { + "x": 2.713340706240203, + "y": 3.416884822801194, + "heading": 2.39650092018746, + "angularVelocity": 0.5022355107959039, + "velocityX": -0.738288661345317, + "velocityY": 0.8201801840047823, + "timestamp": 2.9578159751083413 + }, + { + "x": 2.6762937800618425, + "y": 3.4580316490257124, + "heading": 2.422273262441906, + "angularVelocity": 0.38506043908793497, + "velocityX": -0.5535121922663854, + "velocityY": 0.6147681424009704, + "timestamp": 3.0247466167382404 + }, + { + "x": 2.6516038167671496, + "y": 3.4854487858585643, + "heading": 2.439768796706212, + "angularVelocity": 0.261397976147448, + "velocityX": -0.3688887883552444, + "velocityY": 0.40963505152779395, + "timestamp": 3.0916772583681396 }, { "x": 2.6392624378204346, "y": 3.4991512298583984, "heading": 2.448651867948598, - "angularVelocity": 0.13267802071472284, - "velocityX": -0.18341499822016766, - "velocityY": 0.20571271982335043, - "timestamp": 3.0144358761477683 + "angularVelocity": 0.13272054512051312, + "velocityX": -0.18439056680433819, + "velocityY": 0.20472602183620905, + "timestamp": 3.1586078999980387 }, { "x": 2.6392624378204346, "y": 3.4991512298583984, "heading": 2.448651867948598, - "angularVelocity": 2.0995005926462733e-27, - "velocityX": 2.2917468406814997e-27, - "velocityY": -2.2636416821297325e-27, - "timestamp": 3.0813859762561164 + "angularVelocity": -1.6679097586736474e-24, + "velocityX": 3.3994819825962006e-22, + "velocityY": 2.9924033131311345e-22, + "timestamp": 3.225538541627938 } ], "trajectoryWaypoints": [ @@ -1188,14 +1332,14 @@ "isStopPoint": true, "x": 8.59085464477539, "y": 0.6655267477035522, - "heading": 1.584, + "heading": 0.003579741058468549, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 27 + "controlIntervalCount": 35 }, { - "timestamp": 2.0101843745225465, + "timestamp": 2.1546482755495515, "isStopPoint": false, "x": 3.9970710277557373, "y": 2.0853443145751953, @@ -1206,7 +1350,7 @@ "controlIntervalCount": 16 }, { - "timestamp": 3.0813859762561164, + "timestamp": 3.225538541627938, "isStopPoint": true, "x": 2.6392624378204346, "y": 3.4991512298583984, diff --git a/AmpAuto.chor b/AmpAuto.chor index 7bfee8a..2066a8d 100644 --- a/AmpAuto.chor +++ b/AmpAuto.chor @@ -16,18 +16,27 @@ "Amp 2 Piece 1": { "waypoints": [ { - "x": 0.9050432443618774, - "y": 6.6428680419921875, - "heading": -2.2565255466280507, + "x": 0.5076572895050049, + "y": 7.026061534881592, + "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 7 }, { - "x": 2.8777801990509033, - "y": 6.8983306884765625, - "heading": -1.5707963267948966, + "x": 1.401775598526001, + "y": 7.026061534881592, + "heading": 3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 2.352663278579712, + "y": 7.551178455352783, + "heading": 4.141051517496414, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -36,159 +45,224 @@ ], "trajectory": [ { - "x": 0.9050432443618774, - "y": 6.6428680419921875, - "heading": -2.2565255466280507, - "angularVelocity": -1.4291101663678553e-21, - "velocityX": 4.063581905170547e-21, - "velocityY": 7.609678430321777e-22, + "x": 0.5076572895050049, + "y": 7.026061534881592, + "heading": 3.141592653589793, + "angularVelocity": 5.576382446952711e-28, + "velocityX": -1.7262779855569846e-28, + "velocityY": -3.130496405792362e-27, "timestamp": 0 }, { - "x": 0.9488587964840457, - "y": 6.6487212093785475, - "heading": -2.2170365190388273, - "angularVelocity": 0.5340397711035277, - "velocityX": 0.5925506110077822, - "velocityY": 0.0791567775169907, - "timestamp": 0.07394398268807607 - }, - { - "x": 1.036787588456291, - "y": 6.660437165626341, - "heading": -2.1423875869378732, - "angularVelocity": 1.00953356023372, - "velocityX": 1.1891270766958724, - "velocityY": 0.15844367346548532, - "timestamp": 0.14788796537615215 - }, - { - "x": 1.1692835732429303, - "y": 6.678111777529781, - "heading": -2.0408936155248174, - "angularVelocity": 1.3725791839100139, - "velocityX": 1.791842689132625, - "velocityY": 0.23902704805829636, - "timestamp": 0.22183194806422823 - }, - { - "x": 1.346933296813628, - "y": 6.701897508529881, - "heading": -1.9340009000220366, - "angularVelocity": 1.4455904539750712, - "velocityX": 2.40249060320367, - "velocityY": 0.32167230024149257, - "timestamp": 0.2957759307523043 - }, - { - "x": 1.5625585723226694, - "y": 6.726906508694774, - "heading": -1.9227308281996909, - "angularVelocity": 0.15241364358053452, - "velocityX": 2.916062506650325, - "velocityY": 0.33821548765929105, - "timestamp": 0.36971991344038035 - }, - { - "x": 1.7820773559348524, - "y": 6.756085107088855, - "heading": -1.9227308124237847, - "angularVelocity": 2.1334942281526128e-7, - "velocityX": 2.9687173402436384, - "velocityY": 0.39460409534053714, - "timestamp": 0.4436638961284564 - }, - { - "x": 2.0015961382526735, - "y": 6.785263715245929, - "heading": -1.9227307966478815, - "angularVelocity": 2.1334938403089738e-7, - "velocityX": 2.9687173226950803, - "velocityY": 0.39460422736336004, - "timestamp": 0.5176078788165325 - }, - { - "x": 2.221114921646043, - "y": 6.8144423152746825, - "heading": -1.9227307808719754, - "angularVelocity": 2.1334942614926208e-7, - "velocityX": 2.968717337305404, - "velocityY": 0.39460411744569185, - "timestamp": 0.5915518615046086 - }, - { - "x": 2.436370432370262, - "y": 6.840013832318715, - "heading": -1.910648519871317, - "angularVelocity": 0.1633974876850485, - "velocityX": 2.911061899824357, - "velocityY": 0.3458228257933233, - "timestamp": 0.6654958441926847 - }, - { - "x": 2.613907826780238, - "y": 6.863558295707786, - "heading": -1.7989968132166365, - "angularVelocity": 1.5099498646924372, - "velocityX": 2.4009714915986042, - "velocityY": 0.31840945717834496, - "timestamp": 0.7394398268807608 - }, - { - "x": 2.746285116177599, - "y": 6.881046947319712, - "heading": -1.6928599971823888, - "angularVelocity": 1.435367857882004, - "velocityX": 1.7902374822825093, - "velocityY": 0.23651216740432432, - "timestamp": 0.8133838095688369 - }, - { - "x": 2.834070756139549, - "y": 6.892600291272677, - "heading": -1.6134370485289746, - "angularVelocity": 1.074096170724946, - "velocityX": 1.1871911245679945, - "velocityY": 0.15624454530041104, - "timestamp": 0.887327792256913 - }, - { - "x": 2.8777801990509033, - "y": 6.8983306884765625, - "heading": -1.5707963267948966, - "angularVelocity": 0.5766624975280655, - "velocityX": 0.5911156164696019, - "velocityY": 0.07749646415515937, - "timestamp": 0.9612717749449892 - }, - { - "x": 2.8777801990509033, - "y": 6.8983306884765625, - "heading": -1.5707963267948966, - "angularVelocity": -1.3704110825531436e-21, - "velocityX": 4.248115223834914e-21, - "velocityY": 9.902131829405519e-22, - "timestamp": 1.0352157576330652 + "x": 0.541861263011899, + "y": 7.023858848535928, + "heading": 3.1415926535897727, + "angularVelocity": -3.1650291496821756e-13, + "velocityX": 0.5334864255761352, + "velocityY": -0.03435575299391881, + "timestamp": 0.0641140465194699 + }, + { + "x": 0.6102692085867726, + "y": 7.019453477975782, + "heading": 3.141592653589732, + "angularVelocity": -6.329727254211075e-13, + "velocityX": 1.0669728287092248, + "velocityY": -0.06871147274736708, + "timestamp": 0.1282280930389398 + }, + { + "x": 0.7128811238314352, + "y": 7.012845426753121, + "heading": 3.141592653589671, + "angularVelocity": -9.494390784713861e-13, + "velocityX": 1.60045919443724, + "velocityY": -0.10306713710003898, + "timestamp": 0.19234213955840973 + }, + { + "x": 0.849697003949506, + "y": 7.00403470197188, + "heading": 3.14159265358959, + "angularVelocity": -1.2659835533654102e-12, + "velocityX": 2.133945485355117, + "velocityY": -0.13742269065119764, + "timestamp": 0.2564561860778796 + }, + { + "x": 1.0207168345518518, + "y": 6.99302132494383, + "heading": 3.1415926535894885, + "angularVelocity": -1.5824052622322692e-12, + "velocityX": 2.6674315518427174, + "velocityY": -0.17177791179824162, + "timestamp": 0.3205702325973495 + }, + { + "x": 1.212726563552661, + "y": 6.992468327030024, + "heading": 3.1415926535894885, + "angularVelocity": 6.64805175241919e-17, + "velocityX": 2.9948153240101663, + "velocityY": -0.008625222456331237, + "timestamp": 0.3846842791168194 + }, + { + "x": 1.401775598526001, + "y": 7.026061534881592, + "heading": 3.141592653589793, + "angularVelocity": 4.750903641780761e-12, + "velocityX": 2.948636768947822, + "velocityY": 0.5239601877471003, + "timestamp": 0.4487983256362893 + }, + { + "x": 1.5370249272630554, + "y": 7.069072166745036, + "heading": 3.1415932284442536, + "angularVelocity": 0.000012130387654159235, + "velocityX": 2.8539863567604646, + "velocityY": 0.9075960500518688, + "timestamp": 0.49618794556796086 + }, + { + "x": 1.6654346925708037, + "y": 7.129514350441228, + "heading": 3.1415934952796376, + "angularVelocity": 0.000005630671531509569, + "velocityX": 2.709660163826911, + "velocityY": 1.2754308598241744, + "timestamp": 0.5435775654996324 + }, + { + "x": 1.7855925760230578, + "y": 7.205041921251342, + "heading": 3.1415937037577986, + "angularVelocity": 0.0000043992368250325695, + "velocityX": 2.535531697141759, + "velocityY": 1.5937576819356747, + "timestamp": 0.590967185431304 + }, + { + "x": 1.904036917668605, + "y": 7.272314642412527, + "heading": 3.156119426039146, + "angularVelocity": 0.30651696093556136, + "velocityX": 2.499373107788697, + "velocityY": 1.4195665898604233, + "timestamp": 0.6383568053629756 + }, + { + "x": 2.0095791949782846, + "y": 7.331339137337132, + "heading": 3.210199696093153, + "angularVelocity": 1.1411838738521793, + "velocityX": 2.227118036858182, + "velocityY": 1.245515262829886, + "timestamp": 0.6857464252946471 + }, + { + "x": 2.1018588032452454, + "y": 7.382733805051419, + "heading": 3.305786668196144, + "angularVelocity": 2.0170444971032095, + "velocityX": 1.9472536053256688, + "velocityY": 1.0845131863980089, + "timestamp": 0.7331360452263187 + }, + { + "x": 2.180515398647323, + "y": 7.427471687362473, + "heading": 3.445587312955586, + "angularVelocity": 2.950026713888249, + "velocityX": 1.6597853182930875, + "velocityY": 0.9440439145863256, + "timestamp": 0.7805256651579903 + }, + { + "x": 2.245294901768652, + "y": 7.466899566519577, + "heading": 3.631740569973656, + "angularVelocity": 3.928144122836896, + "velocityX": 1.3669555319230515, + "velocityY": 0.8319939939158001, + "timestamp": 0.8279152850896618 + }, + { + "x": 2.2924148053297366, + "y": 7.500143889930106, + "heading": 3.815249497964916, + "angularVelocity": 3.8723443711059815, + "velocityX": 0.994308534844213, + "velocityY": 0.7015106569426475, + "timestamp": 0.8753049050213334 + }, + { + "x": 2.324405179579958, + "y": 7.525744431799912, + "heading": 3.9691826181905445, + "angularVelocity": 3.2482455113076645, + "velocityX": 0.675050238772683, + "velocityY": 0.5402141208711574, + "timestamp": 0.922694524953005 + }, + { + "x": 2.343773552171984, + "y": 7.542843252672667, + "heading": 4.080963317633744, + "angularVelocity": 2.358759146082405, + "velocityX": 0.4087049573293044, + "velocityY": 0.3608136317068833, + "timestamp": 0.9700841448846765 + }, + { + "x": 2.352663278579712, + "y": 7.551178455352783, + "heading": 4.141051517496414, + "angularVelocity": 1.26796121068935, + "velocityX": 0.18758805030606973, + "velocityY": 0.17588667501732433, + "timestamp": 1.017473764816348 + }, + { + "x": 2.352663278579712, + "y": 7.551178455352783, + "heading": 4.141051517496414, + "angularVelocity": -1.2571692895524854e-27, + "velocityX": -1.885622451182025e-26, + "velocityY": 8.829962183989965e-27, + "timestamp": 1.0648633847480196 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 0.9050432443618774, - "y": 6.6428680419921875, - "heading": -2.2565255466280507, + "x": 0.5076572895050049, + "y": 7.026061534881592, + "heading": 3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "timestamp": 0.4487983256362893, + "isStopPoint": false, + "x": 1.401775598526001, + "y": 7.026061534881592, + "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 13 }, { - "timestamp": 1.0352157576330652, + "timestamp": 1.0648633847480196, "isStopPoint": true, - "x": 2.8777801990509033, - "y": 6.8983306884765625, - "heading": -1.5707963267948966, + "x": 2.352663278579712, + "y": 7.551178455352783, + "heading": 4.141051517496414, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -219,18 +293,36 @@ "Amp 2 Piece 2": { "waypoints": [ { - "x": 3.038259506225586, - "y": 7.043644428253174, - "heading": -1.571, + "x": 2.2391245365142822, + "y": 7.536986351013184, + "heading": -2.3884414433002457, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 1.0043896436691284, + "y": 7.054446220397949, + "heading": -0.024384979707381408, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 2.0120468139648438, + "y": 7.054446220397949, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 7 }, { - "x": 3.1616272926330566, - "y": 6.359021186828613, - "heading": -2.4925966381653493, + "x": 2.8777804374694824, + "y": 7.026061534881592, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -239,132 +331,370 @@ ], "trajectory": [ { - "x": 3.038259506225586, - "y": 7.043644428253174, - "heading": -1.571, - "angularVelocity": 3.377508782026209e-23, - "velocityX": -5.278336527960705e-22, - "velocityY": -2.3473915338301426e-22, + "x": 2.2391245365142822, + "y": 7.536986351013184, + "heading": -2.3884414433002457, + "angularVelocity": 5.721431383504562e-26, + "velocityX": -5.410945273391021e-25, + "velocityY": -1.0088103825907385e-24, "timestamp": 0 }, { - "x": 3.041722845390556, - "y": 7.021424756435917, - "heading": -1.6118736663006001, - "angularVelocity": -0.7477383471577114, - "velocityX": 0.063357945035209, - "velocityY": -0.40648422768911846, - "timestamp": 0.054663060221491123 - }, - { - "x": 3.0486861748029925, - "y": 6.976611694502479, - "heading": -1.689825743738422, - "angularVelocity": -1.4260467145813887, - "velocityX": 0.12738638093480234, - "velocityY": -0.819805216756219, - "timestamp": 0.10932612044298225 - }, - { - "x": 3.059353554396992, - "y": 6.908666049138136, - "heading": -1.7987376751330109, - "angularVelocity": -1.992422871190979, - "velocityX": 0.19514786678198967, - "velocityY": -1.2429901489055193, - "timestamp": 0.16398918066447338 - }, - { - "x": 3.074080389772487, - "y": 6.816859673519979, - "heading": -1.928776333024462, - "angularVelocity": -2.3789128776278363, - "velocityX": 0.26941110350980785, - "velocityY": -1.679495718793705, - "timestamp": 0.2186522408859645 - }, - { - "x": 3.092722859572534, - "y": 6.700859976513945, - "heading": -2.0428573248611555, - "angularVelocity": -2.0869850933051466, - "velocityX": 0.34104328818234536, - "velocityY": -2.122085674238002, - "timestamp": 0.2733153011074556 - }, - { - "x": 3.1151177074959255, - "y": 6.585113556157874, - "heading": -2.1549840120443235, - "angularVelocity": -2.051233259331581, - "velocityX": 0.40968888007091114, - "velocityY": -2.117452259113795, - "timestamp": 0.32797836132894675 - }, - { - "x": 3.1333817091003047, - "y": 6.49366638357125, - "heading": -2.2800565188815622, - "angularVelocity": -2.2880626574958156, - "velocityX": 0.3341196327167695, - "velocityY": -1.6729244981178732, - "timestamp": 0.3826414215504379 - }, - { - "x": 3.147356671497851, - "y": 6.425961612128095, - "heading": -2.3829782913622974, - "angularVelocity": -1.8828395641170315, - "velocityX": 0.25565642210518025, - "velocityY": -1.2385836279348303, - "timestamp": 0.43730448177192904 - }, - { - "x": 3.1568316606137117, - "y": 6.3812429097672245, - "heading": -2.455320963886982, - "angularVelocity": -1.3234288792386915, - "velocityX": 0.17333440677248738, - "velocityY": -0.8180790131337755, - "timestamp": 0.4919675419934202 - }, - { - "x": 3.1616272926330566, - "y": 6.359021186828613, - "heading": -2.4925966381653493, - "angularVelocity": -0.6819170775900275, - "velocityX": 0.08773076369879507, - "velocityY": -0.4065217506771484, - "timestamp": 0.5466306022149113 - }, - { - "x": 3.1616272926330566, - "y": 6.359021186828613, - "heading": -2.4925966381653493, - "angularVelocity": 8.723021914700767e-23, - "velocityX": 5.757885441164143e-22, - "velocityY": -3.00776508275509e-23, - "timestamp": 0.6012936624364025 + "x": 2.2296641876726997, + "y": 7.53164553860975, + "heading": -2.33575280113073, + "angularVelocity": 1.178711973934036, + "velocityX": -0.2116400422939337, + "velocityY": -0.11948076988223226, + "timestamp": 0.044700184043828084 + }, + { + "x": 2.2091363580044265, + "y": 7.5205895962864, + "heading": -2.2405309329659358, + "angularVelocity": 2.130234364839089, + "velocityX": -0.4592336722404892, + "velocityY": -0.2473354989435561, + "timestamp": 0.08940036808765617 + }, + { + "x": 2.1742459343842966, + "y": 7.50751833119898, + "heading": -2.1168186310398647, + "angularVelocity": 2.7676016234020957, + "velocityX": -0.7805431759726248, + "velocityY": -0.2924208337622006, + "timestamp": 0.13410055213148425 + }, + { + "x": 2.1246336868250664, + "y": 7.492684660677987, + "heading": -1.9656445414880221, + "angularVelocity": 3.3819567589166444, + "velocityX": -1.109889111655235, + "velocityY": -0.33184808604924815, + "timestamp": 0.17880073617531234 + }, + { + "x": 2.058976778618434, + "y": 7.475737631665651, + "heading": -1.800162824867856, + "angularVelocity": 3.7020365835163696, + "velocityX": -1.4688285878701581, + "velocityY": -0.3791266048416977, + "timestamp": 0.22350092021914042 + }, + { + "x": 1.9785332458618086, + "y": 7.4586433187632375, + "heading": -1.664425028068561, + "angularVelocity": 3.0366272466843864, + "velocityX": -1.7996241956800805, + "velocityY": -0.38242153288794345, + "timestamp": 0.2682011042629685 + }, + { + "x": 1.8835626315507088, + "y": 7.440435729064373, + "heading": -1.560310319289602, + "angularVelocity": 2.3291785259066433, + "velocityX": -2.1246134964004204, + "velocityY": -0.40732695151796544, + "timestamp": 0.3129012883067966 + }, + { + "x": 1.7746640682844126, + "y": 7.420003143199531, + "heading": -1.4914280558490194, + "angularVelocity": 1.5409838888592533, + "velocityX": -2.4361994384524714, + "velocityY": -0.45710294715583155, + "timestamp": 0.35760147235062467 + }, + { + "x": 1.652640817011798, + "y": 7.396221119364523, + "heading": -1.4615580491684752, + "angularVelocity": 0.6682300603339137, + "velocityX": -2.7298154108934316, + "velocityY": -0.5320341368547818, + "timestamp": 0.40230165639445276 + }, + { + "x": 1.5233049536164358, + "y": 7.361678018164252, + "heading": -1.46155788621968, + "angularVelocity": 0.00000364537190716708, + "velocityX": -2.8934078496981073, + "velocityY": -0.7727731314573986, + "timestamp": 0.44700184043828084 + }, + { + "x": 1.4065653245496021, + "y": 7.328928356244567, + "heading": -1.4235263433901186, + "angularVelocity": 0.8508140098991993, + "velocityX": -2.6116140585097294, + "velocityY": -0.732651612520761, + "timestamp": 0.4917020244821089 + }, + { + "x": 1.3032250484271148, + "y": 7.296949732733987, + "heading": -1.3457880678727918, + "angularVelocity": 1.7391041486787921, + "velocityX": -2.3118534818819345, + "velocityY": -0.7154025021289736, + "timestamp": 0.536402208525937 + }, + { + "x": 1.213100117494602, + "y": 7.264849658847639, + "heading": -1.2286347478417774, + "angularVelocity": 2.6208688518182983, + "velocityX": -2.016209392877359, + "velocityY": -0.7181195015858151, + "timestamp": 0.5811023925697651 + }, + { + "x": 1.1372460206771293, + "y": 7.230088830221739, + "heading": -1.0829990745361808, + "angularVelocity": 3.258055339611178, + "velocityX": -1.6969526734632359, + "velocityY": -0.777643971036398, + "timestamp": 0.6258025766135933 + }, + { + "x": 1.0760806116019437, + "y": 7.195783758543388, + "heading": -0.9097385032115863, + "angularVelocity": 3.876059462187322, + "velocityX": -1.3683480366705796, + "velocityY": -0.7674481081490622, + "timestamp": 0.6705027606574214 + }, + { + "x": 1.0298697641313916, + "y": 7.162338001153043, + "heading": -0.711153238624929, + "angularVelocity": 4.442605077239651, + "velocityX": -1.033795463241102, + "velocityY": -0.7482241540113431, + "timestamp": 0.7152029447012496 + }, + { + "x": 1.0001525071013124, + "y": 7.131235715839601, + "heading": -0.5118465164104274, + "angularVelocity": 4.458745002460914, + "velocityX": -0.6648128562723964, + "velocityY": -0.6957977014803056, + "timestamp": 0.7599031287450777 + }, + { + "x": 0.9864723677403102, + "y": 7.102929584116405, + "heading": -0.32522300497964796, + "angularVelocity": 4.175005437288511, + "velocityX": -0.3060421260813816, + "velocityY": -0.6332441874387541, + "timestamp": 0.8046033127889058 + }, + { + "x": 0.9881450214838234, + "y": 7.077378427353441, + "heading": -0.16014265091379973, + "angularVelocity": 3.6930575924248683, + "velocityX": 0.037419392767447934, + "velocityY": -0.5716118917522006, + "timestamp": 0.849303496832734 + }, + { + "x": 1.0043896436691284, + "y": 7.054446220397949, + "heading": -0.024384979707381408, + "angularVelocity": 3.037071862462787, + "velocityX": 0.3634128702776077, + "velocityY": -0.5130226518308526, + "timestamp": 0.8940036808765621 + }, + { + "x": 1.0505988037906369, + "y": 7.028624623125705, + "heading": 0.09377281742019881, + "angularVelocity": 1.9835385185104277, + "velocityX": 0.7757223918964998, + "velocityY": -0.43347230605238574, + "timestamp": 0.9535728780587337 + }, + { + "x": 1.1215135025956555, + "y": 7.007962378546688, + "heading": 0.15072199016101734, + "angularVelocity": 0.9560171268828643, + "velocityX": 1.1904591997127438, + "velocityY": -0.3468612228536146, + "timestamp": 1.0131420752409053 + }, + { + "x": 1.217577660739459, + "y": 6.992737876712583, + "heading": 0.1503344728353621, + "angularVelocity": -0.006505330674008277, + "velocityX": 1.612648192152475, + "velocityY": -0.25557675030511917, + "timestamp": 1.072711272423077 + }, + { + "x": 1.3396267843628167, + "y": 6.9833714114319, + "heading": 0.10047084389942805, + "angularVelocity": -0.8370706891255146, + "velocityX": 2.0488629929007285, + "velocityY": -0.15723672172447928, + "timestamp": 1.1322804696052486 + }, + { + "x": 1.4890457715649965, + "y": 6.981233099614705, + "heading": 0.01898626882624744, + "angularVelocity": -1.3678978218220477, + "velocityX": 2.5083263543947716, + "velocityY": -0.03589626717070096, + "timestamp": 1.1918496667874203 + }, + { + "x": 1.6615559253103864, + "y": 6.987743789587346, + "heading": 1.6896592561479554e-7, + "angularVelocity": -0.3187234469898841, + "velocityX": 2.8959623749473673, + "velocityY": 0.10929625176465058, + "timestamp": 1.251418863969592 + }, + { + "x": 1.836472158801685, + "y": 7.022824858897805, + "heading": 8.525108932477818e-8, + "angularVelocity": -0.0000014053376619128292, + "velocityX": 2.936353715769886, + "velocityY": 0.5889129108652654, + "timestamp": 1.3109880611517637 + }, + { + "x": 2.012046813964844, + "y": 7.054446220397949, + "heading": 2.2019024313995553e-25, + "angularVelocity": -0.0000014311270481632891, + "velocityX": 2.9474067717620085, + "velocityY": 0.5308341054763681, + "timestamp": 1.3705572583339354 + }, + { + "x": 2.2280675873510725, + "y": 7.049473002137666, + "heading": -7.926750089678431e-16, + "angularVelocity": -1.0986426575996211e-14, + "velocityX": 2.9940345515976707, + "velocityY": -0.06892849734087551, + "timestamp": 1.4427076526141067 + }, + { + "x": 2.444005456892987, + "y": 7.041692003021008, + "heading": -8.024831824871519e-16, + "angularVelocity": -1.359406780437382e-16, + "velocityX": 2.9928855094456446, + "velocityY": -0.10784416626252269, + "timestamp": 1.514858046894278 + }, + { + "x": 2.6175154366538695, + "y": 7.035439816214791, + "heading": -4.813899359992426e-16, + "angularVelocity": 4.45033252681572e-15, + "velocityX": 2.404837582551739, + "velocityY": -0.08665492224391362, + "timestamp": 1.5870084411744494 + }, + { + "x": 2.7476479338367086, + "y": 7.030750675664456, + "heading": -2.405669787611813e-16, + "angularVelocity": 3.3377912850014936e-15, + "velocityX": 1.8036283582528165, + "velocityY": -0.06499119785994299, + "timestamp": 1.6591588354546207 + }, + { + "x": 2.8344029355418954, + "y": 7.02762458183505, + "heading": -8.002384171896392e-17, + "angularVelocity": 2.2251179448655455e-15, + "velocityX": 1.2024189551661102, + "velocityY": -0.043327467030426094, + "timestamp": 1.731309229734792 + }, + { + "x": 2.8777804374694824, + "y": 7.026061534881592, + "heading": -5.744138405308966e-28, + "angularVelocity": 1.1091254942746495e-15, + "velocityX": 0.6012094924824047, + "velocityY": -0.021663734052355155, + "timestamp": 1.8034596240149634 + }, + { + "x": 2.8777804374694824, + "y": 7.026061534881592, + "heading": -2.835172507845664e-28, + "angularVelocity": 1.0227718137074573e-28, + "velocityX": 3.2904914962294794e-27, + "velocityY": 1.9801334803171188e-26, + "timestamp": 1.8756100182951347 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 3.038259506225586, - "y": 7.043644428253174, - "heading": -1.571, + "x": 2.2391245365142822, + "y": 7.536986351013184, + "heading": -2.3884414433002457, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "timestamp": 0.8940036808765621, + "isStopPoint": false, + "x": 1.0043896436691284, + "y": 7.054446220397949, + "heading": -0.024384979707381408, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 1.3705572583339354, + "isStopPoint": false, + "x": 2.0120468139648438, + "y": 7.054446220397949, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 7 }, { - "timestamp": 0.6012936624364025, + "timestamp": 1.8756100182951347, "isStopPoint": true, - "x": 3.1616272926330566, - "y": 6.359021186828613, - "heading": -2.4925966381653493, + "x": 2.8777804374694824, + "y": 7.026061534881592, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -392,7 +722,7 @@ "eventMarkers": [], "isTrajectoryStale": false }, - "Amp 3 Piece 3": { + "Amp 3 Piece 4": { "waypoints": [ { "x": 3.162, @@ -401,21 +731,39 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 8 }, { "x": 3.0722529888153076, "y": 6.29632043838501, - "heading": -3.1159573378389447, + "heading": -1.590018574093363, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 + }, + { + "x": 3.179861068725586, + "y": 5.717745780944824, + "heading": -1.5707963267948966, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "x": 1.8017919063568115, + "y": 4.128912448883057, + "heading": -3.719918188107055, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 3.104857921600342, - "y": 5.3939409255981445, - "heading": -3.1509381423979628, + "x": 2.320594549179077, + "y": 3.221007823944092, + "heading": -3.9579087770321086, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -427,118 +775,415 @@ "x": 3.162, "y": 6.359, "heading": -2.588, - "angularVelocity": 1.4410764077573947e-21, - "velocityX": 4.776931205132025e-21, - "velocityY": -5.947632414862795e-20, + "angularVelocity": 2.3545923598347173e-28, + "velocityX": 1.0132424317562211e-26, + "velocityY": -5.4128073679143105e-28, "timestamp": 0 }, { - "x": 3.1566148081540084, - "y": 6.358254414724499, - "heading": -2.640344449106781, - "angularVelocity": -1.340201944453853, - "velocityX": -0.13787984602783324, - "velocityY": -0.019089604591012607, - "timestamp": 0.03905713562302894 + "x": 3.158541809417635, + "y": 6.361560311364594, + "heading": -2.54828298105256, + "angularVelocity": 1.1612582755337608, + "velocityX": -0.10111162767422677, + "velocityY": 0.074859162114193, + "timestamp": 0.03420171014857538 + }, + { + "x": 3.151551779969113, + "y": 6.366194194823638, + "heading": -2.4681280049011955, + "angularVelocity": 2.3435955630044076, + "velocityX": -0.2043766062619971, + "velocityY": 0.1354868934598289, + "timestamp": 0.06840342029715075 + }, + { + "x": 3.1411730874434873, + "y": 6.371651225656791, + "heading": -2.3456815065418324, + "angularVelocity": 3.580127947621462, + "velocityX": -0.3034553675982788, + "velocityY": 0.15955432665347288, + "timestamp": 0.10260513044572614 + }, + { + "x": 3.1261057120873623, + "y": 6.373387190431622, + "heading": -2.1905074090315306, + "angularVelocity": 4.537027442084355, + "velocityX": -0.44054450174190574, + "velocityY": 0.05075666588861731, + "timestamp": 0.1368068405943015 }, { - "x": 3.143683922809688, - "y": 6.355595068320737, - "heading": -2.736637732363478, - "angularVelocity": -2.465446626350144, - "velocityX": -0.33107613085421683, - "velocityY": -0.06808861841355184, - "timestamp": 0.07811427124605788 + "x": 3.1072783036815137, + "y": 6.366652254015958, + "heading": -2.0240132831100572, + "angularVelocity": 4.86800587450767, + "velocityX": -0.550481491248849, + "velocityY": -0.19691811860888647, + "timestamp": 0.17100855074287688 }, { - "x": 3.121844181931675, - "y": 6.347995260906763, - "heading": -2.863331775306469, - "angularVelocity": -3.2438129658512223, - "velocityX": -0.5591741567739482, - "velocityY": -0.19458179133579442, - "timestamp": 0.11717140686908682 + "x": 3.090847947710989, + "y": 6.350727001128441, + "heading": -1.86543996194514, + "angularVelocity": 4.636414976796781, + "velocityX": -0.4803957433458614, + "velocityY": -0.4656273858335121, + "timestamp": 0.20521026089145225 }, { - "x": 3.096280729497018, - "y": 6.328308215255226, - "heading": -2.9948428591271825, - "angularVelocity": -3.367146149426579, - "velocityX": -0.6545142654952973, - "velocityY": -0.5040575899254738, - "timestamp": 0.15622854249211576 + "x": 3.0789451322686627, + "y": 6.32702824464787, + "heading": -1.7201154118069268, + "angularVelocity": 4.249043381366309, + "velocityX": -0.34801813683056054, + "velocityY": -0.6929114473405495, + "timestamp": 0.23941197104002762 }, { "x": 3.0722529888153076, "y": 6.29632043838501, - "heading": -3.1159573378389447, - "angularVelocity": -3.1009565033322724, - "velocityX": -0.6151946449330228, - "velocityY": -0.8189995594903712, - "timestamp": 0.1952856781151447 - }, - { - "x": 3.0372212203856686, - "y": 6.1694121418451795, - "heading": -3.3044718530781294, - "angularVelocity": -2.1711550223561162, - "velocityX": -0.4034670745195598, - "velocityY": -1.461625302759914, - "timestamp": 0.28211251143569255 - }, - { - "x": 3.0226662530134463, - "y": 5.987674801349684, - "heading": -3.4118328533680193, - "angularVelocity": -1.2364956337119166, - "velocityX": -0.1676321341639648, - "velocityY": -2.093101101874308, - "timestamp": 0.3689393447562404 - }, - { - "x": 3.035431736182884, - "y": 5.7564198497648364, - "heading": -3.423913140535644, - "angularVelocity": -0.13913080444874767, - "velocityX": 0.14702232802053028, - "velocityY": -2.6634041890149223, - "timestamp": 0.45576617807678826 - }, - { - "x": 3.06747307710671, - "y": 5.574098037633144, - "heading": -3.3005437740825374, - "angularVelocity": 1.420866818874432, - "velocityX": 0.3690257918947216, - "velocityY": -2.0998325651080223, - "timestamp": 0.5425930113973362 - }, - { - "x": 3.0919306232912, - "y": 5.453752568769309, - "heading": -3.203252635101642, - "angularVelocity": 1.1205192595440523, - "velocityX": 0.28168188622286433, - "velocityY": -1.3860400553771668, - "timestamp": 0.629419844717884 - }, - { - "x": 3.104857921600342, - "y": 5.3939409255981445, - "heading": -3.1509381423979628, - "angularVelocity": 0.6025152674927592, - "velocityX": 0.1488859816114309, - "velocityY": -0.6888612757573563, - "timestamp": 0.7162466780384319 - }, - { - "x": 3.104857921600342, - "y": 5.3939409255981445, - "heading": -3.1509381423979628, - "angularVelocity": 3.452519064446764e-20, - "velocityX": -7.442188151588243e-20, - "velocityY": -5.2649187007366165e-21, - "timestamp": 0.8030735113589798 + "heading": -1.590018574093363, + "angularVelocity": 3.8038108956660723, + "velocityX": -0.19566692496614022, + "velocityY": -0.8978441759041662, + "timestamp": 0.273613681188603 + }, + { + "x": 3.074208101596113, + "y": 6.231624428620404, + "heading": -1.4310130878973673, + "angularVelocity": 2.956037223790758, + "velocityX": 0.03634708647502808, + "velocityY": -1.2027497771942952, + "timestamp": 0.3274037637072834 + }, + { + "x": 3.08751039213603, + "y": 6.151010677702688, + "heading": -1.32623470104763, + "angularVelocity": 1.9479127367642397, + "velocityX": 0.24730005824581125, + "velocityY": -1.498673122312463, + "timestamp": 0.3811938462259638 + }, + { + "x": 3.1116055405528797, + "y": 6.0548675149224565, + "heading": -1.2804377091115737, + "angularVelocity": 0.8514021505758418, + "velocityX": 0.4479477868152025, + "velocityY": -1.7873771200637798, + "timestamp": 0.4349839287446442 + }, + { + "x": 3.1458684756981237, + "y": 5.943270407786396, + "heading": -1.2961837179515021, + "angularVelocity": -0.292730706156845, + "velocityX": 0.6369749504166531, + "velocityY": -2.0746781174262896, + "timestamp": 0.4887740112633246 + }, + { + "x": 3.173516037313562, + "y": 5.826662668779321, + "heading": -1.4182314122517021, + "angularVelocity": -2.2689627638666443, + "velocityX": 0.5139899461176065, + "velocityY": -2.1678297103667643, + "timestamp": 0.542564093782005 + }, + { + "x": 3.179861068725586, + "y": 5.717745780944824, + "heading": -1.5707963267948966, + "angularVelocity": -2.836301924062139, + "velocityX": 0.11795913140346345, + "velocityY": -2.0248507296241973, + "timestamp": 0.5963541763006854 + }, + { + "x": 3.1696117726950463, + "y": 5.631590772796136, + "heading": -1.715109710694546, + "angularVelocity": -3.1840088084214218, + "velocityX": -0.22613182477966223, + "velocityY": -1.9008514485794035, + "timestamp": 0.641678606157146 + }, + { + "x": 3.145236062623627, + "y": 5.554070753841431, + "heading": -1.8754511624529167, + "angularVelocity": -3.5376385818014935, + "velocityX": -0.5378051119146163, + "velocityY": -1.7103363285584534, + "timestamp": 0.6870030360136066 + }, + { + "x": 3.1081460827237075, + "y": 5.487648602669839, + "heading": -2.046345155928937, + "angularVelocity": -3.770460963706106, + "velocityX": -0.8183220399546215, + "velocityY": -1.4654823321980464, + "timestamp": 0.7323274658700671 + }, + { + "x": 3.0570045176167, + "y": 5.415666219019381, + "heading": -2.1885090431681173, + "angularVelocity": -3.1365841266929104, + "velocityX": -1.1283443668010789, + "velocityY": -1.5881586128809646, + "timestamp": 0.7776518957265277 + }, + { + "x": 2.990954781455389, + "y": 5.342049490783512, + "heading": -2.3020677570341874, + "angularVelocity": -2.5054637030339353, + "velocityX": -1.4572656814544667, + "velocityY": -1.6242174136334224, + "timestamp": 0.8229763255829883 + }, + { + "x": 2.9107728525339303, + "y": 5.26595965350658, + "heading": -2.382373816281858, + "angularVelocity": -1.7718051722215742, + "velocityX": -1.769066465378379, + "velocityY": -1.6787820060374272, + "timestamp": 0.8683007554394488 + }, + { + "x": 2.8194126507306287, + "y": 5.187653621521034, + "heading": -2.432983034334834, + "angularVelocity": -1.116599110308764, + "velocityX": -2.0156944520346625, + "velocityY": -1.7276782572563323, + "timestamp": 0.9136251852959094 + }, + { + "x": 2.7228504033394225, + "y": 5.111823049623562, + "heading": -2.4758781578005773, + "angularVelocity": -0.9464018323360985, + "velocityX": -2.1304679992007083, + "velocityY": -1.6730617933335687, + "timestamp": 0.95894961515237 + }, + { + "x": 2.623412901645993, + "y": 5.040643151344395, + "heading": -2.5203252455458984, + "angularVelocity": -0.9806430634887603, + "velocityX": -2.193905185533298, + "velocityY": -1.570453252353952, + "timestamp": 1.0042740450088306 + }, + { + "x": 2.5233382583202117, + "y": 4.977210159925781, + "heading": -2.5766537672282444, + "angularVelocity": -1.242785002717836, + "velocityX": -2.207962541232429, + "velocityY": -1.3995320320520612, + "timestamp": 1.0495984748652911 + }, + { + "x": 2.4249020255614324, + "y": 4.919535457776485, + "heading": -2.6447412484992725, + "angularVelocity": -1.5022247712030088, + "velocityX": -2.1718140320908645, + "velocityY": -1.2724859933582913, + "timestamp": 1.0949229047217517 + }, + { + "x": 2.328898126856056, + "y": 4.8626542096962675, + "heading": -2.71656901676977, + "angularVelocity": -1.584747309518758, + "velocityX": -2.118149064630577, + "velocityY": -1.2549798918675032, + "timestamp": 1.1402473345782123 + }, + { + "x": 2.2358360141282634, + "y": 4.806747454357658, + "heading": -2.7933938675939385, + "angularVelocity": -1.6949987251349385, + "velocityX": -2.0532439795164334, + "velocityY": -1.2334795057690093, + "timestamp": 1.1855717644346728 + }, + { + "x": 2.146568790469089, + "y": 4.7521619746147765, + "heading": -2.87737657331996, + "angularVelocity": -1.852923599744974, + "velocityX": -1.9695167471908095, + "velocityY": -1.2043279952059003, + "timestamp": 1.2308961942911334 + }, + { + "x": 2.062811347738124, + "y": 4.6996834442356, + "heading": -2.972981859541326, + "angularVelocity": -2.1093544149179886, + "velocityX": -1.8479535869776882, + "velocityY": -1.157842041154702, + "timestamp": 1.276220624147594 + }, + { + "x": 1.9897516685916172, + "y": 4.651936049585479, + "heading": -3.0940724928293957, + "angularVelocity": -2.6716416217822414, + "velocityX": -1.6119271522638414, + "velocityY": -1.0534582520140607, + "timestamp": 1.3215450540040545 + }, + { + "x": 1.9305727803175539, + "y": 4.5950641132377426, + "heading": -3.225567477933253, + "angularVelocity": -2.9011944666550202, + "velocityX": -1.3056730875926967, + "velocityY": -1.2547744456542749, + "timestamp": 1.366869483860515 + }, + { + "x": 1.8816378534728377, + "y": 4.52449736884993, + "heading": -3.353672139095455, + "angularVelocity": -2.8263932181364657, + "velocityX": -1.0796589609552654, + "velocityY": -1.5569251419442536, + "timestamp": 1.4121939137169757 + }, + { + "x": 1.8425411312537379, + "y": 4.439984375501508, + "heading": -3.4765942071733944, + "angularVelocity": -2.7120488546072266, + "velocityX": -0.8625971102762152, + "velocityY": -1.8646234186744175, + "timestamp": 1.4575183435734362 + }, + { + "x": 1.814625048446928, + "y": 4.343895867299826, + "heading": -3.5780401995813476, + "angularVelocity": -2.238218830975388, + "velocityX": -0.6159169104877494, + "velocityY": -2.120015817209132, + "timestamp": 1.5028427734298968 + }, + { + "x": 1.800793601871009, + "y": 4.239870845816041, + "heading": -3.658298645599016, + "angularVelocity": -1.7707546740651992, + "velocityX": -0.30516537372278685, + "velocityY": -2.2951203537082416, + "timestamp": 1.5481672032863574 + }, + { + "x": 1.8017919063568115, + "y": 4.128912448883057, + "heading": -3.719918188107055, + "angularVelocity": -1.359521624501048, + "velocityX": 0.022025748342873373, + "velocityY": -2.448092502969865, + "timestamp": 1.593491633142818 + }, + { + "x": 1.833720071260504, + "y": 3.9595668887755444, + "heading": -3.7690691947954535, + "angularVelocity": -0.7650465428728321, + "velocityX": 0.4969691126511394, + "velocityY": -2.635901969057657, + "timestamp": 1.6577374063526822 + }, + { + "x": 1.8976877544470778, + "y": 3.7830860846248036, + "heading": -3.7807757981913688, + "angularVelocity": -0.18221593127495458, + "velocityX": 0.9956714658506488, + "velocityY": -2.7469636574261713, + "timestamp": 1.7219831795625464 + }, + { + "x": 1.9943801325500514, + "y": 3.616742294018958, + "heading": -3.780775957074689, + "angularVelocity": -0.000002473054837484796, + "velocityX": 1.5050387484188235, + "velocityY": -2.5891787474091115, + "timestamp": 1.7862289527724107 + }, + { + "x": 2.0986811434479353, + "y": 3.48134511313859, + "heading": -3.8280375689759656, + "angularVelocity": -0.7356376854068292, + "velocityX": 1.6234688398437669, + "velocityY": -2.107487763250687, + "timestamp": 1.850474725982275 + }, + { + "x": 2.185971627375577, + "y": 3.3758656680580077, + "heading": -3.8757754572956524, + "angularVelocity": -0.7430510356494172, + "velocityX": 1.3586961377599196, + "velocityY": -1.6418114345985888, + "timestamp": 1.9147204991921392 + }, + { + "x": 2.2527796055270417, + "y": 3.297948908815885, + "heading": -3.9153521792866077, + "angularVelocity": -0.6160206347221387, + "velocityX": 1.0398812997896527, + "velocityY": -1.2127919915852055, + "timestamp": 1.9789662724020034 + }, + { + "x": 2.2978634684080435, + "y": 3.246528407652369, + "heading": -3.943342337095115, + "angularVelocity": -0.4356731409718601, + "velocityX": 0.7017405290419882, + "velocityY": -0.8003717380059724, + "timestamp": 2.0432120456118676 + }, + { + "x": 2.320594549179077, + "y": 3.221007823944092, + "heading": -3.9579087770321086, + "angularVelocity": -0.22672993426993343, + "velocityX": 0.3538144166586729, + "velocityY": -0.39723366119217857, + "timestamp": 2.107457818821732 + }, + { + "x": 2.320594549179077, + "y": 3.221007823944092, + "heading": -3.9579087770321086, + "angularVelocity": 3.2853435384869114e-26, + "velocityX": -5.0541121334764486e-26, + "velocityY": -7.435728149679528e-26, + "timestamp": 2.171703592031596 } ], "trajectoryWaypoints": [ @@ -551,25 +1196,47 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 5 + "controlIntervalCount": 8 }, { - "timestamp": 0.1952856781151447, + "timestamp": 0.273613681188603, "isStopPoint": false, "x": 3.0722529888153076, "y": 6.29632043838501, - "heading": -3.1159573378389447, + "heading": -1.590018574093363, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 + }, + { + "timestamp": 0.5963541763006854, + "isStopPoint": false, + "x": 3.179861068725586, + "y": 5.717745780944824, + "heading": -1.5707963267948966, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "timestamp": 1.593491633142818, + "isStopPoint": false, + "x": 1.8017919063568115, + "y": 4.128912448883057, + "heading": -3.719918188107055, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "timestamp": 0.8030735113589798, + "timestamp": 2.171703592031596, "isStopPoint": true, - "x": 3.104857921600342, - "y": 5.3939409255981445, - "heading": -3.1509381423979628, + "x": 2.320594549179077, + "y": 3.221007823944092, + "heading": -3.9579087770321086, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -597,21 +1264,30 @@ "eventMarkers": [], "isTrajectoryStale": false }, - "Amp 3 Piece 4": { + "Amp 3 Piece 5": { "waypoints": [ { - "x": 2.911, - "y": 5.349, - "heading": 3.132, + "x": 1.785578966140747, + "y": 3.026456832885742, + "heading": 2.2752903910371143, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 19 }, { - "x": 3.133242607116699, - "y": 6.188713073730469, - "heading": 3.402195063790063, + "x": 1.5910279750823975, + "y": 4.08027458190918, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 3.082585573196411, + "y": 4.031636714935303, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -620,105 +1296,296 @@ ], "trajectory": [ { - "x": 2.911, - "y": 5.349, - "heading": 3.132, - "angularVelocity": -4.344195742289857e-21, - "velocityX": -4.7082378602815056e-20, - "velocityY": -1.7784914405892649e-19, + "x": 1.785578966140747, + "y": 3.026456832885742, + "heading": 2.2752903910371143, + "angularVelocity": -1.8954621059563845e-27, + "velocityX": 5.094997374536094e-27, + "velocityY": -1.555088100432084e-26, "timestamp": 0 }, { - "x": 2.924890848236759, - "y": 5.401469647858394, - "heading": 3.149410077214198, - "angularVelocity": 0.2153558954017532, - "velocityX": 0.17182439934715354, - "velocityY": 0.6490291718375661, - "timestamp": 0.08084328121929982 - }, - { - "x": 2.952672232790939, - "y": 5.506414733603586, - "heading": 3.1840022647617863, - "angularVelocity": 0.4278919289996538, - "velocityX": 0.343644940373191, - "velocityY": 1.2981299640784225, - "timestamp": 0.16168656243859963 - }, - { - "x": 2.9943436299649684, - "y": 5.663848700643325, - "heading": 3.2352387316668687, - "angularVelocity": 0.6337752022471191, - "velocityX": 0.5154590034636232, - "velocityY": 1.9473970460535244, - "timestamp": 0.24252984365789945 - }, - { - "x": 3.049899633232633, - "y": 5.873841815086245, - "heading": 3.300049028392088, - "angularVelocity": 0.801678206868071, - "velocityX": 0.6872061899239325, - "velocityY": 2.5975332925104753, - "timestamp": 0.32337312487719927 - }, - { - "x": 3.091570598941423, - "y": 6.031287202367331, - "heading": 3.350741147440927, - "angularVelocity": 0.627041830617058, - "velocityX": 0.5154536664061404, - "velocityY": 1.947538310004935, - "timestamp": 0.4042164060964991 - }, - { - "x": 3.119351858656031, - "y": 6.136239824629046, - "heading": 3.384964766681284, - "angularVelocity": 0.42333288214167747, - "velocityX": 0.3436433961561573, - "velocityY": 1.2982231878616135, - "timestamp": 0.48505968731579896 - }, - { - "x": 3.133242607116699, - "y": 6.188713073730469, - "heading": 3.402195063790063, - "angularVelocity": 0.21313208530019123, - "velocityX": 0.17182316515564514, - "velocityY": 0.6490737178156233, - "timestamp": 0.5659029685350988 - }, - { - "x": 3.133242607116699, - "y": 6.188713073730469, - "heading": 3.402195063790063, - "angularVelocity": 2.97617448838405e-21, - "velocityX": 4.7042355495804444e-20, - "velocityY": 1.783120573732454e-19, - "timestamp": 0.6467462497543986 + "x": 1.782637002969458, + "y": 3.030934879800897, + "heading": 2.2161498525084706, + "angularVelocity": -1.465111298991776, + "velocityX": -0.07288238475180157, + "velocityY": 0.110936377923418, + "timestamp": 0.04036590160033693 + }, + { + "x": 1.776854375639112, + "y": 3.0404392101200512, + "heading": 2.0992045300830253, + "angularVelocity": -2.8971314349254977, + "velocityX": -0.14325525012670193, + "velocityY": 0.2354544291678867, + "timestamp": 0.08073180320067386 + }, + { + "x": 1.768527296831389, + "y": 3.0558164340175304, + "heading": 1.9267427272668538, + "angularVelocity": -4.272462548309133, + "velocityX": -0.2062899248521585, + "velocityY": 0.38094587976082794, + "timestamp": 0.12109770480101079 + }, + { + "x": 1.7577422754879621, + "y": 3.0792567393920285, + "heading": 1.7078934840810325, + "angularVelocity": -5.421636443368693, + "velocityX": -0.26718148030506983, + "velocityY": 0.5806956972392262, + "timestamp": 0.16146360640134771 + }, + { + "x": 1.7418176124047977, + "y": 3.1145654563250176, + "heading": 1.5040219429296366, + "angularVelocity": -5.050588072327222, + "velocityX": -0.39450780118414347, + "velocityY": 0.8747164198778695, + "timestamp": 0.20182950800168464 + }, + { + "x": 1.7176852193747818, + "y": 3.157439084845946, + "heading": 1.3285362877975075, + "angularVelocity": -4.347373604326088, + "velocityX": -0.5978410508193482, + "velocityY": 1.0621248831605514, + "timestamp": 0.24219540960202157 + }, + { + "x": 1.684843526703858, + "y": 3.208127129747163, + "heading": 1.17927498125219, + "angularVelocity": -3.697707734194931, + "velocityX": -0.8135998793261088, + "velocityY": 1.2557144245923109, + "timestamp": 0.2825613112023585 + }, + { + "x": 1.6434576742777403, + "y": 3.26705565218992, + "heading": 1.0553990395550077, + "angularVelocity": -3.0688263307897565, + "velocityX": -1.025267633952027, + "velocityY": 1.459858942981347, + "timestamp": 0.32292721280269543 + }, + { + "x": 1.5943446932187455, + "y": 3.3348722400911304, + "heading": 0.9576696221034526, + "angularVelocity": -2.4210884329842246, + "velocityX": -1.2166947624572502, + "velocityY": 1.6800464058170483, + "timestamp": 0.36329311440303236 + }, + { + "x": 1.5389107935987831, + "y": 3.4123091818648263, + "heading": 0.8877110799809592, + "angularVelocity": -1.7331098612673965, + "velocityX": -1.3732853081993277, + "velocityY": 1.918375131079688, + "timestamp": 0.4036590160033693 + }, + { + "x": 1.4937133432666583, + "y": 3.4984312793538073, + "heading": 0.8250449654269514, + "angularVelocity": -1.5524517493617649, + "velocityX": -1.1196938143392696, + "velocityY": 2.133535832834275, + "timestamp": 0.4440249176037062 + }, + { + "x": 1.4604074486928154, + "y": 3.5812108846355466, + "heading": 0.7421469709224653, + "angularVelocity": -2.0536638900143904, + "velocityX": -0.8250997315408592, + "velocityY": 2.050730988281656, + "timestamp": 0.48439081920404314 + }, + { + "x": 1.4398916806200917, + "y": 3.6618510739711834, + "heading": 0.6449597401080992, + "angularVelocity": -2.4076566349642725, + "velocityX": -0.5082450102527284, + "velocityY": 1.9977304144982566, + "timestamp": 0.5247567208043801 + }, + { + "x": 1.432507206361376, + "y": 3.740336413019291, + "heading": 0.5380390081031117, + "angularVelocity": -2.648788402241338, + "velocityX": -0.18293841995230437, + "velocityY": 1.944347479840561, + "timestamp": 0.565122622404717 + }, + { + "x": 1.4383712466830039, + "y": 3.8162277828812874, + "heading": 0.4256734213660632, + "angularVelocity": -2.7836758819258134, + "velocityX": 0.14527212546093396, + "velocityY": 1.8800860838783595, + "timestamp": 0.6054885240050539 + }, + { + "x": 1.4574240186735574, + "y": 3.888878324566731, + "heading": 0.3119775884105386, + "angularVelocity": -2.816630582941707, + "velocityX": 0.47200164582462106, + "velocityY": 1.7997998014452095, + "timestamp": 0.6458544256053909 + }, + { + "x": 1.4894506756188453, + "y": 3.9575592874387215, + "heading": 0.20081873031766603, + "angularVelocity": -2.753781129267399, + "velocityX": 0.7934086859345777, + "velocityY": 1.7014598992982128, + "timestamp": 0.6862203272057278 + }, + { + "x": 1.5341182594581817, + "y": 4.021562056122661, + "heading": 0.09575326080161849, + "angularVelocity": -2.6028272713019387, + "velocityX": 1.1065672279933332, + "velocityY": 1.585565196031799, + "timestamp": 0.7265862288060647 + }, + { + "x": 1.5910279750823977, + "y": 4.08027458190918, + "heading": -3.14981250987545e-26, + "angularVelocity": -2.372132344513748, + "velocityX": 1.4098462654861337, + "velocityY": 1.4545079747711904, + "timestamp": 0.7669521304064016 + }, + { + "x": 1.765089887743631, + "y": 4.178521763746107, + "heading": -0.13944160626704527, + "angularVelocity": -1.619681245622387, + "velocityX": 2.021812736254959, + "velocityY": 1.1411882157451263, + "timestamp": 0.8530441356351606 + }, + { + "x": 1.9880237520646193, + "y": 4.245061751353186, + "heading": -0.20350198247198673, + "angularVelocity": -0.7440920447226632, + "velocityX": 2.589483933248167, + "velocityY": 0.7728939223830609, + "timestamp": 0.9391361408639195 + }, + { + "x": 2.2451970133288186, + "y": 4.2634626429072915, + "heading": -0.20350197796884295, + "angularVelocity": 5.2306178076003237e-8, + "velocityX": 2.98719097761578, + "velocityY": 0.21373519533215735, + "timestamp": 1.0252281460926784 + }, + { + "x": 2.4993613384299262, + "y": 4.220136155857836, + "heading": -0.20350195456811618, + "angularVelocity": 2.718106834118565e-7, + "velocityX": 2.9522407385651688, + "velocityY": -0.5032579614603062, + "timestamp": 1.1113201513214372 + }, + { + "x": 2.732767913883175, + "y": 4.145066843585559, + "heading": -0.1654270851112631, + "angularVelocity": 0.4422579002043526, + "velocityX": 2.711129504221143, + "velocityY": -0.8719661259231624, + "timestamp": 1.197412156550196 + }, + { + "x": 2.9079624706558325, + "y": 4.088324361045792, + "heading": -0.09227635108510589, + "angularVelocity": 0.8496809178945844, + "velocityX": 2.034968941740179, + "velocityY": -0.6590911942286967, + "timestamp": 1.2835041617789549 + }, + { + "x": 3.0244537137790224, + "y": 4.050518775847017, + "heading": -0.03280274151338361, + "angularVelocity": 0.690814546759508, + "velocityX": 1.3531017521738042, + "velocityY": -0.4391300341805365, + "timestamp": 1.3695961670077137 + }, + { + "x": 3.082585573196411, + "y": 4.031636714935303, + "heading": 1.5144082138928652e-26, + "angularVelocity": 0.3810196013697437, + "velocityX": 0.6752294741297277, + "velocityY": -0.21932420857827498, + "timestamp": 1.4556881722364725 + }, + { + "x": 3.082585573196411, + "y": 4.031636714935303, + "heading": 7.518751599458815e-27, + "angularVelocity": -2.1309887488531084e-27, + "velocityX": 7.927389728497897e-27, + "velocityY": -1.297368142908954e-26, + "timestamp": 1.5417801774652313 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 2.911, - "y": 5.349, - "heading": 3.132, + "x": 1.785578966140747, + "y": 3.026456832885742, + "heading": 2.2752903910371143, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 19 + }, + { + "timestamp": 0.7669521304064016, + "isStopPoint": false, + "x": 1.5910279750823975, + "y": 4.08027458190918, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "timestamp": 0.6467462497543986, + "timestamp": 1.5417801774652313, "isStopPoint": true, - "x": 3.133242607116699, - "y": 6.188713073730469, - "heading": 3.402195063790063, + "x": 3.082585573196411, + "y": 4.031636714935303, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -839,6 +1706,511 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], + "isTrajectoryStale": true + }, + "Amp 2 Piece 3": { + "waypoints": [ + { + "x": 2.8635880947113037, + "y": 7.026061534881592, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 3.104858160018921, + "y": 6.51513671875, + "heading": -2.8321171645728587, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2.8635880947113037, + "y": 7.026061534881592, + "heading": -2.2531013668007383e-23, + "angularVelocity": -1.7770917071805522e-23, + "velocityX": 2.7311091089533584e-24, + "velocityY": -1.1012176566640825e-22, + "timestamp": 0 + }, + { + "x": 2.864676594934698, + "y": 7.022588105747808, + "heading": -0.04985721436534877, + "angularVelocity": -1.3681375614641862, + "velocityX": 0.029869659992932625, + "velocityY": -0.09531476889565428, + "timestamp": 0.03644166768726924 + }, + { + "x": 2.8669918803645973, + "y": 7.015936901032802, + "heading": -0.1499743939379529, + "angularVelocity": -2.74732705516602, + "velocityX": 0.06353401413372199, + "velocityY": -0.1825164746049429, + "timestamp": 0.07288333537453848 + }, + { + "x": 2.871502989712911, + "y": 7.006569710493649, + "heading": -0.2998918812860076, + "angularVelocity": -4.113903036342868, + "velocityX": 0.1237898711723712, + "velocityY": -0.2570461544060021, + "timestamp": 0.10932500306180772 + }, + { + "x": 2.880663781149505, + "y": 6.993184946483063, + "heading": -0.49046362617983097, + "angularVelocity": -5.229501199814709, + "velocityX": 0.25138233286165845, + "velocityY": -0.36729285074021994, + "timestamp": 0.14576667074907695 + }, + { + "x": 2.895739747170883, + "y": 6.971979769048049, + "heading": -0.7007614331389338, + "angularVelocity": -5.7708063408022765, + "velocityX": 0.4137013198944287, + "velocityY": -0.5818937161984139, + "timestamp": 0.1822083384363462 + }, + { + "x": 2.9168343346087933, + "y": 6.94248561361002, + "heading": -0.895115264947589, + "angularVelocity": -5.33328588242826, + "velocityX": 0.5788590033512745, + "velocityY": -0.8093525162223337, + "timestamp": 0.21865000612361546 + }, + { + "x": 2.938346654426208, + "y": 6.902931144993223, + "heading": -1.0726747966927241, + "angularVelocity": -4.872431560182551, + "velocityX": 0.590322045687563, + "velocityY": -1.0854187288090003, + "timestamp": 0.2550916738108847 + }, + { + "x": 2.9553323200852546, + "y": 6.85423688933496, + "heading": -1.2351730413824855, + "angularVelocity": -4.459133047484804, + "velocityX": 0.46610560759215897, + "velocityY": -1.3362246776448974, + "timestamp": 0.29153334149815396 + }, + { + "x": 2.9695732769404084, + "y": 6.79656717365057, + "heading": -1.3761597087915507, + "angularVelocity": -3.8688313778328625, + "velocityX": 0.39078773719588467, + "velocityY": -1.582521309927206, + "timestamp": 0.3279750091854232 + }, + { + "x": 2.987706815188453, + "y": 6.731391881244193, + "heading": -1.4895165523111065, + "angularVelocity": -3.110638198348881, + "velocityX": 0.49760451150759183, + "velocityY": -1.7884827051739542, + "timestamp": 0.36441667687269247 + }, + { + "x": 3.0070752529330744, + "y": 6.676346817724788, + "heading": -1.6191852506186655, + "angularVelocity": -3.5582536842258268, + "velocityX": 0.5314915308166857, + "velocityY": -1.5104979275861887, + "timestamp": 0.4008583445599617 + }, + { + "x": 3.0277127553427983, + "y": 6.631345901214396, + "heading": -1.7659714627857621, + "angularVelocity": -4.027977353472648, + "velocityX": 0.5663160804502753, + "velocityY": -1.2348753327255706, + "timestamp": 0.437300012247231 + }, + { + "x": 3.0496224874417317, + "y": 6.596113415691361, + "heading": -1.9320800211636182, + "angularVelocity": -4.558204081200309, + "velocityX": 0.6012274818747366, + "velocityY": -0.9668186929695299, + "timestamp": 0.4737416799345002 + }, + { + "x": 3.071978131409277, + "y": 6.569951938634911, + "heading": -2.1214170567465316, + "angularVelocity": -5.195619399412323, + "velocityX": 0.6134638008170941, + "velocityY": -0.7179001049282177, + "timestamp": 0.5101833476217694 + }, + { + "x": 3.087603628473299, + "y": 6.549928885874662, + "heading": -2.3361436590449416, + "angularVelocity": -5.892337423773383, + "velocityX": 0.4287810645252342, + "velocityY": -0.5494548968526693, + "timestamp": 0.5466250153090386 + }, + { + "x": 3.095096900142376, + "y": 6.535588060309454, + "heading": -2.5324942002744897, + "angularVelocity": -5.388077815608382, + "velocityX": 0.20562373087262698, + "velocityY": -0.3935282459704378, + "timestamp": 0.5830666829963078 + }, + { + "x": 3.100123151105858, + "y": 6.5252451428378, + "heading": -2.6818042895708434, + "angularVelocity": -4.097235356451997, + "velocityX": 0.13792593156315622, + "velocityY": -0.28382118953540003, + "timestamp": 0.619508350683577 + }, + { + "x": 3.103300439410377, + "y": 6.518482658591172, + "heading": -2.781921479680782, + "angularVelocity": -2.7473273443222164, + "velocityX": 0.08718833429318376, + "velocityY": -0.18557010904826304, + "timestamp": 0.6559500183708462 + }, + { + "x": 3.104858160018921, + "y": 6.51513671875, + "heading": -2.8321171645728587, + "angularVelocity": -1.3774255701698461, + "velocityX": 0.042745590622028554, + "velocityY": -0.0918163205341267, + "timestamp": 0.6923916860581154 + }, + { + "x": 3.104858160018921, + "y": 6.51513671875, + "heading": -2.8321171645728587, + "angularVelocity": 2.0190189525600836e-23, + "velocityX": -7.10768769656816e-23, + "velocityY": 2.4419134906662144e-23, + "timestamp": 0.7288333537453846 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 2.8635880947113037, + "y": 7.026061534881592, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "timestamp": 0.7288333537453846, + "isStopPoint": true, + "x": 3.104858160018921, + "y": 6.51513671875, + "heading": -2.8321171645728587, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "Amp 3 Piece 6": { + "waypoints": [ + { + "x": 2.57999587059021, + "y": 4.08027458190918, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 2.4016571044921875, + "y": 3.2534329891204834, + "heading": 2.487823343289701, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2.57999587059021, + "y": 4.08027458190918, + "heading": 1.0971166550765098e-23, + "angularVelocity": 1.1911120416893396e-24, + "velocityX": 1.392755277940113e-23, + "velocityY": 2.3707203164248613e-24, + "timestamp": 0 + }, + { + "x": 2.579662827473817, + "y": 4.073976766895919, + "heading": 0.05900902390512706, + "angularVelocity": 1.4321204231514015, + "velocityX": -0.00808279509153697, + "velocityY": -0.15284491938419753, + "timestamp": 0.0412039539072258 + }, + { + "x": 2.579370687354804, + "y": 4.061287185375009, + "heading": 0.17643622955934568, + "angularVelocity": 2.84990139341035, + "velocityX": -0.0070900991606351405, + "velocityY": -0.3079699960222945, + "timestamp": 0.0824079078144516 + }, + { + "x": 2.579579590471969, + "y": 4.042200777159572, + "heading": 0.3514246001504735, + "angularVelocity": 4.246882980820942, + "velocityX": 0.005069977450094339, + "velocityY": -0.4632178809444451, + "timestamp": 0.1236118617216774 + }, + { + "x": 2.5750606099681796, + "y": 4.01194105258541, + "heading": 0.5521013175478051, + "angularVelocity": 4.870326713042449, + "velocityX": -0.10967346759885296, + "velocityY": -0.7343888560377853, + "timestamp": 0.1648158156289032 + }, + { + "x": 2.56423762146791, + "y": 3.9690933804161306, + "heading": 0.747472147114967, + "angularVelocity": 4.741555385850978, + "velocityX": -0.2626686876856264, + "velocityY": -1.039892245917828, + "timestamp": 0.20601976953612902 + }, + { + "x": 2.5467043297801593, + "y": 3.9154287676491246, + "heading": 0.9189990053029754, + "angularVelocity": 4.162873751733042, + "velocityX": -0.4255244952275232, + "velocityY": -1.3024141539386382, + "timestamp": 0.24722372344335483 + }, + { + "x": 2.523298157292756, + "y": 3.8503922530083323, + "heading": 1.0672567739768406, + "angularVelocity": 3.5981442219763733, + "velocityX": -0.5680564671075964, + "velocityY": -1.5784047032774475, + "timestamp": 0.2884276773505806 + }, + { + "x": 2.4960522044580586, + "y": 3.773392408577416, + "heading": 1.1901950139788295, + "angularVelocity": 2.9836515272003945, + "velocityX": -0.6612460759480528, + "velocityY": -1.8687489216274693, + "timestamp": 0.3296316312578064 + }, + { + "x": 2.469923707904784, + "y": 3.684349647681216, + "heading": 1.2853707724318124, + "angularVelocity": 2.3098695495893162, + "velocityX": -0.634125953351582, + "velocityY": -2.1610246700277287, + "timestamp": 0.37083558516503223 + }, + { + "x": 2.4521201557482595, + "y": 3.5926952586845426, + "heading": 1.3789369657166912, + "angularVelocity": 2.2708061827161288, + "velocityX": -0.43208358587652174, + "velocityY": -2.2244076188183466, + "timestamp": 0.41203953907225804 + }, + { + "x": 2.4455284878881836, + "y": 3.507277328616976, + "heading": 1.4937827841922569, + "angularVelocity": 2.7872523771420314, + "velocityX": -0.15997658561888023, + "velocityY": -2.0730517818724965, + "timestamp": 0.45324349297948385 + }, + { + "x": 2.4410469344735444, + "y": 3.434231880209267, + "heading": 1.6336884732647516, + "angularVelocity": 3.3954432962308463, + "velocityX": -0.1087651302768112, + "velocityY": -1.772777645858383, + "timestamp": 0.49444744688670966 + }, + { + "x": 2.435553400376216, + "y": 3.374006755150976, + "heading": 1.7961672522867937, + "angularVelocity": 3.943281253733004, + "velocityX": -0.13332541118985913, + "velocityY": -1.4616346089963215, + "timestamp": 0.5356514007939355 + }, + { + "x": 2.427518633441147, + "y": 3.3263479617338527, + "heading": 1.9814413179082384, + "angularVelocity": 4.4965118162835775, + "velocityX": -0.19499990105705994, + "velocityY": -1.1566558278467975, + "timestamp": 0.5768553547011612 + }, + { + "x": 2.418456777536987, + "y": 3.292689475234703, + "heading": 2.160196350293327, + "angularVelocity": 4.3382980377943925, + "velocityX": -0.21992685276183732, + "velocityY": -0.8168751614210406, + "timestamp": 0.618059308608387 + }, + { + "x": 2.40957775522149, + "y": 3.271735169772442, + "heading": 2.3172806828535286, + "angularVelocity": 3.8123606514532646, + "velocityX": -0.21548957013905015, + "velocityY": -0.5085508422187158, + "timestamp": 0.6592632625156127 + }, + { + "x": 2.404195229874686, + "y": 3.2593131748644586, + "heading": 2.4299841661174084, + "angularVelocity": 2.735258939412498, + "velocityX": -0.13063128259300513, + "velocityY": -0.30147579855934886, + "timestamp": 0.7004672164228385 + }, + { + "x": 2.4016571044921875, + "y": 3.2534329891204834, + "heading": 2.487823343289701, + "angularVelocity": 1.4037288096798257, + "velocityX": -0.06159907343390637, + "velocityY": -0.14270925933989237, + "timestamp": 0.7416711703300642 + }, + { + "x": 2.4016571044921875, + "y": 3.2534329891204834, + "heading": 2.487823343289701, + "angularVelocity": -7.318967086218513e-25, + "velocityX": 1.3814485584668107e-23, + "velocityY": -6.25634806032e-24, + "timestamp": 0.78287512423729 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 2.57999587059021, + "y": 4.08027458190918, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "timestamp": 0.78287512423729, + "isStopPoint": true, + "x": 2.4016571044921875, + "y": 3.2534329891204834, + "heading": 2.487823343289701, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], "isTrajectoryStale": false } }, diff --git a/Close2Note.chor b/Close2Note.chor index 379847e..effe215 100644 --- a/Close2Note.chor +++ b/Close2Note.chor @@ -25,9 +25,9 @@ "controlIntervalCount": 13 }, { - "x": 2.059075355529785, - "y": 3.3642866611480713, - "heading": -3.8394954663680236, + "x": 1.9152796268463135, + "y": 3.496621608734131, + "heading": -3.9857466301949205, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -39,127 +39,127 @@ "x": 0.4625682830810547, "y": 4.048226833343506, "heading": -3.141592530780722, - "angularVelocity": 2.456306710221323e-28, - "velocityX": 1.788801844274686e-27, - "velocityY": -7.82277930953085e-28, + "angularVelocity": 5.759379414997178e-23, + "velocityX": 3.901586197109837e-23, + "velocityY": 3.074737368166392e-23, "timestamp": 0 }, { - "x": 0.5005081965427643, - "y": 4.031977412921586, - "heading": -3.1694044453423356, - "angularVelocity": -0.39190233323436413, - "velocityX": 0.534617657314254, - "velocityY": -0.22897329714388057, - "timestamp": 0.07096644291980092 - }, - { - "x": 0.5764839525466269, - "y": 3.9994312658249234, - "heading": -3.2229174521355954, - "angularVelocity": -0.7540607164675686, - "velocityX": 1.0705870673231095, - "velocityY": -0.4586131945973854, - "timestamp": 0.14193288583960184 - }, - { - "x": 0.6906355955542217, - "y": 3.9505270752114026, - "heading": -3.298712638550308, - "angularVelocity": -1.0680426310836606, - "velocityX": 1.6085298672303114, - "velocityY": -0.6891171179142679, - "timestamp": 0.21289932875940276 - }, - { - "x": 0.843174414098898, - "y": 3.8851948830384924, - "heading": -3.390518020683689, - "angularVelocity": -1.2936449729787134, - "velocityX": 2.1494499691503526, - "velocityY": -0.9206068316928638, - "timestamp": 0.2838657716792037 - }, - { - "x": 1.0343457963046487, - "y": 3.803412500195947, - "heading": -3.484313484917418, - "angularVelocity": -1.3216875522382732, - "velocityX": 2.6938278761103085, - "velocityY": -1.1524092159299486, - "timestamp": 0.3548322145990046 - }, - { - "x": 1.2602014683101048, - "y": 3.7073597002170535, - "heading": -3.5049392152200776, - "angularVelocity": -0.29064061060477225, - "velocityX": 3.1825699966489127, - "velocityY": -1.353496047243663, - "timestamp": 0.4257986575188055 - }, - { - "x": 1.486615288654731, - "y": 3.610382149167226, - "heading": -3.5229483465773357, - "angularVelocity": -0.2537696778406977, - "velocityX": 3.190434958118108, - "velocityY": -1.3665268690361414, - "timestamp": 0.49676510043860644 - }, - { - "x": 1.6779759070617781, - "y": 3.5282783831779434, - "heading": -3.6088796447422054, - "angularVelocity": -1.2108722746887648, - "velocityX": 2.6964944350290114, - "velocityY": -1.156937879528049, - "timestamp": 0.5677315433584074 - }, - { - "x": 1.8307040868272946, - "y": 3.4626308105116546, - "heading": -3.6935584190600492, - "angularVelocity": -1.193222751963764, - "velocityX": 2.1521182897403275, - "velocityY": -0.9250509108999159, - "timestamp": 0.6386979862782083 - }, - { - "x": 1.9450099109679613, - "y": 3.4134373887939176, - "heading": -3.763746700694216, - "angularVelocity": -0.9890347993556098, - "velocityX": 1.6107024593277632, - "velocityY": -0.6931927217111649, - "timestamp": 0.7096644291980092 - }, - { - "x": 2.0210874791239872, - "y": 3.380664133786014, - "heading": -3.8135102476904286, - "angularVelocity": -0.701226452232503, - "velocityX": 1.0720217193639106, - "velocityY": -0.461813410106245, - "timestamp": 0.7806308721178101 - }, - { - "x": 2.059075355529785, - "y": 3.3642866611480713, - "heading": -3.839495466368024, - "angularVelocity": -0.36616205643788113, - "velocityX": 0.5352935111701761, - "velocityY": -0.23077770230714698, - "timestamp": 0.851597315037611 - }, - { - "x": 2.059075355529785, - "y": 3.3642866611480713, - "heading": -3.839495466368024, - "angularVelocity": -2.0248765755018437e-27, - "velocityX": 6.656997760587202e-28, - "velocityY": -7.813409565467552e-28, - "timestamp": 0.922563757957412 + "x": 0.49719645689563025, + "y": 4.035311227037971, + "heading": -3.1685878765684166, + "angularVelocity": -0.40141935074427276, + "velocityX": 0.5149190960333256, + "velocityY": -0.19205437627695554, + "timestamp": 0.06724973705837163 + }, + { + "x": 0.5665241731222844, + "y": 4.009400966165659, + "heading": -3.2208545680350382, + "angularVelocity": -0.7772029119051421, + "velocityX": 1.0308994393015785, + "velocityY": -0.38528419597866803, + "timestamp": 0.13449947411674326 + }, + { + "x": 0.6706558658845477, + "y": 3.9703839131162932, + "heading": -3.295610335882003, + "angularVelocity": -1.1116142771246529, + "velocityX": 1.5484327124116424, + "velocityY": -0.5801814959588587, + "timestamp": 0.2017492111751149 + }, + { + "x": 0.8097504058175827, + "y": 3.918094176827053, + "heading": -3.387768258841931, + "angularVelocity": -1.370383394658279, + "velocityX": 2.06832838338539, + "velocityY": -0.7775455871872714, + "timestamp": 0.2689989482334865 + }, + { + "x": 0.9839944627129134, + "y": 3.8522600039621713, + "heading": -3.486174940623861, + "angularVelocity": -1.4633021047578973, + "velocityX": 2.5909998241939562, + "velocityY": -0.9789506360112324, + "timestamp": 0.33624868529185814 + }, + { + "x": 1.1913257283544783, + "y": 3.772870322561364, + "heading": -3.5398918687364964, + "angularVelocity": -0.7987678534119754, + "velocityX": 3.0830048519238766, + "velocityY": -1.180520324293636, + "timestamp": 0.40349842235022976 + }, + { + "x": 1.3968116539650544, + "y": 3.6932929076649863, + "heading": -3.596464396112961, + "angularVelocity": -0.8412304620218959, + "velocityX": 3.055564744174655, + "velocityY": -1.1833119113507133, + "timestamp": 0.4707481594086014 + }, + { + "x": 1.5702772156145808, + "y": 3.627313135982868, + "heading": -3.7079920004982503, + "angularVelocity": -1.6584095234228897, + "velocityX": 2.5794236414480194, + "velocityY": -0.9811156826509139, + "timestamp": 0.537997896466973 + }, + { + "x": 1.7086059487553384, + "y": 3.5748707707048286, + "heading": -3.812251230661302, + "angularVelocity": -1.5503291867529019, + "velocityX": 2.0569408772660527, + "velocityY": -0.7798151721027226, + "timestamp": 0.6052476335253447 + }, + { + "x": 1.8120769868060278, + "y": 3.5356871260751257, + "heading": -3.8965156601796274, + "angularVelocity": -1.2530075685676696, + "velocityX": 1.5386088121188892, + "velocityY": -0.5826587038652028, + "timestamp": 0.6724973705837164 + }, + { + "x": 1.880917107973145, + "y": 3.5096285360361232, + "heading": -3.9553366407492576, + "angularVelocity": -0.8746648409728991, + "velocityX": 1.023648926796031, + "velocityY": -0.38748984276893983, + "timestamp": 0.7397471076420881 + }, + { + "x": 1.9152796268463137, + "y": 3.496621608734131, + "heading": -3.985746630194921, + "angularVelocity": -0.4521949196510347, + "velocityX": 0.5109688212363194, + "velocityY": -0.19341231461921185, + "timestamp": 0.8069968447004597 + }, + { + "x": 1.9152796268463137, + "y": 3.496621608734131, + "heading": -3.985746630194921, + "angularVelocity": -7.373947357741041e-23, + "velocityX": 1.569986147768196e-24, + "velocityY": 8.224120381678207e-23, + "timestamp": 0.8742465817588314 } ], "trajectoryWaypoints": [ @@ -175,11 +175,11 @@ "controlIntervalCount": 13 }, { - "timestamp": 0.922563757957412, + "timestamp": 0.8742465817588314, "isStopPoint": true, - "x": 2.059075355529785, - "y": 3.3642866611480713, - "heading": -3.8394954663680236, + "x": 1.9152796268463135, + "y": 3.496621608734131, + "heading": -3.9857466301949205, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -210,27 +210,27 @@ "Close 2 Note 2": { "waypoints": [ { - "x": 2.162928342819214, - "y": 3.453303337097168, + "x": 2.3668556213378906, + "y": 4.003089904785156, "heading": 2.647, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 16 }, { - "x": 1.6146607398986816, - "y": 4.187590599060059, - "heading": 1.8033532796155989, + "x": 1.8275461196899414, + "y": 4.499822616577148, + "heading": 0.7720656380191331, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 13 }, { - "x": 2.771209239959717, - "y": 4.120928764343262, - "heading": 1.5462110292578024, + "x": 2.9771268367767334, + "y": 5.308786869049072, + "heading": -0.007825065024634177, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -239,224 +239,305 @@ ], "trajectory": [ { - "x": 2.162928342819214, - "y": 3.453303337097168, + "x": 2.36685562133789, + "y": 4.003089904785156, "heading": 2.647, - "angularVelocity": 7.022113809915831e-24, - "velocityX": -3.4078579548884074e-24, - "velocityY": 1.7716860021550897e-23, + "angularVelocity": 1.515129418425611e-22, + "velocityX": -1.1699452732974461e-22, + "velocityY": 8.049673997142983e-22, "timestamp": 0 }, { - "x": 2.1425389520968783, - "y": 3.467250631324219, - "heading": 2.6339713166813827, - "angularVelocity": -0.23809760264625404, - "velocityX": -0.37261363498404976, - "velocityY": 0.2548851052444871, - "timestamp": 0.05471992650834813 - }, - { - "x": 2.1020164250363758, - "y": 3.495473507452917, - "heading": 2.607355879948924, - "angularVelocity": -0.48639386839085036, - "velocityX": -0.7405442522719768, - "velocityY": 0.5157696278044568, - "timestamp": 0.10943985301669626 - }, - { - "x": 2.0417796658114202, - "y": 3.538482116702539, - "heading": 2.566266686397403, - "angularVelocity": -0.7509000134576662, - "velocityX": -1.1008194467469847, - "velocityY": 0.7859771018343888, - "timestamp": 0.1641597795250444 - }, - { - "x": 1.9626298042618509, - "y": 3.597171847629263, - "heading": 2.50909305220371, - "angularVelocity": -1.044841209444435, - "velocityX": -1.446454090860199, - "velocityY": 1.0725476928001763, - "timestamp": 0.21887970603339252 - }, - { - "x": 1.8666391340457336, - "y": 3.673472522298835, - "heading": 2.432100025693096, - "angularVelocity": -1.4070381929125568, - "velocityX": -1.7542178204766536, - "velocityY": 1.3943855472454167, - "timestamp": 0.27359963254174063 - }, - { - "x": 1.7650455993955068, - "y": 3.7727148333401828, - "heading": 2.326049396431328, - "angularVelocity": -1.9380623481939154, - "velocityX": -1.8566094863948293, - "velocityY": 1.8136411609801288, - "timestamp": 0.32831955905008875 - }, - { - "x": 1.6882967509494158, - "y": 3.87295238025058, - "heading": 2.211959011327005, - "angularVelocity": -2.0849879081419744, - "velocityX": -1.4025758684888243, - "velocityY": 1.8318289754118107, - "timestamp": 0.38303948555843687 - }, - { - "x": 1.6354914799537714, - "y": 3.9661527572411814, - "heading": 2.1004292198963976, - "angularVelocity": -2.0381933702632526, - "velocityX": -0.9650099034323164, - "velocityY": 1.7032255512328316, - "timestamp": 0.437759412066785 - }, - { - "x": 1.6058693049320312, - "y": 4.050214103514873, - "heading": 1.9943710354808801, - "angularVelocity": -1.938200417709569, - "velocityX": -0.5413416448434165, - "velocityY": 1.536210876687981, - "timestamp": 0.4924793385751331 - }, - { - "x": 1.5990118210425874, - "y": 4.124201694067412, - "heading": 1.895096592298551, - "angularVelocity": -1.814228371947535, - "velocityX": -0.12531968383396322, - "velocityY": 1.3521142164043614, - "timestamp": 0.5471992650834813 - }, - { - "x": 1.6146607398986816, - "y": 4.187590599060059, - "heading": 1.8033532796155989, - "angularVelocity": -1.6765978782693645, - "velocityX": 0.28598208832950334, - "velocityY": 1.158424527177966, - "timestamp": 0.6019191915918294 - }, - { - "x": 1.6905120212342348, - "y": 4.258849164477337, - "heading": 1.6820774903470974, - "angularVelocity": -1.4527350214804762, - "velocityX": 0.9086052004688632, - "velocityY": 0.8535901038990373, - "timestamp": 0.6854002032527928 - }, - { - "x": 1.8179066252511533, - "y": 4.303857525516893, - "heading": 1.5806423784124122, - "angularVelocity": -1.2150680725652636, - "velocityX": 1.5260309078943428, - "velocityY": 0.5391448922821541, - "timestamp": 0.7688812149137563 - }, - { - "x": 1.9959545196143786, - "y": 4.321064540954123, - "heading": 1.501233745672766, - "angularVelocity": -0.9512179016486254, - "velocityX": 2.1327951209590146, - "velocityY": 0.20611891368915536, - "timestamp": 0.8523622265747197 - }, - { - "x": 2.221863436317024, - "y": 4.306254552246914, - "heading": 1.4497152819618502, - "angularVelocity": -0.6171279274878088, - "velocityX": 2.706111392374063, - "velocityY": -0.1774054771563637, - "timestamp": 0.9358432382356832 - }, - { - "x": 2.443298757309218, - "y": 4.237785772933149, - "heading": 1.481002566027846, - "angularVelocity": 0.37478324044587696, - "velocityX": 2.652523209607188, - "velocityY": -0.8201718924039002, - "timestamp": 1.0193242498966466 - }, - { - "x": 2.6077641513687433, - "y": 4.180765687739868, - "heading": 1.5117621140320452, - "angularVelocity": 0.3684616105171448, - "velocityX": 1.9700934474471774, - "velocityY": -0.6830305965247938, - "timestamp": 1.10280526155761 - }, - { - "x": 2.7168458549001153, - "y": 4.14118234012958, - "heading": 1.5343226948830555, - "angularVelocity": 0.2702480528462504, - "velocityX": 1.3066648494196378, - "velocityY": -0.47415989364199385, - "timestamp": 1.1862862732185735 - }, - { - "x": 2.771209239959717, - "y": 4.120928764343262, - "heading": 1.5462110292578024, - "angularVelocity": 0.14240764622053664, - "velocityX": 0.651206591510705, - "velocityY": -0.24261296531207407, - "timestamp": 1.269767284879537 - }, - { - "x": 2.771209239959717, - "y": 4.120928764343262, - "heading": 1.5462110292578024, - "angularVelocity": 1.3345092530168404e-23, - "velocityX": -1.0052627512755276e-23, - "velocityY": 2.977034014839026e-23, - "timestamp": 1.3532482965405004 + "x": 2.3582182665685285, + "y": 4.005140044275413, + "heading": 2.626572128421542, + "angularVelocity": -0.5800138561279072, + "velocityX": -0.24524265424721656, + "velocityY": 0.05821014229392081, + "timestamp": 0.0352196268462121 + }, + { + "x": 2.340949746064586, + "y": 4.0093581184378255, + "heading": 2.5859880369259955, + "angularVelocity": -1.1523146361759788, + "velocityX": -0.4903095816263469, + "velocityY": 0.11976487374015862, + "timestamp": 0.0704392536924242 + }, + { + "x": 2.315025310616676, + "y": 4.0158942358801255, + "heading": 2.5258878520188563, + "angularVelocity": -1.7064401383231338, + "velocityX": -0.7360792197199094, + "velocityY": 0.18558167782148904, + "timestamp": 0.10565888053863629 + }, + { + "x": 2.2803832616948174, + "y": 4.024926514000347, + "heading": 2.4473235520751233, + "angularVelocity": -2.230696545616096, + "velocityX": -0.9836006801903231, + "velocityY": 0.25645581537904877, + "timestamp": 0.1408785073848484 + }, + { + "x": 2.2369349430382, + "y": 4.036673464094862, + "heading": 2.351697628937878, + "angularVelocity": -2.7151316382424904, + "velocityX": -1.2336393808559734, + "velocityY": 0.3335342008518269, + "timestamp": 0.1760981342310605 + }, + { + "x": 2.184628025978604, + "y": 4.05150435046694, + "heading": 2.240519075284163, + "angularVelocity": -3.1567215104004607, + "velocityX": -1.485163862980013, + "velocityY": 0.42109720346804624, + "timestamp": 0.2113177610772726 + }, + { + "x": 2.1236673593002187, + "y": 4.070437803424587, + "heading": 2.115442529811484, + "angularVelocity": -3.5513307968545753, + "velocityX": -1.7308720204371333, + "velocityY": 0.5375824406184673, + "timestamp": 0.2465373879234847 + }, + { + "x": 2.056940171048295, + "y": 4.097882582880828, + "heading": 1.9863138442222075, + "angularVelocity": -3.6663842621934117, + "velocityX": -1.8946023631436484, + "velocityY": 0.779246741484578, + "timestamp": 0.2817570147696968 + }, + { + "x": 1.9961028754422039, + "y": 4.13376425540664, + "heading": 1.8534458383236918, + "angularVelocity": -3.7725557536054866, + "velocityX": -1.727369113586192, + "velocityY": 1.0187976346956282, + "timestamp": 0.3169766416159089 + }, + { + "x": 1.9435464476369881, + "y": 4.17533041611823, + "heading": 1.7117204090724802, + "angularVelocity": -4.0240468722187535, + "velocityX": -1.4922482862928892, + "velocityY": 1.1801987821502666, + "timestamp": 0.352196268462121 + }, + { + "x": 1.8999454238017455, + "y": 4.221481056301366, + "heading": 1.5613962576553257, + "angularVelocity": -4.268192620936928, + "velocityX": -1.2379751786044981, + "velocityY": 1.3103670968625691, + "timestamp": 0.3874158953083331 + }, + { + "x": 1.8656935910383048, + "y": 4.271628463138376, + "heading": 1.4043136517982262, + "angularVelocity": -4.460087170798457, + "velocityX": -0.9725211715890388, + "velocityY": 1.4238483291143482, + "timestamp": 0.4226355221545452 + }, + { + "x": 1.8411062322404879, + "y": 4.325262090117391, + "heading": 1.2429884596951064, + "angularVelocity": -4.580548022486237, + "velocityX": -0.6981152555995893, + "velocityY": 1.5228334818326028, + "timestamp": 0.4578551490007573 + }, + { + "x": 1.8264926299504727, + "y": 4.381633415364751, + "heading": 1.0814768977873637, + "angularVelocity": -4.585839668687841, + "velocityX": -0.4149278001666941, + "velocityY": 1.6005656588446922, + "timestamp": 0.4930747758469694 + }, + { + "x": 1.8219761515922521, + "y": 4.439993109865742, + "heading": 0.9236480211125673, + "angularVelocity": -4.48127623168645, + "velocityX": -0.1282375414691572, + "velocityY": 1.657021942787722, + "timestamp": 0.5282944026931815 + }, + { + "x": 1.8275461196899416, + "y": 4.499822616577148, + "heading": 0.7720656380191331, + "angularVelocity": -4.303917919270544, + "velocityX": 0.1581495488866161, + "velocityY": 1.6987547021055334, + "timestamp": 0.5635140295393936 + }, + { + "x": 1.863631380068701, + "y": 4.6009476633877675, + "heading": 0.5458535834271052, + "angularVelocity": -3.9160123080463913, + "velocityX": 0.624680784749141, + "velocityY": 1.750600465020928, + "timestamp": 0.6212799497657474 + }, + { + "x": 1.9264611646703016, + "y": 4.704317253907965, + "heading": 0.34642581420132385, + "angularVelocity": -3.452342980849784, + "velocityX": 1.0876617970494085, + "velocityY": 1.7894563111806223, + "timestamp": 0.6790458699921013 + }, + { + "x": 2.015600664927885, + "y": 4.808735070353454, + "heading": 0.18046889819718334, + "angularVelocity": -2.8729208390317975, + "velocityX": 1.5431157317030553, + "velocityY": 1.8076024070304682, + "timestamp": 0.7368117902184551 + }, + { + "x": 2.1302379264615894, + "y": 4.912412090690155, + "heading": 0.05682984585261984, + "angularVelocity": -2.1403459316511855, + "velocityX": 1.98451372512552, + "velocityY": 1.7947783040671845, + "timestamp": 0.794577710444809 + }, + { + "x": 2.2701009625494035, + "y": 5.012833103004185, + "heading": -0.02381934322313901, + "angularVelocity": -1.3961378743684443, + "velocityX": 2.4212032897555655, + "velocityY": 1.7384127513329013, + "timestamp": 0.8523436306711628 + }, + { + "x": 2.432555238928894, + "y": 5.102059929533658, + "heading": -0.06105728015599246, + "angularVelocity": -0.6446350510290115, + "velocityX": 2.8122857862028177, + "velocityY": 1.5446274581939572, + "timestamp": 0.9101095508975167 + }, + { + "x": 2.5850691468596714, + "y": 5.167502593301118, + "heading": -0.059583445184643466, + "angularVelocity": 0.025513918337556375, + "velocityX": 2.6402056321984575, + "velocityY": 1.132893988549338, + "timestamp": 0.9678754711238705 + }, + { + "x": 2.714659135727784, + "y": 5.217422693936502, + "heading": -0.0482224195291295, + "angularVelocity": 0.1966734990284264, + "velocityX": 2.2433640520283893, + "velocityY": 0.8641790945216953, + "timestamp": 1.0256413913502245 + }, + { + "x": 2.8192112721940457, + "y": 5.255223857981348, + "heading": -0.034645348058129244, + "angularVelocity": 0.23503601116019512, + "velocityX": 1.8099276538238398, + "velocityY": 0.6543852135776276, + "timestamp": 1.0834073115765783 + }, + { + "x": 2.8980093999029637, + "y": 5.282500252165217, + "heading": -0.022245861805103868, + "angularVelocity": 0.2146505448963401, + "velocityX": 1.3640936974626836, + "velocityY": 0.4721883435253398, + "timestamp": 1.1411732318029322 + }, + { + "x": 2.950712826385929, + "y": 5.300160015762531, + "heading": -0.012907289054399772, + "angularVelocity": 0.1616623212113855, + "velocityX": 0.9123619302949761, + "velocityY": 0.3057124949811315, + "timestamp": 1.198939152029286 + }, + { + "x": 2.9771268367767334, + "y": 5.308786869049072, + "heading": -0.007825065024634177, + "angularVelocity": 0.08797962552749206, + "velocityX": 0.4572594063643587, + "velocityY": 0.14934157116754843, + "timestamp": 1.2567050722556399 + }, + { + "x": 2.9771268367767334, + "y": 5.308786869049072, + "heading": -0.007825065024634177, + "angularVelocity": -9.033896437363216e-23, + "velocityX": 2.001984212704135e-22, + "velocityY": -4.919331709829794e-22, + "timestamp": 1.3144709924819937 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 2.162928342819214, - "y": 3.453303337097168, + "x": 2.3668556213378906, + "y": 4.003089904785156, "heading": 2.647, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 11 + "controlIntervalCount": 16 }, { - "timestamp": 0.6019191915918294, + "timestamp": 0.5635140295393936, "isStopPoint": false, - "x": 1.6146607398986816, - "y": 4.187590599060059, - "heading": 1.8033532796155989, + "x": 1.8275461196899414, + "y": 4.499822616577148, + "heading": 0.7720656380191331, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 13 }, { - "timestamp": 1.3532482965405004, + "timestamp": 1.3144709924819937, "isStopPoint": true, - "x": 2.771209239959717, - "y": 4.120928764343262, - "heading": 1.5462110292578024, + "x": 2.9771268367767334, + "y": 5.308786869049072, + "heading": -0.007825065024634177, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -482,23 +563,23 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "Close 2 Note 3": { "waypoints": [ { - "x": 2.629, - "y": 4.107, - "heading": 1.546, + "x": 3.048088550567627, + "y": 5.33717155456543, + "heading": -0.01821858883733709, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 21 }, { - "x": 2.501993417739868, - "y": 3.440321445465088, - "heading": 2.3895683761463378, + "x": 2.551356077194214, + "y": 4.457245349884033, + "heading": 2.647920421723555, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -507,123 +588,222 @@ ], "trajectory": [ { - "x": 2.629, - "y": 4.107, - "heading": 1.546, - "angularVelocity": -8.203167337623884e-22, - "velocityX": 4.329369978962889e-21, - "velocityY": 1.525295443830001e-20, + "x": 3.048088550567627, + "y": 5.33717155456543, + "heading": -0.01821858883733709, + "angularVelocity": 1.98280707847529e-26, + "velocityX": -3.686176979860547e-26, + "velocityY": -9.942929150065618e-26, "timestamp": 0 }, { - "x": 2.6242456552823756, - "y": 4.0804929655510955, - "heading": 1.58312669994352, - "angularVelocity": 0.6327197789954236, - "velocityX": -0.0810243825489381, - "velocityY": -0.451737563636128, - "timestamp": 0.05867795061261784 - }, - { - "x": 2.614797212256109, - "y": 4.027426968793948, - "heading": 1.656708910709224, - "angularVelocity": 1.254001034417197, - "velocityX": -0.1610220351532649, - "velocityY": -0.904360091024302, - "timestamp": 0.11735590122523568 - }, - { - "x": 2.600686059565033, - "y": 3.947699552686434, - "heading": 1.7652887549030123, - "angularVelocity": 1.8504368857497178, - "velocityX": -0.2404847569444782, - "velocityY": -1.3587287094237785, - "timestamp": 0.17603385183785353 - }, - { - "x": 2.58182712139046, - "y": 3.841172258879563, - "heading": 1.9066425962383542, - "angularVelocity": 2.408977134674257, - "velocityX": -0.3213973558666907, - "velocityY": -1.8154569594658583, - "timestamp": 0.23471180245047135 - }, - { - "x": 2.5554465223661467, - "y": 3.707426020625259, - "heading": 2.066347870244412, - "angularVelocity": 2.721725492091665, - "velocityX": -0.4495828287950398, - "velocityY": -2.2793270190582233, - "timestamp": 0.2933897530630892 - }, - { - "x": 2.5341409307456813, - "y": 3.6005266015036144, - "heading": 2.1949908224625454, - "angularVelocity": 2.1923559169169495, - "velocityX": -0.363093656101234, - "velocityY": -1.8217987848174317, - "timestamp": 0.352067703675707 - }, - { - "x": 2.518082931719197, - "y": 3.5204124576524585, - "heading": 2.2921355834233315, - "angularVelocity": 1.6555581772465078, - "velocityX": -0.2736632561095736, - "velocityY": -1.3653193919476883, - "timestamp": 0.4107456542883248 - }, - { - "x": 2.5073565100987056, - "y": 3.467021273525448, - "heading": 2.3571203843486037, - "angularVelocity": 1.1074824571548374, - "velocityX": -0.18280157211529632, - "velocityY": -0.9099019916269808, - "timestamp": 0.46942360490094265 - }, - { - "x": 2.501993417739868, - "y": 3.440321445465088, - "heading": 2.3895683761463378, - "angularVelocity": 0.5529844082652147, - "velocityX": -0.09139876738784368, - "velocityY": -0.45502318642019196, - "timestamp": 0.5281015555135605 - }, - { - "x": 2.501993417739868, - "y": 3.440321445465088, - "heading": 2.3895683761463378, - "angularVelocity": 6.196323349296843e-22, - "velocityX": -4.670139220367797e-21, - "velocityY": -1.5443840632789222e-20, - "timestamp": 0.5867795061261784 + "x": 3.0461249759665745, + "y": 5.329789651433795, + "heading": 0.022783459542432594, + "angularVelocity": 1.066293792007848, + "velocityX": -0.051064458727859946, + "velocityY": -0.19197278656813405, + "timestamp": 0.038452862322833355 + }, + { + "x": 3.0423993082753173, + "y": 5.314779984880276, + "heading": 0.10329419369255852, + "angularVelocity": 2.093751395518834, + "velocityX": -0.09688921620393233, + "velocityY": -0.3903393829958584, + "timestamp": 0.07690572464566671 + }, + { + "x": 3.032907918028394, + "y": 5.290887968080895, + "heading": 0.20545892573589697, + "angularVelocity": 2.656882371606259, + "velocityX": -0.2468318266462912, + "velocityY": -0.6213325967464687, + "timestamp": 0.11535858696850007 + }, + { + "x": 3.0165602178325317, + "y": 5.259789389738606, + "heading": 0.33465802025849095, + "angularVelocity": 3.3599343902645034, + "velocityX": -0.42513610712811417, + "velocityY": -0.8087454733850095, + "timestamp": 0.15381144929133342 + }, + { + "x": 2.9936431691568814, + "y": 5.220807962711857, + "heading": 0.48978809219771535, + "angularVelocity": 4.034291924403973, + "velocityX": -0.5959777059832116, + "velocityY": -1.0137457830701675, + "timestamp": 0.19226431161416677 + }, + { + "x": 2.9642904690085223, + "y": 5.172884357745661, + "heading": 0.6658699909928164, + "angularVelocity": 4.579162334309335, + "velocityX": -0.7633423983350427, + "velocityY": -1.246294867826739, + "timestamp": 0.23071717393700011 + }, + { + "x": 2.928089447111606, + "y": 5.114902261857335, + "heading": 0.8491068027757267, + "angularVelocity": 4.7652320455245825, + "velocityX": -0.9414389387450126, + "velocityY": -1.507874638863897, + "timestamp": 0.2691700362598335 + }, + { + "x": 2.8859836301958794, + "y": 5.047128174361195, + "heading": 1.0148826733109657, + "angularVelocity": 4.311145140339815, + "velocityX": -1.0949982490828563, + "velocityY": -1.7625238643391223, + "timestamp": 0.30762289858266684 + }, + { + "x": 2.8416687526149524, + "y": 4.969882073543266, + "heading": 1.150055375667919, + "angularVelocity": 3.5152832374896503, + "velocityX": -1.1524467855963136, + "velocityY": -2.008851777259247, + "timestamp": 0.3460757609055002 + }, + { + "x": 2.794651136420651, + "y": 4.883085920156056, + "heading": 1.2554742450273566, + "angularVelocity": 2.7415090318735467, + "velocityX": -1.222733844871212, + "velocityY": -2.2572091684230795, + "timestamp": 0.38452862322833353 + }, + { + "x": 2.7509500334287784, + "y": 4.798093290887787, + "heading": 1.3652052247880782, + "angularVelocity": 2.8536491988416444, + "velocityX": -1.1364850456378852, + "velocityY": -2.2103069611491293, + "timestamp": 0.4229814855511669 + }, + { + "x": 2.7108561849571107, + "y": 4.723392024777697, + "heading": 1.4968704984881265, + "angularVelocity": 3.424069516454839, + "velocityX": -1.0426752665395174, + "velocityY": -1.9426711458546686, + "timestamp": 0.46143434787400023 + }, + { + "x": 2.6740931092193048, + "y": 4.658985356507111, + "heading": 1.650582325216758, + "angularVelocity": 3.9974092289446306, + "velocityX": -0.9560556358369164, + "velocityY": -1.674951209869765, + "timestamp": 0.4998872101968336 + }, + { + "x": 2.640223792764239, + "y": 4.60465818865713, + "heading": 1.8276925817226235, + "angularVelocity": 4.605905667539798, + "velocityX": -0.8808009185561768, + "velocityY": -1.4128250686223396, + "timestamp": 0.538340072519667 + }, + { + "x": 2.6122072946516184, + "y": 4.558661386609658, + "heading": 2.0303012950028507, + "angularVelocity": 5.269015127644162, + "velocityX": -0.7285933067194444, + "velocityY": -1.1961866885565553, + "timestamp": 0.5767929348425004 + }, + { + "x": 2.590171635106383, + "y": 4.523029597001796, + "heading": 2.222212243469772, + "angularVelocity": 4.990810485204496, + "velocityX": -0.573056418017296, + "velocityY": -0.9266355598892387, + "timestamp": 0.6152457971653338 + }, + { + "x": 2.5738089300274605, + "y": 4.495952446465408, + "heading": 2.387016152370131, + "angularVelocity": 4.285868435923901, + "velocityX": -0.42552632211221264, + "velocityY": -0.7041647591552526, + "timestamp": 0.6536986594881672 + }, + { + "x": 2.562290479492898, + "y": 4.476281411764592, + "heading": 2.515465208376393, + "angularVelocity": 3.340428988819089, + "velocityX": -0.29954728565739847, + "velocityY": -0.5115623002435097, + "timestamp": 0.6921515218110006 + }, + { + "x": 2.5549432582798937, + "y": 4.463492222573091, + "heading": 2.6032428246599313, + "angularVelocity": 2.282732961374791, + "velocityX": -0.19107085322595763, + "velocityY": -0.332593945390306, + "timestamp": 0.730604384133834 + }, + { + "x": 2.551356077194214, + "y": 4.457245349884033, + "heading": 2.647920421723555, + "angularVelocity": 1.1618796200014927, + "velocityX": -0.09328775204205611, + "velocityY": -0.16245533652637065, + "timestamp": 0.7690572464566674 + }, + { + "x": 2.551356077194214, + "y": 4.457245349884033, + "heading": 2.647920421723555, + "angularVelocity": -2.0111425344083367e-26, + "velocityX": 8.099203026682335e-26, + "velocityY": 2.8915510028596647e-26, + "timestamp": 0.8075101087795008 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 2.629, - "y": 4.107, - "heading": 1.546, + "x": 3.048088550567627, + "y": 5.33717155456543, + "heading": -0.01821858883733709, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 21 }, { - "timestamp": 0.5867795061261784, + "timestamp": 0.8075101087795008, "isStopPoint": true, - "x": 2.501993417739868, - "y": 3.440321445465088, - "heading": 2.3895683761463378, + "x": 2.551356077194214, + "y": 4.457245349884033, + "heading": 2.647920421723555, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -656,25 +836,25 @@ { "x": 2.502, "y": 3.44, - "heading": 2.39, + "heading": 2.5104836654523037, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 27 }, { "x": 5.263677597045898, "y": 1.4059187173843384, - "heading": 1.5922215933347457, + "heading": -0.031824505216569196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 15 }, { - "x": 9.04688835144043, - "y": 1.242721438407898, - "heading": 1.5707963267948966, + "x": 8.724565505981445, + "y": 1.2106475830078125, + "heading": 0.012440027539999704, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -685,317 +865,389 @@ { "x": 2.502, "y": 3.44, - "heading": 2.39, - "angularVelocity": 1.6143473252561374e-27, - "velocityX": -4.497600413655855e-27, - "velocityY": 7.73733480239798e-27, + "heading": 2.5104836654523037, + "angularVelocity": 1.9281904905287756e-29, + "velocityX": -4.71586515216861e-27, + "velocityY": 2.3810934236992728e-27, "timestamp": 0 }, { - "x": 2.525916739364106, - "y": 3.420694982582511, - "heading": 2.32354752179907, - "angularVelocity": -1.023433961876349, - "velocityX": 0.3683414672446169, - "velocityY": -0.29731638299377017, - "timestamp": 0.06493089019549111 - }, - { - "x": 2.5746652987077048, - "y": 3.3812894837618575, - "heading": 2.2018139650877506, - "angularVelocity": -1.8748173072140173, - "velocityX": 0.7507760820285803, - "velocityY": -0.6068836989915484, - "timestamp": 0.12986178039098223 - }, - { - "x": 2.649625766646246, - "y": 3.321219957909989, - "heading": 2.0401525298056455, - "angularVelocity": -2.4897461715892337, - "velocityX": 1.1544654279781719, - "velocityY": -0.925130175653128, - "timestamp": 0.19479267058647334 - }, - { - "x": 2.7520459663591446, - "y": 3.239983238174216, - "heading": 1.8578167867140025, - "angularVelocity": -2.8081509824164503, - "velocityX": 1.5773724864164937, - "velocityY": -1.2511259200542133, - "timestamp": 0.25972356078196446 - }, - { - "x": 2.8818046504752663, - "y": 3.1376357565206274, - "heading": 1.6986985692029641, - "angularVelocity": -2.4505780997607194, - "velocityX": 1.9984122152869, - "velocityY": -1.5762525563017087, - "timestamp": 0.3246544509774556 - }, - { - "x": 3.0370045621229718, - "y": 3.020948186403271, - "heading": 1.6099501280880217, - "angularVelocity": -1.3668138669859953, - "velocityX": 2.390232309774843, - "velocityY": -1.7971041174091287, - "timestamp": 0.38958534117294674 - }, - { - "x": 3.217315269794052, - "y": 2.8895687443365436, - "heading": 1.5922226066527227, - "angularVelocity": -0.2730213829184488, - "velocityX": 2.776963431861306, - "velocityY": -2.0233734925114266, - "timestamp": 0.4545162313684379 - }, - { - "x": 3.400066011553934, - "y": 2.749461510644502, - "heading": 1.5922225170352793, - "angularVelocity": -0.0000013801973627609237, - "velocityX": 2.814542372816138, - "velocityY": -2.1577901253195986, - "timestamp": 0.519447121563929 - }, - { - "x": 3.582816703850075, - "y": 2.6093542124337037, - "heading": 1.5922224274179715, - "angularVelocity": -0.0000013801952753049937, - "velocityX": 2.814541611025554, - "velocityY": -2.15779111897233, - "timestamp": 0.5843780117594202 - }, - { - "x": 3.7655673961455407, - "y": 2.4692469142220252, - "heading": 1.5922223378006626, - "angularVelocity": -0.0000013801952903393593, - "velocityX": 2.8145416110151595, - "velocityY": -2.1577911189858847, - "timestamp": 0.6493089019549113 - }, - { - "x": 3.948318088441007, - "y": 2.3291396160103477, - "heading": 1.592222248183353, - "angularVelocity": -0.0000013801953035277997, - "velocityX": 2.8145416110151644, - "velocityY": -2.157791118985876, - "timestamp": 0.7142397921504025 - }, - { - "x": 4.131068780736474, - "y": 2.1890323177986706, - "heading": 1.5922221585660423, - "angularVelocity": -0.0000013801953177064064, - "velocityX": 2.81454161101517, - "velocityY": -2.157791118985867, - "timestamp": 0.7791706823458936 - }, - { - "x": 4.31381947303194, - "y": 2.0489250195869935, - "heading": 1.5922220689487308, - "angularVelocity": -0.000001380195332153733, - "velocityX": 2.814541611015174, - "velocityY": -2.1577911189858576, - "timestamp": 0.8441015725413847 - }, - { - "x": 4.496570165327408, - "y": 1.9088177213753177, - "heading": 1.5922219793314183, - "angularVelocity": -0.0000013801953476150256, - "velocityX": 2.8145416110151804, - "velocityY": -2.157791118985848, - "timestamp": 0.9090324627368759 - }, - { - "x": 4.679320857625259, - "y": 1.7687104231667514, - "heading": 1.5922218897141047, - "angularVelocity": -0.0000013801953620228687, - "velocityX": 2.8145416110518964, - "velocityY": -2.1577911189379546, - "timestamp": 0.973963352932367 - }, - { - "x": 4.86207172453282, - "y": 1.6286033527128316, - "heading": 1.5922218000967605, - "angularVelocity": -0.0000013801958387075775, - "velocityX": 2.8145443002142994, - "velocityY": -2.1577876112908903, - "timestamp": 1.038894243127858 - }, - { - "x": 5.054351274988585, - "y": 1.5018893260974921, - "heading": 1.592221707127701, - "angularVelocity": -0.000001431815567947987, - "velocityX": 2.9612954616340366, - "velocityY": -1.9515214751227716, - "timestamp": 1.103825133323349 + "x": 2.5078330623275535, + "y": 3.4353964642834374, + "heading": 2.421550371026829, + "angularVelocity": -1.8125731387547213, + "velocityX": 0.11888519547047234, + "velocityY": -0.09382588643594957, + "timestamp": 0.04906466532245639 + }, + { + "x": 2.5190564983313566, + "y": 3.4257216835023843, + "heading": 2.2438316854082814, + "angularVelocity": -3.622131822372945, + "velocityX": 0.22874783573966653, + "velocityY": -0.19718428154904508, + "timestamp": 0.09812933064491278 + }, + { + "x": 2.5349008514646854, + "y": 3.4113020995219303, + "heading": 1.9758859051359892, + "angularVelocity": -5.461074247859102, + "velocityX": 0.32292797737839174, + "velocityY": -0.2938893781438674, + "timestamp": 0.14719399596736915 + }, + { + "x": 2.5604353108293316, + "y": 3.394955070383565, + "heading": 1.6293166979187397, + "angularVelocity": -7.063519234047003, + "velocityX": 0.5204246110073755, + "velocityY": -0.3331731507986746, + "timestamp": 0.19625866128982555 + }, + { + "x": 2.603992889740609, + "y": 3.3729904224492855, + "heading": 1.3083335355675372, + "angularVelocity": -6.54204324520872, + "velocityX": 0.8877586064230485, + "velocityY": -0.44766733432149164, + "timestamp": 0.24532332661228196 + }, + { + "x": 2.6607200682182177, + "y": 3.3376856037523086, + "heading": 1.014097923240798, + "angularVelocity": -5.996894310661298, + "velocityX": 1.1561717196029637, + "velocityY": -0.7195569044433685, + "timestamp": 0.29438799193473836 + }, + { + "x": 2.71502692076517, + "y": 3.2838347448517657, + "heading": 0.745483337859882, + "angularVelocity": -5.474705342746399, + "velocityX": 1.1068424127637233, + "velocityY": -1.0975486849167542, + "timestamp": 0.34345265725719476 + }, + { + "x": 2.7744606723157283, + "y": 3.214460375697132, + "heading": 0.5232634756481663, + "angularVelocity": -4.529122144241102, + "velocityX": 1.2113351056194037, + "velocityY": -1.4139374781974092, + "timestamp": 0.39251732257965116 + }, + { + "x": 2.84873633367785, + "y": 3.1410738657590347, + "heading": 0.34689629796158034, + "angularVelocity": -3.594586379576598, + "velocityX": 1.513832018907633, + "velocityY": -1.4957099871322082, + "timestamp": 0.44158198790210756 + }, + { + "x": 2.940226570979376, + "y": 3.0662458182374484, + "heading": 0.20973540275964336, + "angularVelocity": -2.7955127035006972, + "velocityX": 1.8646868719117038, + "velocityY": -1.5250903482131433, + "timestamp": 0.49064665322456397 + }, + { + "x": 3.0480227715570303, + "y": 2.9863881956756497, + "heading": 0.11214926277131038, + "angularVelocity": -1.9889290866857052, + "velocityX": 2.197023048444546, + "velocityY": -1.6275994554730684, + "timestamp": 0.5397113185470204 + }, + { + "x": 3.169811200590146, + "y": 2.8977558044744653, + "heading": 0.054667190153068854, + "angularVelocity": -1.1715574179598567, + "velocityX": 2.4822023799146242, + "velocityY": -1.8064403500703765, + "timestamp": 0.5887759838694767 + }, + { + "x": 3.303879745950196, + "y": 2.7983821340483717, + "heading": 0.037123590068076556, + "angularVelocity": -0.3575607816683257, + "velocityX": 2.7324866985017113, + "velocityY": -2.02536122019794, + "timestamp": 0.6378406491919331 + }, + { + "x": 3.4431520829592457, + "y": 2.6940705143750954, + "heading": 0.037115475659100644, + "angularVelocity": -0.00016538192857494248, + "velocityX": 2.838546560824209, + "velocityY": -2.126002877788589, + "timestamp": 0.6869053145143894 + }, + { + "x": 3.5824244012585216, + "y": 2.5897588683797643, + "heading": 0.03710736322260343, + "angularVelocity": -0.0001653417269616524, + "velocityX": 2.838546179495335, + "velocityY": -2.126003414265387, + "timestamp": 0.7359699798368458 + }, + { + "x": 3.7216967195717463, + "y": 2.485447222406277, + "heading": 0.03709925077439075, + "angularVelocity": -0.00016534196573771254, + "velocityX": 2.8385461797796308, + "velocityY": -2.12600341382019, + "timestamp": 0.7850346451593021 + }, + { + "x": 3.8609690378995296, + "y": 2.381135576455448, + "heading": 0.03709113831446088, + "angularVelocity": -0.00016534220454895188, + "velocityX": 2.8385461800763494, + "velocityY": -2.1260034133583896, + "timestamp": 0.8340993104817584 + }, + { + "x": 4.000241356241871, + "y": 2.276823930527277, + "heading": 0.03708302584281285, + "angularVelocity": -0.0001653424433797745, + "velocityX": 2.8385461803730676, + "velocityY": -2.126003412896584, + "timestamp": 0.8831639758042148 + }, + { + "x": 4.139513674598771, + "y": 2.172512284621764, + "heading": 0.03707491335944571, + "angularVelocity": -0.00016534268223024974, + "velocityX": 2.838546180669785, + "velocityY": -2.1260034124347746, + "timestamp": 0.9322286411266711 + }, + { + "x": 4.278785992970229, + "y": 2.0682006387389102, + "heading": 0.03706680086435853, + "angularVelocity": -0.00016534292109930652, + "velocityX": 2.838546180966501, + "velocityY": -2.1260034119729605, + "timestamp": 0.9812933064491275 + }, + { + "x": 4.418058311356245, + "y": 1.9638889928787158, + "heading": 0.03705868835755037, + "angularVelocity": -0.00016534315998779618, + "velocityX": 2.8385461812632156, + "velocityY": -2.1260034115111432, + "timestamp": 1.0303579717715838 + }, + { + "x": 4.5573306297568195, + "y": 1.85957734704118, + "heading": 0.03705057583902021, + "angularVelocity": -0.00016534339889699094, + "velocityX": 2.8385461815599298, + "velocityY": -2.1260034110493207, + "timestamp": 1.0794226370940403 + }, + { + "x": 4.696602948173282, + "y": 1.7552657012280795, + "heading": 0.03704246330876568, + "angularVelocity": -0.00016534363785453005, + "velocityX": 2.838546181883735, + "velocityY": -2.126003410551304, + "timestamp": 1.1284873024164968 + }, + { + "x": 4.835875308437814, + "y": 1.650954112123811, + "heading": 0.03703434953912002, + "angularVelocity": -0.00016536889821494152, + "velocityX": 2.8385470348003916, + "velocityY": -2.1260022547535105, + "timestamp": 1.1775519677389532 + }, + { + "x": 4.972500711386871, + "y": 1.5511534257066393, + "heading": 0.025156273343944498, + "angularVelocity": -0.2420902316791928, + "velocityX": 2.7845986934007563, + "velocityY": -2.034064346740661, + "timestamp": 1.2266166330614097 + }, + { + "x": 5.112272239054231, + "y": 1.4704127227705035, + "heading": -0.006717749301178638, + "angularVelocity": -0.64963293717882, + "velocityX": 2.8487206984654128, + "velocityY": -1.6455977515693263, + "timestamp": 1.2756812983838661 }, { "x": 5.263677597045898, "y": 1.4059187173843384, - "heading": 1.5922215933347457, - "angularVelocity": -0.0000017525241831714588, - "velocityX": 3.2238326230717362, - "velocityY": -1.4780423989908298, - "timestamp": 1.1687560235188401 - }, - { - "x": 5.549175877605322, - "y": 1.3375763530891083, - "heading": 1.5922215924257537, - "angularVelocity": -1.0981400650486354e-8, - "velocityX": 3.449063323110087, - "velocityY": -0.8256341917136142, - "timestamp": 1.2515316219389887 - }, - { - "x": 5.84250473577961, - "y": 1.325823529497918, - "heading": 1.5922215915939637, - "angularVelocity": -1.0048733277896019e-8, - "velocityX": 3.5436633956473873, - "velocityY": -0.14198415735439168, - "timestamp": 1.3343072203591373 - }, - { - "x": 6.135970278596253, - "y": 1.3182128061287934, - "heading": 1.5922215907646338, - "angularVelocity": -1.0019015771647788e-8, - "velocityX": 3.5453146629890213, - "velocityY": -0.0919440452788315, - "timestamp": 1.4170828187792859 - }, - { - "x": 6.4294358220976315, - "y": 1.3106021091627869, - "heading": 1.5922215899353038, - "angularVelocity": -1.0019015824110795e-8, - "velocityX": 3.54531467126122, - "velocityY": -0.09194372630659559, - "timestamp": 1.4998584171994345 - }, - { - "x": 6.722901365599015, - "y": 1.3029914121969473, - "heading": 1.5922215891059737, - "angularVelocity": -1.0019016426747296e-8, - "velocityX": 3.545314671261272, - "velocityY": -0.09194372630457344, - "timestamp": 1.582634015619583 - }, - { - "x": 7.016366909100399, - "y": 1.2953807152311076, - "heading": 1.5922215882766437, - "angularVelocity": -1.0019015739011674e-8, - "velocityX": 3.545314671261273, - "velocityY": -0.09194372630457337, - "timestamp": 1.6654096140397316 - }, - { - "x": 7.309832452601783, - "y": 1.2877700182652683, - "heading": 1.5922215874473136, - "angularVelocity": -1.00190160592321e-8, - "velocityX": 3.5453146712612726, - "velocityY": -0.09194372630457343, - "timestamp": 1.7481852124598802 - }, - { - "x": 7.603297996103167, - "y": 1.2801593212994287, - "heading": 1.5922215866179836, - "angularVelocity": -1.0019015458013803e-8, - "velocityX": 3.545314671261272, - "velocityY": -0.0919437263045734, - "timestamp": 1.8309608108800288 - }, - { - "x": 7.89676353960455, - "y": 1.272548624333589, - "heading": 1.5922215857886535, - "angularVelocity": -1.0019015102150109e-8, - "velocityX": 3.545314671261272, - "velocityY": -0.09194372630457341, - "timestamp": 1.9137364093001774 - }, - { - "x": 8.190229083105928, - "y": 1.264937927367751, - "heading": 1.5922215849593204, - "angularVelocity": -1.0019051996053813e-8, - "velocityX": 3.545314671261198, - "velocityY": -0.09194372630455969, - "timestamp": 1.996512007720326 - }, - { - "x": 8.475782151148142, - "y": 1.2575324316094862, - "heading": 1.5850799971897327, - "angularVelocity": -0.08627648613712059, - "velocityX": 3.44972520274412, - "velocityY": -0.08946472027512459, - "timestamp": 2.0792876061404746 - }, - { - "x": 8.70422462444052, - "y": 1.2516080345083516, - "heading": 1.5793665822224103, - "angularVelocity": -0.06902293763341408, - "velocityX": 2.7597803900234106, - "velocityY": -0.07157178219436212, - "timestamp": 2.162063204560623 - }, - { - "x": 8.875556485627149, - "y": 1.2471647365187968, - "heading": 1.5750814725763596, - "angularVelocity": -0.051767788186808314, - "velocityX": 2.0698353676283796, - "velocityY": -0.0536788386234512, - "timestamp": 2.2448388029807713 - }, - { - "x": 8.989777728924334, - "y": 1.2442025377932433, - "heading": 1.572224713236977, - "angularVelocity": -0.034512095278156824, - "velocityX": 1.3798902753614153, - "velocityY": -0.03578589321116078, - "timestamp": 2.3276144014009197 - }, - { - "x": 9.04688835144043, - "y": 1.242721438407898, - "heading": 1.5707963267948966, - "angularVelocity": -0.017256129455330296, - "velocityX": 0.6899451481608816, - "velocityY": -0.017892946878229225, - "timestamp": 2.410389999821068 - }, - { - "x": 9.04688835144043, - "y": 1.242721438407898, - "heading": 1.5707963267948966, - "angularVelocity": -5.163840199570749e-28, - "velocityX": -6.169448749202353e-27, - "velocityY": 2.9824086172169885e-27, - "timestamp": 2.4931655982412164 + "heading": -0.031824505216569196, + "angularVelocity": -0.5117074731965909, + "velocityX": 3.0858328900568646, + "velocityY": -1.31446948557187, + "timestamp": 1.3247459637063226 + }, + { + "x": 5.548529116436538, + "y": 1.343358066124809, + "heading": -0.03182450881816663, + "angularVelocity": -4.379736737746259e-8, + "velocityX": 3.4639481169894566, + "velocityY": -0.7607712628377938, + "timestamp": 1.4069791582491455 + }, + { + "x": 5.839915359315492, + "y": 1.3311809328886355, + "heading": -0.03182450708796022, + "angularVelocity": 2.1040243198202156e-8, + "velocityX": 3.5434138792603314, + "velocityY": -0.14808050816792953, + "timestamp": 1.4892123527919685 + }, + { + "x": 6.131301674589235, + "y": 1.3190055321081195, + "heading": -0.031824505357755536, + "angularVelocity": 2.1040222208336333e-8, + "velocityX": 3.5434147596200054, + "velocityY": -0.1480594405726954, + "timestamp": 1.5714455473347915 + }, + { + "x": 6.4226879898634435, + "y": 1.3068301313387687, + "heading": -0.03182450362755091, + "angularVelocity": 2.10402215194868e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.14805944043692082, + "timestamp": 1.6536787418776144 + }, + { + "x": 6.714074305137652, + "y": 1.294654730569418, + "heading": -0.03182450189734621, + "angularVelocity": 2.1040222382135106e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.1480594404369199, + "timestamp": 1.7359119364204374 + }, + { + "x": 7.005460620411861, + "y": 1.2824793298000672, + "heading": -0.031824500167141584, + "angularVelocity": 2.1040221454378006e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.14805944043691993, + "timestamp": 1.8181451309632604 + }, + { + "x": 7.2968469356860695, + "y": 1.2703039290307165, + "heading": -0.03182449843693696, + "angularVelocity": 2.104022148411524e-8, + "velocityX": 3.543414759625678, + "velocityY": -0.1480594404369199, + "timestamp": 1.9003783255060833 + }, + { + "x": 7.588233250960278, + "y": 1.2581285282613657, + "heading": -0.03182449670673225, + "angularVelocity": 2.104022241858639e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.14805944043691993, + "timestamp": 1.9826115200489063 + }, + { + "x": 7.879619566234481, + "y": 1.2459531274920435, + "heading": -0.03182449497652102, + "angularVelocity": 2.104030178948416e-8, + "velocityX": 3.5434147596255965, + "velocityY": -0.14805944043657504, + "timestamp": 2.0648447145917292 + }, + { + "x": 8.161268192201323, + "y": 1.2341846133473202, + "heading": -0.017069904161984, + "angularVelocity": 0.1794237825316825, + "velocityX": 3.424999205402077, + "velocityY": -0.14311147961781526, + "timestamp": 2.1470779091345524 + }, + { + "x": 8.386587111141134, + "y": 1.2247698014142092, + "heading": -0.005266011713750806, + "angularVelocity": 0.14354169887059787, + "velocityX": 2.739999585234113, + "velocityY": -0.11448919120134828, + "timestamp": 2.2293111036773756 + }, + { + "x": 8.555576306335714, + "y": 1.217708692286378, + "heading": 0.0035869801538412527, + "angularVelocity": 0.10765715617410258, + "velocityX": 2.0549997617638245, + "velocityY": -0.08586689556556354, + "timestamp": 2.311544298220199 + }, + { + "x": 8.668235772211057, + "y": 1.213001286124164, + "heading": 0.00948900448730265, + "angularVelocity": 0.07177179928706179, + "velocityX": 1.3699998705106224, + "velocityY": -0.05724459797998689, + "timestamp": 2.393777492763022 + }, + { + "x": 8.724565505981445, + "y": 1.2106475830078125, + "heading": 0.012440027539999704, + "angularVelocity": 0.03588603202275334, + "velocityX": 0.6849999453816004, + "velocityY": -0.028622299418584626, + "timestamp": 2.476010687305845 + }, + { + "x": 8.724565505981445, + "y": 1.2106475830078125, + "heading": 0.012440027539999704, + "angularVelocity": 1.1793246737780391e-29, + "velocityX": -2.6822762319390616e-28, + "velocityY": 3.69381070917653e-27, + "timestamp": 2.5582438818486684 } ], "trajectoryWaypoints": [ @@ -1004,29 +1256,29 @@ "isStopPoint": true, "x": 2.502, "y": 3.44, - "heading": 2.39, + "heading": 2.5104836654523037, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 27 }, { - "timestamp": 1.1687560235188401, + "timestamp": 1.3247459637063226, "isStopPoint": false, "x": 5.263677597045898, "y": 1.4059187173843384, - "heading": 1.5922215933347457, + "heading": -0.031824505216569196, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 15 }, { - "timestamp": 2.4931655982412164, + "timestamp": 2.5582438818486684, "isStopPoint": true, - "x": 9.04688835144043, - "y": 1.242721438407898, - "heading": 1.5707963267948966, + "x": 8.724565505981445, + "y": 1.2106475830078125, + "heading": 0.012440027539999704, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -1059,24 +1311,24 @@ { "x": 8.972707748413086, "y": 1.1833769083023071, - "heading": 1.571, + "heading": 0.009898490937098423, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 27 }, { - "x": 5.026299476623535, - "y": 1.5691161155700684, - "heading": 2.111216138366583, + "x": 5.174009799957275, + "y": 1.9726389646530151, + "heading": 2.3183527865287403, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 15 }, { - "x": 2.640187978744507, - "y": 3.4790618419647217, + "x": 2.223318576812744, + "y": 3.7398104667663574, "heading": 2.203545115911647, "isInitialGuess": false, "translationConstrained": true, @@ -1088,308 +1340,389 @@ { "x": 8.972707748413086, "y": 1.1833769083023071, - "heading": 1.571, - "angularVelocity": -9.767399252006736e-24, - "velocityX": 5.907264225115041e-23, - "velocityY": 1.299820018625526e-22, + "heading": 0.009898490937098424, + "angularVelocity": -3.594444463136562e-23, + "velocityX": 2.661376445930665e-23, + "velocityY": 1.6196216041750108e-24, "timestamp": 0 }, { - "x": 8.935090507110717, - "y": 1.1863475460806576, - "heading": 1.6136708412192649, - "angularVelocity": 0.6202915722960445, - "velocityX": -0.5468291012353428, - "velocityY": 0.04318315565391617, - "timestamp": 0.06879158628790716 - }, - { - "x": 8.859516182153305, - "y": 1.192305430427452, - "heading": 1.6942785937843952, - "angularVelocity": 1.1717676087271771, - "velocityX": -1.0985983756955169, - "velocityY": 0.08660774766640515, - "timestamp": 0.13758317257581432 - }, - { - "x": 8.745545936468519, - "y": 1.2013296641436748, - "heading": 1.8058392839348794, - "angularVelocity": 1.6217199830743718, - "velocityX": -1.6567468761048345, - "velocityY": 0.13118223031598755, - "timestamp": 0.20637475886372147 - }, - { - "x": 8.592647322978339, - "y": 1.2135389725367793, - "heading": 1.9374414012084449, - "angularVelocity": 1.9130554239988473, - "velocityX": -2.2226353794236933, - "velocityY": 0.17748258256475968, - "timestamp": 0.27516634515162863 - }, - { - "x": 8.40044069338619, - "y": 1.228982218641201, - "heading": 2.064842015198071, - "angularVelocity": 1.8519795932082177, - "velocityX": -2.7940427014972795, - "velocityY": 0.2244932402022014, - "timestamp": 0.3439579314395358 - }, - { - "x": 8.175541171514828, - "y": 1.2426911633087852, - "heading": 2.111215871319779, - "angularVelocity": 0.6741210462515563, - "velocityX": -3.2692882081554697, - "velocityY": 0.1992822873746392, - "timestamp": 0.41274951772744295 - }, - { - "x": 7.932395229110654, - "y": 1.262724401465717, - "heading": 2.1112158916774773, - "angularVelocity": 2.9593296428696675e-7, - "velocityX": -3.534530245989049, - "velocityY": 0.2912164006960001, - "timestamp": 0.4815411040153501 - }, - { - "x": 7.68924928943743, - "y": 1.2827576727686798, - "heading": 2.1112159120351404, - "angularVelocity": 2.9593245340453995e-7, - "velocityX": -3.534530206290155, - "velocityY": 0.2912168825286064, - "timestamp": 0.5503326903032573 - }, - { - "x": 7.446103349764219, - "y": 1.3027909440717906, - "heading": 2.111215932392803, - "angularVelocity": 2.95932453993162e-7, - "velocityX": -3.5345302062899764, - "velocityY": 0.291216882530778, - "timestamp": 0.6191242765911644 - }, - { - "x": 7.202957410091008, - "y": 1.3228242153749026, - "heading": 2.111215952750466, - "angularVelocity": 2.959324524383632e-7, - "velocityX": -3.5345302062899755, - "velocityY": 0.29121688253078115, - "timestamp": 0.6879158628790716 - }, - { - "x": 6.9598114704177965, - "y": 1.342857486678014, - "heading": 2.111215973108129, - "angularVelocity": 2.9593245270626844e-7, - "velocityX": -3.534530206289975, - "velocityY": 0.2912168825307812, - "timestamp": 0.7567074491669787 - }, - { - "x": 6.716665530744585, - "y": 1.3628907579811254, - "heading": 2.1112159934657915, - "angularVelocity": 2.9593245074455596e-7, - "velocityX": -3.5345302062899755, - "velocityY": 0.29121688253078176, - "timestamp": 0.8254990354548859 - }, - { - "x": 6.4735195910713745, - "y": 1.3829240292842375, - "heading": 2.111216013823454, - "angularVelocity": 2.959324507471835e-7, - "velocityX": -3.5345302062899755, - "velocityY": 0.2912168825307816, - "timestamp": 0.8942906217427931 - }, - { - "x": 6.230373651398163, - "y": 1.402957300587349, - "heading": 2.1112160341811164, - "angularVelocity": 2.9593244991278353e-7, - "velocityX": -3.534530206289976, - "velocityY": 0.29121688253078176, - "timestamp": 0.9630822080307002 - }, - { - "x": 5.987227711724952, - "y": 1.4229905718904616, - "heading": 2.1112160545387795, - "angularVelocity": 2.9593244925906865e-7, - "velocityX": -3.534530206289975, - "velocityY": 0.29121688253078837, - "timestamp": 1.0318737943186074 - }, - { - "x": 5.744081772051767, - "y": 1.4430238431938993, - "heading": 2.111216074896442, - "angularVelocity": 2.9593244949024157e-7, - "velocityX": -3.534530206289585, - "velocityY": 0.291216882535523, - "timestamp": 1.1006653806065145 - }, - { - "x": 5.500935838264892, - "y": 1.4630571859399728, - "heading": 2.111216095254104, - "angularVelocity": 2.959324407133209e-7, - "velocityX": -3.5345301207222937, - "velocityY": 0.29121792107293887, - "timestamp": 1.1694569668944217 - }, - { - "x": 5.259315238985184, - "y": 1.4968324118293073, - "heading": 2.111216115741956, - "angularVelocity": 2.9782496007778795e-7, - "velocityX": -3.512356849404178, - "velocityY": 0.49097902391693365, - "timestamp": 1.2382485531823288 - }, - { - "x": 5.026299476623535, - "y": 1.5691161155700684, - "heading": 2.111216138366583, - "angularVelocity": 3.2888654369729736e-7, - "velocityX": -3.3872712483533762, - "velocityY": 1.0507637291316205, - "timestamp": 1.307040139470236 - }, - { - "x": 4.776317244480285, - "y": 1.702059385073433, - "heading": 2.111216140017271, - "angularVelocity": 2.06763249457757e-8, - "velocityX": -3.1312480899349673, - "velocityY": 1.6652317852077385, - "timestamp": 1.3868748331653122 - }, - { - "x": 4.555581127297633, - "y": 1.8793743483839014, - "heading": 2.1112161415313353, - "angularVelocity": 1.8964991666679164e-8, - "velocityX": -2.7649146876637363, - "velocityY": 2.2210264122476873, - "timestamp": 1.4667095268603885 - }, - { - "x": 4.3382681622702, - "y": 2.0608684388916934, - "heading": 2.111216143039461, - "angularVelocity": 1.889060440086873e-8, - "velocityX": -2.7220366856725855, - "velocityY": 2.2733736688587687, - "timestamp": 1.5465442205554647 - }, - { - "x": 4.120955206951703, - "y": 2.2423625410245305, - "heading": 2.111216144547586, - "angularVelocity": 1.8890603633646163e-8, - "velocityX": -2.7220365640596182, - "velocityY": 2.273373814472728, - "timestamp": 1.626378914250541 - }, - { - "x": 3.903642251633233, - "y": 2.423856643157401, - "heading": 2.1112161460557117, - "angularVelocity": 1.8890603742361353e-8, - "velocityX": -2.722036564059269, - "velocityY": 2.2733738144731452, - "timestamp": 1.7062136079456172 - }, - { - "x": 3.6863292963147622, - "y": 2.605350745290272, - "heading": 2.1112161475638374, - "angularVelocity": 1.8890604477781255e-8, - "velocityX": -2.7220365640592696, - "velocityY": 2.273373814473146, - "timestamp": 1.7860483016406934 - }, - { - "x": 3.4690163409962946, - "y": 2.786844847423142, - "heading": 2.111216149071963, - "angularVelocity": 1.8890603977643177e-8, - "velocityX": -2.722036564059269, - "velocityY": 2.273373814473146, - "timestamp": 1.8658829953357696 - }, - { - "x": 3.251703385681568, - "y": 2.968338949560493, - "heading": 2.1112161505800917, - "angularVelocity": 1.8890642534870236e-8, - "velocityX": -2.7220365640123627, - "velocityY": 2.273373814529255, - "timestamp": 1.9457176890308459 - }, - { - "x": 3.0478649217251967, - "y": 3.1385799156225445, - "heading": 2.1419916080333983, - "angularVelocity": 0.385489766777984, - "velocityX": -2.5532566672694883, - "velocityY": 2.1324183532572523, - "timestamp": 2.0255523827259223 - }, - { - "x": 2.8847941463462403, - "y": 3.274772687216185, - "heading": 2.1666127065045155, - "angularVelocity": 0.308400988737502, - "velocityX": -2.042605386597918, - "velocityY": 1.705934666873273, - "timestamp": 2.1053870764209988 - }, - { - "x": 2.7624910633351236, - "y": 3.376917264599957, - "heading": 2.1850788550552847, - "angularVelocity": 0.23130480867502362, - "velocityX": -1.5319540584478795, - "velocityY": 1.2794509837277892, - "timestamp": 2.185221770116075 - }, - { - "x": 2.680955673869497, - "y": 3.445013649295216, - "heading": 2.197389707457713, - "angularVelocity": 0.15420429180136633, - "velocityX": -1.02130271554678, - "velocityY": 0.8529673196386245, - "timestamp": 2.2650564638111517 - }, - { - "x": 2.640187978744507, - "y": 3.4790618419647217, + "x": 8.9642414023468, + "y": 1.1838504232385874, + "heading": 0.11269422494821685, + "angularVelocity": 1.9483306196990626, + "velocityX": -0.1604662045229367, + "velocityY": 0.008974726997325759, + "timestamp": 0.05276092926517583 + }, + { + "x": 8.948083227066952, + "y": 1.1844896520896235, + "heading": 0.3198575365997856, + "angularVelocity": 3.926453050331398, + "velocityX": -0.30625266660175565, + "velocityY": 0.012115572260365658, + "timestamp": 0.10552185853035166 + }, + { + "x": 8.924965236199132, + "y": 1.188814526065175, + "heading": 0.6300158546364878, + "angularVelocity": 5.878560562833341, + "velocityX": -0.4381649677099328, + "velocityY": 0.08197114864703657, + "timestamp": 0.15828278779552749 + }, + { + "x": 8.884295423196336, + "y": 1.2001880729953864, + "heading": 0.9938364812875791, + "angularVelocity": 6.895644783330729, + "velocityX": -0.7708320071162389, + "velocityY": 0.21556760065858568, + "timestamp": 0.21104371706070332 + }, + { + "x": 8.822221884064051, + "y": 1.2155938935187947, + "heading": 1.32752447859539, + "angularVelocity": 6.324528433354513, + "velocityX": -1.1765057969374584, + "velocityY": 0.2919929716548175, + "timestamp": 0.26380464632587913 + }, + { + "x": 8.74361650376665, + "y": 1.2194863977685344, + "heading": 1.619938928469106, + "angularVelocity": 5.542253594587851, + "velocityX": -1.489840709634433, + "velocityY": 0.0737762640641897, + "timestamp": 0.31656557559105497 + }, + { + "x": 8.646843774366385, + "y": 1.2218814186876117, + "heading": 1.8543200673475446, + "angularVelocity": 4.442323934448573, + "velocityX": -1.8341740895784047, + "velocityY": 0.04539383502970571, + "timestamp": 0.3693265048562308 + }, + { + "x": 8.535183983723305, + "y": 1.2361352435496769, + "heading": 2.0387764035405076, + "angularVelocity": 3.496078229894084, + "velocityX": -2.116334799220068, + "velocityY": 0.2701587151815607, + "timestamp": 0.42208743412140665 + }, + { + "x": 8.40747247720944, + "y": 1.2621673609702526, + "heading": 2.1777614957327995, + "angularVelocity": 2.6342426892019786, + "velocityX": -2.4205696960337497, + "velocityY": 0.4933976293279214, + "timestamp": 0.4748483633865825 + }, + { + "x": 8.261948828020005, + "y": 1.2964340038650055, + "heading": 2.270600320754398, + "angularVelocity": 1.7596131515233795, + "velocityX": -2.758170699724305, + "velocityY": 0.6494700410322398, + "timestamp": 0.5276092926517583 + }, + { + "x": 8.09782277606961, + "y": 1.3349380681436944, + "heading": 2.3167167289348747, + "angularVelocity": 0.8740636077256377, + "velocityX": -3.1107498339443698, + "velocityY": 0.7297836640664107, + "timestamp": 0.5803702219169341 + }, + { + "x": 7.914829584256359, + "y": 1.3739987658766613, + "heading": 2.3167202209384494, + "angularVelocity": 0.00006618540695237648, + "velocityX": -3.4683466413855033, + "velocityY": 0.7403337711632821, + "timestamp": 0.6331311511821099 + }, + { + "x": 7.731792737090714, + "y": 1.412856687245149, + "heading": 2.3167225237878513, + "angularVelocity": 0.00004364686964296262, + "velocityX": -3.4691740595716793, + "velocityY": 0.7364904657609108, + "timestamp": 0.6858920804472858 + }, + { + "x": 7.548755888844399, + "y": 1.451714603522748, + "heading": 2.3167248266383997, + "angularVelocity": 0.00004364689137005159, + "velocityX": -3.469174080054087, + "velocityY": 0.7364903692711618, + "timestamp": 0.7386530097124616 + }, + { + "x": 7.365719040598746, + "y": 1.4905725198022486, + "heading": 2.316727129490484, + "angularVelocity": 0.00004364692048352743, + "velocityX": -3.469174080041539, + "velocityY": 0.7364903693072045, + "timestamp": 0.7914139389776375 + }, + { + "x": 7.182682192353782, + "y": 1.529430436083779, + "heading": 2.3167294323441046, + "angularVelocity": 0.00004364694959712505, + "velocityX": -3.4691740800284765, + "velocityY": 0.7364903693456704, + "timestamp": 0.8441748682428133 + }, + { + "x": 6.999645344109507, + "y": 1.5682883523673385, + "heading": 2.3167317351992613, + "angularVelocity": 0.00004364697871207036, + "velocityX": -3.469174080015414, + "velocityY": 0.7364903693841357, + "timestamp": 0.8969357975079891 + }, + { + "x": 6.816608495865921, + "y": 1.6071462686529279, + "heading": 2.316734038055954, + "angularVelocity": 0.00004364700782609749, + "velocityX": -3.4691740800023507, + "velocityY": 0.7364903694226017, + "timestamp": 0.949696726773165 + }, + { + "x": 6.633571647623024, + "y": 1.6460041849405465, + "heading": 2.316736340914183, + "angularVelocity": 0.00004364703694171292, + "velocityX": -3.469174079989288, + "velocityY": 0.7364903694610679, + "timestamp": 1.0024576560383407 + }, + { + "x": 6.450534799380817, + "y": 1.6848621012301948, + "heading": 2.316738643773948, + "angularVelocity": 0.000043647066056982235, + "velocityX": -3.469174079976225, + "velocityY": 0.736490369499534, + "timestamp": 1.0552185853035165 + }, + { + "x": 6.267497951139299, + "y": 1.7237200175218723, + "heading": 2.3167409466352487, + "angularVelocity": 0.00004364709517405661, + "velocityX": -3.4691740799631625, + "velocityY": 0.7364903695379998, + "timestamp": 1.1079795145686924 + }, + { + "x": 6.084461102898469, + "y": 1.7625779338155794, + "heading": 2.316743249498086, + "angularVelocity": 0.000043647124290962285, + "velocityX": -3.4691740799500987, + "velocityY": 0.7364903695764661, + "timestamp": 1.1607404438338682 + }, + { + "x": 5.90142425465833, + "y": 1.8014358501113161, + "heading": 2.31674555236246, + "angularVelocity": 0.000043647153406607097, + "velocityX": -3.469174079937036, + "velocityY": 0.7364903696149331, + "timestamp": 1.213501373099044 + }, + { + "x": 5.718387406418881, + "y": 1.8402937664090826, + "heading": 2.31674785522837, + "angularVelocity": 0.000043647182524781595, + "velocityX": -3.4691740799239703, + "velocityY": 0.7364903696534086, + "timestamp": 1.2662623023642199 + }, + { + "x": 5.535350558184445, + "y": 1.879151682729246, + "heading": 2.316750158095817, + "angularVelocity": 0.00004364721167212726, + "velocityX": -3.469174079828943, + "velocityY": 0.7364903700779036, + "timestamp": 1.3190232316293957 + }, + { + "x": 5.352313882264269, + "y": 1.9180104105293494, + "heading": 2.316752461060097, + "angularVelocity": 0.0000436490469716496, + "velocityX": -3.4691708138845505, + "velocityY": 0.7365057503972241, + "timestamp": 1.3717841608945716 + }, + { + "x": 5.174009799957275, + "y": 1.9726389646530151, + "heading": 2.3183527865287403, + "angularVelocity": 0.030331639168071065, + "velocityX": -3.3794719841047485, + "velocityY": 1.0353978765063663, + "timestamp": 1.4245450901597474 + }, + { + "x": 4.918462925408197, + "y": 2.1088992111590095, + "heading": 2.318352780187111, + "angularVelocity": -7.76596778188732e-8, + "velocityX": -3.1294303624461395, + "velocityY": 1.6686447578851502, + "timestamp": 1.506204320419573 + }, + { + "x": 4.6706807831887005, + "y": 2.2588160882042634, + "heading": 2.318352773860442, + "angularVelocity": -7.747647131185673e-8, + "velocityX": -3.03434334895268, + "velocityY": 1.8358840337858229, + "timestamp": 1.5878635506793985 + }, + { + "x": 4.42289869878705, + "y": 2.4087330608106337, + "heading": 2.318352767533773, + "angularVelocity": -7.747646978575146e-8, + "velocityX": -3.0343426409145584, + "velocityY": 1.8358852040285008, + "timestamp": 1.6695227809392241 + }, + { + "x": 4.17511661438578, + "y": 2.5586500334176288, + "heading": 2.318352761207104, + "angularVelocity": -7.747646959889163e-8, + "velocityX": -3.034342640909924, + "velocityY": 1.835885204036159, + "timestamp": 1.7511820111990497 + }, + { + "x": 3.9273345299845093, + "y": 2.7085670060246243, + "heading": 2.318352754880435, + "angularVelocity": -7.747646933531489e-8, + "velocityX": -3.0343426409099234, + "velocityY": 1.835885204036159, + "timestamp": 1.8328412414588753 + }, + { + "x": 3.679552445583239, + "y": 2.85848397863162, + "heading": 2.318352748553766, + "angularVelocity": -7.747646958961607e-8, + "velocityX": -3.034342640909924, + "velocityY": 1.8358852040361588, + "timestamp": 1.9145004717187009 + }, + { + "x": 3.431770361181968, + "y": 3.0084009512386154, + "heading": 2.318352742227097, + "angularVelocity": -7.747647015947937e-8, + "velocityX": -3.034342640909924, + "velocityY": 1.8358852040361597, + "timestamp": 1.9961597019785264 + }, + { + "x": 3.1839882767807, + "y": 3.158317923845615, + "heading": 2.318352735900428, + "angularVelocity": -7.747647018934798e-8, + "velocityX": -3.034342640909895, + "velocityY": 1.835885204036207, + "timestamp": 2.077818932238352 + }, + { + "x": 2.936206192742126, + "y": 3.308234897052054, + "heading": 2.3183527295737423, + "angularVelocity": -7.74766777065616e-8, + "velocityX": -3.0343426364683324, + "velocityY": 1.8358852113769544, + "timestamp": 2.1594781624981776 + }, + { + "x": 2.6985520828578236, + "y": 3.452139832457651, + "heading": 2.2861229229565243, + "angularVelocity": -0.39468663266440823, + "velocityX": -2.91031533273248, + "velocityY": 1.7622617179676583, + "timestamp": 2.241137392758003 + }, + { + "x": 2.5084457239359947, + "y": 3.567225663105015, + "heading": 2.2549856266803583, + "angularVelocity": -0.3813077367627909, + "velocityX": -2.328044953607123, + "velocityY": 1.4093425848024874, + "timestamp": 2.3227966230178287 + }, + { + "x": 2.3658771899178763, + "y": 3.653524329460914, + "heading": 2.2299036649035693, + "angularVelocity": -0.3071540314179127, + "velocityX": -1.7458961291269777, + "velocityY": 1.0568145950104009, + "timestamp": 2.4044558532776543 + }, + { + "x": 2.270836799349362, + "y": 3.7110500271012747, + "heading": 2.2124904722719037, + "angularVelocity": -0.213242184334339, + "velocityX": -1.1638658638602495, + "velocityY": 0.7044604444264757, + "timestamp": 2.48611508353748 + }, + { + "x": 2.223318576812744, + "y": 3.7398104667663574, "heading": 2.203545115911647, - "angularVelocity": 0.07710192360033451, - "velocityX": -0.5106513626857504, - "velocityY": 0.42648366385108344, - "timestamp": 2.344891157506228 + "angularVelocity": -0.10954495078871034, + "velocityX": -0.5819087736367691, + "velocityY": 0.35220071966845595, + "timestamp": 2.5677743137973055 }, { - "x": 2.640187978744507, - "y": 3.4790618419647217, + "x": 2.223318576812744, + "y": 3.7398104667663574, "heading": 2.203545115911647, - "angularVelocity": -2.5642698021713453e-24, - "velocityX": -3.130979253679926e-22, - "velocityY": -1.883332935459204e-22, - "timestamp": 2.4247258512013046 + "angularVelocity": 4.719095338364679e-26, + "velocityX": 4.055221098284943e-24, + "velocityY": 5.8881582869771646e-24, + "timestamp": 2.649433544057131 } ], "trajectoryWaypoints": [ @@ -1398,28 +1731,28 @@ "isStopPoint": true, "x": 8.972707748413086, "y": 1.1833769083023071, - "heading": 1.571, + "heading": 0.009898490937098423, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 19 + "controlIntervalCount": 27 }, { - "timestamp": 1.307040139470236, + "timestamp": 1.4245450901597474, "isStopPoint": false, - "x": 5.026299476623535, - "y": 1.5691161155700684, - "heading": 2.111216138366583, + "x": 5.174009799957275, + "y": 1.9726389646530151, + "heading": 2.3183527865287403, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 15 }, { - "timestamp": 2.4247258512013046, + "timestamp": 2.649433544057131, "isStopPoint": true, - "x": 2.640187978744507, - "y": 3.4790618419647217, + "x": 2.223318576812744, + "y": 3.7398104667663574, "heading": 2.203545115911647, "isInitialGuess": false, "translationConstrained": true, @@ -1447,6 +1780,994 @@ "circleObstacles": [], "eventMarkers": [], "isTrajectoryStale": false + }, + "Close 2 Note 6": { + "waypoints": [ + { + "x": 2.7096962928771973, + "y": 3.496621608734131, + "heading": 2.3370885589294867, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 5.4171977043151855, + "y": 2.8156931400299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 6.973606586456299, + "y": 3.399346351623535, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 8.8218412399292, + "y": 4.128912448883057, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2.7096962928771973, + "y": 3.496621608734131, + "heading": 2.3370885589294867, + "angularVelocity": -3.26643471533185e-27, + "velocityX": -8.603201911023632e-26, + "velocityY": 2.4805779170240995e-26, + "timestamp": 0 + }, + { + "x": 2.7146740314887152, + "y": 3.4937673012218937, + "heading": 2.2533967162557134, + "angularVelocity": -1.7826089205421576, + "velocityX": 0.10602420701389548, + "velocityY": -0.06079581797616154, + "timestamp": 0.04694907655253943 + }, + { + "x": 2.724452218978781, + "y": 3.488820892027521, + "heading": 2.0851306096050566, + "angularVelocity": -3.5840131266981397, + "velocityX": 0.20827220060704882, + "velocityY": -0.10535690065889125, + "timestamp": 0.09389815310507886 + }, + { + "x": 2.7406560932871677, + "y": 3.4832680280351447, + "heading": 1.8355772431800976, + "angularVelocity": -5.315405216664708, + "velocityX": 0.3451372316184537, + "velocityY": -0.11827418982697585, + "timestamp": 0.14084722965761828 + }, + { + "x": 2.767585684695567, + "y": 3.475579797799871, + "heading": 1.520977813967397, + "angularVelocity": -6.700865114154927, + "velocityX": 0.5735915035147274, + "velocityY": -0.1637567935265024, + "timestamp": 0.18779630621015772 + }, + { + "x": 2.810498710473928, + "y": 3.4589447346262188, + "heading": 1.2009849249020366, + "angularVelocity": -6.815744047857154, + "velocityX": 0.9140334364263394, + "velocityY": -0.3543214136498727, + "timestamp": 0.23474538276269716 + }, + { + "x": 2.8647081763617166, + "y": 3.432538014521098, + "heading": 0.9243397881091598, + "angularVelocity": -5.892451079059896, + "velocityX": 1.1546439220614773, + "velocityY": -0.5624545154912587, + "timestamp": 0.28169445931523657 + }, + { + "x": 2.932084724896856, + "y": 3.4027137974797674, + "heading": 0.6918755798834426, + "angularVelocity": -4.951411727248202, + "velocityX": 1.4350984829220317, + "velocityY": -0.6352460842963507, + "timestamp": 0.328643535867776 + }, + { + "x": 3.0147211413516173, + "y": 3.376471633610061, + "heading": 0.49655237322236306, + "angularVelocity": -4.1603205217998, + "velocityX": 1.7601286867119736, + "velocityY": -0.5589495214104997, + "timestamp": 0.37559261242031544 + }, + { + "x": 3.1133145878182975, + "y": 3.353150170071545, + "heading": 0.33467121013791984, + "angularVelocity": -3.448015913652453, + "velocityX": 2.100008215419251, + "velocityY": -0.49673955807026315, + "timestamp": 0.4225416889728549 + }, + { + "x": 3.2280064438607416, + "y": 3.3301514191120454, + "heading": 0.2070951624637037, + "angularVelocity": -2.7173281572737493, + "velocityX": 2.442899082670891, + "velocityY": -0.4898658855145381, + "timestamp": 0.4694907655253943 + }, + { + "x": 3.358041797859201, + "y": 3.3040376340853572, + "heading": 0.11640881453382437, + "angularVelocity": -1.9315895985386364, + "velocityX": 2.7697105789277994, + "velocityY": -0.5562150939745335, + "timestamp": 0.5164398420779337 + }, + { + "x": 3.502044068421766, + "y": 3.272023000424053, + "heading": 0.0642726455079616, + "angularVelocity": -1.1104833759087593, + "velocityX": 3.067201341040561, + "velocityY": -0.6819012430516593, + "timestamp": 0.5633889186304731 + }, + { + "x": 3.658545816104221, + "y": 3.2320987294497567, + "heading": 0.05052094344595785, + "angularVelocity": -0.2929067634933054, + "velocityX": 3.333436122163514, + "velocityY": -0.8503739350361508, + "timestamp": 0.6103379951830126 + }, + { + "x": 3.818497445143101, + "y": 3.1858458228841244, + "heading": 0.05051984068744808, + "angularVelocity": -0.000023488396167744368, + "velocityX": 3.406917468544511, + "velocityY": -0.9851718066035188, + "timestamp": 0.657287071735552 + }, + { + "x": 3.978449011059529, + "y": 3.139592697974801, + "heading": 0.05051873797026558, + "angularVelocity": -0.000023487515910291847, + "velocityX": 3.406916124056895, + "velocityY": -0.9851764572528472, + "timestamp": 0.7042361482880914 + }, + { + "x": 4.138400576973649, + "y": 3.0933395730582816, + "heading": 0.05051763525189827, + "angularVelocity": -0.000023487541146302523, + "velocityX": 3.4069161240077346, + "velocityY": -0.9851764574061228, + "timestamp": 0.7511852248406309 + }, + { + "x": 4.298352142887712, + "y": 3.047086448142349, + "heading": 0.05051653253234576, + "angularVelocity": -0.000023487566390649612, + "velocityX": 3.406916124006511, + "velocityY": -0.9851764573936154, + "timestamp": 0.7981343013931703 + }, + { + "x": 4.458303708801718, + "y": 3.0008333232270044, + "heading": 0.05051542981160801, + "angularVelocity": -0.000023487591635953872, + "velocityX": 3.4069161240052885, + "velocityY": -0.9851764573811018, + "timestamp": 0.8450833779457098 + }, + { + "x": 4.618255274715666, + "y": 2.9545801983122475, + "heading": 0.050514327089685056, + "angularVelocity": -0.000023487616880439936, + "velocityX": 3.4069161240040726, + "velocityY": -0.9851764573685653, + "timestamp": 0.8920324544982492 + }, + { + "x": 4.778206840638278, + "y": 2.908327073428245, + "heading": 0.050513224366575286, + "angularVelocity": -0.000023487642159216305, + "velocityX": 3.4069161241886405, + "velocityY": -0.985176456713516, + "timestamp": 0.9389815310507886 + }, + { + "x": 4.938158651224666, + "y": 2.8620747947569334, + "heading": 0.050512121554286644, + "angularVelocity": -0.000023489541639992042, + "velocityX": 3.4069213354471506, + "velocityY": -0.9851584326595224, + "timestamp": 0.9859306076033281 + }, + { + "x": 5.097496996112395, + "y": 2.828606048631961, + "heading": 0.040035508366877214, + "angularVelocity": -0.22314843989924574, + "velocityX": 3.393854716384416, + "velocityY": -0.7128733637075573, + "timestamp": 1.0328796841558674 + }, + { + "x": 5.255731410284399, + "y": 2.813146210355405, + "heading": 0.016867711475476607, + "angularVelocity": -0.4934665086644284, + "velocityX": 3.3703413526127197, + "velocityY": -0.3292895070951061, + "timestamp": 1.0798287607084067 + }, + { + "x": 5.4171977043151855, + "y": 2.8156931400299072, + "heading": -4.112071684642348e-22, + "angularVelocity": -0.359276746510667, + "velocityX": 3.4391793382792955, + "velocityY": 0.05424877040238669, + "timestamp": 1.126777837260946 + }, + { + "x": 5.601031770417355, + "y": 2.8403910645192147, + "heading": -1.6475843781367288e-8, + "angularVelocity": -3.15019851866737e-7, + "velocityX": 3.5149265215212115, + "velocityY": 0.4722268927335189, + "timestamp": 1.1790788071323186 + }, + { + "x": 5.7804448855599615, + "y": 2.887464289884782, + "heading": -1.4325422036977554e-8, + "angularVelocity": 4.1116288085647864e-8, + "velocityX": 3.430397477978901, + "velocityY": 0.9000449796884818, + "timestamp": 1.2313797770036912 + }, + { + "x": 5.952724706601957, + "y": 2.956200146604807, + "heading": -1.2270848156308066e-8, + "angularVelocity": 3.928366693242696e-8, + "velocityX": 3.2940081506269547, + "velocityY": 1.3142367510405981, + "timestamp": 1.2836807468750637 + }, + { + "x": 6.122871731907656, + "y": 3.0300577430139204, + "heading": -1.0225706785848107e-8, + "angularVelocity": 3.910331635321734e-8, + "velocityX": 3.2532288736547965, + "velocityY": 1.4121649481980245, + "timestamp": 1.3359817167464363 + }, + { + "x": 6.293018697456439, + "y": 3.103915477085925, + "heading": -8.180565446039309e-9, + "angularVelocity": 3.910331576716392e-8, + "velocityX": 3.253227731096333, + "velocityY": 1.4121675803268703, + "timestamp": 1.3882826866178088 + }, + { + "x": 6.463165663003702, + "y": 3.177773211161434, + "heading": -6.13542410268984e-9, + "angularVelocity": 3.9103315834861925e-8, + "velocityX": 3.2532277310672533, + "velocityY": 1.4121675803938631, + "timestamp": 1.4405836564891814 + }, + { + "x": 6.633312628550963, + "y": 3.251630945236942, + "heading": -4.090282764746313e-9, + "angularVelocity": 3.910331573149977e-8, + "velocityX": 3.253227731067253, + "velocityY": 1.4121675803938638, + "timestamp": 1.492884626360554 + }, + { + "x": 6.803459594098908, + "y": 3.3254886793108787, + "heading": -2.045141412996072e-9, + "angularVelocity": 3.910331599548557e-8, + "velocityX": 3.253227731080299, + "velocityY": 1.4121675803638085, + "timestamp": 1.5451855962319265 + }, + { + "x": 6.973606586456299, + "y": 3.399346351623535, + "heading": 4.112281238380375e-22, + "angularVelocity": 3.910331716652011e-8, + "velocityX": 3.2532282436797044, + "velocityY": 1.4121663994816895, + "timestamp": 1.597486566103299 + }, + { + "x": 7.242468138651456, + "y": 3.505475940728197, + "heading": 2.585437014143955e-17, + "angularVelocity": 3.17215609296563e-16, + "velocityX": 3.2988016026124845, + "velocityY": 1.3021588835020073, + "timestamp": 1.6789893615373233 + }, + { + "x": 7.511329714849845, + "y": 3.6116054690246653, + "heading": 3.276598422574725e-17, + "angularVelocity": 8.480217208701784e-17, + "velocityX": 3.298801897120578, + "velocityY": 1.3021581374147986, + "timestamp": 1.7604921569713476 + }, + { + "x": 7.780191291048392, + "y": 3.7177349973207363, + "heading": 3.5721707298172814e-17, + "angularVelocity": 3.6265296869230005e-17, + "velocityX": 3.2988018971225013, + "velocityY": 1.3021581374099265, + "timestamp": 1.841994952405372 + }, + { + "x": 8.049052867246928, + "y": 3.8238645256168042, + "heading": 2.8432311760247846e-17, + "angularVelocity": -8.94373683629846e-17, + "velocityX": 3.2988018971223876, + "velocityY": 1.3021581374098816, + "timestamp": 1.9234977478393964 + }, + { + "x": 8.306648971596287, + "y": 3.9255471588589534, + "heading": 1.1537732349557526e-17, + "angularVelocity": -2.0728834294140422e-16, + "velocityX": 3.1605799896505293, + "velocityY": 1.247596879354403, + "timestamp": 2.005000543273421 + }, + { + "x": 8.512725872589964, + "y": 4.006893272366167, + "heading": 4.56264194487887e-18, + "angularVelocity": -8.55809959333942e-17, + "velocityX": 2.52846420661111, + "velocityY": 0.9980775883087605, + "timestamp": 2.086503338707445 + }, + { + "x": 8.667283554110604, + "y": 4.067902859776331, + "heading": 3.1467275701042136e-19, + "angularVelocity": -5.212053359848234e-17, + "velocityX": 1.8963482258194635, + "velocityY": 0.7485582192030588, + "timestamp": 2.1680061341414696 + }, + { + "x": 8.770322010785756, + "y": 4.108575918968745, + "heading": -1.2062044578319972e-18, + "angularVelocity": -1.8660430070701055e-17, + "velocityX": 1.264232179110475, + "velocityY": 0.4990388240773634, + "timestamp": 2.249508929575494 + }, + { + "x": 8.8218412399292, + "y": 4.128912448883057, + "heading": 1.614787833327321e-28, + "angularVelocity": 1.4799547078713896e-17, + "velocityX": 0.6321160994428312, + "velocityY": 0.24951941594167767, + "timestamp": 2.3310117250095184 + }, + { + "x": 8.8218412399292, + "y": 4.128912448883057, + "heading": 7.916184096548804e-29, + "angularVelocity": -3.8711572835683144e-29, + "velocityX": -5.492029296440765e-27, + "velocityY": 6.5647493687471005e-28, + "timestamp": 2.4125145204435428 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 2.7096962928771973, + "y": 3.496621608734131, + "heading": 2.3370885589294867, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "timestamp": 1.126777837260946, + "isStopPoint": false, + "x": 5.4171977043151855, + "y": 2.8156931400299072, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "timestamp": 1.597486566103299, + "isStopPoint": false, + "x": 6.973606586456299, + "y": 3.399346351623535, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "timestamp": 2.4125145204435428, + "isStopPoint": true, + "x": 8.8218412399292, + "y": 4.128912448883057, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "Close 2 Note 7": { + "waypoints": [ + { + "x": 8.465164184570312, + "y": 4.534226894378662, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 26 + }, + { + "x": 5.871150970458984, + "y": 2.9778192043304443, + "heading": 2.569623693858617, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.4178707599639893, + "y": 5.182730197906494, + "heading": 2.4155561764362563, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 8.465164184570312, + "y": 4.534226894378662, + "heading": -2.8449855627415714e-26, + "angularVelocity": -6.1230157259157575e-27, + "velocityX": 2.3963430003994368e-26, + "velocityY": -3.084479188733402e-26, + "timestamp": 0 + }, + { + "x": 8.460367187528206, + "y": 4.531767118727852, + "heading": 0.08450384633322554, + "angularVelocity": 1.8016904366905349, + "velocityX": -0.1022758616396483, + "velocityY": -0.05244440884966881, + "timestamp": 0.0469025336497028 + }, + { + "x": 8.449742946270945, + "y": 4.5264220063428615, + "heading": 0.2507113382987055, + "angularVelocity": 3.5436783267791148, + "velocityX": -0.22651742732301222, + "velocityY": -0.11396212462446169, + "timestamp": 0.0938050672994056 + }, + { + "x": 8.432270313112136, + "y": 4.516718694694835, + "heading": 0.49372727846173775, + "angularVelocity": 5.181296643333298, + "velocityX": -0.37253068862558697, + "velocityY": -0.20688246226733464, + "timestamp": 0.1407076009491084 + }, + { + "x": 8.406880343130178, + "y": 4.499531714826417, + "heading": 0.8023679750542415, + "angularVelocity": 6.580469594619851, + "velocityX": -0.5413347213092073, + "velocityY": -0.3664403291468338, + "timestamp": 0.1876101345988112 + }, + { + "x": 8.369319015581828, + "y": 4.46918191846546, + "heading": 1.0969671011562374, + "angularVelocity": 6.281091940624037, + "velocityX": -0.8008379212278784, + "velocityY": -0.6470822362738113, + "timestamp": 0.23451266824851402 + }, + { + "x": 8.31733458470586, + "y": 4.435074642393435, + "heading": 1.3519084583358731, + "angularVelocity": 5.435556191563035, + "velocityX": -1.1083501642836833, + "velocityY": -0.7271947465942838, + "timestamp": 0.28141520189821684 + }, + { + "x": 8.249546558176377, + "y": 4.397567790061388, + "heading": 1.5734769473555086, + "angularVelocity": 4.7240196163910095, + "velocityX": -1.4452956216772148, + "velocityY": -0.7996764655012261, + "timestamp": 0.32831773554791965 + }, + { + "x": 8.16571602730043, + "y": 4.355751955292342, + "heading": 1.7651793028229739, + "angularVelocity": 4.087249462880137, + "velocityX": -1.7873348058772967, + "velocityY": -0.8915474605562327, + "timestamp": 0.37522026919762247 + }, + { + "x": 8.066870437319741, + "y": 4.307122077538631, + "heading": 1.9259060954825424, + "angularVelocity": 3.42682537919968, + "velocityX": -2.107468025478739, + "velocityY": -1.0368283751344498, + "timestamp": 0.4221228028473253 + }, + { + "x": 7.955957471746159, + "y": 4.248022091743418, + "heading": 2.0518724582720194, + "angularVelocity": 2.6857048646939137, + "velocityX": -2.364754245515796, + "velocityY": -1.2600595574774145, + "timestamp": 0.4690253364970281 + }, + { + "x": 7.841127991815943, + "y": 4.1735531848919445, + "heading": 2.1461285868842412, + "angularVelocity": 2.00961699246751, + "velocityX": -2.448257503269065, + "velocityY": -1.5877374004494726, + "timestamp": 0.5159278701467309 + }, + { + "x": 7.720416086674602, + "y": 4.0853653777627885, + "heading": 2.2017425392035563, + "angularVelocity": 1.1857345007132167, + "velocityX": -2.5736755724732037, + "velocityY": -1.880235464203213, + "timestamp": 0.5628304037964337 + }, + { + "x": 7.594525449972563, + "y": 3.988428321875803, + "heading": 2.226983293645896, + "angularVelocity": 0.5381533251668943, + "velocityX": -2.684090323185251, + "velocityY": -2.066776533032764, + "timestamp": 0.6097329374461364 + }, + { + "x": 7.473855661362909, + "y": 3.8889244490440706, + "heading": 2.2604012205898147, + "angularVelocity": 0.7124972649346369, + "velocityX": -2.572777613910837, + "velocityY": -2.1215031489533054, + "timestamp": 0.6566354710958392 + }, + { + "x": 7.353858527255753, + "y": 3.789691370515812, + "heading": 2.2950724550213937, + "angularVelocity": 0.7392187955244643, + "velocityX": -2.5584360751888, + "velocityY": -2.1157295951087174, + "timestamp": 0.703538004745542 + }, + { + "x": 7.234194488365969, + "y": 3.690951634795317, + "heading": 2.330505605791114, + "angularVelocity": 0.7554634688683831, + "velocityX": -2.5513342154074086, + "velocityY": -2.105211126928559, + "timestamp": 0.7504405383952447 + }, + { + "x": 7.114885343623766, + "y": 3.5927409732758844, + "heading": 2.3667975931007965, + "angularVelocity": 0.7737745594029797, + "velocityX": -2.5437675847807513, + "velocityY": -2.0939308365072624, + "timestamp": 0.7973430720449475 + }, + { + "x": 6.995958632853738, + "y": 3.4951017727711147, + "heading": 2.404066159199295, + "angularVelocity": 0.7945960100331263, + "velocityX": -2.5356137827914957, + "velocityY": -2.081746824894364, + "timestamp": 0.8442456056946502 + }, + { + "x": 6.877449452771626, + "y": 3.3980853132150073, + "heading": 2.4424566069983173, + "angularVelocity": 0.8185154364100276, + "velocityX": -2.526711690400603, + "velocityY": -2.0684694835610897, + "timestamp": 0.891148139344353 + }, + { + "x": 6.759397930576002, + "y": 3.3017545649144626, + "heading": 2.482139466223676, + "angularVelocity": 0.8460706946395502, + "velocityX": -2.516954053640389, + "velocityY": -2.0538495642901125, + "timestamp": 0.9380506729940558 + }, + { + "x": 6.630091339801819, + "y": 3.2069465515769977, + "heading": 2.4987006025081633, + "angularVelocity": 0.35309683711706574, + "velocityX": -2.756921230309734, + "velocityY": -2.0213836217367276, + "timestamp": 0.9849532066437585 + }, + { + "x": 6.489136479391131, + "y": 3.1261931906999445, + "heading": 2.508890104109657, + "angularVelocity": 0.21724842580137385, + "velocityX": -3.0052717719564495, + "velocityY": -1.721727049548525, + "timestamp": 1.0318557402934614 + }, + { + "x": 6.3415058716452215, + "y": 3.0624912033101053, + "heading": 2.5228679049585003, + "angularVelocity": 0.2980180335936202, + "velocityX": -3.1476041112939677, + "velocityY": -1.3581779582656586, + "timestamp": 1.0787582739431643 + }, + { + "x": 6.188700179225271, + "y": 3.0163717053414074, + "heading": 2.5394254687500224, + "angularVelocity": 0.35302066867398807, + "velocityX": -3.257941107429268, + "velocityY": -0.9833050451633768, + "timestamp": 1.1256608075928671 + }, + { + "x": 6.031658683642738, + "y": 2.988099001678993, + "heading": 2.55600757529583, + "angularVelocity": 0.35354394007053697, + "velocityX": -3.3482518610916934, + "velocityY": -0.6027969378706126, + "timestamp": 1.17256334124257 + }, + { + "x": 5.871150970458984, + "y": 2.9778192043304443, + "heading": 2.569623693858617, + "angularVelocity": 0.29030667435753743, + "velocityX": -3.4221544273604505, + "velocityY": -0.21917360425184526, + "timestamp": 1.2194658748922729 + }, + { + "x": 5.576841136148987, + "y": 3.016980693416113, + "heading": 2.5696236883953287, + "angularVelocity": -6.525879318384462e-8, + "velocityX": -3.5155208959382085, + "velocityY": 0.4677826465415135, + "timestamp": 1.3031831512282035 + }, + { + "x": 5.2959027238957885, + "y": 3.113030268725703, + "heading": 2.5696236701163304, + "angularVelocity": -2.1834200821596536e-7, + "velocityX": -3.355799717203925, + "velocityY": 1.14730888907773, + "timestamp": 1.3869004275641341 + }, + { + "x": 5.039219321333584, + "y": 3.2622467062205915, + "heading": 2.569623654672457, + "angularVelocity": -1.8447654007901228e-7, + "velocityX": -3.0660744567491185, + "velocityY": 1.782385237858559, + "timestamp": 1.4706177039000647 + }, + { + "x": 4.799769406634373, + "y": 3.437790584146643, + "heading": 2.569623639730028, + "angularVelocity": -1.7848680691589113e-7, + "velocityX": -2.8602210341671395, + "velocityY": 2.0968656125606664, + "timestamp": 1.5543349802359954 + }, + { + "x": 4.560319652029815, + "y": 3.613334680448953, + "heading": 2.5696236247876003, + "angularVelocity": -1.7848678782979664e-7, + "velocityX": -2.860219121841975, + "velocityY": 2.096868221057592, + "timestamp": 1.638052256571926 + }, + { + "x": 4.320869897426237, + "y": 3.7888787767525987, + "heading": 2.5696236098451726, + "angularVelocity": -1.784867870189022e-7, + "velocityX": -2.860219121830273, + "velocityY": 2.0968682210735534, + "timestamp": 1.7217695329078566 + }, + { + "x": 4.081420142822659, + "y": 3.9644228730562445, + "heading": 2.569623594902745, + "angularVelocity": -1.784867875533888e-7, + "velocityX": -2.8602191218302733, + "velocityY": 2.096868221073554, + "timestamp": 1.8054868092437872 + }, + { + "x": 3.8419703882190817, + "y": 4.139966969359891, + "heading": 2.569623579960317, + "angularVelocity": -1.7848678683767884e-7, + "velocityX": -2.8602191218302733, + "velocityY": 2.0968682210735534, + "timestamp": 1.8892040855797179 + }, + { + "x": 3.6025206336155033, + "y": 4.315511065663538, + "heading": 2.5696235650178894, + "angularVelocity": -1.7848678777394938e-7, + "velocityX": -2.8602191218302737, + "velocityY": 2.096868221073554, + "timestamp": 1.9729213619156485 + }, + { + "x": 3.363070879011915, + "y": 4.491055161967169, + "heading": 2.5696235500754616, + "angularVelocity": -1.7848678692480314e-7, + "velocityX": -2.8602191218304047, + "velocityY": 2.0968682210733744, + "timestamp": 2.056638638251579 + }, + { + "x": 3.123621122611819, + "y": 4.666599255820278, + "heading": 2.5696235351330237, + "angularVelocity": -1.784869102061119e-7, + "velocityX": -2.860219143289622, + "velocityY": 2.096868191801975, + "timestamp": 2.1403559145875097 + }, + { + "x": 2.888392901577879, + "y": 4.838443251060152, + "heading": 2.5511544908862556, + "angularVelocity": -0.22061210129027164, + "velocityX": -2.809793047853641, + "velocityY": 2.0526706405296773, + "timestamp": 2.2240731909234404 + }, + { + "x": 2.7000575025773665, + "y": 4.976197095777676, + "heading": 2.5062705544048423, + "angularVelocity": -0.5361370847912993, + "velocityX": -2.2496598939122543, + "velocityY": 1.645464959523549, + "timestamp": 2.307790467259371 + }, + { + "x": 2.5589096957108692, + "y": 5.079485700111359, + "heading": 2.463997463224374, + "angularVelocity": -0.504950627046677, + "velocityX": -1.6860057212099981, + "velocityY": 1.233778842962105, + "timestamp": 2.3915077435953016 + }, + { + "x": 2.4648687186972724, + "y": 5.148321942855847, + "heading": 2.4324643694891153, + "angularVelocity": -0.37666172521818037, + "velocityX": -1.1233162511909789, + "velocityY": 0.822246563161818, + "timestamp": 2.4752250199312322 + }, + { + "x": 2.4178707599639893, + "y": 5.182730197906494, + "heading": 2.4155561764362563, + "angularVelocity": -0.20196778721051775, + "velocityX": -0.5613890082220943, + "velocityY": 0.41100542870718687, + "timestamp": 2.558942296267163 + }, + { + "x": 2.4178707599639893, + "y": 5.182730197906494, + "heading": 2.4155561764362563, + "angularVelocity": -2.09894229579262e-27, + "velocityX": 2.3772952918690395e-26, + "velocityY": 2.3276233341918227e-26, + "timestamp": 2.6426595726030935 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 8.465164184570312, + "y": 4.534226894378662, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 26 + }, + { + "timestamp": 1.2194658748922729, + "isStopPoint": false, + "x": 5.871150970458984, + "y": 2.9778192043304443, + "heading": 2.569623693858617, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "timestamp": 2.6426595726030935, + "isStopPoint": true, + "x": 2.4178707599639893, + "y": 5.182730197906494, + "heading": 2.4155561764362563, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": false, diff --git a/MiddleAuto.chor b/MiddleAuto.chor index 244919c..b277f51 100644 --- a/MiddleAuto.chor +++ b/MiddleAuto.chor @@ -31,21 +31,21 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 11 }, { "x": 1.6146607398986816, "y": 5.578441143035889, - "heading": 4.267969754957633, + "heading": 1.5263811612249127, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 16 }, { "x": 2.891972541809082, "y": 5.5500569343566895, - "heading": 4.71238898038469, + "heading": 0.014491481540703788, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -57,244 +57,298 @@ "x": 1.3875831365585327, "y": 5.578441619873047, "heading": 3.141592653589793, - "angularVelocity": -1.1291873234278378e-20, - "velocityX": 1.3203072273457026e-19, - "velocityY": -7.78556777115951e-20, + "angularVelocity": 2.5697719393979616e-20, + "velocityX": -1.1084704093663195e-19, + "velocityY": 3.5588051044447295e-21, "timestamp": 0 }, { - "x": 1.4336237451382812, - "y": 5.57779899487797, - "heading": 3.0785239487284644, - "angularVelocity": -0.8225912321878043, - "velocityX": 0.600497521323188, - "velocityY": -0.008381616329320548, - "timestamp": 0.0766707720596395 + "x": 1.435497168845919, + "y": 5.578814866768434, + "heading": 3.202269926322516, + "angularVelocity": 0.7789200950398169, + "velocityX": 0.6150771269405634, + "velocityY": 0.004791406965653385, + "timestamp": 0.0778992262738075 }, { - "x": 1.5259556376145589, - "y": 5.576358757232951, - "heading": 2.955577173518951, - "angularVelocity": -1.6035677208764376, - "velocityX": 1.2042645456140169, - "velocityY": -0.018784702518695878, - "timestamp": 0.153341544119279 + "x": 1.5313833167159634, + "y": 5.57976146384972, + "heading": 3.322629311801515, + "angularVelocity": 1.5450652238309606, + "velocityX": 1.23089987483335, + "velocityY": 0.012151559477090946, + "timestamp": 0.155798452547615 }, { - "x": 1.657752548636916, - "y": 5.576152010354836, - "heading": 2.9483871516288307, - "angularVelocity": -0.09377787254480233, - "velocityX": 1.718998093821681, - "velocityY": -0.0026965540134869003, - "timestamp": 0.23001231617891849 + "x": 1.6638110997885283, + "y": 5.579730251476359, + "heading": 3.3230709468305517, + "angularVelocity": 0.005669312137769006, + "velocityX": 1.699988426163507, + "velocityY": -0.0004006762949166178, + "timestamp": 0.2336976788214225 }, { - "x": 1.744151163856094, - "y": 5.576992225920646, - "heading": 3.0109375917958325, - "angularVelocity": 0.8158316198817746, - "velocityX": 1.1268781166305677, - "velocityY": 0.01095874664151184, - "timestamp": 0.306683088238558 + "x": 1.7483290860755216, + "y": 5.579216882713837, + "heading": 3.262903789160066, + "angularVelocity": -0.7723716980064068, + "velocityX": 1.0849656707747208, + "velocityY": -0.006590165102761797, + "timestamp": 0.31159690509523 }, { "x": 1.7849689722061155, "y": 5.578441143035889, "heading": 3.141592653589793, - "angularVelocity": 1.7041052057272719, - "velocityX": 0.532377687787867, - "velocityY": 0.018897906937929525, - "timestamp": 0.3833538602981975 - }, - { - "x": 1.793473158716207, - "y": 5.579365807303268, - "heading": 3.2362487120966916, - "angularVelocity": 2.19466706141234, - "velocityX": 0.1971755248655801, - "velocityY": 0.02143898914124505, - "timestamp": 0.4264838913499161 - }, - { - "x": 1.7871836885671384, - "y": 5.580329664394791, - "heading": 3.348096786420172, - "angularVelocity": 2.5932759980942377, - "velocityX": -0.14582577372890423, - "velocityY": 0.022347702239475742, - "timestamp": 0.4696139224016347 - }, - { - "x": 1.7656852765643356, - "y": 5.581260377422058, - "heading": 3.4709165543645333, - "angularVelocity": 2.847662404812178, - "velocityX": -0.498455750635179, - "velocityY": 0.021579233878850173, - "timestamp": 0.5127439534533532 - }, - { - "x": 1.7286997359077372, - "y": 5.581806558753597, - "heading": 3.5910196586486482, - "angularVelocity": 2.7846746537255114, - "velocityX": -0.8575356834834563, - "velocityY": 0.01266359699311981, - "timestamp": 0.5558739845050717 - }, - { - "x": 1.678880135296367, - "y": 5.580447353738231, - "heading": 3.678953949653364, - "angularVelocity": 2.038818170551998, - "velocityX": -1.1551023589950564, - "velocityY": -0.031514120955215656, - "timestamp": 0.5990040155567903 - }, - { - "x": 1.642003204232439, - "y": 5.580155414936893, - "heading": 3.7971584641789797, - "angularVelocity": 2.740654519443145, - "velocityX": -0.8550174939523657, - "velocityY": -0.0067688057304568965, - "timestamp": 0.6421340466085088 - }, - { - "x": 1.6177973573716273, - "y": 5.580015376090363, - "heading": 3.9458939820250976, - "angularVelocity": 3.4485372307700124, - "velocityX": -0.5612295254734593, - "velocityY": -0.003246898810787891, - "timestamp": 0.6852640776602273 - }, - { - "x": 1.6085134315165317, - "y": 5.579433916920347, - "heading": 4.109633152642141, - "angularVelocity": 3.796407436403154, - "velocityX": -0.21525432810291645, - "velocityY": -0.013481538404617919, - "timestamp": 0.7283941087119459 + "angularVelocity": -1.5572829330021465, + "velocityX": 0.47034980812015426, + "velocityY": -0.009958246250379041, + "timestamp": 0.38949613136903755 + }, + { + "x": 1.7918077455407007, + "y": 5.578070537407724, + "heading": 3.07603165300874, + "angularVelocity": -1.903502222336055, + "velocityX": 0.19855737595617648, + "velocityY": -0.010760187162647133, + "timestamp": 0.42393843451517954 + }, + { + "x": 1.7893368559672658, + "y": 5.577687740411084, + "heading": 2.997989156370227, + "angularVelocity": -2.265890765416316, + "velocityX": -0.07173996358347022, + "velocityY": -0.01111415212321765, + "timestamp": 0.45838073766132154 + }, + { + "x": 1.7776291594278917, + "y": 5.577317580799147, + "heading": 2.906758179496928, + "angularVelocity": -2.6488059316532158, + "velocityX": -0.33992199910955634, + "velocityY": -0.010747237499325827, + "timestamp": 0.49282304080746353 + }, + { + "x": 1.7568034196465414, + "y": 5.577003452812707, + "heading": 2.8013208897259188, + "angularVelocity": -3.0612729155663203, + "velocityX": -0.6046558411899725, + "velocityY": -0.009120411753767155, + "timestamp": 0.5272653439536056 + }, + { + "x": 1.7271154460348663, + "y": 5.576861684230968, + "heading": 2.6797808328137562, + "angularVelocity": -3.528801671492646, + "velocityX": -0.8619624966921092, + "velocityY": -0.004116117936055084, + "timestamp": 0.5617076470997476 + }, + { + "x": 1.691069043849476, + "y": 5.578432787106803, + "heading": 2.528205165864953, + "angularVelocity": -4.400857466054248, + "velocityX": -1.0465735125912459, + "velocityY": 0.04561549990339373, + "timestamp": 0.5961499502458897 + }, + { + "x": 1.656140821204661, + "y": 5.579323072829806, + "heading": 2.3275870071573586, + "angularVelocity": -5.824760262295832, + "velocityX": -1.0141082173457174, + "velocityY": 0.02584861178490341, + "timestamp": 0.6305922533920317 + }, + { + "x": 1.631008856377759, + "y": 5.579569155022308, + "heading": 2.1226675607510446, + "angularVelocity": -5.94964412039522, + "velocityX": -0.7296830505284353, + "velocityY": 0.007144765884526293, + "timestamp": 0.6650345565381738 + }, + { + "x": 1.615749078145331, + "y": 5.579444205722283, + "heading": 1.9194967328972732, + "angularVelocity": -5.898874619147794, + "velocityX": -0.44305336282766067, + "velocityY": -0.003627785850877333, + "timestamp": 0.6994768596843158 + }, + { + "x": 1.6103138655470886, + "y": 5.579051278101532, + "heading": 1.720326094951214, + "angularVelocity": -5.782732853286816, + "velocityX": -0.15780630508883814, + "velocityY": -0.01140828530204804, + "timestamp": 0.7339191628304579 }, { "x": 1.6146607398986816, "y": 5.578441143035889, - "heading": 4.267969754957633, - "angularVelocity": 3.6711451036431124, - "velocityX": 0.14252965352096647, - "velocityY": -0.023018158351596252, - "timestamp": 0.7715241397636644 - }, - { - "x": 1.6690373330299817, - "y": 5.575429312080012, - "heading": 4.4984754181934505, - "angularVelocity": 3.129429714872091, - "velocityX": 0.7382366400451945, - "velocityY": -0.040889725472165635, - "timestamp": 0.8451815419863054 - }, - { - "x": 1.7636030315477318, - "y": 5.5708764490251905, - "heading": 4.648544403662639, - "angularVelocity": 2.0373917751753106, - "velocityX": 1.2838587251816274, - "velocityY": -0.061811344378666935, - "timestamp": 0.9188389442089462 - }, - { - "x": 1.8951140247808684, - "y": 5.5669762523657225, - "heading": 4.693072397728858, - "angularVelocity": 0.6045284346524559, - "velocityX": 1.7854416428592532, - "velocityY": -0.05295050520081145, - "timestamp": 0.992496346431587 - }, - { - "x": 2.040218953265875, - "y": 5.564513492820825, - "heading": 4.69307239826707, - "angularVelocity": 7.306959148578081e-9, - "velocityX": 1.9699979106839063, - "velocityY": -0.03343532992724096, - "timestamp": 1.0661537486542279 - }, - { - "x": 2.1853238811191047, - "y": 5.562050696052041, - "heading": 4.693072398805279, - "angularVelocity": 7.30692484830781e-9, - "velocityX": 1.9699979021066683, - "velocityY": -0.03343583529242484, - "timestamp": 1.1398111508768687 - }, - { - "x": 2.3304288085405163, - "y": 5.559587873841206, - "heading": 4.693072399343489, - "angularVelocity": 7.306924923849905e-9, - "velocityX": 1.969997896244148, - "velocityY": -0.03343618070307283, - "timestamp": 1.2134685530995095 - }, - { - "x": 2.475533735803122, - "y": 5.557125042273845, - "heading": 4.693072399881698, - "angularVelocity": 7.30692520769984e-9, - "velocityX": 1.9699978940881417, - "velocityY": -0.03343630773069961, - "timestamp": 1.2871259553221504 - }, - { - "x": 2.6206386630670626, - "y": 5.554662210785155, - "heading": 4.69307240041991, - "angularVelocity": 7.306954868223136e-9, - "velocityX": 1.9699978941062621, - "velocityY": -0.03343630666264002, - "timestamp": 1.3607833575447912 - }, - { - "x": 2.7563056000189468, - "y": 5.552359572937684, - "heading": 4.70273064510102, - "angularVelocity": 0.131123884221667, - "velocityX": 1.841864264256972, - "velocityY": -0.031261458834935876, - "timestamp": 1.434440759767432 - }, - { - "x": 2.846750227458183, - "y": 5.55082448040246, - "heading": 4.709169530239947, - "angularVelocity": 0.08741667428705875, - "velocityX": 1.2279095475815647, - "velocityY": -0.020840981203541834, - "timestamp": 1.5080981619900728 + "heading": 1.5263811612249127, + "angularVelocity": -5.631009427661504, + "velocityX": 0.12620742385167788, + "velocityY": -0.017714699944832876, + "timestamp": 0.7683614659765999 + }, + { + "x": 1.6401584005796046, + "y": 5.577198410041313, + "heading": 1.2647274815001233, + "angularVelocity": -5.390569723612346, + "velocityX": 0.5253009162114343, + "velocityY": -0.025602693079409197, + "timestamp": 0.816900617335438 + }, + { + "x": 1.684994320964394, + "y": 5.575581354594258, + "heading": 1.0153473511131832, + "angularVelocity": -5.137710969508591, + "velocityX": 0.9237063098472489, + "velocityY": -0.03331445651163336, + "timestamp": 0.865439768694276 + }, + { + "x": 1.7491124987051623, + "y": 5.573626178659196, + "heading": 0.7792584484534197, + "angularVelocity": -4.863886080628321, + "velocityX": 1.3209579472611341, + "velocityY": -0.04028038975397266, + "timestamp": 0.913978920053114 + }, + { + "x": 1.832394833509839, + "y": 5.57140698926971, + "heading": 0.55854280647037, + "angularVelocity": -4.547167303180748, + "velocityX": 1.7157764912079099, + "velocityY": -0.04571957538111955, + "timestamp": 0.962518071411952 + }, + { + "x": 1.9345711443290823, + "y": 5.569045004771247, + "heading": 0.3573383968625533, + "angularVelocity": -4.145198339385086, + "velocityX": 2.105028785194018, + "velocityY": -0.04866142963647802, + "timestamp": 1.0110572227707901 + }, + { + "x": 2.0549191287043285, + "y": 5.566727666479068, + "heading": 0.1842377602443393, + "angularVelocity": -3.5662064904786552, + "velocityX": 2.4794002574446163, + "velocityY": -0.047741631802519585, + "timestamp": 1.0595963741296281 + }, + { + "x": 2.192617094377459, + "y": 5.564252863678173, + "heading": 0.047019931683506766, + "angularVelocity": -2.8269515374592107, + "velocityX": 2.8368432866731306, + "velocityY": -0.05098570394439828, + "timestamp": 1.1081355254884662 + }, + { + "x": 2.342632262023748, + "y": 5.5613797511301, + "heading": -0.022930505967036604, + "angularVelocity": -1.4411137338066946, + "velocityX": 3.090601369135268, + "velocityY": -0.059191651844776, + "timestamp": 1.1566746768473042 + }, + { + "x": 2.479810836982564, + "y": 5.558675249403878, + "heading": -0.028156785630628813, + "angularVelocity": -0.1076714264111378, + "velocityX": 2.826142837658786, + "velocityY": -0.05571794418547554, + "timestamp": 1.2052138282061422 + }, + { + "x": 2.5975798577346256, + "y": 5.556269094299155, + "heading": -0.02249245533146242, + "angularVelocity": 0.11669611314980328, + "velocityX": 2.4262686399567257, + "velocityY": -0.04957142919403416, + "timestamp": 1.2537529795649802 + }, + { + "x": 2.69573124985327, + "y": 5.554225549899802, + "heading": -0.013355296818420646, + "angularVelocity": 0.18824306270814264, + "velocityX": 2.0221077083329138, + "velocityY": -0.04210095030804235, + "timestamp": 1.3022921309238182 + }, + { + "x": 2.7742419202395245, + "y": 5.552570833433737, + "heading": -0.0037262727394932653, + "angularVelocity": 0.1983764406539037, + "velocityX": 1.6174710143950495, + "velocityY": -0.03409034603492016, + "timestamp": 1.3508312822826563 + }, + { + "x": 2.833114272929124, + "y": 5.551319078090283, + "heading": 0.0047616375661535875, + "angularVelocity": 0.17486729924258082, + "velocityX": 1.2128838482232662, + "velocityY": -0.025788570842536836, + "timestamp": 1.3993704336414943 + }, + { + "x": 2.872355354708483, + "y": 5.550479104815246, + "heading": 0.011073016059497023, + "angularVelocity": 0.13002655210605069, + "velocityX": 0.8084418594231156, + "velocityY": -0.017305067178193873, + "timestamp": 1.4479095850003323 }, { "x": 2.891972541809082, "y": 5.5500569343566895, - "heading": 4.71238898038469, - "angularVelocity": 0.04370843998830303, - "velocityX": 0.6139547823612851, - "velocityY": -0.010420487589966523, - "timestamp": 1.5817555642127137 + "heading": 0.014491481540703788, + "angularVelocity": 0.07042697256766788, + "velocityX": 0.4041518351974077, + "velocityY": -0.0086975245083139, + "timestamp": 1.4964487363591703 }, { "x": 2.891972541809082, "y": 5.5500569343566895, - "heading": 4.71238898038469, - "angularVelocity": -7.307364922132298e-21, - "velocityX": 3.835648569289986e-21, - "velocityY": 6.13563548284752e-20, - "timestamp": 1.6554129664353545 + "heading": 0.014491481540703788, + "angularVelocity": 1.3928075939596226e-22, + "velocityX": -3.188487813654877e-20, + "velocityY": 1.8586974314537386e-21, + "timestamp": 1.5449878877180083 } ], "trajectoryWaypoints": [ @@ -310,7 +364,7 @@ "controlIntervalCount": 5 }, { - "timestamp": 0.3833538602981975, + "timestamp": 0.38949613136903755, "isStopPoint": false, "x": 1.7849689722061157, "y": 5.578441143035889, @@ -318,25 +372,25 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 11 }, { - "timestamp": 0.7715241397636644, + "timestamp": 0.7683614659765999, "isStopPoint": false, "x": 1.6146607398986816, "y": 5.578441143035889, - "heading": 4.267969754957633, + "heading": 1.5263811612249127, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 16 }, { - "timestamp": 1.6554129664353545, + "timestamp": 1.5449878877180083, "isStopPoint": true, "x": 2.891972541809082, "y": 5.5500569343566895, - "heading": 4.71238898038469, + "heading": 0.014491481540703788, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -369,16 +423,16 @@ { "x": 2.8635878562927246, "y": 5.578441619873047, - "heading": -1.5707963267948966, + "heading": 0.009614917466831607, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 21 }, { "x": 3.4596667289733887, "y": 5.5500569343566895, - "heading": -3.166256562885956, + "heading": 3.1169287442936304, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -389,137 +443,200 @@ { "x": 2.8635878562927246, "y": 5.578441619873047, - "heading": -1.5707963267948966, - "angularVelocity": 3.07668011019845e-22, - "velocityX": 6.225343802913688e-21, - "velocityY": -7.419414269605286e-22, + "heading": 0.009614917466831609, + "angularVelocity": -1.528531437446765e-23, + "velocityX": -2.453817422824562e-23, + "velocityY": 1.50998248167019e-22, "timestamp": 0 }, { - "x": 2.8753743714707505, - "y": 5.578008878952115, - "heading": -1.6071749538697528, - "angularVelocity": -0.864093629223167, - "velocityX": 0.27996253556015577, - "velocityY": -0.010278801124406404, - "timestamp": 0.04210033015468626 - }, - { - "x": 2.8989864993914374, - "y": 5.576913727810355, - "heading": -1.6795591223435373, - "angularVelocity": -1.7193254354022516, - "velocityX": 0.5608537470833614, - "velocityY": -0.02601288725612288, - "timestamp": 0.08420066030937252 - }, - { - "x": 2.934525294888728, - "y": 5.574838164969817, - "heading": -1.786903685335108, - "angularVelocity": -2.5497320946691384, - "velocityX": 0.8441452921320711, - "velocityY": -0.04930039343896005, - "timestamp": 0.1263009904640588 - }, - { - "x": 2.9822550006530326, - "y": 5.571466907965597, - "heading": -1.9266329908587956, - "angularVelocity": -3.318959851628004, - "velocityX": 1.1337133364258365, - "velocityY": -0.08007673554654467, - "timestamp": 0.16840132061874505 - }, - { - "x": 3.0427779474067433, - "y": 5.5667931144149465, - "heading": -2.0934132586364123, - "angularVelocity": -3.9614954838792804, - "velocityX": 1.4375884115719717, - "velocityY": -0.1110156032859041, - "timestamp": 0.2105016507734313 - }, - { - "x": 3.1169264950584123, - "y": 5.561652784032428, - "heading": -2.2802480853445712, - "angularVelocity": -4.437847067272045, - "velocityX": 1.7612343508747772, - "velocityY": -0.12209715134364663, - "timestamp": 0.2526019809281176 - }, - { - "x": 3.2052828649293703, - "y": 5.558124125052269, - "heading": -2.45377086401179, - "angularVelocity": -4.12164888089134, - "velocityX": 2.0987096667967022, - "velocityY": -0.08381547050092075, - "timestamp": 0.29470231108280387 - }, - { - "x": 3.2796524843692785, - "y": 5.55600822933581, - "heading": -2.643502979834551, - "angularVelocity": -4.506665746459506, - "velocityX": 1.7664854210572272, - "velocityY": -0.05025841148224034, - "timestamp": 0.33680264123749015 - }, - { - "x": 3.3403881246799387, - "y": 5.554336823360145, - "heading": -2.812261145556071, - "angularVelocity": -4.008476064236641, - "velocityX": 1.442640475442267, - "velocityY": -0.03970054319134659, - "timestamp": 0.37890297139217644 - }, - { - "x": 3.388390555860858, - "y": 5.552788836167647, - "heading": -2.9518024547103603, - "angularVelocity": -3.3144944146894546, - "velocityX": 1.1401913240242543, - "velocityY": -0.036769003635058306, - "timestamp": 0.4210033015468627 - }, - { - "x": 3.4241306673086034, - "y": 5.551464604674301, - "heading": -3.058336722149922, - "angularVelocity": -2.5304853203794417, - "velocityX": 0.8489271061869301, - "velocityY": -0.03145418310203628, - "timestamp": 0.463103631701549 - }, - { - "x": 3.4478466303449795, - "y": 5.550522718395356, - "heading": -3.1301129064142854, - "angularVelocity": -1.7048841185007717, - "velocityX": 0.5633201200391078, - "velocityY": -0.022372420251377964, - "timestamp": 0.5052039618562353 + "x": 2.8683152245735273, + "y": 5.578274982110834, + "heading": 0.04618464340580782, + "angularVelocity": 1.0952855735453868, + "velocityX": 0.14158756036180598, + "velocityY": -0.004990902509465985, + "timestamp": 0.03338830239551295 + }, + { + "x": 2.877792114944196, + "y": 5.578133507964627, + "heading": 0.11915603160618048, + "angularVelocity": 2.18553753754726, + "velocityX": 0.28383864080319215, + "velocityY": -0.004237236878087561, + "timestamp": 0.0667766047910259 + }, + { + "x": 2.8919946410560815, + "y": 5.578297396627518, + "heading": 0.22838911975350412, + "angularVelocity": 3.271597544953453, + "velocityX": 0.4253743105487484, + "velocityY": 0.004908565311320537, + "timestamp": 0.10016490718653885 + }, + { + "x": 2.9107812374502178, + "y": 5.579092613322564, + "heading": 0.3739274892257765, + "angularVelocity": 4.3589628411845, + "velocityX": 0.5626700085439953, + "velocityY": 0.02381722453953736, + "timestamp": 0.1335532095820518 + }, + { + "x": 2.9332106538040255, + "y": 5.58025607529439, + "heading": 0.5585664198828683, + "angularVelocity": 5.530048472362744, + "velocityX": 0.6717746858795545, + "velocityY": 0.03484639494245511, + "timestamp": 0.16694151197756474 + }, + { + "x": 2.9622622193818153, + "y": 5.578513064570307, + "heading": 0.7657200090473316, + "angularVelocity": 6.204376212679287, + "velocityX": 0.8701120899668712, + "velocityY": -0.052204233187781474, + "timestamp": 0.20032981437307767 + }, + { + "x": 2.9994929664814656, + "y": 5.574221954382459, + "heading": 0.9863349814940863, + "angularVelocity": 6.6075528439086835, + "velocityX": 1.1150835600622937, + "velocityY": -0.12852136467529682, + "timestamp": 0.2337181167685906 + }, + { + "x": 3.0447312800587603, + "y": 5.5663721092988325, + "heading": 1.1953383351835791, + "angularVelocity": 6.259777787252248, + "velocityX": 1.354915055021299, + "velocityY": -0.2351076431069595, + "timestamp": 0.26710641916410355 + }, + { + "x": 3.0987733800833377, + "y": 5.5575866982672455, + "heading": 1.3938213283520493, + "angularVelocity": 5.944686579667022, + "velocityX": 1.6185938232027546, + "velocityY": -0.26312841328559256, + "timestamp": 0.3004947215596165 + }, + { + "x": 3.1585920860884475, + "y": 5.554797235578974, + "heading": 1.5779705261768464, + "angularVelocity": 5.515380675644807, + "velocityX": 1.7916066919633906, + "velocityY": -0.08354610711348699, + "timestamp": 0.3338830239551294 + }, + { + "x": 3.221965507535244, + "y": 5.559958063123297, + "heading": 1.750590432066646, + "angularVelocity": 5.170071357476323, + "velocityX": 1.898072585304906, + "velocityY": 0.15456992941021028, + "timestamp": 0.36727132635064236 + }, + { + "x": 3.2769241618398413, + "y": 5.5622903490720015, + "heading": 1.9334313224180195, + "angularVelocity": 5.476196069673357, + "velocityX": 1.6460451823384379, + "velocityY": 0.06985338521021929, + "timestamp": 0.4006596287461553 + }, + { + "x": 3.323773131257663, + "y": 5.561780428146509, + "heading": 2.129567464043533, + "angularVelocity": 5.874396946035567, + "velocityX": 1.4031551788066186, + "velocityY": -0.01527244241141609, + "timestamp": 0.43404793114166823 + }, + { + "x": 3.362968545080011, + "y": 5.558640440834612, + "heading": 2.342597979084741, + "angularVelocity": 6.380393723456773, + "velocityX": 1.1739265254654228, + "velocityY": -0.09404453316405703, + "timestamp": 0.46743623353718117 + }, + { + "x": 3.3929228094560866, + "y": 5.555144446568466, + "heading": 2.552295167463074, + "angularVelocity": 6.2805585589315465, + "velocityX": 0.8971484689831585, + "velocityY": -0.10470715835674735, + "timestamp": 0.5008245359326942 + }, + { + "x": 3.4160537194412997, + "y": 5.552868431782891, + "heading": 2.736680731758176, + "angularVelocity": 5.522459995446828, + "velocityX": 0.692784847555345, + "velocityY": -0.068168029586149, + "timestamp": 0.5342128383282071 + }, + { + "x": 3.433893136182045, + "y": 5.55152245545275, + "heading": 2.887212003334188, + "angularVelocity": 4.50850330133112, + "velocityX": 0.5343014008149213, + "velocityY": -0.040312811181968014, + "timestamp": 0.56760114072372 + }, + { + "x": 3.4469382938570345, + "y": 5.550737144467633, + "heading": 3.0014747717216315, + "angularVelocity": 3.4222395326933257, + "velocityX": 0.3907104206872714, + "velocityY": -0.0235205424891011, + "timestamp": 0.600989443119233 + }, + { + "x": 3.4554666542736996, + "y": 5.550283886264018, + "heading": 3.0782886390389157, + "angularVelocity": 2.300622128293862, + "velocityX": 0.25542959074821336, + "velocityY": -0.01357535936459554, + "timestamp": 0.6343777455147459 }, { "x": 3.4596667289733887, "y": 5.5500569343566895, - "heading": -3.166256562885956, - "angularVelocity": -0.8585124235099837, - "velocityX": 0.28076023596367544, - "velocityY": -0.011063667124691125, - "timestamp": 0.5473042920109216 + "heading": 3.1169287442936304, + "angularVelocity": 1.1572946955191077, + "velocityX": 0.12579479633122612, + "velocityY": -0.006797347904669093, + "timestamp": 0.6677660479102588 }, { "x": 3.4596667289733887, "y": 5.5500569343566895, - "heading": -3.166256562885956, - "angularVelocity": 8.614597867332237e-22, - "velocityX": -7.306610025433993e-22, - "velocityY": 6.593534096827558e-23, - "timestamp": 0.5894046221656078 + "heading": 3.1169287442936304, + "angularVelocity": 1.752573179508138e-24, + "velocityX": 1.210114208680477e-22, + "velocityY": 2.4080448202682696e-22, + "timestamp": 0.7011543503057718 } ], "trajectoryWaypoints": [ @@ -528,18 +645,18 @@ "isStopPoint": true, "x": 2.8635878562927246, "y": 5.578441619873047, - "heading": -1.5707963267948966, + "heading": 0.009614917466831607, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 21 }, { - "timestamp": 0.5894046221656078, + "timestamp": 0.7011543503057718, "isStopPoint": true, "x": 3.4596667289733887, "y": 5.5500569343566895, - "heading": -3.166256562885956, + "heading": 3.1169287442936304, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -565,7 +682,7 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [], "eventMarkers": [], - "isTrajectoryStale": false + "isTrajectoryStale": true }, "Mid 3 3": { "waypoints": [ @@ -585,12 +702,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 22 }, { "x": 7.149678707122803, "y": 4.102436542510986, - "heading": -1.6226976913440598, + "heading": -0.02423765959758657, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -599,7 +716,7 @@ { "x": 8.557295799255371, "y": 4.120928764343262, - "heading": -1.5940483219094344, + "heading": -0.002446756286149136, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -611,343 +728,415 @@ "x": 3.4596667289733887, "y": 5.5500569343566895, "heading": -3.0698845150991874, - "angularVelocity": 3.7072815015004976e-26, - "velocityX": 1.0418795881976682e-23, - "velocityY": 6.901816656311587e-24, + "angularVelocity": 1.2312735419469557e-26, + "velocityX": 1.812191248734851e-27, + "velocityY": 6.527407921509027e-26, "timestamp": 0 }, { - "x": 3.470351081239337, - "y": 5.533890502292575, - "heading": -3.040759961328588, - "angularVelocity": 0.5805525842398843, - "velocityX": 0.2129759091858508, - "velocityY": -0.32225262528260434, - "timestamp": 0.05016695224728367 - }, - { - "x": 3.491872536796006, - "y": 5.501531193150436, - "heading": -2.9832552756508868, - "angularVelocity": 1.1462662789289626, - "velocityX": 0.4289966719641501, - "velocityY": -0.6450323906988166, - "timestamp": 0.10033390449456733 - }, - { - "x": 3.5244383644938826, - "y": 5.4529510964403825, - "heading": -2.8984389928959944, - "angularVelocity": 1.6906803972626214, - "velocityX": 0.6491490162159438, - "velocityY": -0.9683685082281274, - "timestamp": 0.15050085674185099 - }, - { - "x": 3.568347380928225, - "y": 5.388082581800036, - "heading": -2.7884682822947835, - "angularVelocity": 2.192094709264814, - "velocityX": 0.8752578035417661, - "velocityY": -1.2930527316189402, - "timestamp": 0.20066780898913467 - }, - { - "x": 3.623990295586385, - "y": 5.306759607475579, - "heading": -2.6577142680126014, - "angularVelocity": 2.606377474112186, - "velocityX": 1.1091547755160458, - "velocityY": -1.6210467385700176, - "timestamp": 0.25083476123641835 - }, - { - "x": 3.691810783485903, - "y": 5.208713512634915, - "heading": -2.5142236168221443, - "angularVelocity": 2.860262478835876, - "velocityX": 1.3518957174280044, - "velocityY": -1.9543960804589637, - "timestamp": 0.301001713483702 - }, - { - "x": 3.7722356432776443, - "y": 5.093905970122991, - "heading": -2.373366389937741, - "angularVelocity": 2.8077692698988006, - "velocityX": 1.6031442252124417, - "velocityY": -2.288509414445041, - "timestamp": 0.3511686657309857 - }, - { - "x": 3.8638571513314606, - "y": 4.9663546159375045, - "heading": -2.2823158901507146, - "angularVelocity": 1.8149497967948816, - "velocityX": 1.826331956587555, - "velocityY": -2.542537436931775, - "timestamp": 0.4013356179782694 - }, - { - "x": 3.966502090333123, - "y": 4.825614406095303, - "heading": -2.2390967768789967, - "angularVelocity": 0.8615056593169522, - "velocityX": 2.046066870789829, - "velocityY": -2.8054367175518826, - "timestamp": 0.45150257022555307 - }, - { - "x": 4.07406031947327, - "y": 4.6692585851239965, - "heading": -2.239096739696053, - "angularVelocity": 7.411840268057862e-7, - "velocityX": 2.144005651568587, - "velocityY": -3.1167095860357494, - "timestamp": 0.5016695224728367 - }, - { - "x": 4.192225193562204, - "y": 4.520756134824619, - "heading": -2.239096700182899, - "angularVelocity": 7.876331299799558e-7, - "velocityX": 2.3554325865058465, - "velocityY": -2.960164882398619, - "timestamp": 0.5518364747201203 - }, - { - "x": 4.326063058683468, - "y": 4.386207500276538, - "heading": -2.2390966080686847, - "angularVelocity": 0.000001836153294318911, - "velocityX": 2.667849233924928, - "velocityY": -2.6820173146030837, - "timestamp": 0.6020034269674039 - }, - { - "x": 4.46665632097007, - "y": 4.270349550388192, - "heading": -2.214327295386285, - "angularVelocity": 0.4937376414717436, - "velocityX": 2.802507547071767, - "velocityY": -2.3094476482696806, - "timestamp": 0.6521703792146876 + "x": 3.470351701902805, + "y": 5.533821202631173, + "heading": -3.0410502376141126, + "angularVelocity": 0.5744467520987541, + "velocityX": 0.21286983864060433, + "velocityY": -0.32345403357157865, + "timestamp": 0.050194865546246525 + }, + { + "x": 3.4918683639146386, + "y": 5.5013221164284385, + "heading": -2.984122678919303, + "angularVelocity": 1.1341311123218276, + "velocityX": 0.4286626087684034, + "velocityY": -0.6474583774468244, + "timestamp": 0.10038973109249305 + }, + { + "x": 3.524415476543047, + "y": 5.452530110487052, + "heading": -2.900159466928913, + "angularVelocity": 1.6727450323187332, + "velocityX": 0.6484151770149017, + "velocityY": -0.9720517309969351, + "timestamp": 0.15058459663873958 + }, + { + "x": 3.568279107796025, + "y": 5.387377079170257, + "heading": -2.7912705513728184, + "angularVelocity": 2.169323781847179, + "velocityX": 0.8738668940663786, + "velocityY": -1.2980019093141477, + "timestamp": 0.2007794621849861 + }, + { + "x": 3.6238335021807875, + "y": 5.305700107177474, + "heading": -2.661703338416061, + "angularVelocity": 2.58128419205311, + "velocityX": 1.1067744435649114, + "velocityY": -1.627197744309744, + "timestamp": 0.2509743277312326 + }, + { + "x": 3.6915048154681016, + "y": 5.207234717854563, + "heading": -2.519260477416935, + "angularVelocity": 2.837797441012924, + "velocityX": 1.348172020203257, + "velocityY": -1.961662577464042, + "timestamp": 0.30116919327747915 + }, + { + "x": 3.771709210218125, + "y": 5.09191800317261, + "heading": -2.378757934963842, + "angularVelocity": 2.799141723442651, + "velocityX": 1.597860535678248, + "velocityY": -2.297380686789684, + "timestamp": 0.3513640588237257 + }, + { + "x": 3.8630702426232038, + "y": 4.963801416536227, + "heading": -2.2881483630208987, + "angularVelocity": 1.8051561839419887, + "velocityX": 1.8201270470762463, + "velocityY": -2.5523842975203284, + "timestamp": 0.4015589243699722 + }, + { + "x": 3.9654177804635915, + "y": 4.822444228754285, + "heading": -2.245465067240904, + "angularVelocity": 0.8503518301223242, + "velocityX": 2.0390041237602423, + "velocityY": -2.8161682722649046, + "timestamp": 0.45175378991621873 + }, + { + "x": 4.072604442672708, + "y": 4.665705551178546, + "heading": -2.2454649759091194, + "angularVelocity": 0.0000018195443548764102, + "velocityX": 2.1354108840148447, + "velocityY": -3.1226037936356046, + "timestamp": 0.5019486554624653 + }, + { + "x": 4.191866544117303, + "y": 4.517947153047455, + "heading": -2.245464878096673, + "angularVelocity": 0.000001948654414335729, + "velocityX": 2.375982087943118, + "velocityY": -2.9436954661220507, + "timestamp": 0.5521435210087118 + }, + { + "x": 4.326720805180037, + "y": 4.384267316197636, + "heading": -2.245464583471803, + "angularVelocity": 0.0000058696216512510084, + "velocityX": 2.686614648633487, + "velocityY": -2.6632173509191657, + "timestamp": 0.6023383865549583 + }, + { + "x": 4.4671387450953555, + "y": 4.269390431902348, + "heading": -2.217765688783718, + "angularVelocity": 0.5518272513862013, + "velocityX": 2.7974562415342206, + "velocityY": -2.288618229078576, + "timestamp": 0.6525332521012048 }, { "x": 4.60924768447876, "y": 4.173398017883301, "heading": -2.157308519530683, - "angularVelocity": 1.1365804239919575, - "velocityX": 2.84233658058051, - "velocityY": -1.9325776863421187, - "timestamp": 0.7023373314619712 - }, - { - "x": 4.757244359209621, - "y": 4.093623523820721, - "heading": -2.0655111962458994, - "angularVelocity": 1.767636272422969, - "velocityX": 2.8498030344597014, - "velocityY": -1.5361263735516977, - "timestamp": 0.7542695783370328 - }, - { - "x": 4.905332920228958, - "y": 4.0351168726459035, - "heading": -1.9470536057388323, - "angularVelocity": 2.281002606955405, - "velocityX": 2.8515723838332723, - "velocityY": -1.126595799245347, - "timestamp": 0.8062018252120944 - }, - { - "x": 5.055400824091604, - "y": 3.998698661500805, - "heading": -1.8151511335185468, - "angularVelocity": 2.539895347444063, - "velocityX": 2.889686329645598, - "velocityY": -0.7012639224471425, - "timestamp": 0.858134072087156 - }, - { - "x": 5.216908347866735, - "y": 3.980352876038938, - "heading": -1.7083842074089743, - "angularVelocity": 2.0558888269639497, - "velocityX": 3.109966032543999, - "velocityY": -0.3532638498388685, - "timestamp": 0.9100663189622176 - }, - { - "x": 5.392426431970856, - "y": 3.975108601121907, - "heading": -1.641860902012389, - "angularVelocity": 1.2809633589824587, - "velocityX": 3.3797514004426206, - "velocityY": -0.1009830159986892, - "timestamp": 0.9619985658372792 - }, - { - "x": 5.582811407311731, - "y": 3.979892618198808, - "heading": -1.6227004666535643, - "angularVelocity": 0.3689506330223126, - "velocityX": 3.6660261551729536, - "velocityY": 0.0921203561326904, - "timestamp": 1.0139308127123408 - }, - { - "x": 5.778669831046716, - "y": 3.995210464766022, - "heading": -1.6227001197359128, - "angularVelocity": 0.000006680197224880823, - "velocityX": 3.7714221032296127, - "velocityY": 0.29495828678596464, - "timestamp": 1.0658630595874024 - }, - { - "x": 5.974528240815056, - "y": 4.010528489936178, - "heading": -1.6226997728238337, - "angularVelocity": 0.000006680089923071776, - "velocityX": 3.7714218342899026, - "velocityY": 0.2949617259389604, - "timestamp": 1.117795306462464 - }, - { - "x": 6.1703866505831755, - "y": 4.025846515108401, - "heading": -1.6226994259114784, - "angularVelocity": 0.0000066800952405189, - "velocityX": 3.7714218342856256, - "velocityY": 0.2949617259787309, - "timestamp": 1.1697275533375255 - }, - { - "x": 6.366245060351238, - "y": 4.0411645402805725, - "heading": -1.6226990789988471, - "angularVelocity": 0.000006680100558868112, - "velocityX": 3.7714218342845336, - "velocityY": 0.2949617259777772, - "timestamp": 1.2216598002125871 - }, - { - "x": 6.562103470119243, - "y": 4.056482565452695, - "heading": -1.6226987320859396, - "angularVelocity": 0.000006680105877144824, - "velocityX": 3.7714218342834416, - "velocityY": 0.2949617259768231, - "timestamp": 1.2735920470876487 - }, - { - "x": 6.757961879887191, - "y": 4.071800590624768, - "heading": -1.6226983851727559, - "angularVelocity": 0.000006680111194481284, - "velocityX": 3.7714218342823496, - "velocityY": 0.29496172597586884, - "timestamp": 1.3255242939627103 - }, - { - "x": 6.953820289655174, - "y": 4.087118615795626, - "heading": -1.622698038259296, - "angularVelocity": 0.000006680116513213821, - "velocityX": 3.7714218342830135, - "velocityY": 0.2949617259524653, - "timestamp": 1.377456540837772 + "angularVelocity": 1.204449271755367, + "velocityX": 2.8311449355806007, + "velocityY": -1.9123950821345668, + "timestamp": 0.7027281176474514 + }, + { + "x": 4.722976104309171, + "y": 4.108925075993853, + "heading": -2.0891694778953918, + "angularVelocity": 1.6986564221053766, + "velocityX": 2.8351662436762037, + "velocityY": -1.6072632394614674, + "timestamp": 0.7428416101398513 + }, + { + "x": 4.836477408039613, + "y": 4.056947172457928, + "heading": -2.0033151883417575, + "angularVelocity": 2.1402845830464856, + "velocityX": 2.8295044055799075, + "velocityY": -1.2957710811586096, + "timestamp": 0.7829551026322512 + }, + { + "x": 4.949920984616205, + "y": 4.0177960509685695, + "heading": -1.903114928393686, + "angularVelocity": 2.4979191220274792, + "velocityX": 2.828065309897542, + "velocityY": -0.9760087954639355, + "timestamp": 0.8230685951246511 + }, + { + "x": 5.064065362215247, + "y": 3.991835755777838, + "heading": -1.794426344777262, + "angularVelocity": 2.7095268166195297, + "velocityX": 2.845535766317746, + "velocityY": -0.647171153088953, + "timestamp": 0.8631820876170511 + }, + { + "x": 5.180659510984554, + "y": 3.978933247316211, + "heading": -1.6868519677774367, + "angularVelocity": 2.681750461399157, + "velocityX": 2.906606768069287, + "velocityY": -0.32165008978141457, + "timestamp": 0.903295580109451 + }, + { + "x": 5.304982214386576, + "y": 3.975371793740181, + "heading": -1.599436041565328, + "angularVelocity": 2.179215041638947, + "velocityX": 3.0992739768439947, + "velocityY": -0.08878443024388993, + "timestamp": 0.9434090726018509 + }, + { + "x": 5.437286305059447, + "y": 3.9799465783185837, + "heading": -1.5362326476191075, + "angularVelocity": 1.5756143386965153, + "velocityX": 3.298244118183867, + "velocityY": 0.11404603025451941, + "timestamp": 0.9835225650942508 + }, + { + "x": 5.574519249175071, + "y": 3.9911087281656146, + "heading": -1.4896376312982285, + "angularVelocity": 1.1615796437995773, + "velocityX": 3.421116826006249, + "velocityY": 0.278264223668521, + "timestamp": 1.0236360575866508 + }, + { + "x": 5.710295358289056, + "y": 4.008878734245578, + "heading": -1.4406573828911584, + "angularVelocity": 1.2210417334354526, + "velocityX": 3.3847989959914093, + "velocityY": 0.44299324182080874, + "timestamp": 1.0637495500790508 + }, + { + "x": 5.839309895761849, + "y": 4.034487620002502, + "heading": -1.374832058502612, + "angularVelocity": 1.6409771450595625, + "velocityX": 3.216237965249132, + "velocityY": 0.6384107731775158, + "timestamp": 1.1038630425714508 + }, + { + "x": 5.9651423912202, + "y": 4.060367188933271, + "heading": -1.3030479833599253, + "angularVelocity": 1.789524438847772, + "velocityX": 3.1369119874613967, + "velocityY": 0.6451587065292861, + "timestamp": 1.1439765350638509 + }, + { + "x": 6.088325953926389, + "y": 4.085030282261382, + "heading": -1.226883005971526, + "angularVelocity": 1.8987371244932074, + "velocityX": 3.0708760332829894, + "velocityY": 0.6148328603594958, + "timestamp": 1.184090027556251 + }, + { + "x": 6.2080954642882755, + "y": 4.108236900279935, + "heading": -1.1444073412911668, + "angularVelocity": 2.056057938509982, + "velocityX": 2.9857662078309084, + "velocityY": 0.5785239972049276, + "timestamp": 1.224203520048651 + }, + { + "x": 6.31677879626179, + "y": 4.1302920762013215, + "heading": -1.0366273178562313, + "angularVelocity": 2.686877076468872, + "velocityX": 2.7093958970066208, + "velocityY": 0.5498193887147775, + "timestamp": 1.264317012541051 + }, + { + "x": 6.414746281726902, + "y": 4.147226598563455, + "heading": -0.9049619897221106, + "angularVelocity": 3.2823202357427825, + "velocityX": 2.4422576888231124, + "velocityY": 0.4221652444085389, + "timestamp": 1.304430505033451 + }, + { + "x": 6.503675902170543, + "y": 4.155946147636541, + "heading": -0.7520815391315856, + "angularVelocity": 3.8111977065943705, + "velocityX": 2.216950330627282, + "velocityY": 0.21737197464765756, + "timestamp": 1.344543997525851 + }, + { + "x": 6.586090114201575, + "y": 4.1540858277756465, + "heading": -0.580040497638246, + "angularVelocity": 4.288857209975799, + "velocityX": 2.0545259689528748, + "velocityY": -0.0463764121572527, + "timestamp": 1.3846574900182511 + }, + { + "x": 6.676170358790984, + "y": 4.141704344881373, + "heading": -0.41948306620614584, + "angularVelocity": 4.002579218514071, + "velocityX": 2.245634548187899, + "velocityY": -0.3086613038398522, + "timestamp": 1.4247709825106512 + }, + { + "x": 6.778119596986203, + "y": 4.12741918933359, + "heading": -0.2818617425003748, + "angularVelocity": 3.4307988448486575, + "velocityX": 2.54151986926928, + "velocityY": -0.3561184693775817, + "timestamp": 1.4648844750030512 + }, + { + "x": 6.891681505117521, + "y": 4.114615252299952, + "heading": -0.16923343203441635, + "angularVelocity": 2.80774132263097, + "velocityX": 2.831015228924134, + "velocityY": -0.3191927762476476, + "timestamp": 1.5049979674954512 + }, + { + "x": 7.015942193168223, + "y": 4.105678430574392, + "heading": -0.0831259035248278, + "angularVelocity": 2.1465976448174597, + "velocityX": 3.097727979538158, + "velocityY": -0.22278842280444786, + "timestamp": 1.5451114599878513 }, { "x": 7.149678707122803, "y": 4.102436542510986, - "heading": -1.6226976913440598, - "angularVelocity": 0.000006680150717856603, - "velocityX": 3.7714219825462947, - "velocityY": 0.29495983010733695, - "timestamp": 1.4293887877128335 - }, - { - "x": 7.416831851800406, - "y": 4.105946276559182, - "heading": -1.6226976909008946, - "angularVelocity": 6.274765917736025e-9, - "velocityX": 3.7826143141948623, - "velocityY": 0.04969423162037773, - "timestamp": 1.5000153762273085 - }, - { - "x": 7.683984997699984, - "y": 4.109455917591789, - "heading": -1.6226976904577277, - "angularVelocity": 6.274788999649594e-9, - "velocityX": 3.7826143314967657, - "velocityY": 0.049692914615112965, - "timestamp": 1.5706419647417835 - }, - { - "x": 7.933502359055495, - "y": 4.112733873648306, - "heading": -1.6145121736520152, - "angularVelocity": 0.11589851609546277, - "velocityX": 3.532909724280016, - "velocityY": 0.0464124931624786, - "timestamp": 1.6412685532562585 - }, - { - "x": 8.141433501977613, - "y": 4.115465503819871, - "heading": -1.60769089632527, - "angularVelocity": 0.09658228537184779, - "velocityX": 2.944091556673475, - "velocityY": 0.038677079397722, - "timestamp": 1.7118951417707335 - }, - { - "x": 8.307778419316605, - "y": 4.117650808004561, - "heading": -1.6022338692720377, - "angularVelocity": 0.07726590180855333, - "velocityX": 2.355273287834106, - "velocityY": 0.030941664189867397, - "timestamp": 1.7825217302852085 - }, - { - "x": 8.432537108690411, - "y": 4.119289786165083, - "heading": -1.5981410965811054, - "angularVelocity": 0.057949460352219886, - "velocityX": 1.7664549852671345, - "velocityY": 0.023206248453940526, - "timestamp": 1.8531483187996836 - }, - { - "x": 8.515709568908191, - "y": 4.120382438281781, - "heading": -1.5954125803793526, - "angularVelocity": 0.03863298878146346, - "velocityX": 1.1776366658391457, - "velocityY": 0.015470832439747922, - "timestamp": 1.9237749073141586 + "heading": -0.02423765959758657, + "angularVelocity": 1.4680408079251241, + "velocityX": 3.3339533818930946, + "velocityY": -0.0808178959740319, + "timestamp": 1.5852249524802513 + }, + { + "x": 7.416849021016848, + "y": 4.104072679647048, + "heading": -0.02423765672113594, + "angularVelocity": 4.072771835592131e-8, + "velocityX": 3.7828694770841707, + "velocityY": 0.023166096345521754, + "timestamp": 1.655851315750913 + }, + { + "x": 7.683995167692338, + "y": 4.108021145887048, + "heading": -0.024237655875962986, + "angularVelocity": 1.1966819636377302e-8, + "velocityX": 3.7825272929841307, + "velocityY": 0.05590640742563016, + "timestamp": 1.7264776790215746 + }, + { + "x": 7.933509607434154, + "y": 4.111709036478853, + "heading": -0.01801171480387418, + "angularVelocity": 0.08815321621798794, + "velocityX": 3.5328796243635003, + "velocityY": 0.05221691194365583, + "timestamp": 1.7971040422922362 + }, + { + "x": 8.141438328466565, + "y": 4.114782278956212, + "heading": -0.012823406858938095, + "angularVelocity": 0.07346134934136293, + "velocityX": 2.9440666544809257, + "velocityY": 0.04351409778216477, + "timestamp": 1.8677304055628978 + }, + { + "x": 8.307781312832681, + "y": 4.11724087305162, + "heading": -0.008672751470206637, + "angularVelocity": 0.05876920736842818, + "velocityX": 2.3552534303463926, + "velocityY": 0.034811279833078496, + "timestamp": 1.9383567688335595 + }, + { + "x": 8.432538554548055, + "y": 4.119084818675069, + "heading": -0.00555975571941177, + "angularVelocity": 0.044076965125117926, + "velocityX": 1.7664401214779493, + "velocityY": 0.02610846060956428, + "timestamp": 2.008983132104221 + }, + { + "x": 8.515710050620672, + "y": 4.120314115781301, + "heading": -0.0034844232543666874, + "angularVelocity": 0.029384671232352395, + "velocityX": 1.1776267702455339, + "velocityY": 0.01740564074522844, + "timestamp": 2.079609495374883 }, { "x": 8.557295799255371, "y": 4.120928764343262, - "heading": -1.5940483219094344, - "angularVelocity": 0.019316499615988192, - "velocityX": 0.5888183362935331, - "velocityY": 0.007735416264214447, - "timestamp": 1.9944014958286336 + "heading": -0.002446756286149136, + "angularVelocity": 0.014692346032895615, + "velocityX": 0.5888133935953765, + "velocityY": 0.008702820497862328, + "timestamp": 2.150235858645545 }, { "x": 8.557295799255371, "y": 4.120928764343262, - "heading": -1.5940483219094344, - "angularVelocity": 6.715491935087819e-29, - "velocityX": 4.935316924694466e-27, - "velocityY": -9.408823776589858e-28, - "timestamp": 2.065028084343109 + "heading": -0.002446756286149136, + "angularVelocity": -1.1375698231513232e-28, + "velocityX": -3.986760367825934e-26, + "velocityY": 7.675786204695289e-26, + "timestamp": 2.2208622219162066 } ], "trajectoryWaypoints": [ @@ -963,7 +1152,7 @@ "controlIntervalCount": 14 }, { - "timestamp": 0.7023373314619712, + "timestamp": 0.7027281176474514, "isStopPoint": false, "x": 4.60924768447876, "y": 4.173398017883301, @@ -971,25 +1160,25 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 22 }, { - "timestamp": 1.4293887877128335, + "timestamp": 1.5852249524802513, "isStopPoint": false, "x": 7.149678707122803, "y": 4.102436542510986, - "heading": -1.6226976913440598, + "heading": -0.02423765959758657, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 9 }, { - "timestamp": 2.065028084343109, + "timestamp": 2.2208622219162066, "isStopPoint": true, "x": 8.557295799255371, "y": 4.120928764343262, - "heading": -1.5940483219094344, + "heading": -0.002446756286149136, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -1022,11 +1211,11 @@ { "x": 8.270874977111816, "y": 3.5915117263793945, - "heading": -1.5707963267948966, + "heading": -0.04647811321922418, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 26 }, { "x": 4.680209159851074, @@ -1051,281 +1240,353 @@ { "x": 8.270874977111816, "y": 3.5915117263793945, - "heading": -1.5707963267948966, - "angularVelocity": 5.354414694519943e-27, - "velocityX": 1.1934033992121382e-26, - "velocityY": 2.4414828428621674e-26, + "heading": -0.04647811321922418, + "angularVelocity": 1.3316191465946648e-27, + "velocityX": 8.288424720789902e-27, + "velocityY": -2.121860390602647e-27, "timestamp": 0 }, { - "x": 8.23848292883025, - "y": 3.5938924779782577, - "heading": -1.6238948429612086, - "angularVelocity": -0.8155689384136132, - "velocityX": -0.4975270560724381, - "velocityY": 0.0365672563811382, - "timestamp": 0.06510610405246105 - }, - { - "x": 8.17314182032488, - "y": 3.5988858589902613, - "heading": -1.7240953242981718, - "angularVelocity": -1.539033594395756, - "velocityX": -1.0036095609824784, - "velocityY": 0.07669605000446453, - "timestamp": 0.1302122081049221 - }, - { - "x": 8.074106083315115, - "y": 3.6065972666192967, - "heading": -1.8622827836674782, - "angularVelocity": -2.1224962141484913, - "velocityX": -1.5211436539032237, - "velocityY": 0.11844369650535398, - "timestamp": 0.19531831215738316 - }, - { - "x": 7.940524553703249, - "y": 3.6169600723778745, - "heading": -2.0255788872462053, - "angularVelocity": -2.508153512720506, - "velocityX": -2.0517512383205907, - "velocityY": 0.159167959892478, - "timestamp": 0.2604244162098442 - }, - { - "x": 7.771710550914322, - "y": 3.62995093841072, - "heading": -2.1923426679447573, - "angularVelocity": -2.561415448299248, - "velocityX": -2.5929059225061466, - "velocityY": 0.1995337644896925, - "timestamp": 0.3255305202623052 - }, - { - "x": 7.571402537694921, - "y": 3.6467516463163245, - "heading": -2.304001906692002, - "angularVelocity": -1.7150348707284366, - "velocityX": -3.0766395276546734, - "velocityY": 0.25805119427921946, - "timestamp": 0.39063662431476626 - }, - { - "x": 7.342572310447652, - "y": 3.66977975125525, - "heading": -2.3441463215039384, - "angularVelocity": -0.6165998625810742, - "velocityX": -3.5147276983872913, - "velocityY": 0.35370116633564747, - "timestamp": 0.4557427283672273 - }, - { - "x": 7.096978065151491, - "y": 3.688312829536684, - "heading": -2.3441463518162298, - "angularVelocity": -4.65582937531096e-7, - "velocityX": -3.772215353237325, - "velocityY": 0.28465961143213553, - "timestamp": 0.5208488324196884 - }, - { - "x": 6.85138381589539, - "y": 3.7068458553425065, - "heading": -2.3441463821284625, - "angularVelocity": -4.6558204127672377e-7, - "velocityX": -3.7722154140602266, - "velocityY": 0.28465880543073424, - "timestamp": 0.5859549364721495 - }, - { - "x": 6.605789566639263, - "y": 3.7253788811480093, - "heading": -2.344146412440695, - "angularVelocity": -4.6558204038728883e-7, - "velocityX": -3.772215414060598, - "velocityY": 0.28465880542582156, - "timestamp": 0.6510610405246106 - }, - { - "x": 6.360195317383137, - "y": 3.743911906953512, - "heading": -2.344146442752928, - "angularVelocity": -4.655820403555088e-7, - "velocityX": -3.7722154140605975, - "velocityY": 0.2846588054258209, - "timestamp": 0.7161671445770716 - }, - { - "x": 6.11460106812701, - "y": 3.762444932759015, - "heading": -2.3441464730651607, - "angularVelocity": -4.6558203882329284e-7, - "velocityX": -3.7722154140605975, - "velocityY": 0.2846588054258197, - "timestamp": 0.7812732486295327 - }, - { - "x": 5.869006818870883, - "y": 3.780977958564517, - "heading": -2.3441465033773934, - "angularVelocity": -4.655820376871189e-7, - "velocityX": -3.772215414060598, - "velocityY": 0.28465880542581967, - "timestamp": 0.8463793526819938 - }, - { - "x": 5.623412569614877, - "y": 3.799510984371621, - "heading": -2.344146533689626, - "angularVelocity": -4.65582035307202e-7, - "velocityX": -3.7722154140587425, - "velocityY": 0.2846588054504081, - "timestamp": 0.9114854567344549 - }, - { - "x": 5.3778183401842155, - "y": 3.818044272896504, - "heading": -2.344146564001866, - "angularVelocity": -4.6558214602936475e-7, - "velocityX": -3.7722151095505105, - "velocityY": 0.2846628406754266, - "timestamp": 0.976591560786916 - }, - { - "x": 5.135150406557795, - "y": 3.860142753669275, - "heading": -2.3441465956568845, - "angularVelocity": -4.862066176822936e-7, - "velocityX": -3.7272685435283313, - "velocityY": 0.6466134225885977, - "timestamp": 1.041697664839377 - }, - { - "x": 4.901006760007509, - "y": 3.936541444082641, - "heading": -2.3441466329967953, - "angularVelocity": -5.735239590740333e-7, - "velocityX": -3.5963393902608347, - "velocityY": 1.1734489649665605, - "timestamp": 1.106803768891838 + "x": 8.261714565981372, + "y": 3.5923814975091863, + "heading": -0.13089503027674879, + "angularVelocity": -1.710158870612933, + "velocityX": -0.18557605393852974, + "velocityY": 0.01762024561976423, + "timestamp": 0.04936203209428664 + }, + { + "x": 8.243251196384476, + "y": 3.595057930838583, + "heading": -0.2984548440137123, + "angularVelocity": -3.3945080181647858, + "velocityX": -0.3740399009835839, + "velocityY": 0.054220485175413155, + "timestamp": 0.09872406418857328 + }, + { + "x": 8.215818750608202, + "y": 3.6007267499258475, + "heading": -0.5482060630272272, + "angularVelocity": -5.05958139114824, + "velocityX": -0.5557397986345999, + "velocityY": 0.11484168796853474, + "timestamp": 0.14808609628285993 + }, + { + "x": 8.172797656458112, + "y": 3.6028495640275278, + "heading": -0.8441726532098716, + "angularVelocity": -5.995834807151321, + "velocityX": -0.8715422020697032, + "velocityY": 0.04300499820642603, + "timestamp": 0.19744812837714656 + }, + { + "x": 8.109804374431524, + "y": 3.604125050259272, + "heading": -1.1531707681026144, + "angularVelocity": -6.259833758515535, + "velocityX": -1.2761484759432886, + "velocityY": 0.025839419035829633, + "timestamp": 0.2468101604714332 + }, + { + "x": 8.028058626769713, + "y": 3.605873396143341, + "heading": -1.4327138691793657, + "angularVelocity": -5.66311979504399, + "velocityX": -1.6560450247604885, + "velocityY": 0.035418839336477916, + "timestamp": 0.29617219256571986 + }, + { + "x": 7.929944698522907, + "y": 3.617190837314496, + "heading": -1.6856390841183952, + "angularVelocity": -5.123881740847214, + "velocityX": -1.9876395700120058, + "velocityY": 0.22927421524172362, + "timestamp": 0.3455342246600065 + }, + { + "x": 7.81789322279875, + "y": 3.6363759065331616, + "heading": -1.8880322520166901, + "angularVelocity": -4.100179010290798, + "velocityX": -2.2699931702594154, + "velocityY": 0.38866044213942363, + "timestamp": 0.39489625675429313 + }, + { + "x": 7.690304977256709, + "y": 3.6594593247549176, + "heading": -2.0402246489251294, + "angularVelocity": -3.083187430730889, + "velocityX": -2.584744592733444, + "velocityY": 0.4676350879895761, + "timestamp": 0.44425828884857976 + }, + { + "x": 7.546813675021453, + "y": 3.6826342543625548, + "heading": -2.1470758213139596, + "angularVelocity": -2.1646429017495277, + "velocityX": -2.9069164324753056, + "velocityY": 0.46948897005234025, + "timestamp": 0.4936203209428664 + }, + { + "x": 7.387459875875891, + "y": 3.7001461214884253, + "heading": -2.213536834034917, + "angularVelocity": -1.3463994471299328, + "velocityX": -3.228266592452671, + "velocityY": 0.3547639021914809, + "timestamp": 0.542982353037153 + }, + { + "x": 7.21184251301541, + "y": 3.7140792269378826, + "heading": -2.2398648640063543, + "angularVelocity": -0.5333660073221452, + "velocityX": -3.5577417583829254, + "velocityY": 0.28226361149077894, + "timestamp": 0.5923443851314397 + }, + { + "x": 7.025484740409283, + "y": 3.725907654379097, + "heading": -2.2398670079946887, + "angularVelocity": -0.0000434339560853095, + "velocityX": -3.775326190991562, + "velocityY": 0.23962602306608316, + "timestamp": 0.6417064172257264 + }, + { + "x": 6.83912695720996, + "y": 3.7377359168228743, + "heading": -2.2398691516824063, + "angularVelocity": -0.0000434278660421668, + "velocityX": -3.775326405593678, + "velocityY": 0.2396226804679459, + "timestamp": 0.6910684493200131 + }, + { + "x": 6.652769174009924, + "y": 3.749564179259098, + "heading": -2.2398712953687108, + "angularVelocity": -0.00004342783741540177, + "velocityX": -3.775326405608112, + "velocityY": 0.2396226803149126, + "timestamp": 0.7404304814142998 + }, + { + "x": 6.46641139080954, + "y": 3.761392441693503, + "heading": -2.239873439053602, + "angularVelocity": -0.00004342780879696299, + "velocityX": -3.775326405615169, + "velocityY": 0.23962268027806155, + "timestamp": 0.7897925135085865 + }, + { + "x": 6.280053607608808, + "y": 3.773220704126089, + "heading": -2.239875582737081, + "angularVelocity": -0.00004342778017790842, + "velocityX": -3.7753264056222258, + "velocityY": 0.2396226802412151, + "timestamp": 0.8391545456028732 + }, + { + "x": 6.093695824407728, + "y": 3.7850489665568556, + "heading": -2.239877726419147, + "angularVelocity": -0.000043427751560344506, + "velocityX": -3.775326405629282, + "velocityY": 0.23962268020436814, + "timestamp": 0.8885165776971599 + }, + { + "x": 5.907338041206299, + "y": 3.796877228985804, + "heading": -2.2398798700998004, + "angularVelocity": -0.00004342772294180255, + "velocityX": -3.7753264056363385, + "velocityY": 0.23962268016752136, + "timestamp": 0.9378786097914465 + }, + { + "x": 5.720980258004522, + "y": 3.8087054914129355, + "heading": -2.2398820137790416, + "angularVelocity": -0.000043427694325368904, + "velocityX": -3.7753264056433924, + "velocityY": 0.2396226801307148, + "timestamp": 0.9872406418857332 + }, + { + "x": 5.534622474806013, + "y": 3.820533753895205, + "heading": -2.2398841574568737, + "angularVelocity": -0.000043427665782203, + "velocityX": -3.775326405577184, + "velocityY": 0.23962268124772898, + "timestamp": 1.0366026739800198 + }, + { + "x": 5.348264795875393, + "y": 3.8323636550337796, + "heading": -2.2398863017677915, + "angularVelocity": -0.0000434404911395521, + "velocityX": -3.7753242932677007, + "velocityY": 0.23965587794235257, + "timestamp": 1.0859647060743065 + }, + { + "x": 5.171853769012511, + "y": 3.8569512152531384, + "heading": -2.2615346019691334, + "angularVelocity": -0.4385617707146157, + "velocityX": -3.573820188883595, + "velocityY": 0.49810672648957277, + "timestamp": 1.1353267381685932 + }, + { + "x": 5.002868859035653, + "y": 3.900247608117125, + "heading": -2.2930504927284256, + "angularVelocity": -0.6384642086673774, + "velocityX": -3.4233783093467514, + "velocityY": 0.8771193370095877, + "timestamp": 1.1846887702628799 + }, + { + "x": 4.839142910274149, + "y": 3.9631633228826875, + "heading": -2.323008869125164, + "angularVelocity": -0.6069113268982622, + "velocityX": -3.316839721038412, + "velocityY": 1.2745770807284975, + "timestamp": 1.2340508023571666 }, { "x": 4.680209159851074, "y": 4.0456671714782715, "heading": -2.3441466852834205, - "angularVelocity": -8.030986646480626e-7, - "velocityX": -3.3913502177694665, - "velocityY": 1.6761212943674095, - "timestamp": 1.171909872944299 - }, - { - "x": 4.472372375309597, - "y": 4.189862041622909, - "heading": -2.3441466910141098, - "angularVelocity": -8.570105231495362e-8, - "velocityX": -3.108148039650104, - "velocityY": 2.1563988490122856, - "timestamp": 1.238778240181039 - }, - { - "x": 4.287987685649001, - "y": 4.363039917097588, - "heading": -2.3441466957763613, - "angularVelocity": -7.121830279054471e-8, - "velocityX": -2.7574277237516953, - "velocityY": 2.5898325715291013, - "timestamp": 1.305646607417779 - }, - { - "x": 4.131060347221029, - "y": 4.561438760621924, - "heading": -2.3441467001447087, - "angularVelocity": -6.532756518877877e-8, - "velocityX": -2.3468097833522306, - "velocityY": 2.9670059509891678, - "timestamp": 1.372514974654519 - }, - { - "x": 3.9877899693707293, - "y": 4.769913913918279, - "heading": -2.3441467044472297, - "angularVelocity": -6.434314457198755e-8, - "velocityX": -2.1425732939323447, - "velocityY": 3.117694687508523, - "timestamp": 1.4393833418912592 - }, - { - "x": 3.8445196551450387, - "y": 4.978389110939239, - "heading": -2.3441467087499444, - "angularVelocity": -6.434603866942448e-8, - "velocityX": -2.14257234244195, - "velocityY": 3.117695341399279, - "timestamp": 1.5062517091279992 - }, - { - "x": 3.7183406970943045, - "y": 5.161996657346525, - "heading": -2.4164805017111153, - "angularVelocity": -1.08173409865207, - "velocityX": -1.8869753108224054, - "velocityY": 2.7458057373712084, - "timestamp": 1.5731200763647393 - }, - { - "x": 3.61319236178323, - "y": 5.315003459544194, - "heading": -2.4767605080431827, - "angularVelocity": -0.9014726816740256, - "velocityX": -1.5724675157508774, - "velocityY": 2.2881791274484944, - "timestamp": 1.6399884436014793 - }, - { - "x": 3.5290741458237123, - "y": 5.437409251753405, - "heading": -2.5249829334569385, - "angularVelocity": -0.721154522033258, - "velocityX": -1.257967248724744, - "velocityY": 1.8305485428685142, - "timestamp": 1.7068568108382194 - }, - { - "x": 3.465985698821635, - "y": 5.529213783971836, - "heading": -2.561148047037766, - "angularVelocity": -0.5408403864982843, - "velocityX": -0.9434722217565042, - "velocityY": 1.3729142195652448, - "timestamp": 1.7737251780749594 - }, - { - "x": 3.423926804037115, - "y": 5.590416869126759, - "heading": -2.5852574378563697, - "angularVelocity": -0.36055001512518836, - "velocityX": -0.628980436079966, - "velocityY": 0.915277098635298, - "timestamp": 1.8405935453116995 + "angularVelocity": -0.42822013724802277, + "velocityX": -3.2197570415961705, + "velocityY": 1.6714030013592756, + "timestamp": 1.2834128344514533 + }, + { + "x": 4.474436093505845, + "y": 4.192482234656094, + "heading": -2.344146743629121, + "angularVelocity": -8.731670658673155e-7, + "velocityX": -3.0794773957732224, + "velocityY": 2.1971469660494822, + "timestamp": 1.35023360950493 + }, + { + "x": 4.292459307252108, + "y": 4.367929309366008, + "heading": -2.3441467553603754, + "angularVelocity": -1.7556298637551318e-7, + "velocityX": -2.723356412853638, + "velocityY": 2.6256366312647916, + "timestamp": 1.417054384558407 + }, + { + "x": 4.138225756164794, + "y": 4.5682022210646345, + "heading": -2.344146766260572, + "angularVelocity": -1.631258670814988e-7, + "velocityX": -2.3081676464225445, + "velocityY": 2.9971653507213616, + "timestamp": 1.4838751596118838 + }, + { + "x": 3.993481705313873, + "y": 4.7754373225031195, + "heading": -2.3441467770570332, + "angularVelocity": -1.6157341933246893e-7, + "velocityX": -2.1661534266114293, + "velocityY": 3.1013573439193687, + "timestamp": 1.5506959346653606 + }, + { + "x": 3.8487377220414505, + "y": 4.982672471141195, + "heading": -2.344146787854829, + "angularVelocity": -1.615933934586182e-7, + "velocityX": -2.1661524152717977, + "velocityY": 3.1013580502803935, + "timestamp": 1.6175167097188374 + }, + { + "x": 3.7213536241170644, + "y": 5.165056170282478, + "heading": -2.4164814183686705, + "angularVelocity": -1.082517083286617, + "velocityX": -1.9063546901759274, + "velocityY": 2.7294460292524283, + "timestamp": 1.6843374847723143 + }, + { + "x": 3.6152009914473524, + "y": 5.317043121214145, + "heading": -2.476761601610272, + "angularVelocity": -0.9021173907869166, + "velocityX": -1.5886172015328703, + "velocityY": 2.274546363912, + "timestamp": 1.751158259825791 + }, + { + "x": 3.530279328218681, + "y": 5.43863304450566, + "heading": -2.5249837389973546, + "angularVelocity": -0.7216638440438612, + "velocityX": -1.2708871329419387, + "velocityY": 1.8196425167802273, + "timestamp": 1.817979034879268 + }, + { + "x": 3.4665882908564125, + "y": 5.52982568005271, + "heading": -2.561148443081316, + "angularVelocity": -0.5412194643809262, + "velocityX": -0.953162206084802, + "velocityY": 1.3647347770819482, + "timestamp": 1.8847998099327448 + }, + { + "x": 3.4241276677886727, + "y": 5.590620834962734, + "heading": -2.585257539099172, + "angularVelocity": -0.3608024001302229, + "velocityX": -0.6354404454865786, + "velocityY": 0.9098241506682598, + "timestamp": 1.9516205849862216 }, { "x": 3.402897357940674, "y": 5.621018409729004, "heading": -2.59731223675972, - "angularVelocity": -0.18027655529066144, - "velocityX": -0.31449019866133116, - "velocityY": 0.45763851977875486, - "timestamp": 1.9074619125484396 + "angularVelocity": -0.18040343966229697, + "velocityX": -0.3177201975135489, + "velocityY": 0.45491203509600503, + "timestamp": 2.0184413600396987 }, { "x": 3.402897357940674, "y": 5.621018409729004, "heading": -2.59731223675972, - "angularVelocity": 2.7558604882702093e-25, - "velocityX": -2.598680883455403e-25, - "velocityY": -4.201313047715582e-25, - "timestamp": 1.9743302797851796 + "angularVelocity": 3.854629324739845e-28, + "velocityX": 2.481722314351389e-27, + "velocityY": -5.327025844101886e-28, + "timestamp": 2.0852621350931755 } ], "trajectoryWaypoints": [ @@ -1334,14 +1595,14 @@ "isStopPoint": true, "x": 8.270874977111816, "y": 3.5915117263793945, - "heading": -1.5707963267948966, + "heading": -0.04647811321922418, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 18 + "controlIntervalCount": 26 }, { - "timestamp": 1.171909872944299, + "timestamp": 1.2834128344514533, "isStopPoint": false, "x": 4.680209159851074, "y": 4.0456671714782715, @@ -1352,7 +1613,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 1.9743302797851796, + "timestamp": 2.0852621350931755, "isStopPoint": true, "x": 3.402897357940674, "y": 5.621018409729004, diff --git a/src/main/deploy/choreo/Amp 2 Piece 1.1.traj b/src/main/deploy/choreo/Amp 2 Piece 1.1.traj index 8a03c8b..2c3d605 100644 --- a/src/main/deploy/choreo/Amp 2 Piece 1.1.traj +++ b/src/main/deploy/choreo/Amp 2 Piece 1.1.traj @@ -1,139 +1,193 @@ { "samples": [ { - "x": 0.9050432443618774, - "y": 6.6428680419921875, - "heading": -2.2565255466280507, - "angularVelocity": -1.4291101663678553e-21, - "velocityX": 4.063581905170547e-21, - "velocityY": 7.609678430321777e-22, + "x": 0.5076572895050049, + "y": 7.026061534881592, + "heading": 3.141592653589793, + "angularVelocity": 5.576382446952711e-28, + "velocityX": -1.7262779855569846e-28, + "velocityY": -3.130496405792362e-27, "timestamp": 0 }, { - "x": 0.9488587964840457, - "y": 6.6487212093785475, - "heading": -2.2170365190388273, - "angularVelocity": 0.5340397711035277, - "velocityX": 0.5925506110077822, - "velocityY": 0.0791567775169907, - "timestamp": 0.07394398268807607 - }, - { - "x": 1.036787588456291, - "y": 6.660437165626341, - "heading": -2.1423875869378732, - "angularVelocity": 1.00953356023372, - "velocityX": 1.1891270766958724, - "velocityY": 0.15844367346548532, - "timestamp": 0.14788796537615215 - }, - { - "x": 1.1692835732429303, - "y": 6.678111777529781, - "heading": -2.0408936155248174, - "angularVelocity": 1.3725791839100139, - "velocityX": 1.791842689132625, - "velocityY": 0.23902704805829636, - "timestamp": 0.22183194806422823 - }, - { - "x": 1.346933296813628, - "y": 6.701897508529881, - "heading": -1.9340009000220366, - "angularVelocity": 1.4455904539750712, - "velocityX": 2.40249060320367, - "velocityY": 0.32167230024149257, - "timestamp": 0.2957759307523043 - }, - { - "x": 1.5625585723226694, - "y": 6.726906508694774, - "heading": -1.9227308281996909, - "angularVelocity": 0.15241364358053452, - "velocityX": 2.916062506650325, - "velocityY": 0.33821548765929105, - "timestamp": 0.36971991344038035 - }, - { - "x": 1.7820773559348524, - "y": 6.756085107088855, - "heading": -1.9227308124237847, - "angularVelocity": 2.1334942281526128e-7, - "velocityX": 2.9687173402436384, - "velocityY": 0.39460409534053714, - "timestamp": 0.4436638961284564 - }, - { - "x": 2.0015961382526735, - "y": 6.785263715245929, - "heading": -1.9227307966478815, - "angularVelocity": 2.1334938403089738e-7, - "velocityX": 2.9687173226950803, - "velocityY": 0.39460422736336004, - "timestamp": 0.5176078788165325 - }, - { - "x": 2.221114921646043, - "y": 6.8144423152746825, - "heading": -1.9227307808719754, - "angularVelocity": 2.1334942614926208e-7, - "velocityX": 2.968717337305404, - "velocityY": 0.39460411744569185, - "timestamp": 0.5915518615046086 - }, - { - "x": 2.436370432370262, - "y": 6.840013832318715, - "heading": -1.910648519871317, - "angularVelocity": 0.1633974876850485, - "velocityX": 2.911061899824357, - "velocityY": 0.3458228257933233, - "timestamp": 0.6654958441926847 - }, - { - "x": 2.613907826780238, - "y": 6.863558295707786, - "heading": -1.7989968132166365, - "angularVelocity": 1.5099498646924372, - "velocityX": 2.4009714915986042, - "velocityY": 0.31840945717834496, - "timestamp": 0.7394398268807608 - }, - { - "x": 2.746285116177599, - "y": 6.881046947319712, - "heading": -1.6928599971823888, - "angularVelocity": 1.435367857882004, - "velocityX": 1.7902374822825093, - "velocityY": 0.23651216740432432, - "timestamp": 0.8133838095688369 - }, - { - "x": 2.834070756139549, - "y": 6.892600291272677, - "heading": -1.6134370485289746, - "angularVelocity": 1.074096170724946, - "velocityX": 1.1871911245679945, - "velocityY": 0.15624454530041104, - "timestamp": 0.887327792256913 - }, - { - "x": 2.8777801990509033, - "y": 6.8983306884765625, - "heading": -1.5707963267948966, - "angularVelocity": 0.5766624975280655, - "velocityX": 0.5911156164696019, - "velocityY": 0.07749646415515937, - "timestamp": 0.9612717749449892 - }, - { - "x": 2.8777801990509033, - "y": 6.8983306884765625, - "heading": -1.5707963267948966, - "angularVelocity": -1.3704110825531436e-21, - "velocityX": 4.248115223834914e-21, - "velocityY": 9.902131829405519e-22, - "timestamp": 1.0352157576330652 + "x": 0.541861263011899, + "y": 7.023858848535928, + "heading": 3.1415926535897727, + "angularVelocity": -3.1650291496821756e-13, + "velocityX": 0.5334864255761352, + "velocityY": -0.03435575299391881, + "timestamp": 0.0641140465194699 + }, + { + "x": 0.6102692085867726, + "y": 7.019453477975782, + "heading": 3.141592653589732, + "angularVelocity": -6.329727254211075e-13, + "velocityX": 1.0669728287092248, + "velocityY": -0.06871147274736708, + "timestamp": 0.1282280930389398 + }, + { + "x": 0.7128811238314352, + "y": 7.012845426753121, + "heading": 3.141592653589671, + "angularVelocity": -9.494390784713861e-13, + "velocityX": 1.60045919443724, + "velocityY": -0.10306713710003898, + "timestamp": 0.19234213955840973 + }, + { + "x": 0.849697003949506, + "y": 7.00403470197188, + "heading": 3.14159265358959, + "angularVelocity": -1.2659835533654102e-12, + "velocityX": 2.133945485355117, + "velocityY": -0.13742269065119764, + "timestamp": 0.2564561860778796 + }, + { + "x": 1.0207168345518518, + "y": 6.99302132494383, + "heading": 3.1415926535894885, + "angularVelocity": -1.5824052622322692e-12, + "velocityX": 2.6674315518427174, + "velocityY": -0.17177791179824162, + "timestamp": 0.3205702325973495 + }, + { + "x": 1.212726563552661, + "y": 6.992468327030024, + "heading": 3.1415926535894885, + "angularVelocity": 6.64805175241919e-17, + "velocityX": 2.9948153240101663, + "velocityY": -0.008625222456331237, + "timestamp": 0.3846842791168194 + }, + { + "x": 1.401775598526001, + "y": 7.026061534881592, + "heading": 3.141592653589793, + "angularVelocity": 4.750903641780761e-12, + "velocityX": 2.948636768947822, + "velocityY": 0.5239601877471003, + "timestamp": 0.4487983256362893 + }, + { + "x": 1.5370249272630554, + "y": 7.069072166745036, + "heading": 3.1415932284442536, + "angularVelocity": 0.000012130387654159235, + "velocityX": 2.8539863567604646, + "velocityY": 0.9075960500518688, + "timestamp": 0.49618794556796086 + }, + { + "x": 1.6654346925708037, + "y": 7.129514350441228, + "heading": 3.1415934952796376, + "angularVelocity": 0.000005630671531509569, + "velocityX": 2.709660163826911, + "velocityY": 1.2754308598241744, + "timestamp": 0.5435775654996324 + }, + { + "x": 1.7855925760230578, + "y": 7.205041921251342, + "heading": 3.1415937037577986, + "angularVelocity": 0.0000043992368250325695, + "velocityX": 2.535531697141759, + "velocityY": 1.5937576819356747, + "timestamp": 0.590967185431304 + }, + { + "x": 1.904036917668605, + "y": 7.272314642412527, + "heading": 3.156119426039146, + "angularVelocity": 0.30651696093556136, + "velocityX": 2.499373107788697, + "velocityY": 1.4195665898604233, + "timestamp": 0.6383568053629756 + }, + { + "x": 2.0095791949782846, + "y": 7.331339137337132, + "heading": 3.210199696093153, + "angularVelocity": 1.1411838738521793, + "velocityX": 2.227118036858182, + "velocityY": 1.245515262829886, + "timestamp": 0.6857464252946471 + }, + { + "x": 2.1018588032452454, + "y": 7.382733805051419, + "heading": 3.305786668196144, + "angularVelocity": 2.0170444971032095, + "velocityX": 1.9472536053256688, + "velocityY": 1.0845131863980089, + "timestamp": 0.7331360452263187 + }, + { + "x": 2.180515398647323, + "y": 7.427471687362473, + "heading": 3.445587312955586, + "angularVelocity": 2.950026713888249, + "velocityX": 1.6597853182930875, + "velocityY": 0.9440439145863256, + "timestamp": 0.7805256651579903 + }, + { + "x": 2.245294901768652, + "y": 7.466899566519577, + "heading": 3.631740569973656, + "angularVelocity": 3.928144122836896, + "velocityX": 1.3669555319230515, + "velocityY": 0.8319939939158001, + "timestamp": 0.8279152850896618 + }, + { + "x": 2.2924148053297366, + "y": 7.500143889930106, + "heading": 3.815249497964916, + "angularVelocity": 3.8723443711059815, + "velocityX": 0.994308534844213, + "velocityY": 0.7015106569426475, + "timestamp": 0.8753049050213334 + }, + { + "x": 2.324405179579958, + "y": 7.525744431799912, + "heading": 3.9691826181905445, + "angularVelocity": 3.2482455113076645, + "velocityX": 0.675050238772683, + "velocityY": 0.5402141208711574, + "timestamp": 0.922694524953005 + }, + { + "x": 2.343773552171984, + "y": 7.542843252672667, + "heading": 4.080963317633744, + "angularVelocity": 2.358759146082405, + "velocityX": 0.4087049573293044, + "velocityY": 0.3608136317068833, + "timestamp": 0.9700841448846765 + }, + { + "x": 2.352663278579712, + "y": 7.551178455352783, + "heading": 4.141051517496414, + "angularVelocity": 1.26796121068935, + "velocityX": 0.18758805030606973, + "velocityY": 0.17588667501732433, + "timestamp": 1.017473764816348 + }, + { + "x": 2.352663278579712, + "y": 7.551178455352783, + "heading": 4.141051517496414, + "angularVelocity": -1.2571692895524854e-27, + "velocityX": -1.885622451182025e-26, + "velocityY": 8.829962183989965e-27, + "timestamp": 1.0648633847480196 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Amp 2 Piece 1.traj b/src/main/deploy/choreo/Amp 2 Piece 1.traj index 8a03c8b..2c3d605 100644 --- a/src/main/deploy/choreo/Amp 2 Piece 1.traj +++ b/src/main/deploy/choreo/Amp 2 Piece 1.traj @@ -1,139 +1,193 @@ { "samples": [ { - "x": 0.9050432443618774, - "y": 6.6428680419921875, - "heading": -2.2565255466280507, - "angularVelocity": -1.4291101663678553e-21, - "velocityX": 4.063581905170547e-21, - "velocityY": 7.609678430321777e-22, + "x": 0.5076572895050049, + "y": 7.026061534881592, + "heading": 3.141592653589793, + "angularVelocity": 5.576382446952711e-28, + "velocityX": -1.7262779855569846e-28, + "velocityY": -3.130496405792362e-27, "timestamp": 0 }, { - "x": 0.9488587964840457, - "y": 6.6487212093785475, - "heading": -2.2170365190388273, - "angularVelocity": 0.5340397711035277, - "velocityX": 0.5925506110077822, - "velocityY": 0.0791567775169907, - "timestamp": 0.07394398268807607 - }, - { - "x": 1.036787588456291, - "y": 6.660437165626341, - "heading": -2.1423875869378732, - "angularVelocity": 1.00953356023372, - "velocityX": 1.1891270766958724, - "velocityY": 0.15844367346548532, - "timestamp": 0.14788796537615215 - }, - { - "x": 1.1692835732429303, - "y": 6.678111777529781, - "heading": -2.0408936155248174, - "angularVelocity": 1.3725791839100139, - "velocityX": 1.791842689132625, - "velocityY": 0.23902704805829636, - "timestamp": 0.22183194806422823 - }, - { - "x": 1.346933296813628, - "y": 6.701897508529881, - "heading": -1.9340009000220366, - "angularVelocity": 1.4455904539750712, - "velocityX": 2.40249060320367, - "velocityY": 0.32167230024149257, - "timestamp": 0.2957759307523043 - }, - { - "x": 1.5625585723226694, - "y": 6.726906508694774, - "heading": -1.9227308281996909, - "angularVelocity": 0.15241364358053452, - "velocityX": 2.916062506650325, - "velocityY": 0.33821548765929105, - "timestamp": 0.36971991344038035 - }, - { - "x": 1.7820773559348524, - "y": 6.756085107088855, - "heading": -1.9227308124237847, - "angularVelocity": 2.1334942281526128e-7, - "velocityX": 2.9687173402436384, - "velocityY": 0.39460409534053714, - "timestamp": 0.4436638961284564 - }, - { - "x": 2.0015961382526735, - "y": 6.785263715245929, - "heading": -1.9227307966478815, - "angularVelocity": 2.1334938403089738e-7, - "velocityX": 2.9687173226950803, - "velocityY": 0.39460422736336004, - "timestamp": 0.5176078788165325 - }, - { - "x": 2.221114921646043, - "y": 6.8144423152746825, - "heading": -1.9227307808719754, - "angularVelocity": 2.1334942614926208e-7, - "velocityX": 2.968717337305404, - "velocityY": 0.39460411744569185, - "timestamp": 0.5915518615046086 - }, - { - "x": 2.436370432370262, - "y": 6.840013832318715, - "heading": -1.910648519871317, - "angularVelocity": 0.1633974876850485, - "velocityX": 2.911061899824357, - "velocityY": 0.3458228257933233, - "timestamp": 0.6654958441926847 - }, - { - "x": 2.613907826780238, - "y": 6.863558295707786, - "heading": -1.7989968132166365, - "angularVelocity": 1.5099498646924372, - "velocityX": 2.4009714915986042, - "velocityY": 0.31840945717834496, - "timestamp": 0.7394398268807608 - }, - { - "x": 2.746285116177599, - "y": 6.881046947319712, - "heading": -1.6928599971823888, - "angularVelocity": 1.435367857882004, - "velocityX": 1.7902374822825093, - "velocityY": 0.23651216740432432, - "timestamp": 0.8133838095688369 - }, - { - "x": 2.834070756139549, - "y": 6.892600291272677, - "heading": -1.6134370485289746, - "angularVelocity": 1.074096170724946, - "velocityX": 1.1871911245679945, - "velocityY": 0.15624454530041104, - "timestamp": 0.887327792256913 - }, - { - "x": 2.8777801990509033, - "y": 6.8983306884765625, - "heading": -1.5707963267948966, - "angularVelocity": 0.5766624975280655, - "velocityX": 0.5911156164696019, - "velocityY": 0.07749646415515937, - "timestamp": 0.9612717749449892 - }, - { - "x": 2.8777801990509033, - "y": 6.8983306884765625, - "heading": -1.5707963267948966, - "angularVelocity": -1.3704110825531436e-21, - "velocityX": 4.248115223834914e-21, - "velocityY": 9.902131829405519e-22, - "timestamp": 1.0352157576330652 + "x": 0.541861263011899, + "y": 7.023858848535928, + "heading": 3.1415926535897727, + "angularVelocity": -3.1650291496821756e-13, + "velocityX": 0.5334864255761352, + "velocityY": -0.03435575299391881, + "timestamp": 0.0641140465194699 + }, + { + "x": 0.6102692085867726, + "y": 7.019453477975782, + "heading": 3.141592653589732, + "angularVelocity": -6.329727254211075e-13, + "velocityX": 1.0669728287092248, + "velocityY": -0.06871147274736708, + "timestamp": 0.1282280930389398 + }, + { + "x": 0.7128811238314352, + "y": 7.012845426753121, + "heading": 3.141592653589671, + "angularVelocity": -9.494390784713861e-13, + "velocityX": 1.60045919443724, + "velocityY": -0.10306713710003898, + "timestamp": 0.19234213955840973 + }, + { + "x": 0.849697003949506, + "y": 7.00403470197188, + "heading": 3.14159265358959, + "angularVelocity": -1.2659835533654102e-12, + "velocityX": 2.133945485355117, + "velocityY": -0.13742269065119764, + "timestamp": 0.2564561860778796 + }, + { + "x": 1.0207168345518518, + "y": 6.99302132494383, + "heading": 3.1415926535894885, + "angularVelocity": -1.5824052622322692e-12, + "velocityX": 2.6674315518427174, + "velocityY": -0.17177791179824162, + "timestamp": 0.3205702325973495 + }, + { + "x": 1.212726563552661, + "y": 6.992468327030024, + "heading": 3.1415926535894885, + "angularVelocity": 6.64805175241919e-17, + "velocityX": 2.9948153240101663, + "velocityY": -0.008625222456331237, + "timestamp": 0.3846842791168194 + }, + { + "x": 1.401775598526001, + "y": 7.026061534881592, + "heading": 3.141592653589793, + "angularVelocity": 4.750903641780761e-12, + "velocityX": 2.948636768947822, + "velocityY": 0.5239601877471003, + "timestamp": 0.4487983256362893 + }, + { + "x": 1.5370249272630554, + "y": 7.069072166745036, + "heading": 3.1415932284442536, + "angularVelocity": 0.000012130387654159235, + "velocityX": 2.8539863567604646, + "velocityY": 0.9075960500518688, + "timestamp": 0.49618794556796086 + }, + { + "x": 1.6654346925708037, + "y": 7.129514350441228, + "heading": 3.1415934952796376, + "angularVelocity": 0.000005630671531509569, + "velocityX": 2.709660163826911, + "velocityY": 1.2754308598241744, + "timestamp": 0.5435775654996324 + }, + { + "x": 1.7855925760230578, + "y": 7.205041921251342, + "heading": 3.1415937037577986, + "angularVelocity": 0.0000043992368250325695, + "velocityX": 2.535531697141759, + "velocityY": 1.5937576819356747, + "timestamp": 0.590967185431304 + }, + { + "x": 1.904036917668605, + "y": 7.272314642412527, + "heading": 3.156119426039146, + "angularVelocity": 0.30651696093556136, + "velocityX": 2.499373107788697, + "velocityY": 1.4195665898604233, + "timestamp": 0.6383568053629756 + }, + { + "x": 2.0095791949782846, + "y": 7.331339137337132, + "heading": 3.210199696093153, + "angularVelocity": 1.1411838738521793, + "velocityX": 2.227118036858182, + "velocityY": 1.245515262829886, + "timestamp": 0.6857464252946471 + }, + { + "x": 2.1018588032452454, + "y": 7.382733805051419, + "heading": 3.305786668196144, + "angularVelocity": 2.0170444971032095, + "velocityX": 1.9472536053256688, + "velocityY": 1.0845131863980089, + "timestamp": 0.7331360452263187 + }, + { + "x": 2.180515398647323, + "y": 7.427471687362473, + "heading": 3.445587312955586, + "angularVelocity": 2.950026713888249, + "velocityX": 1.6597853182930875, + "velocityY": 0.9440439145863256, + "timestamp": 0.7805256651579903 + }, + { + "x": 2.245294901768652, + "y": 7.466899566519577, + "heading": 3.631740569973656, + "angularVelocity": 3.928144122836896, + "velocityX": 1.3669555319230515, + "velocityY": 0.8319939939158001, + "timestamp": 0.8279152850896618 + }, + { + "x": 2.2924148053297366, + "y": 7.500143889930106, + "heading": 3.815249497964916, + "angularVelocity": 3.8723443711059815, + "velocityX": 0.994308534844213, + "velocityY": 0.7015106569426475, + "timestamp": 0.8753049050213334 + }, + { + "x": 2.324405179579958, + "y": 7.525744431799912, + "heading": 3.9691826181905445, + "angularVelocity": 3.2482455113076645, + "velocityX": 0.675050238772683, + "velocityY": 0.5402141208711574, + "timestamp": 0.922694524953005 + }, + { + "x": 2.343773552171984, + "y": 7.542843252672667, + "heading": 4.080963317633744, + "angularVelocity": 2.358759146082405, + "velocityX": 0.4087049573293044, + "velocityY": 0.3608136317068833, + "timestamp": 0.9700841448846765 + }, + { + "x": 2.352663278579712, + "y": 7.551178455352783, + "heading": 4.141051517496414, + "angularVelocity": 1.26796121068935, + "velocityX": 0.18758805030606973, + "velocityY": 0.17588667501732433, + "timestamp": 1.017473764816348 + }, + { + "x": 2.352663278579712, + "y": 7.551178455352783, + "heading": 4.141051517496414, + "angularVelocity": -1.2571692895524854e-27, + "velocityX": -1.885622451182025e-26, + "velocityY": 8.829962183989965e-27, + "timestamp": 1.0648633847480196 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Amp 2 Piece 2.1.traj b/src/main/deploy/choreo/Amp 2 Piece 2.1.traj index 51db90e..dc99d2e 100644 --- a/src/main/deploy/choreo/Amp 2 Piece 2.1.traj +++ b/src/main/deploy/choreo/Amp 2 Piece 2.1.traj @@ -1,112 +1,328 @@ { "samples": [ { - "x": 3.038259506225586, - "y": 7.043644428253174, - "heading": -1.571, - "angularVelocity": 3.377508782026209e-23, - "velocityX": -5.278336527960705e-22, - "velocityY": -2.3473915338301426e-22, + "x": 2.2391245365142822, + "y": 7.536986351013184, + "heading": -2.3884414433002457, + "angularVelocity": 5.721431383504562e-26, + "velocityX": -5.410945273391021e-25, + "velocityY": -1.0088103825907385e-24, "timestamp": 0 }, { - "x": 3.041722845390556, - "y": 7.021424756435917, - "heading": -1.6118736663006001, - "angularVelocity": -0.7477383471577114, - "velocityX": 0.063357945035209, - "velocityY": -0.40648422768911846, - "timestamp": 0.054663060221491123 - }, - { - "x": 3.0486861748029925, - "y": 6.976611694502479, - "heading": -1.689825743738422, - "angularVelocity": -1.4260467145813887, - "velocityX": 0.12738638093480234, - "velocityY": -0.819805216756219, - "timestamp": 0.10932612044298225 - }, - { - "x": 3.059353554396992, - "y": 6.908666049138136, - "heading": -1.7987376751330109, - "angularVelocity": -1.992422871190979, - "velocityX": 0.19514786678198967, - "velocityY": -1.2429901489055193, - "timestamp": 0.16398918066447338 - }, - { - "x": 3.074080389772487, - "y": 6.816859673519979, - "heading": -1.928776333024462, - "angularVelocity": -2.3789128776278363, - "velocityX": 0.26941110350980785, - "velocityY": -1.679495718793705, - "timestamp": 0.2186522408859645 - }, - { - "x": 3.092722859572534, - "y": 6.700859976513945, - "heading": -2.0428573248611555, - "angularVelocity": -2.0869850933051466, - "velocityX": 0.34104328818234536, - "velocityY": -2.122085674238002, - "timestamp": 0.2733153011074556 - }, - { - "x": 3.1151177074959255, - "y": 6.585113556157874, - "heading": -2.1549840120443235, - "angularVelocity": -2.051233259331581, - "velocityX": 0.40968888007091114, - "velocityY": -2.117452259113795, - "timestamp": 0.32797836132894675 - }, - { - "x": 3.1333817091003047, - "y": 6.49366638357125, - "heading": -2.2800565188815622, - "angularVelocity": -2.2880626574958156, - "velocityX": 0.3341196327167695, - "velocityY": -1.6729244981178732, - "timestamp": 0.3826414215504379 - }, - { - "x": 3.147356671497851, - "y": 6.425961612128095, - "heading": -2.3829782913622974, - "angularVelocity": -1.8828395641170315, - "velocityX": 0.25565642210518025, - "velocityY": -1.2385836279348303, - "timestamp": 0.43730448177192904 - }, - { - "x": 3.1568316606137117, - "y": 6.3812429097672245, - "heading": -2.455320963886982, - "angularVelocity": -1.3234288792386915, - "velocityX": 0.17333440677248738, - "velocityY": -0.8180790131337755, - "timestamp": 0.4919675419934202 - }, - { - "x": 3.1616272926330566, - "y": 6.359021186828613, - "heading": -2.4925966381653493, - "angularVelocity": -0.6819170775900275, - "velocityX": 0.08773076369879507, - "velocityY": -0.4065217506771484, - "timestamp": 0.5466306022149113 - }, - { - "x": 3.1616272926330566, - "y": 6.359021186828613, - "heading": -2.4925966381653493, - "angularVelocity": 8.723021914700767e-23, - "velocityX": 5.757885441164143e-22, - "velocityY": -3.00776508275509e-23, - "timestamp": 0.6012936624364025 + "x": 2.2296641876726997, + "y": 7.53164553860975, + "heading": -2.33575280113073, + "angularVelocity": 1.178711973934036, + "velocityX": -0.2116400422939337, + "velocityY": -0.11948076988223226, + "timestamp": 0.044700184043828084 + }, + { + "x": 2.2091363580044265, + "y": 7.5205895962864, + "heading": -2.2405309329659358, + "angularVelocity": 2.130234364839089, + "velocityX": -0.4592336722404892, + "velocityY": -0.2473354989435561, + "timestamp": 0.08940036808765617 + }, + { + "x": 2.1742459343842966, + "y": 7.50751833119898, + "heading": -2.1168186310398647, + "angularVelocity": 2.7676016234020957, + "velocityX": -0.7805431759726248, + "velocityY": -0.2924208337622006, + "timestamp": 0.13410055213148425 + }, + { + "x": 2.1246336868250664, + "y": 7.492684660677987, + "heading": -1.9656445414880221, + "angularVelocity": 3.3819567589166444, + "velocityX": -1.109889111655235, + "velocityY": -0.33184808604924815, + "timestamp": 0.17880073617531234 + }, + { + "x": 2.058976778618434, + "y": 7.475737631665651, + "heading": -1.800162824867856, + "angularVelocity": 3.7020365835163696, + "velocityX": -1.4688285878701581, + "velocityY": -0.3791266048416977, + "timestamp": 0.22350092021914042 + }, + { + "x": 1.9785332458618086, + "y": 7.4586433187632375, + "heading": -1.664425028068561, + "angularVelocity": 3.0366272466843864, + "velocityX": -1.7996241956800805, + "velocityY": -0.38242153288794345, + "timestamp": 0.2682011042629685 + }, + { + "x": 1.8835626315507088, + "y": 7.440435729064373, + "heading": -1.560310319289602, + "angularVelocity": 2.3291785259066433, + "velocityX": -2.1246134964004204, + "velocityY": -0.40732695151796544, + "timestamp": 0.3129012883067966 + }, + { + "x": 1.7746640682844126, + "y": 7.420003143199531, + "heading": -1.4914280558490194, + "angularVelocity": 1.5409838888592533, + "velocityX": -2.4361994384524714, + "velocityY": -0.45710294715583155, + "timestamp": 0.35760147235062467 + }, + { + "x": 1.652640817011798, + "y": 7.396221119364523, + "heading": -1.4615580491684752, + "angularVelocity": 0.6682300603339137, + "velocityX": -2.7298154108934316, + "velocityY": -0.5320341368547818, + "timestamp": 0.40230165639445276 + }, + { + "x": 1.5233049536164358, + "y": 7.361678018164252, + "heading": -1.46155788621968, + "angularVelocity": 0.00000364537190716708, + "velocityX": -2.8934078496981073, + "velocityY": -0.7727731314573986, + "timestamp": 0.44700184043828084 + }, + { + "x": 1.4065653245496021, + "y": 7.328928356244567, + "heading": -1.4235263433901186, + "angularVelocity": 0.8508140098991993, + "velocityX": -2.6116140585097294, + "velocityY": -0.732651612520761, + "timestamp": 0.4917020244821089 + }, + { + "x": 1.3032250484271148, + "y": 7.296949732733987, + "heading": -1.3457880678727918, + "angularVelocity": 1.7391041486787921, + "velocityX": -2.3118534818819345, + "velocityY": -0.7154025021289736, + "timestamp": 0.536402208525937 + }, + { + "x": 1.213100117494602, + "y": 7.264849658847639, + "heading": -1.2286347478417774, + "angularVelocity": 2.6208688518182983, + "velocityX": -2.016209392877359, + "velocityY": -0.7181195015858151, + "timestamp": 0.5811023925697651 + }, + { + "x": 1.1372460206771293, + "y": 7.230088830221739, + "heading": -1.0829990745361808, + "angularVelocity": 3.258055339611178, + "velocityX": -1.6969526734632359, + "velocityY": -0.777643971036398, + "timestamp": 0.6258025766135933 + }, + { + "x": 1.0760806116019437, + "y": 7.195783758543388, + "heading": -0.9097385032115863, + "angularVelocity": 3.876059462187322, + "velocityX": -1.3683480366705796, + "velocityY": -0.7674481081490622, + "timestamp": 0.6705027606574214 + }, + { + "x": 1.0298697641313916, + "y": 7.162338001153043, + "heading": -0.711153238624929, + "angularVelocity": 4.442605077239651, + "velocityX": -1.033795463241102, + "velocityY": -0.7482241540113431, + "timestamp": 0.7152029447012496 + }, + { + "x": 1.0001525071013124, + "y": 7.131235715839601, + "heading": -0.5118465164104274, + "angularVelocity": 4.458745002460914, + "velocityX": -0.6648128562723964, + "velocityY": -0.6957977014803056, + "timestamp": 0.7599031287450777 + }, + { + "x": 0.9864723677403102, + "y": 7.102929584116405, + "heading": -0.32522300497964796, + "angularVelocity": 4.175005437288511, + "velocityX": -0.3060421260813816, + "velocityY": -0.6332441874387541, + "timestamp": 0.8046033127889058 + }, + { + "x": 0.9881450214838234, + "y": 7.077378427353441, + "heading": -0.16014265091379973, + "angularVelocity": 3.6930575924248683, + "velocityX": 0.037419392767447934, + "velocityY": -0.5716118917522006, + "timestamp": 0.849303496832734 + }, + { + "x": 1.0043896436691284, + "y": 7.054446220397949, + "heading": -0.024384979707381408, + "angularVelocity": 3.037071862462787, + "velocityX": 0.3634128702776077, + "velocityY": -0.5130226518308526, + "timestamp": 0.8940036808765621 + }, + { + "x": 1.0505988037906369, + "y": 7.028624623125705, + "heading": 0.09377281742019881, + "angularVelocity": 1.9835385185104277, + "velocityX": 0.7757223918964998, + "velocityY": -0.43347230605238574, + "timestamp": 0.9535728780587337 + }, + { + "x": 1.1215135025956555, + "y": 7.007962378546688, + "heading": 0.15072199016101734, + "angularVelocity": 0.9560171268828643, + "velocityX": 1.1904591997127438, + "velocityY": -0.3468612228536146, + "timestamp": 1.0131420752409053 + }, + { + "x": 1.217577660739459, + "y": 6.992737876712583, + "heading": 0.1503344728353621, + "angularVelocity": -0.006505330674008277, + "velocityX": 1.612648192152475, + "velocityY": -0.25557675030511917, + "timestamp": 1.072711272423077 + }, + { + "x": 1.3396267843628167, + "y": 6.9833714114319, + "heading": 0.10047084389942805, + "angularVelocity": -0.8370706891255146, + "velocityX": 2.0488629929007285, + "velocityY": -0.15723672172447928, + "timestamp": 1.1322804696052486 + }, + { + "x": 1.4890457715649965, + "y": 6.981233099614705, + "heading": 0.01898626882624744, + "angularVelocity": -1.3678978218220477, + "velocityX": 2.5083263543947716, + "velocityY": -0.03589626717070096, + "timestamp": 1.1918496667874203 + }, + { + "x": 1.6615559253103864, + "y": 6.987743789587346, + "heading": 1.6896592561479554e-7, + "angularVelocity": -0.3187234469898841, + "velocityX": 2.8959623749473673, + "velocityY": 0.10929625176465058, + "timestamp": 1.251418863969592 + }, + { + "x": 1.836472158801685, + "y": 7.022824858897805, + "heading": 8.525108932477818e-8, + "angularVelocity": -0.0000014053376619128292, + "velocityX": 2.936353715769886, + "velocityY": 0.5889129108652654, + "timestamp": 1.3109880611517637 + }, + { + "x": 2.012046813964844, + "y": 7.054446220397949, + "heading": 2.2019024313995553e-25, + "angularVelocity": -0.0000014311270481632891, + "velocityX": 2.9474067717620085, + "velocityY": 0.5308341054763681, + "timestamp": 1.3705572583339354 + }, + { + "x": 2.2280675873510725, + "y": 7.049473002137666, + "heading": -7.926750089678431e-16, + "angularVelocity": -1.0986426575996211e-14, + "velocityX": 2.9940345515976707, + "velocityY": -0.06892849734087551, + "timestamp": 1.4427076526141067 + }, + { + "x": 2.444005456892987, + "y": 7.041692003021008, + "heading": -8.024831824871519e-16, + "angularVelocity": -1.359406780437382e-16, + "velocityX": 2.9928855094456446, + "velocityY": -0.10784416626252269, + "timestamp": 1.514858046894278 + }, + { + "x": 2.6175154366538695, + "y": 7.035439816214791, + "heading": -4.813899359992426e-16, + "angularVelocity": 4.45033252681572e-15, + "velocityX": 2.404837582551739, + "velocityY": -0.08665492224391362, + "timestamp": 1.5870084411744494 + }, + { + "x": 2.7476479338367086, + "y": 7.030750675664456, + "heading": -2.405669787611813e-16, + "angularVelocity": 3.3377912850014936e-15, + "velocityX": 1.8036283582528165, + "velocityY": -0.06499119785994299, + "timestamp": 1.6591588354546207 + }, + { + "x": 2.8344029355418954, + "y": 7.02762458183505, + "heading": -8.002384171896392e-17, + "angularVelocity": 2.2251179448655455e-15, + "velocityX": 1.2024189551661102, + "velocityY": -0.043327467030426094, + "timestamp": 1.731309229734792 + }, + { + "x": 2.8777804374694824, + "y": 7.026061534881592, + "heading": -5.744138405308966e-28, + "angularVelocity": 1.1091254942746495e-15, + "velocityX": 0.6012094924824047, + "velocityY": -0.021663734052355155, + "timestamp": 1.8034596240149634 + }, + { + "x": 2.8777804374694824, + "y": 7.026061534881592, + "heading": -2.835172507845664e-28, + "angularVelocity": 1.0227718137074573e-28, + "velocityX": 3.2904914962294794e-27, + "velocityY": 1.9801334803171188e-26, + "timestamp": 1.8756100182951347 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Amp 2 Piece 2.traj b/src/main/deploy/choreo/Amp 2 Piece 2.traj index 51db90e..dc99d2e 100644 --- a/src/main/deploy/choreo/Amp 2 Piece 2.traj +++ b/src/main/deploy/choreo/Amp 2 Piece 2.traj @@ -1,112 +1,328 @@ { "samples": [ { - "x": 3.038259506225586, - "y": 7.043644428253174, - "heading": -1.571, - "angularVelocity": 3.377508782026209e-23, - "velocityX": -5.278336527960705e-22, - "velocityY": -2.3473915338301426e-22, + "x": 2.2391245365142822, + "y": 7.536986351013184, + "heading": -2.3884414433002457, + "angularVelocity": 5.721431383504562e-26, + "velocityX": -5.410945273391021e-25, + "velocityY": -1.0088103825907385e-24, "timestamp": 0 }, { - "x": 3.041722845390556, - "y": 7.021424756435917, - "heading": -1.6118736663006001, - "angularVelocity": -0.7477383471577114, - "velocityX": 0.063357945035209, - "velocityY": -0.40648422768911846, - "timestamp": 0.054663060221491123 - }, - { - "x": 3.0486861748029925, - "y": 6.976611694502479, - "heading": -1.689825743738422, - "angularVelocity": -1.4260467145813887, - "velocityX": 0.12738638093480234, - "velocityY": -0.819805216756219, - "timestamp": 0.10932612044298225 - }, - { - "x": 3.059353554396992, - "y": 6.908666049138136, - "heading": -1.7987376751330109, - "angularVelocity": -1.992422871190979, - "velocityX": 0.19514786678198967, - "velocityY": -1.2429901489055193, - "timestamp": 0.16398918066447338 - }, - { - "x": 3.074080389772487, - "y": 6.816859673519979, - "heading": -1.928776333024462, - "angularVelocity": -2.3789128776278363, - "velocityX": 0.26941110350980785, - "velocityY": -1.679495718793705, - "timestamp": 0.2186522408859645 - }, - { - "x": 3.092722859572534, - "y": 6.700859976513945, - "heading": -2.0428573248611555, - "angularVelocity": -2.0869850933051466, - "velocityX": 0.34104328818234536, - "velocityY": -2.122085674238002, - "timestamp": 0.2733153011074556 - }, - { - "x": 3.1151177074959255, - "y": 6.585113556157874, - "heading": -2.1549840120443235, - "angularVelocity": -2.051233259331581, - "velocityX": 0.40968888007091114, - "velocityY": -2.117452259113795, - "timestamp": 0.32797836132894675 - }, - { - "x": 3.1333817091003047, - "y": 6.49366638357125, - "heading": -2.2800565188815622, - "angularVelocity": -2.2880626574958156, - "velocityX": 0.3341196327167695, - "velocityY": -1.6729244981178732, - "timestamp": 0.3826414215504379 - }, - { - "x": 3.147356671497851, - "y": 6.425961612128095, - "heading": -2.3829782913622974, - "angularVelocity": -1.8828395641170315, - "velocityX": 0.25565642210518025, - "velocityY": -1.2385836279348303, - "timestamp": 0.43730448177192904 - }, - { - "x": 3.1568316606137117, - "y": 6.3812429097672245, - "heading": -2.455320963886982, - "angularVelocity": -1.3234288792386915, - "velocityX": 0.17333440677248738, - "velocityY": -0.8180790131337755, - "timestamp": 0.4919675419934202 - }, - { - "x": 3.1616272926330566, - "y": 6.359021186828613, - "heading": -2.4925966381653493, - "angularVelocity": -0.6819170775900275, - "velocityX": 0.08773076369879507, - "velocityY": -0.4065217506771484, - "timestamp": 0.5466306022149113 - }, - { - "x": 3.1616272926330566, - "y": 6.359021186828613, - "heading": -2.4925966381653493, - "angularVelocity": 8.723021914700767e-23, - "velocityX": 5.757885441164143e-22, - "velocityY": -3.00776508275509e-23, - "timestamp": 0.6012936624364025 + "x": 2.2296641876726997, + "y": 7.53164553860975, + "heading": -2.33575280113073, + "angularVelocity": 1.178711973934036, + "velocityX": -0.2116400422939337, + "velocityY": -0.11948076988223226, + "timestamp": 0.044700184043828084 + }, + { + "x": 2.2091363580044265, + "y": 7.5205895962864, + "heading": -2.2405309329659358, + "angularVelocity": 2.130234364839089, + "velocityX": -0.4592336722404892, + "velocityY": -0.2473354989435561, + "timestamp": 0.08940036808765617 + }, + { + "x": 2.1742459343842966, + "y": 7.50751833119898, + "heading": -2.1168186310398647, + "angularVelocity": 2.7676016234020957, + "velocityX": -0.7805431759726248, + "velocityY": -0.2924208337622006, + "timestamp": 0.13410055213148425 + }, + { + "x": 2.1246336868250664, + "y": 7.492684660677987, + "heading": -1.9656445414880221, + "angularVelocity": 3.3819567589166444, + "velocityX": -1.109889111655235, + "velocityY": -0.33184808604924815, + "timestamp": 0.17880073617531234 + }, + { + "x": 2.058976778618434, + "y": 7.475737631665651, + "heading": -1.800162824867856, + "angularVelocity": 3.7020365835163696, + "velocityX": -1.4688285878701581, + "velocityY": -0.3791266048416977, + "timestamp": 0.22350092021914042 + }, + { + "x": 1.9785332458618086, + "y": 7.4586433187632375, + "heading": -1.664425028068561, + "angularVelocity": 3.0366272466843864, + "velocityX": -1.7996241956800805, + "velocityY": -0.38242153288794345, + "timestamp": 0.2682011042629685 + }, + { + "x": 1.8835626315507088, + "y": 7.440435729064373, + "heading": -1.560310319289602, + "angularVelocity": 2.3291785259066433, + "velocityX": -2.1246134964004204, + "velocityY": -0.40732695151796544, + "timestamp": 0.3129012883067966 + }, + { + "x": 1.7746640682844126, + "y": 7.420003143199531, + "heading": -1.4914280558490194, + "angularVelocity": 1.5409838888592533, + "velocityX": -2.4361994384524714, + "velocityY": -0.45710294715583155, + "timestamp": 0.35760147235062467 + }, + { + "x": 1.652640817011798, + "y": 7.396221119364523, + "heading": -1.4615580491684752, + "angularVelocity": 0.6682300603339137, + "velocityX": -2.7298154108934316, + "velocityY": -0.5320341368547818, + "timestamp": 0.40230165639445276 + }, + { + "x": 1.5233049536164358, + "y": 7.361678018164252, + "heading": -1.46155788621968, + "angularVelocity": 0.00000364537190716708, + "velocityX": -2.8934078496981073, + "velocityY": -0.7727731314573986, + "timestamp": 0.44700184043828084 + }, + { + "x": 1.4065653245496021, + "y": 7.328928356244567, + "heading": -1.4235263433901186, + "angularVelocity": 0.8508140098991993, + "velocityX": -2.6116140585097294, + "velocityY": -0.732651612520761, + "timestamp": 0.4917020244821089 + }, + { + "x": 1.3032250484271148, + "y": 7.296949732733987, + "heading": -1.3457880678727918, + "angularVelocity": 1.7391041486787921, + "velocityX": -2.3118534818819345, + "velocityY": -0.7154025021289736, + "timestamp": 0.536402208525937 + }, + { + "x": 1.213100117494602, + "y": 7.264849658847639, + "heading": -1.2286347478417774, + "angularVelocity": 2.6208688518182983, + "velocityX": -2.016209392877359, + "velocityY": -0.7181195015858151, + "timestamp": 0.5811023925697651 + }, + { + "x": 1.1372460206771293, + "y": 7.230088830221739, + "heading": -1.0829990745361808, + "angularVelocity": 3.258055339611178, + "velocityX": -1.6969526734632359, + "velocityY": -0.777643971036398, + "timestamp": 0.6258025766135933 + }, + { + "x": 1.0760806116019437, + "y": 7.195783758543388, + "heading": -0.9097385032115863, + "angularVelocity": 3.876059462187322, + "velocityX": -1.3683480366705796, + "velocityY": -0.7674481081490622, + "timestamp": 0.6705027606574214 + }, + { + "x": 1.0298697641313916, + "y": 7.162338001153043, + "heading": -0.711153238624929, + "angularVelocity": 4.442605077239651, + "velocityX": -1.033795463241102, + "velocityY": -0.7482241540113431, + "timestamp": 0.7152029447012496 + }, + { + "x": 1.0001525071013124, + "y": 7.131235715839601, + "heading": -0.5118465164104274, + "angularVelocity": 4.458745002460914, + "velocityX": -0.6648128562723964, + "velocityY": -0.6957977014803056, + "timestamp": 0.7599031287450777 + }, + { + "x": 0.9864723677403102, + "y": 7.102929584116405, + "heading": -0.32522300497964796, + "angularVelocity": 4.175005437288511, + "velocityX": -0.3060421260813816, + "velocityY": -0.6332441874387541, + "timestamp": 0.8046033127889058 + }, + { + "x": 0.9881450214838234, + "y": 7.077378427353441, + "heading": -0.16014265091379973, + "angularVelocity": 3.6930575924248683, + "velocityX": 0.037419392767447934, + "velocityY": -0.5716118917522006, + "timestamp": 0.849303496832734 + }, + { + "x": 1.0043896436691284, + "y": 7.054446220397949, + "heading": -0.024384979707381408, + "angularVelocity": 3.037071862462787, + "velocityX": 0.3634128702776077, + "velocityY": -0.5130226518308526, + "timestamp": 0.8940036808765621 + }, + { + "x": 1.0505988037906369, + "y": 7.028624623125705, + "heading": 0.09377281742019881, + "angularVelocity": 1.9835385185104277, + "velocityX": 0.7757223918964998, + "velocityY": -0.43347230605238574, + "timestamp": 0.9535728780587337 + }, + { + "x": 1.1215135025956555, + "y": 7.007962378546688, + "heading": 0.15072199016101734, + "angularVelocity": 0.9560171268828643, + "velocityX": 1.1904591997127438, + "velocityY": -0.3468612228536146, + "timestamp": 1.0131420752409053 + }, + { + "x": 1.217577660739459, + "y": 6.992737876712583, + "heading": 0.1503344728353621, + "angularVelocity": -0.006505330674008277, + "velocityX": 1.612648192152475, + "velocityY": -0.25557675030511917, + "timestamp": 1.072711272423077 + }, + { + "x": 1.3396267843628167, + "y": 6.9833714114319, + "heading": 0.10047084389942805, + "angularVelocity": -0.8370706891255146, + "velocityX": 2.0488629929007285, + "velocityY": -0.15723672172447928, + "timestamp": 1.1322804696052486 + }, + { + "x": 1.4890457715649965, + "y": 6.981233099614705, + "heading": 0.01898626882624744, + "angularVelocity": -1.3678978218220477, + "velocityX": 2.5083263543947716, + "velocityY": -0.03589626717070096, + "timestamp": 1.1918496667874203 + }, + { + "x": 1.6615559253103864, + "y": 6.987743789587346, + "heading": 1.6896592561479554e-7, + "angularVelocity": -0.3187234469898841, + "velocityX": 2.8959623749473673, + "velocityY": 0.10929625176465058, + "timestamp": 1.251418863969592 + }, + { + "x": 1.836472158801685, + "y": 7.022824858897805, + "heading": 8.525108932477818e-8, + "angularVelocity": -0.0000014053376619128292, + "velocityX": 2.936353715769886, + "velocityY": 0.5889129108652654, + "timestamp": 1.3109880611517637 + }, + { + "x": 2.012046813964844, + "y": 7.054446220397949, + "heading": 2.2019024313995553e-25, + "angularVelocity": -0.0000014311270481632891, + "velocityX": 2.9474067717620085, + "velocityY": 0.5308341054763681, + "timestamp": 1.3705572583339354 + }, + { + "x": 2.2280675873510725, + "y": 7.049473002137666, + "heading": -7.926750089678431e-16, + "angularVelocity": -1.0986426575996211e-14, + "velocityX": 2.9940345515976707, + "velocityY": -0.06892849734087551, + "timestamp": 1.4427076526141067 + }, + { + "x": 2.444005456892987, + "y": 7.041692003021008, + "heading": -8.024831824871519e-16, + "angularVelocity": -1.359406780437382e-16, + "velocityX": 2.9928855094456446, + "velocityY": -0.10784416626252269, + "timestamp": 1.514858046894278 + }, + { + "x": 2.6175154366538695, + "y": 7.035439816214791, + "heading": -4.813899359992426e-16, + "angularVelocity": 4.45033252681572e-15, + "velocityX": 2.404837582551739, + "velocityY": -0.08665492224391362, + "timestamp": 1.5870084411744494 + }, + { + "x": 2.7476479338367086, + "y": 7.030750675664456, + "heading": -2.405669787611813e-16, + "angularVelocity": 3.3377912850014936e-15, + "velocityX": 1.8036283582528165, + "velocityY": -0.06499119785994299, + "timestamp": 1.6591588354546207 + }, + { + "x": 2.8344029355418954, + "y": 7.02762458183505, + "heading": -8.002384171896392e-17, + "angularVelocity": 2.2251179448655455e-15, + "velocityX": 1.2024189551661102, + "velocityY": -0.043327467030426094, + "timestamp": 1.731309229734792 + }, + { + "x": 2.8777804374694824, + "y": 7.026061534881592, + "heading": -5.744138405308966e-28, + "angularVelocity": 1.1091254942746495e-15, + "velocityX": 0.6012094924824047, + "velocityY": -0.021663734052355155, + "timestamp": 1.8034596240149634 + }, + { + "x": 2.8777804374694824, + "y": 7.026061534881592, + "heading": -2.835172507845664e-28, + "angularVelocity": 1.0227718137074573e-28, + "velocityX": 3.2904914962294794e-27, + "velocityY": 1.9801334803171188e-26, + "timestamp": 1.8756100182951347 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Amp 2 Piece 3.1.traj b/src/main/deploy/choreo/Amp 2 Piece 3.1.traj new file mode 100644 index 0000000..82cd037 --- /dev/null +++ b/src/main/deploy/choreo/Amp 2 Piece 3.1.traj @@ -0,0 +1,194 @@ +{ + "samples": [ + { + "x": 2.8635880947113037, + "y": 7.026061534881592, + "heading": -2.2531013668007383e-23, + "angularVelocity": -1.7770917071805522e-23, + "velocityX": 2.7311091089533584e-24, + "velocityY": -1.1012176566640825e-22, + "timestamp": 0 + }, + { + "x": 2.864676594934698, + "y": 7.022588105747808, + "heading": -0.04985721436534877, + "angularVelocity": -1.3681375614641862, + "velocityX": 0.029869659992932625, + "velocityY": -0.09531476889565428, + "timestamp": 0.03644166768726924 + }, + { + "x": 2.8669918803645973, + "y": 7.015936901032802, + "heading": -0.1499743939379529, + "angularVelocity": -2.74732705516602, + "velocityX": 0.06353401413372199, + "velocityY": -0.1825164746049429, + "timestamp": 0.07288333537453848 + }, + { + "x": 2.871502989712911, + "y": 7.006569710493649, + "heading": -0.2998918812860076, + "angularVelocity": -4.113903036342868, + "velocityX": 0.1237898711723712, + "velocityY": -0.2570461544060021, + "timestamp": 0.10932500306180772 + }, + { + "x": 2.880663781149505, + "y": 6.993184946483063, + "heading": -0.49046362617983097, + "angularVelocity": -5.229501199814709, + "velocityX": 0.25138233286165845, + "velocityY": -0.36729285074021994, + "timestamp": 0.14576667074907695 + }, + { + "x": 2.895739747170883, + "y": 6.971979769048049, + "heading": -0.7007614331389338, + "angularVelocity": -5.7708063408022765, + "velocityX": 0.4137013198944287, + "velocityY": -0.5818937161984139, + "timestamp": 0.1822083384363462 + }, + { + "x": 2.9168343346087933, + "y": 6.94248561361002, + "heading": -0.895115264947589, + "angularVelocity": -5.33328588242826, + "velocityX": 0.5788590033512745, + "velocityY": -0.8093525162223337, + "timestamp": 0.21865000612361546 + }, + { + "x": 2.938346654426208, + "y": 6.902931144993223, + "heading": -1.0726747966927241, + "angularVelocity": -4.872431560182551, + "velocityX": 0.590322045687563, + "velocityY": -1.0854187288090003, + "timestamp": 0.2550916738108847 + }, + { + "x": 2.9553323200852546, + "y": 6.85423688933496, + "heading": -1.2351730413824855, + "angularVelocity": -4.459133047484804, + "velocityX": 0.46610560759215897, + "velocityY": -1.3362246776448974, + "timestamp": 0.29153334149815396 + }, + { + "x": 2.9695732769404084, + "y": 6.79656717365057, + "heading": -1.3761597087915507, + "angularVelocity": -3.8688313778328625, + "velocityX": 0.39078773719588467, + "velocityY": -1.582521309927206, + "timestamp": 0.3279750091854232 + }, + { + "x": 2.987706815188453, + "y": 6.731391881244193, + "heading": -1.4895165523111065, + "angularVelocity": -3.110638198348881, + "velocityX": 0.49760451150759183, + "velocityY": -1.7884827051739542, + "timestamp": 0.36441667687269247 + }, + { + "x": 3.0070752529330744, + "y": 6.676346817724788, + "heading": -1.6191852506186655, + "angularVelocity": -3.5582536842258268, + "velocityX": 0.5314915308166857, + "velocityY": -1.5104979275861887, + "timestamp": 0.4008583445599617 + }, + { + "x": 3.0277127553427983, + "y": 6.631345901214396, + "heading": -1.7659714627857621, + "angularVelocity": -4.027977353472648, + "velocityX": 0.5663160804502753, + "velocityY": -1.2348753327255706, + "timestamp": 0.437300012247231 + }, + { + "x": 3.0496224874417317, + "y": 6.596113415691361, + "heading": -1.9320800211636182, + "angularVelocity": -4.558204081200309, + "velocityX": 0.6012274818747366, + "velocityY": -0.9668186929695299, + "timestamp": 0.4737416799345002 + }, + { + "x": 3.071978131409277, + "y": 6.569951938634911, + "heading": -2.1214170567465316, + "angularVelocity": -5.195619399412323, + "velocityX": 0.6134638008170941, + "velocityY": -0.7179001049282177, + "timestamp": 0.5101833476217694 + }, + { + "x": 3.087603628473299, + "y": 6.549928885874662, + "heading": -2.3361436590449416, + "angularVelocity": -5.892337423773383, + "velocityX": 0.4287810645252342, + "velocityY": -0.5494548968526693, + "timestamp": 0.5466250153090386 + }, + { + "x": 3.095096900142376, + "y": 6.535588060309454, + "heading": -2.5324942002744897, + "angularVelocity": -5.388077815608382, + "velocityX": 0.20562373087262698, + "velocityY": -0.3935282459704378, + "timestamp": 0.5830666829963078 + }, + { + "x": 3.100123151105858, + "y": 6.5252451428378, + "heading": -2.6818042895708434, + "angularVelocity": -4.097235356451997, + "velocityX": 0.13792593156315622, + "velocityY": -0.28382118953540003, + "timestamp": 0.619508350683577 + }, + { + "x": 3.103300439410377, + "y": 6.518482658591172, + "heading": -2.781921479680782, + "angularVelocity": -2.7473273443222164, + "velocityX": 0.08718833429318376, + "velocityY": -0.18557010904826304, + "timestamp": 0.6559500183708462 + }, + { + "x": 3.104858160018921, + "y": 6.51513671875, + "heading": -2.8321171645728587, + "angularVelocity": -1.3774255701698461, + "velocityX": 0.042745590622028554, + "velocityY": -0.0918163205341267, + "timestamp": 0.6923916860581154 + }, + { + "x": 3.104858160018921, + "y": 6.51513671875, + "heading": -2.8321171645728587, + "angularVelocity": 2.0190189525600836e-23, + "velocityX": -7.10768769656816e-23, + "velocityY": 2.4419134906662144e-23, + "timestamp": 0.7288333537453846 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Amp 2 Piece 3.traj b/src/main/deploy/choreo/Amp 2 Piece 3.traj new file mode 100644 index 0000000..82cd037 --- /dev/null +++ b/src/main/deploy/choreo/Amp 2 Piece 3.traj @@ -0,0 +1,194 @@ +{ + "samples": [ + { + "x": 2.8635880947113037, + "y": 7.026061534881592, + "heading": -2.2531013668007383e-23, + "angularVelocity": -1.7770917071805522e-23, + "velocityX": 2.7311091089533584e-24, + "velocityY": -1.1012176566640825e-22, + "timestamp": 0 + }, + { + "x": 2.864676594934698, + "y": 7.022588105747808, + "heading": -0.04985721436534877, + "angularVelocity": -1.3681375614641862, + "velocityX": 0.029869659992932625, + "velocityY": -0.09531476889565428, + "timestamp": 0.03644166768726924 + }, + { + "x": 2.8669918803645973, + "y": 7.015936901032802, + "heading": -0.1499743939379529, + "angularVelocity": -2.74732705516602, + "velocityX": 0.06353401413372199, + "velocityY": -0.1825164746049429, + "timestamp": 0.07288333537453848 + }, + { + "x": 2.871502989712911, + "y": 7.006569710493649, + "heading": -0.2998918812860076, + "angularVelocity": -4.113903036342868, + "velocityX": 0.1237898711723712, + "velocityY": -0.2570461544060021, + "timestamp": 0.10932500306180772 + }, + { + "x": 2.880663781149505, + "y": 6.993184946483063, + "heading": -0.49046362617983097, + "angularVelocity": -5.229501199814709, + "velocityX": 0.25138233286165845, + "velocityY": -0.36729285074021994, + "timestamp": 0.14576667074907695 + }, + { + "x": 2.895739747170883, + "y": 6.971979769048049, + "heading": -0.7007614331389338, + "angularVelocity": -5.7708063408022765, + "velocityX": 0.4137013198944287, + "velocityY": -0.5818937161984139, + "timestamp": 0.1822083384363462 + }, + { + "x": 2.9168343346087933, + "y": 6.94248561361002, + "heading": -0.895115264947589, + "angularVelocity": -5.33328588242826, + "velocityX": 0.5788590033512745, + "velocityY": -0.8093525162223337, + "timestamp": 0.21865000612361546 + }, + { + "x": 2.938346654426208, + "y": 6.902931144993223, + "heading": -1.0726747966927241, + "angularVelocity": -4.872431560182551, + "velocityX": 0.590322045687563, + "velocityY": -1.0854187288090003, + "timestamp": 0.2550916738108847 + }, + { + "x": 2.9553323200852546, + "y": 6.85423688933496, + "heading": -1.2351730413824855, + "angularVelocity": -4.459133047484804, + "velocityX": 0.46610560759215897, + "velocityY": -1.3362246776448974, + "timestamp": 0.29153334149815396 + }, + { + "x": 2.9695732769404084, + "y": 6.79656717365057, + "heading": -1.3761597087915507, + "angularVelocity": -3.8688313778328625, + "velocityX": 0.39078773719588467, + "velocityY": -1.582521309927206, + "timestamp": 0.3279750091854232 + }, + { + "x": 2.987706815188453, + "y": 6.731391881244193, + "heading": -1.4895165523111065, + "angularVelocity": -3.110638198348881, + "velocityX": 0.49760451150759183, + "velocityY": -1.7884827051739542, + "timestamp": 0.36441667687269247 + }, + { + "x": 3.0070752529330744, + "y": 6.676346817724788, + "heading": -1.6191852506186655, + "angularVelocity": -3.5582536842258268, + "velocityX": 0.5314915308166857, + "velocityY": -1.5104979275861887, + "timestamp": 0.4008583445599617 + }, + { + "x": 3.0277127553427983, + "y": 6.631345901214396, + "heading": -1.7659714627857621, + "angularVelocity": -4.027977353472648, + "velocityX": 0.5663160804502753, + "velocityY": -1.2348753327255706, + "timestamp": 0.437300012247231 + }, + { + "x": 3.0496224874417317, + "y": 6.596113415691361, + "heading": -1.9320800211636182, + "angularVelocity": -4.558204081200309, + "velocityX": 0.6012274818747366, + "velocityY": -0.9668186929695299, + "timestamp": 0.4737416799345002 + }, + { + "x": 3.071978131409277, + "y": 6.569951938634911, + "heading": -2.1214170567465316, + "angularVelocity": -5.195619399412323, + "velocityX": 0.6134638008170941, + "velocityY": -0.7179001049282177, + "timestamp": 0.5101833476217694 + }, + { + "x": 3.087603628473299, + "y": 6.549928885874662, + "heading": -2.3361436590449416, + "angularVelocity": -5.892337423773383, + "velocityX": 0.4287810645252342, + "velocityY": -0.5494548968526693, + "timestamp": 0.5466250153090386 + }, + { + "x": 3.095096900142376, + "y": 6.535588060309454, + "heading": -2.5324942002744897, + "angularVelocity": -5.388077815608382, + "velocityX": 0.20562373087262698, + "velocityY": -0.3935282459704378, + "timestamp": 0.5830666829963078 + }, + { + "x": 3.100123151105858, + "y": 6.5252451428378, + "heading": -2.6818042895708434, + "angularVelocity": -4.097235356451997, + "velocityX": 0.13792593156315622, + "velocityY": -0.28382118953540003, + "timestamp": 0.619508350683577 + }, + { + "x": 3.103300439410377, + "y": 6.518482658591172, + "heading": -2.781921479680782, + "angularVelocity": -2.7473273443222164, + "velocityX": 0.08718833429318376, + "velocityY": -0.18557010904826304, + "timestamp": 0.6559500183708462 + }, + { + "x": 3.104858160018921, + "y": 6.51513671875, + "heading": -2.8321171645728587, + "angularVelocity": -1.3774255701698461, + "velocityX": 0.042745590622028554, + "velocityY": -0.0918163205341267, + "timestamp": 0.6923916860581154 + }, + { + "x": 3.104858160018921, + "y": 6.51513671875, + "heading": -2.8321171645728587, + "angularVelocity": 2.0190189525600836e-23, + "velocityX": -7.10768769656816e-23, + "velocityY": 2.4419134906662144e-23, + "timestamp": 0.7288333537453846 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Amp 3 Piece 3.1.traj b/src/main/deploy/choreo/Amp 3 Piece 3.1.traj deleted file mode 100644 index d7d854f..0000000 --- a/src/main/deploy/choreo/Amp 3 Piece 3.1.traj +++ /dev/null @@ -1,122 +0,0 @@ -{ - "samples": [ - { - "x": 3.162, - "y": 6.359, - "heading": -2.588, - "angularVelocity": 1.4410764077573947e-21, - "velocityX": 4.776931205132025e-21, - "velocityY": -5.947632414862795e-20, - "timestamp": 0 - }, - { - "x": 3.1566148081540084, - "y": 6.358254414724499, - "heading": -2.640344449106781, - "angularVelocity": -1.340201944453853, - "velocityX": -0.13787984602783324, - "velocityY": -0.019089604591012607, - "timestamp": 0.03905713562302894 - }, - { - "x": 3.143683922809688, - "y": 6.355595068320737, - "heading": -2.736637732363478, - "angularVelocity": -2.465446626350144, - "velocityX": -0.33107613085421683, - "velocityY": -0.06808861841355184, - "timestamp": 0.07811427124605788 - }, - { - "x": 3.121844181931675, - "y": 6.347995260906763, - "heading": -2.863331775306469, - "angularVelocity": -3.2438129658512223, - "velocityX": -0.5591741567739482, - "velocityY": -0.19458179133579442, - "timestamp": 0.11717140686908682 - }, - { - "x": 3.096280729497018, - "y": 6.328308215255226, - "heading": -2.9948428591271825, - "angularVelocity": -3.367146149426579, - "velocityX": -0.6545142654952973, - "velocityY": -0.5040575899254738, - "timestamp": 0.15622854249211576 - }, - { - "x": 3.0722529888153076, - "y": 6.29632043838501, - "heading": -3.1159573378389447, - "angularVelocity": -3.1009565033322724, - "velocityX": -0.6151946449330228, - "velocityY": -0.8189995594903712, - "timestamp": 0.1952856781151447 - }, - { - "x": 3.0372212203856686, - "y": 6.1694121418451795, - "heading": -3.3044718530781294, - "angularVelocity": -2.1711550223561162, - "velocityX": -0.4034670745195598, - "velocityY": -1.461625302759914, - "timestamp": 0.28211251143569255 - }, - { - "x": 3.0226662530134463, - "y": 5.987674801349684, - "heading": -3.4118328533680193, - "angularVelocity": -1.2364956337119166, - "velocityX": -0.1676321341639648, - "velocityY": -2.093101101874308, - "timestamp": 0.3689393447562404 - }, - { - "x": 3.035431736182884, - "y": 5.7564198497648364, - "heading": -3.423913140535644, - "angularVelocity": -0.13913080444874767, - "velocityX": 0.14702232802053028, - "velocityY": -2.6634041890149223, - "timestamp": 0.45576617807678826 - }, - { - "x": 3.06747307710671, - "y": 5.574098037633144, - "heading": -3.3005437740825374, - "angularVelocity": 1.420866818874432, - "velocityX": 0.3690257918947216, - "velocityY": -2.0998325651080223, - "timestamp": 0.5425930113973362 - }, - { - "x": 3.0919306232912, - "y": 5.453752568769309, - "heading": -3.203252635101642, - "angularVelocity": 1.1205192595440523, - "velocityX": 0.28168188622286433, - "velocityY": -1.3860400553771668, - "timestamp": 0.629419844717884 - }, - { - "x": 3.104857921600342, - "y": 5.3939409255981445, - "heading": -3.1509381423979628, - "angularVelocity": 0.6025152674927592, - "velocityX": 0.1488859816114309, - "velocityY": -0.6888612757573563, - "timestamp": 0.7162466780384319 - }, - { - "x": 3.104857921600342, - "y": 5.3939409255981445, - "heading": -3.1509381423979628, - "angularVelocity": 3.452519064446764e-20, - "velocityX": -7.442188151588243e-20, - "velocityY": -5.2649187007366165e-21, - "timestamp": 0.8030735113589798 - } - ], - "eventMarkers": [] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Amp 3 Piece 3.traj b/src/main/deploy/choreo/Amp 3 Piece 3.traj deleted file mode 100644 index d7d854f..0000000 --- a/src/main/deploy/choreo/Amp 3 Piece 3.traj +++ /dev/null @@ -1,122 +0,0 @@ -{ - "samples": [ - { - "x": 3.162, - "y": 6.359, - "heading": -2.588, - "angularVelocity": 1.4410764077573947e-21, - "velocityX": 4.776931205132025e-21, - "velocityY": -5.947632414862795e-20, - "timestamp": 0 - }, - { - "x": 3.1566148081540084, - "y": 6.358254414724499, - "heading": -2.640344449106781, - "angularVelocity": -1.340201944453853, - "velocityX": -0.13787984602783324, - "velocityY": -0.019089604591012607, - "timestamp": 0.03905713562302894 - }, - { - "x": 3.143683922809688, - "y": 6.355595068320737, - "heading": -2.736637732363478, - "angularVelocity": -2.465446626350144, - "velocityX": -0.33107613085421683, - "velocityY": -0.06808861841355184, - "timestamp": 0.07811427124605788 - }, - { - "x": 3.121844181931675, - "y": 6.347995260906763, - "heading": -2.863331775306469, - "angularVelocity": -3.2438129658512223, - "velocityX": -0.5591741567739482, - "velocityY": -0.19458179133579442, - "timestamp": 0.11717140686908682 - }, - { - "x": 3.096280729497018, - "y": 6.328308215255226, - "heading": -2.9948428591271825, - "angularVelocity": -3.367146149426579, - "velocityX": -0.6545142654952973, - "velocityY": -0.5040575899254738, - "timestamp": 0.15622854249211576 - }, - { - "x": 3.0722529888153076, - "y": 6.29632043838501, - "heading": -3.1159573378389447, - "angularVelocity": -3.1009565033322724, - "velocityX": -0.6151946449330228, - "velocityY": -0.8189995594903712, - "timestamp": 0.1952856781151447 - }, - { - "x": 3.0372212203856686, - "y": 6.1694121418451795, - "heading": -3.3044718530781294, - "angularVelocity": -2.1711550223561162, - "velocityX": -0.4034670745195598, - "velocityY": -1.461625302759914, - "timestamp": 0.28211251143569255 - }, - { - "x": 3.0226662530134463, - "y": 5.987674801349684, - "heading": -3.4118328533680193, - "angularVelocity": -1.2364956337119166, - "velocityX": -0.1676321341639648, - "velocityY": -2.093101101874308, - "timestamp": 0.3689393447562404 - }, - { - "x": 3.035431736182884, - "y": 5.7564198497648364, - "heading": -3.423913140535644, - "angularVelocity": -0.13913080444874767, - "velocityX": 0.14702232802053028, - "velocityY": -2.6634041890149223, - "timestamp": 0.45576617807678826 - }, - { - "x": 3.06747307710671, - "y": 5.574098037633144, - "heading": -3.3005437740825374, - "angularVelocity": 1.420866818874432, - "velocityX": 0.3690257918947216, - "velocityY": -2.0998325651080223, - "timestamp": 0.5425930113973362 - }, - { - "x": 3.0919306232912, - "y": 5.453752568769309, - "heading": -3.203252635101642, - "angularVelocity": 1.1205192595440523, - "velocityX": 0.28168188622286433, - "velocityY": -1.3860400553771668, - "timestamp": 0.629419844717884 - }, - { - "x": 3.104857921600342, - "y": 5.3939409255981445, - "heading": -3.1509381423979628, - "angularVelocity": 0.6025152674927592, - "velocityX": 0.1488859816114309, - "velocityY": -0.6888612757573563, - "timestamp": 0.7162466780384319 - }, - { - "x": 3.104857921600342, - "y": 5.3939409255981445, - "heading": -3.1509381423979628, - "angularVelocity": 3.452519064446764e-20, - "velocityX": -7.442188151588243e-20, - "velocityY": -5.2649187007366165e-21, - "timestamp": 0.8030735113589798 - } - ], - "eventMarkers": [] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Amp 3 Piece 4.1.traj b/src/main/deploy/choreo/Amp 3 Piece 4.1.traj index b3a05b3..2015fb3 100644 --- a/src/main/deploy/choreo/Amp 3 Piece 4.1.traj +++ b/src/main/deploy/choreo/Amp 3 Piece 4.1.traj @@ -1,85 +1,418 @@ { "samples": [ { - "x": 2.911, - "y": 5.349, - "heading": 3.132, - "angularVelocity": -4.344195742289857e-21, - "velocityX": -4.7082378602815056e-20, - "velocityY": -1.7784914405892649e-19, + "x": 3.162, + "y": 6.359, + "heading": -2.588, + "angularVelocity": 2.3545923598347173e-28, + "velocityX": 1.0132424317562211e-26, + "velocityY": -5.4128073679143105e-28, "timestamp": 0 }, { - "x": 2.924890848236759, - "y": 5.401469647858394, - "heading": 3.149410077214198, - "angularVelocity": 0.2153558954017532, - "velocityX": 0.17182439934715354, - "velocityY": 0.6490291718375661, - "timestamp": 0.08084328121929982 - }, - { - "x": 2.952672232790939, - "y": 5.506414733603586, - "heading": 3.1840022647617863, - "angularVelocity": 0.4278919289996538, - "velocityX": 0.343644940373191, - "velocityY": 1.2981299640784225, - "timestamp": 0.16168656243859963 - }, - { - "x": 2.9943436299649684, - "y": 5.663848700643325, - "heading": 3.2352387316668687, - "angularVelocity": 0.6337752022471191, - "velocityX": 0.5154590034636232, - "velocityY": 1.9473970460535244, - "timestamp": 0.24252984365789945 - }, - { - "x": 3.049899633232633, - "y": 5.873841815086245, - "heading": 3.300049028392088, - "angularVelocity": 0.801678206868071, - "velocityX": 0.6872061899239325, - "velocityY": 2.5975332925104753, - "timestamp": 0.32337312487719927 - }, - { - "x": 3.091570598941423, - "y": 6.031287202367331, - "heading": 3.350741147440927, - "angularVelocity": 0.627041830617058, - "velocityX": 0.5154536664061404, - "velocityY": 1.947538310004935, - "timestamp": 0.4042164060964991 - }, - { - "x": 3.119351858656031, - "y": 6.136239824629046, - "heading": 3.384964766681284, - "angularVelocity": 0.42333288214167747, - "velocityX": 0.3436433961561573, - "velocityY": 1.2982231878616135, - "timestamp": 0.48505968731579896 - }, - { - "x": 3.133242607116699, - "y": 6.188713073730469, - "heading": 3.402195063790063, - "angularVelocity": 0.21313208530019123, - "velocityX": 0.17182316515564514, - "velocityY": 0.6490737178156233, - "timestamp": 0.5659029685350988 - }, - { - "x": 3.133242607116699, - "y": 6.188713073730469, - "heading": 3.402195063790063, - "angularVelocity": 2.97617448838405e-21, - "velocityX": 4.7042355495804444e-20, - "velocityY": 1.783120573732454e-19, - "timestamp": 0.6467462497543986 + "x": 3.158541809417635, + "y": 6.361560311364594, + "heading": -2.54828298105256, + "angularVelocity": 1.1612582755337608, + "velocityX": -0.10111162767422677, + "velocityY": 0.074859162114193, + "timestamp": 0.03420171014857538 + }, + { + "x": 3.151551779969113, + "y": 6.366194194823638, + "heading": -2.4681280049011955, + "angularVelocity": 2.3435955630044076, + "velocityX": -0.2043766062619971, + "velocityY": 0.1354868934598289, + "timestamp": 0.06840342029715075 + }, + { + "x": 3.1411730874434873, + "y": 6.371651225656791, + "heading": -2.3456815065418324, + "angularVelocity": 3.580127947621462, + "velocityX": -0.3034553675982788, + "velocityY": 0.15955432665347288, + "timestamp": 0.10260513044572614 + }, + { + "x": 3.1261057120873623, + "y": 6.373387190431622, + "heading": -2.1905074090315306, + "angularVelocity": 4.537027442084355, + "velocityX": -0.44054450174190574, + "velocityY": 0.05075666588861731, + "timestamp": 0.1368068405943015 + }, + { + "x": 3.1072783036815137, + "y": 6.366652254015958, + "heading": -2.0240132831100572, + "angularVelocity": 4.86800587450767, + "velocityX": -0.550481491248849, + "velocityY": -0.19691811860888647, + "timestamp": 0.17100855074287688 + }, + { + "x": 3.090847947710989, + "y": 6.350727001128441, + "heading": -1.86543996194514, + "angularVelocity": 4.636414976796781, + "velocityX": -0.4803957433458614, + "velocityY": -0.4656273858335121, + "timestamp": 0.20521026089145225 + }, + { + "x": 3.0789451322686627, + "y": 6.32702824464787, + "heading": -1.7201154118069268, + "angularVelocity": 4.249043381366309, + "velocityX": -0.34801813683056054, + "velocityY": -0.6929114473405495, + "timestamp": 0.23941197104002762 + }, + { + "x": 3.0722529888153076, + "y": 6.29632043838501, + "heading": -1.590018574093363, + "angularVelocity": 3.8038108956660723, + "velocityX": -0.19566692496614022, + "velocityY": -0.8978441759041662, + "timestamp": 0.273613681188603 + }, + { + "x": 3.074208101596113, + "y": 6.231624428620404, + "heading": -1.4310130878973673, + "angularVelocity": 2.956037223790758, + "velocityX": 0.03634708647502808, + "velocityY": -1.2027497771942952, + "timestamp": 0.3274037637072834 + }, + { + "x": 3.08751039213603, + "y": 6.151010677702688, + "heading": -1.32623470104763, + "angularVelocity": 1.9479127367642397, + "velocityX": 0.24730005824581125, + "velocityY": -1.498673122312463, + "timestamp": 0.3811938462259638 + }, + { + "x": 3.1116055405528797, + "y": 6.0548675149224565, + "heading": -1.2804377091115737, + "angularVelocity": 0.8514021505758418, + "velocityX": 0.4479477868152025, + "velocityY": -1.7873771200637798, + "timestamp": 0.4349839287446442 + }, + { + "x": 3.1458684756981237, + "y": 5.943270407786396, + "heading": -1.2961837179515021, + "angularVelocity": -0.292730706156845, + "velocityX": 0.6369749504166531, + "velocityY": -2.0746781174262896, + "timestamp": 0.4887740112633246 + }, + { + "x": 3.173516037313562, + "y": 5.826662668779321, + "heading": -1.4182314122517021, + "angularVelocity": -2.2689627638666443, + "velocityX": 0.5139899461176065, + "velocityY": -2.1678297103667643, + "timestamp": 0.542564093782005 + }, + { + "x": 3.179861068725586, + "y": 5.717745780944824, + "heading": -1.5707963267948966, + "angularVelocity": -2.836301924062139, + "velocityX": 0.11795913140346345, + "velocityY": -2.0248507296241973, + "timestamp": 0.5963541763006854 + }, + { + "x": 3.1696117726950463, + "y": 5.631590772796136, + "heading": -1.715109710694546, + "angularVelocity": -3.1840088084214218, + "velocityX": -0.22613182477966223, + "velocityY": -1.9008514485794035, + "timestamp": 0.641678606157146 + }, + { + "x": 3.145236062623627, + "y": 5.554070753841431, + "heading": -1.8754511624529167, + "angularVelocity": -3.5376385818014935, + "velocityX": -0.5378051119146163, + "velocityY": -1.7103363285584534, + "timestamp": 0.6870030360136066 + }, + { + "x": 3.1081460827237075, + "y": 5.487648602669839, + "heading": -2.046345155928937, + "angularVelocity": -3.770460963706106, + "velocityX": -0.8183220399546215, + "velocityY": -1.4654823321980464, + "timestamp": 0.7323274658700671 + }, + { + "x": 3.0570045176167, + "y": 5.415666219019381, + "heading": -2.1885090431681173, + "angularVelocity": -3.1365841266929104, + "velocityX": -1.1283443668010789, + "velocityY": -1.5881586128809646, + "timestamp": 0.7776518957265277 + }, + { + "x": 2.990954781455389, + "y": 5.342049490783512, + "heading": -2.3020677570341874, + "angularVelocity": -2.5054637030339353, + "velocityX": -1.4572656814544667, + "velocityY": -1.6242174136334224, + "timestamp": 0.8229763255829883 + }, + { + "x": 2.9107728525339303, + "y": 5.26595965350658, + "heading": -2.382373816281858, + "angularVelocity": -1.7718051722215742, + "velocityX": -1.769066465378379, + "velocityY": -1.6787820060374272, + "timestamp": 0.8683007554394488 + }, + { + "x": 2.8194126507306287, + "y": 5.187653621521034, + "heading": -2.432983034334834, + "angularVelocity": -1.116599110308764, + "velocityX": -2.0156944520346625, + "velocityY": -1.7276782572563323, + "timestamp": 0.9136251852959094 + }, + { + "x": 2.7228504033394225, + "y": 5.111823049623562, + "heading": -2.4758781578005773, + "angularVelocity": -0.9464018323360985, + "velocityX": -2.1304679992007083, + "velocityY": -1.6730617933335687, + "timestamp": 0.95894961515237 + }, + { + "x": 2.623412901645993, + "y": 5.040643151344395, + "heading": -2.5203252455458984, + "angularVelocity": -0.9806430634887603, + "velocityX": -2.193905185533298, + "velocityY": -1.570453252353952, + "timestamp": 1.0042740450088306 + }, + { + "x": 2.5233382583202117, + "y": 4.977210159925781, + "heading": -2.5766537672282444, + "angularVelocity": -1.242785002717836, + "velocityX": -2.207962541232429, + "velocityY": -1.3995320320520612, + "timestamp": 1.0495984748652911 + }, + { + "x": 2.4249020255614324, + "y": 4.919535457776485, + "heading": -2.6447412484992725, + "angularVelocity": -1.5022247712030088, + "velocityX": -2.1718140320908645, + "velocityY": -1.2724859933582913, + "timestamp": 1.0949229047217517 + }, + { + "x": 2.328898126856056, + "y": 4.8626542096962675, + "heading": -2.71656901676977, + "angularVelocity": -1.584747309518758, + "velocityX": -2.118149064630577, + "velocityY": -1.2549798918675032, + "timestamp": 1.1402473345782123 + }, + { + "x": 2.2358360141282634, + "y": 4.806747454357658, + "heading": -2.7933938675939385, + "angularVelocity": -1.6949987251349385, + "velocityX": -2.0532439795164334, + "velocityY": -1.2334795057690093, + "timestamp": 1.1855717644346728 + }, + { + "x": 2.146568790469089, + "y": 4.7521619746147765, + "heading": -2.87737657331996, + "angularVelocity": -1.852923599744974, + "velocityX": -1.9695167471908095, + "velocityY": -1.2043279952059003, + "timestamp": 1.2308961942911334 + }, + { + "x": 2.062811347738124, + "y": 4.6996834442356, + "heading": -2.972981859541326, + "angularVelocity": -2.1093544149179886, + "velocityX": -1.8479535869776882, + "velocityY": -1.157842041154702, + "timestamp": 1.276220624147594 + }, + { + "x": 1.9897516685916172, + "y": 4.651936049585479, + "heading": -3.0940724928293957, + "angularVelocity": -2.6716416217822414, + "velocityX": -1.6119271522638414, + "velocityY": -1.0534582520140607, + "timestamp": 1.3215450540040545 + }, + { + "x": 1.9305727803175539, + "y": 4.5950641132377426, + "heading": -3.225567477933253, + "angularVelocity": -2.9011944666550202, + "velocityX": -1.3056730875926967, + "velocityY": -1.2547744456542749, + "timestamp": 1.366869483860515 + }, + { + "x": 1.8816378534728377, + "y": 4.52449736884993, + "heading": -3.353672139095455, + "angularVelocity": -2.8263932181364657, + "velocityX": -1.0796589609552654, + "velocityY": -1.5569251419442536, + "timestamp": 1.4121939137169757 + }, + { + "x": 1.8425411312537379, + "y": 4.439984375501508, + "heading": -3.4765942071733944, + "angularVelocity": -2.7120488546072266, + "velocityX": -0.8625971102762152, + "velocityY": -1.8646234186744175, + "timestamp": 1.4575183435734362 + }, + { + "x": 1.814625048446928, + "y": 4.343895867299826, + "heading": -3.5780401995813476, + "angularVelocity": -2.238218830975388, + "velocityX": -0.6159169104877494, + "velocityY": -2.120015817209132, + "timestamp": 1.5028427734298968 + }, + { + "x": 1.800793601871009, + "y": 4.239870845816041, + "heading": -3.658298645599016, + "angularVelocity": -1.7707546740651992, + "velocityX": -0.30516537372278685, + "velocityY": -2.2951203537082416, + "timestamp": 1.5481672032863574 + }, + { + "x": 1.8017919063568115, + "y": 4.128912448883057, + "heading": -3.719918188107055, + "angularVelocity": -1.359521624501048, + "velocityX": 0.022025748342873373, + "velocityY": -2.448092502969865, + "timestamp": 1.593491633142818 + }, + { + "x": 1.833720071260504, + "y": 3.9595668887755444, + "heading": -3.7690691947954535, + "angularVelocity": -0.7650465428728321, + "velocityX": 0.4969691126511394, + "velocityY": -2.635901969057657, + "timestamp": 1.6577374063526822 + }, + { + "x": 1.8976877544470778, + "y": 3.7830860846248036, + "heading": -3.7807757981913688, + "angularVelocity": -0.18221593127495458, + "velocityX": 0.9956714658506488, + "velocityY": -2.7469636574261713, + "timestamp": 1.7219831795625464 + }, + { + "x": 1.9943801325500514, + "y": 3.616742294018958, + "heading": -3.780775957074689, + "angularVelocity": -0.000002473054837484796, + "velocityX": 1.5050387484188235, + "velocityY": -2.5891787474091115, + "timestamp": 1.7862289527724107 + }, + { + "x": 2.0986811434479353, + "y": 3.48134511313859, + "heading": -3.8280375689759656, + "angularVelocity": -0.7356376854068292, + "velocityX": 1.6234688398437669, + "velocityY": -2.107487763250687, + "timestamp": 1.850474725982275 + }, + { + "x": 2.185971627375577, + "y": 3.3758656680580077, + "heading": -3.8757754572956524, + "angularVelocity": -0.7430510356494172, + "velocityX": 1.3586961377599196, + "velocityY": -1.6418114345985888, + "timestamp": 1.9147204991921392 + }, + { + "x": 2.2527796055270417, + "y": 3.297948908815885, + "heading": -3.9153521792866077, + "angularVelocity": -0.6160206347221387, + "velocityX": 1.0398812997896527, + "velocityY": -1.2127919915852055, + "timestamp": 1.9789662724020034 + }, + { + "x": 2.2978634684080435, + "y": 3.246528407652369, + "heading": -3.943342337095115, + "angularVelocity": -0.4356731409718601, + "velocityX": 0.7017405290419882, + "velocityY": -0.8003717380059724, + "timestamp": 2.0432120456118676 + }, + { + "x": 2.320594549179077, + "y": 3.221007823944092, + "heading": -3.9579087770321086, + "angularVelocity": -0.22672993426993343, + "velocityX": 0.3538144166586729, + "velocityY": -0.39723366119217857, + "timestamp": 2.107457818821732 + }, + { + "x": 2.320594549179077, + "y": 3.221007823944092, + "heading": -3.9579087770321086, + "angularVelocity": 3.2853435384869114e-26, + "velocityX": -5.0541121334764486e-26, + "velocityY": -7.435728149679528e-26, + "timestamp": 2.171703592031596 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Amp 3 Piece 4.traj b/src/main/deploy/choreo/Amp 3 Piece 4.traj index b3a05b3..2015fb3 100644 --- a/src/main/deploy/choreo/Amp 3 Piece 4.traj +++ b/src/main/deploy/choreo/Amp 3 Piece 4.traj @@ -1,85 +1,418 @@ { "samples": [ { - "x": 2.911, - "y": 5.349, - "heading": 3.132, - "angularVelocity": -4.344195742289857e-21, - "velocityX": -4.7082378602815056e-20, - "velocityY": -1.7784914405892649e-19, + "x": 3.162, + "y": 6.359, + "heading": -2.588, + "angularVelocity": 2.3545923598347173e-28, + "velocityX": 1.0132424317562211e-26, + "velocityY": -5.4128073679143105e-28, "timestamp": 0 }, { - "x": 2.924890848236759, - "y": 5.401469647858394, - "heading": 3.149410077214198, - "angularVelocity": 0.2153558954017532, - "velocityX": 0.17182439934715354, - "velocityY": 0.6490291718375661, - "timestamp": 0.08084328121929982 - }, - { - "x": 2.952672232790939, - "y": 5.506414733603586, - "heading": 3.1840022647617863, - "angularVelocity": 0.4278919289996538, - "velocityX": 0.343644940373191, - "velocityY": 1.2981299640784225, - "timestamp": 0.16168656243859963 - }, - { - "x": 2.9943436299649684, - "y": 5.663848700643325, - "heading": 3.2352387316668687, - "angularVelocity": 0.6337752022471191, - "velocityX": 0.5154590034636232, - "velocityY": 1.9473970460535244, - "timestamp": 0.24252984365789945 - }, - { - "x": 3.049899633232633, - "y": 5.873841815086245, - "heading": 3.300049028392088, - "angularVelocity": 0.801678206868071, - "velocityX": 0.6872061899239325, - "velocityY": 2.5975332925104753, - "timestamp": 0.32337312487719927 - }, - { - "x": 3.091570598941423, - "y": 6.031287202367331, - "heading": 3.350741147440927, - "angularVelocity": 0.627041830617058, - "velocityX": 0.5154536664061404, - "velocityY": 1.947538310004935, - "timestamp": 0.4042164060964991 - }, - { - "x": 3.119351858656031, - "y": 6.136239824629046, - "heading": 3.384964766681284, - "angularVelocity": 0.42333288214167747, - "velocityX": 0.3436433961561573, - "velocityY": 1.2982231878616135, - "timestamp": 0.48505968731579896 - }, - { - "x": 3.133242607116699, - "y": 6.188713073730469, - "heading": 3.402195063790063, - "angularVelocity": 0.21313208530019123, - "velocityX": 0.17182316515564514, - "velocityY": 0.6490737178156233, - "timestamp": 0.5659029685350988 - }, - { - "x": 3.133242607116699, - "y": 6.188713073730469, - "heading": 3.402195063790063, - "angularVelocity": 2.97617448838405e-21, - "velocityX": 4.7042355495804444e-20, - "velocityY": 1.783120573732454e-19, - "timestamp": 0.6467462497543986 + "x": 3.158541809417635, + "y": 6.361560311364594, + "heading": -2.54828298105256, + "angularVelocity": 1.1612582755337608, + "velocityX": -0.10111162767422677, + "velocityY": 0.074859162114193, + "timestamp": 0.03420171014857538 + }, + { + "x": 3.151551779969113, + "y": 6.366194194823638, + "heading": -2.4681280049011955, + "angularVelocity": 2.3435955630044076, + "velocityX": -0.2043766062619971, + "velocityY": 0.1354868934598289, + "timestamp": 0.06840342029715075 + }, + { + "x": 3.1411730874434873, + "y": 6.371651225656791, + "heading": -2.3456815065418324, + "angularVelocity": 3.580127947621462, + "velocityX": -0.3034553675982788, + "velocityY": 0.15955432665347288, + "timestamp": 0.10260513044572614 + }, + { + "x": 3.1261057120873623, + "y": 6.373387190431622, + "heading": -2.1905074090315306, + "angularVelocity": 4.537027442084355, + "velocityX": -0.44054450174190574, + "velocityY": 0.05075666588861731, + "timestamp": 0.1368068405943015 + }, + { + "x": 3.1072783036815137, + "y": 6.366652254015958, + "heading": -2.0240132831100572, + "angularVelocity": 4.86800587450767, + "velocityX": -0.550481491248849, + "velocityY": -0.19691811860888647, + "timestamp": 0.17100855074287688 + }, + { + "x": 3.090847947710989, + "y": 6.350727001128441, + "heading": -1.86543996194514, + "angularVelocity": 4.636414976796781, + "velocityX": -0.4803957433458614, + "velocityY": -0.4656273858335121, + "timestamp": 0.20521026089145225 + }, + { + "x": 3.0789451322686627, + "y": 6.32702824464787, + "heading": -1.7201154118069268, + "angularVelocity": 4.249043381366309, + "velocityX": -0.34801813683056054, + "velocityY": -0.6929114473405495, + "timestamp": 0.23941197104002762 + }, + { + "x": 3.0722529888153076, + "y": 6.29632043838501, + "heading": -1.590018574093363, + "angularVelocity": 3.8038108956660723, + "velocityX": -0.19566692496614022, + "velocityY": -0.8978441759041662, + "timestamp": 0.273613681188603 + }, + { + "x": 3.074208101596113, + "y": 6.231624428620404, + "heading": -1.4310130878973673, + "angularVelocity": 2.956037223790758, + "velocityX": 0.03634708647502808, + "velocityY": -1.2027497771942952, + "timestamp": 0.3274037637072834 + }, + { + "x": 3.08751039213603, + "y": 6.151010677702688, + "heading": -1.32623470104763, + "angularVelocity": 1.9479127367642397, + "velocityX": 0.24730005824581125, + "velocityY": -1.498673122312463, + "timestamp": 0.3811938462259638 + }, + { + "x": 3.1116055405528797, + "y": 6.0548675149224565, + "heading": -1.2804377091115737, + "angularVelocity": 0.8514021505758418, + "velocityX": 0.4479477868152025, + "velocityY": -1.7873771200637798, + "timestamp": 0.4349839287446442 + }, + { + "x": 3.1458684756981237, + "y": 5.943270407786396, + "heading": -1.2961837179515021, + "angularVelocity": -0.292730706156845, + "velocityX": 0.6369749504166531, + "velocityY": -2.0746781174262896, + "timestamp": 0.4887740112633246 + }, + { + "x": 3.173516037313562, + "y": 5.826662668779321, + "heading": -1.4182314122517021, + "angularVelocity": -2.2689627638666443, + "velocityX": 0.5139899461176065, + "velocityY": -2.1678297103667643, + "timestamp": 0.542564093782005 + }, + { + "x": 3.179861068725586, + "y": 5.717745780944824, + "heading": -1.5707963267948966, + "angularVelocity": -2.836301924062139, + "velocityX": 0.11795913140346345, + "velocityY": -2.0248507296241973, + "timestamp": 0.5963541763006854 + }, + { + "x": 3.1696117726950463, + "y": 5.631590772796136, + "heading": -1.715109710694546, + "angularVelocity": -3.1840088084214218, + "velocityX": -0.22613182477966223, + "velocityY": -1.9008514485794035, + "timestamp": 0.641678606157146 + }, + { + "x": 3.145236062623627, + "y": 5.554070753841431, + "heading": -1.8754511624529167, + "angularVelocity": -3.5376385818014935, + "velocityX": -0.5378051119146163, + "velocityY": -1.7103363285584534, + "timestamp": 0.6870030360136066 + }, + { + "x": 3.1081460827237075, + "y": 5.487648602669839, + "heading": -2.046345155928937, + "angularVelocity": -3.770460963706106, + "velocityX": -0.8183220399546215, + "velocityY": -1.4654823321980464, + "timestamp": 0.7323274658700671 + }, + { + "x": 3.0570045176167, + "y": 5.415666219019381, + "heading": -2.1885090431681173, + "angularVelocity": -3.1365841266929104, + "velocityX": -1.1283443668010789, + "velocityY": -1.5881586128809646, + "timestamp": 0.7776518957265277 + }, + { + "x": 2.990954781455389, + "y": 5.342049490783512, + "heading": -2.3020677570341874, + "angularVelocity": -2.5054637030339353, + "velocityX": -1.4572656814544667, + "velocityY": -1.6242174136334224, + "timestamp": 0.8229763255829883 + }, + { + "x": 2.9107728525339303, + "y": 5.26595965350658, + "heading": -2.382373816281858, + "angularVelocity": -1.7718051722215742, + "velocityX": -1.769066465378379, + "velocityY": -1.6787820060374272, + "timestamp": 0.8683007554394488 + }, + { + "x": 2.8194126507306287, + "y": 5.187653621521034, + "heading": -2.432983034334834, + "angularVelocity": -1.116599110308764, + "velocityX": -2.0156944520346625, + "velocityY": -1.7276782572563323, + "timestamp": 0.9136251852959094 + }, + { + "x": 2.7228504033394225, + "y": 5.111823049623562, + "heading": -2.4758781578005773, + "angularVelocity": -0.9464018323360985, + "velocityX": -2.1304679992007083, + "velocityY": -1.6730617933335687, + "timestamp": 0.95894961515237 + }, + { + "x": 2.623412901645993, + "y": 5.040643151344395, + "heading": -2.5203252455458984, + "angularVelocity": -0.9806430634887603, + "velocityX": -2.193905185533298, + "velocityY": -1.570453252353952, + "timestamp": 1.0042740450088306 + }, + { + "x": 2.5233382583202117, + "y": 4.977210159925781, + "heading": -2.5766537672282444, + "angularVelocity": -1.242785002717836, + "velocityX": -2.207962541232429, + "velocityY": -1.3995320320520612, + "timestamp": 1.0495984748652911 + }, + { + "x": 2.4249020255614324, + "y": 4.919535457776485, + "heading": -2.6447412484992725, + "angularVelocity": -1.5022247712030088, + "velocityX": -2.1718140320908645, + "velocityY": -1.2724859933582913, + "timestamp": 1.0949229047217517 + }, + { + "x": 2.328898126856056, + "y": 4.8626542096962675, + "heading": -2.71656901676977, + "angularVelocity": -1.584747309518758, + "velocityX": -2.118149064630577, + "velocityY": -1.2549798918675032, + "timestamp": 1.1402473345782123 + }, + { + "x": 2.2358360141282634, + "y": 4.806747454357658, + "heading": -2.7933938675939385, + "angularVelocity": -1.6949987251349385, + "velocityX": -2.0532439795164334, + "velocityY": -1.2334795057690093, + "timestamp": 1.1855717644346728 + }, + { + "x": 2.146568790469089, + "y": 4.7521619746147765, + "heading": -2.87737657331996, + "angularVelocity": -1.852923599744974, + "velocityX": -1.9695167471908095, + "velocityY": -1.2043279952059003, + "timestamp": 1.2308961942911334 + }, + { + "x": 2.062811347738124, + "y": 4.6996834442356, + "heading": -2.972981859541326, + "angularVelocity": -2.1093544149179886, + "velocityX": -1.8479535869776882, + "velocityY": -1.157842041154702, + "timestamp": 1.276220624147594 + }, + { + "x": 1.9897516685916172, + "y": 4.651936049585479, + "heading": -3.0940724928293957, + "angularVelocity": -2.6716416217822414, + "velocityX": -1.6119271522638414, + "velocityY": -1.0534582520140607, + "timestamp": 1.3215450540040545 + }, + { + "x": 1.9305727803175539, + "y": 4.5950641132377426, + "heading": -3.225567477933253, + "angularVelocity": -2.9011944666550202, + "velocityX": -1.3056730875926967, + "velocityY": -1.2547744456542749, + "timestamp": 1.366869483860515 + }, + { + "x": 1.8816378534728377, + "y": 4.52449736884993, + "heading": -3.353672139095455, + "angularVelocity": -2.8263932181364657, + "velocityX": -1.0796589609552654, + "velocityY": -1.5569251419442536, + "timestamp": 1.4121939137169757 + }, + { + "x": 1.8425411312537379, + "y": 4.439984375501508, + "heading": -3.4765942071733944, + "angularVelocity": -2.7120488546072266, + "velocityX": -0.8625971102762152, + "velocityY": -1.8646234186744175, + "timestamp": 1.4575183435734362 + }, + { + "x": 1.814625048446928, + "y": 4.343895867299826, + "heading": -3.5780401995813476, + "angularVelocity": -2.238218830975388, + "velocityX": -0.6159169104877494, + "velocityY": -2.120015817209132, + "timestamp": 1.5028427734298968 + }, + { + "x": 1.800793601871009, + "y": 4.239870845816041, + "heading": -3.658298645599016, + "angularVelocity": -1.7707546740651992, + "velocityX": -0.30516537372278685, + "velocityY": -2.2951203537082416, + "timestamp": 1.5481672032863574 + }, + { + "x": 1.8017919063568115, + "y": 4.128912448883057, + "heading": -3.719918188107055, + "angularVelocity": -1.359521624501048, + "velocityX": 0.022025748342873373, + "velocityY": -2.448092502969865, + "timestamp": 1.593491633142818 + }, + { + "x": 1.833720071260504, + "y": 3.9595668887755444, + "heading": -3.7690691947954535, + "angularVelocity": -0.7650465428728321, + "velocityX": 0.4969691126511394, + "velocityY": -2.635901969057657, + "timestamp": 1.6577374063526822 + }, + { + "x": 1.8976877544470778, + "y": 3.7830860846248036, + "heading": -3.7807757981913688, + "angularVelocity": -0.18221593127495458, + "velocityX": 0.9956714658506488, + "velocityY": -2.7469636574261713, + "timestamp": 1.7219831795625464 + }, + { + "x": 1.9943801325500514, + "y": 3.616742294018958, + "heading": -3.780775957074689, + "angularVelocity": -0.000002473054837484796, + "velocityX": 1.5050387484188235, + "velocityY": -2.5891787474091115, + "timestamp": 1.7862289527724107 + }, + { + "x": 2.0986811434479353, + "y": 3.48134511313859, + "heading": -3.8280375689759656, + "angularVelocity": -0.7356376854068292, + "velocityX": 1.6234688398437669, + "velocityY": -2.107487763250687, + "timestamp": 1.850474725982275 + }, + { + "x": 2.185971627375577, + "y": 3.3758656680580077, + "heading": -3.8757754572956524, + "angularVelocity": -0.7430510356494172, + "velocityX": 1.3586961377599196, + "velocityY": -1.6418114345985888, + "timestamp": 1.9147204991921392 + }, + { + "x": 2.2527796055270417, + "y": 3.297948908815885, + "heading": -3.9153521792866077, + "angularVelocity": -0.6160206347221387, + "velocityX": 1.0398812997896527, + "velocityY": -1.2127919915852055, + "timestamp": 1.9789662724020034 + }, + { + "x": 2.2978634684080435, + "y": 3.246528407652369, + "heading": -3.943342337095115, + "angularVelocity": -0.4356731409718601, + "velocityX": 0.7017405290419882, + "velocityY": -0.8003717380059724, + "timestamp": 2.0432120456118676 + }, + { + "x": 2.320594549179077, + "y": 3.221007823944092, + "heading": -3.9579087770321086, + "angularVelocity": -0.22672993426993343, + "velocityX": 0.3538144166586729, + "velocityY": -0.39723366119217857, + "timestamp": 2.107457818821732 + }, + { + "x": 2.320594549179077, + "y": 3.221007823944092, + "heading": -3.9579087770321086, + "angularVelocity": 3.2853435384869114e-26, + "velocityX": -5.0541121334764486e-26, + "velocityY": -7.435728149679528e-26, + "timestamp": 2.171703592031596 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Amp 3 Piece 5.1.traj b/src/main/deploy/choreo/Amp 3 Piece 5.1.traj new file mode 100644 index 0000000..18e9bd5 --- /dev/null +++ b/src/main/deploy/choreo/Amp 3 Piece 5.1.traj @@ -0,0 +1,266 @@ +{ + "samples": [ + { + "x": 1.785578966140747, + "y": 3.026456832885742, + "heading": 2.2752903910371143, + "angularVelocity": -1.8954621059563845e-27, + "velocityX": 5.094997374536094e-27, + "velocityY": -1.555088100432084e-26, + "timestamp": 0 + }, + { + "x": 1.782637002969458, + "y": 3.030934879800897, + "heading": 2.2161498525084706, + "angularVelocity": -1.465111298991776, + "velocityX": -0.07288238475180157, + "velocityY": 0.110936377923418, + "timestamp": 0.04036590160033693 + }, + { + "x": 1.776854375639112, + "y": 3.0404392101200512, + "heading": 2.0992045300830253, + "angularVelocity": -2.8971314349254977, + "velocityX": -0.14325525012670193, + "velocityY": 0.2354544291678867, + "timestamp": 0.08073180320067386 + }, + { + "x": 1.768527296831389, + "y": 3.0558164340175304, + "heading": 1.9267427272668538, + "angularVelocity": -4.272462548309133, + "velocityX": -0.2062899248521585, + "velocityY": 0.38094587976082794, + "timestamp": 0.12109770480101079 + }, + { + "x": 1.7577422754879621, + "y": 3.0792567393920285, + "heading": 1.7078934840810325, + "angularVelocity": -5.421636443368693, + "velocityX": -0.26718148030506983, + "velocityY": 0.5806956972392262, + "timestamp": 0.16146360640134771 + }, + { + "x": 1.7418176124047977, + "y": 3.1145654563250176, + "heading": 1.5040219429296366, + "angularVelocity": -5.050588072327222, + "velocityX": -0.39450780118414347, + "velocityY": 0.8747164198778695, + "timestamp": 0.20182950800168464 + }, + { + "x": 1.7176852193747818, + "y": 3.157439084845946, + "heading": 1.3285362877975075, + "angularVelocity": -4.347373604326088, + "velocityX": -0.5978410508193482, + "velocityY": 1.0621248831605514, + "timestamp": 0.24219540960202157 + }, + { + "x": 1.684843526703858, + "y": 3.208127129747163, + "heading": 1.17927498125219, + "angularVelocity": -3.697707734194931, + "velocityX": -0.8135998793261088, + "velocityY": 1.2557144245923109, + "timestamp": 0.2825613112023585 + }, + { + "x": 1.6434576742777403, + "y": 3.26705565218992, + "heading": 1.0553990395550077, + "angularVelocity": -3.0688263307897565, + "velocityX": -1.025267633952027, + "velocityY": 1.459858942981347, + "timestamp": 0.32292721280269543 + }, + { + "x": 1.5943446932187455, + "y": 3.3348722400911304, + "heading": 0.9576696221034526, + "angularVelocity": -2.4210884329842246, + "velocityX": -1.2166947624572502, + "velocityY": 1.6800464058170483, + "timestamp": 0.36329311440303236 + }, + { + "x": 1.5389107935987831, + "y": 3.4123091818648263, + "heading": 0.8877110799809592, + "angularVelocity": -1.7331098612673965, + "velocityX": -1.3732853081993277, + "velocityY": 1.918375131079688, + "timestamp": 0.4036590160033693 + }, + { + "x": 1.4937133432666583, + "y": 3.4984312793538073, + "heading": 0.8250449654269514, + "angularVelocity": -1.5524517493617649, + "velocityX": -1.1196938143392696, + "velocityY": 2.133535832834275, + "timestamp": 0.4440249176037062 + }, + { + "x": 1.4604074486928154, + "y": 3.5812108846355466, + "heading": 0.7421469709224653, + "angularVelocity": -2.0536638900143904, + "velocityX": -0.8250997315408592, + "velocityY": 2.050730988281656, + "timestamp": 0.48439081920404314 + }, + { + "x": 1.4398916806200917, + "y": 3.6618510739711834, + "heading": 0.6449597401080992, + "angularVelocity": -2.4076566349642725, + "velocityX": -0.5082450102527284, + "velocityY": 1.9977304144982566, + "timestamp": 0.5247567208043801 + }, + { + "x": 1.432507206361376, + "y": 3.740336413019291, + "heading": 0.5380390081031117, + "angularVelocity": -2.648788402241338, + "velocityX": -0.18293841995230437, + "velocityY": 1.944347479840561, + "timestamp": 0.565122622404717 + }, + { + "x": 1.4383712466830039, + "y": 3.8162277828812874, + "heading": 0.4256734213660632, + "angularVelocity": -2.7836758819258134, + "velocityX": 0.14527212546093396, + "velocityY": 1.8800860838783595, + "timestamp": 0.6054885240050539 + }, + { + "x": 1.4574240186735574, + "y": 3.888878324566731, + "heading": 0.3119775884105386, + "angularVelocity": -2.816630582941707, + "velocityX": 0.47200164582462106, + "velocityY": 1.7997998014452095, + "timestamp": 0.6458544256053909 + }, + { + "x": 1.4894506756188453, + "y": 3.9575592874387215, + "heading": 0.20081873031766603, + "angularVelocity": -2.753781129267399, + "velocityX": 0.7934086859345777, + "velocityY": 1.7014598992982128, + "timestamp": 0.6862203272057278 + }, + { + "x": 1.5341182594581817, + "y": 4.021562056122661, + "heading": 0.09575326080161849, + "angularVelocity": -2.6028272713019387, + "velocityX": 1.1065672279933332, + "velocityY": 1.585565196031799, + "timestamp": 0.7265862288060647 + }, + { + "x": 1.5910279750823977, + "y": 4.08027458190918, + "heading": -3.14981250987545e-26, + "angularVelocity": -2.372132344513748, + "velocityX": 1.4098462654861337, + "velocityY": 1.4545079747711904, + "timestamp": 0.7669521304064016 + }, + { + "x": 1.765089887743631, + "y": 4.178521763746107, + "heading": -0.13944160626704527, + "angularVelocity": -1.619681245622387, + "velocityX": 2.021812736254959, + "velocityY": 1.1411882157451263, + "timestamp": 0.8530441356351606 + }, + { + "x": 1.9880237520646193, + "y": 4.245061751353186, + "heading": -0.20350198247198673, + "angularVelocity": -0.7440920447226632, + "velocityX": 2.589483933248167, + "velocityY": 0.7728939223830609, + "timestamp": 0.9391361408639195 + }, + { + "x": 2.2451970133288186, + "y": 4.2634626429072915, + "heading": -0.20350197796884295, + "angularVelocity": 5.2306178076003237e-8, + "velocityX": 2.98719097761578, + "velocityY": 0.21373519533215735, + "timestamp": 1.0252281460926784 + }, + { + "x": 2.4993613384299262, + "y": 4.220136155857836, + "heading": -0.20350195456811618, + "angularVelocity": 2.718106834118565e-7, + "velocityX": 2.9522407385651688, + "velocityY": -0.5032579614603062, + "timestamp": 1.1113201513214372 + }, + { + "x": 2.732767913883175, + "y": 4.145066843585559, + "heading": -0.1654270851112631, + "angularVelocity": 0.4422579002043526, + "velocityX": 2.711129504221143, + "velocityY": -0.8719661259231624, + "timestamp": 1.197412156550196 + }, + { + "x": 2.9079624706558325, + "y": 4.088324361045792, + "heading": -0.09227635108510589, + "angularVelocity": 0.8496809178945844, + "velocityX": 2.034968941740179, + "velocityY": -0.6590911942286967, + "timestamp": 1.2835041617789549 + }, + { + "x": 3.0244537137790224, + "y": 4.050518775847017, + "heading": -0.03280274151338361, + "angularVelocity": 0.690814546759508, + "velocityX": 1.3531017521738042, + "velocityY": -0.4391300341805365, + "timestamp": 1.3695961670077137 + }, + { + "x": 3.082585573196411, + "y": 4.031636714935303, + "heading": 1.5144082138928652e-26, + "angularVelocity": 0.3810196013697437, + "velocityX": 0.6752294741297277, + "velocityY": -0.21932420857827498, + "timestamp": 1.4556881722364725 + }, + { + "x": 3.082585573196411, + "y": 4.031636714935303, + "heading": 7.518751599458815e-27, + "angularVelocity": -2.1309887488531084e-27, + "velocityX": 7.927389728497897e-27, + "velocityY": -1.297368142908954e-26, + "timestamp": 1.5417801774652313 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Amp 3 Piece 5.traj b/src/main/deploy/choreo/Amp 3 Piece 5.traj new file mode 100644 index 0000000..18e9bd5 --- /dev/null +++ b/src/main/deploy/choreo/Amp 3 Piece 5.traj @@ -0,0 +1,266 @@ +{ + "samples": [ + { + "x": 1.785578966140747, + "y": 3.026456832885742, + "heading": 2.2752903910371143, + "angularVelocity": -1.8954621059563845e-27, + "velocityX": 5.094997374536094e-27, + "velocityY": -1.555088100432084e-26, + "timestamp": 0 + }, + { + "x": 1.782637002969458, + "y": 3.030934879800897, + "heading": 2.2161498525084706, + "angularVelocity": -1.465111298991776, + "velocityX": -0.07288238475180157, + "velocityY": 0.110936377923418, + "timestamp": 0.04036590160033693 + }, + { + "x": 1.776854375639112, + "y": 3.0404392101200512, + "heading": 2.0992045300830253, + "angularVelocity": -2.8971314349254977, + "velocityX": -0.14325525012670193, + "velocityY": 0.2354544291678867, + "timestamp": 0.08073180320067386 + }, + { + "x": 1.768527296831389, + "y": 3.0558164340175304, + "heading": 1.9267427272668538, + "angularVelocity": -4.272462548309133, + "velocityX": -0.2062899248521585, + "velocityY": 0.38094587976082794, + "timestamp": 0.12109770480101079 + }, + { + "x": 1.7577422754879621, + "y": 3.0792567393920285, + "heading": 1.7078934840810325, + "angularVelocity": -5.421636443368693, + "velocityX": -0.26718148030506983, + "velocityY": 0.5806956972392262, + "timestamp": 0.16146360640134771 + }, + { + "x": 1.7418176124047977, + "y": 3.1145654563250176, + "heading": 1.5040219429296366, + "angularVelocity": -5.050588072327222, + "velocityX": -0.39450780118414347, + "velocityY": 0.8747164198778695, + "timestamp": 0.20182950800168464 + }, + { + "x": 1.7176852193747818, + "y": 3.157439084845946, + "heading": 1.3285362877975075, + "angularVelocity": -4.347373604326088, + "velocityX": -0.5978410508193482, + "velocityY": 1.0621248831605514, + "timestamp": 0.24219540960202157 + }, + { + "x": 1.684843526703858, + "y": 3.208127129747163, + "heading": 1.17927498125219, + "angularVelocity": -3.697707734194931, + "velocityX": -0.8135998793261088, + "velocityY": 1.2557144245923109, + "timestamp": 0.2825613112023585 + }, + { + "x": 1.6434576742777403, + "y": 3.26705565218992, + "heading": 1.0553990395550077, + "angularVelocity": -3.0688263307897565, + "velocityX": -1.025267633952027, + "velocityY": 1.459858942981347, + "timestamp": 0.32292721280269543 + }, + { + "x": 1.5943446932187455, + "y": 3.3348722400911304, + "heading": 0.9576696221034526, + "angularVelocity": -2.4210884329842246, + "velocityX": -1.2166947624572502, + "velocityY": 1.6800464058170483, + "timestamp": 0.36329311440303236 + }, + { + "x": 1.5389107935987831, + "y": 3.4123091818648263, + "heading": 0.8877110799809592, + "angularVelocity": -1.7331098612673965, + "velocityX": -1.3732853081993277, + "velocityY": 1.918375131079688, + "timestamp": 0.4036590160033693 + }, + { + "x": 1.4937133432666583, + "y": 3.4984312793538073, + "heading": 0.8250449654269514, + "angularVelocity": -1.5524517493617649, + "velocityX": -1.1196938143392696, + "velocityY": 2.133535832834275, + "timestamp": 0.4440249176037062 + }, + { + "x": 1.4604074486928154, + "y": 3.5812108846355466, + "heading": 0.7421469709224653, + "angularVelocity": -2.0536638900143904, + "velocityX": -0.8250997315408592, + "velocityY": 2.050730988281656, + "timestamp": 0.48439081920404314 + }, + { + "x": 1.4398916806200917, + "y": 3.6618510739711834, + "heading": 0.6449597401080992, + "angularVelocity": -2.4076566349642725, + "velocityX": -0.5082450102527284, + "velocityY": 1.9977304144982566, + "timestamp": 0.5247567208043801 + }, + { + "x": 1.432507206361376, + "y": 3.740336413019291, + "heading": 0.5380390081031117, + "angularVelocity": -2.648788402241338, + "velocityX": -0.18293841995230437, + "velocityY": 1.944347479840561, + "timestamp": 0.565122622404717 + }, + { + "x": 1.4383712466830039, + "y": 3.8162277828812874, + "heading": 0.4256734213660632, + "angularVelocity": -2.7836758819258134, + "velocityX": 0.14527212546093396, + "velocityY": 1.8800860838783595, + "timestamp": 0.6054885240050539 + }, + { + "x": 1.4574240186735574, + "y": 3.888878324566731, + "heading": 0.3119775884105386, + "angularVelocity": -2.816630582941707, + "velocityX": 0.47200164582462106, + "velocityY": 1.7997998014452095, + "timestamp": 0.6458544256053909 + }, + { + "x": 1.4894506756188453, + "y": 3.9575592874387215, + "heading": 0.20081873031766603, + "angularVelocity": -2.753781129267399, + "velocityX": 0.7934086859345777, + "velocityY": 1.7014598992982128, + "timestamp": 0.6862203272057278 + }, + { + "x": 1.5341182594581817, + "y": 4.021562056122661, + "heading": 0.09575326080161849, + "angularVelocity": -2.6028272713019387, + "velocityX": 1.1065672279933332, + "velocityY": 1.585565196031799, + "timestamp": 0.7265862288060647 + }, + { + "x": 1.5910279750823977, + "y": 4.08027458190918, + "heading": -3.14981250987545e-26, + "angularVelocity": -2.372132344513748, + "velocityX": 1.4098462654861337, + "velocityY": 1.4545079747711904, + "timestamp": 0.7669521304064016 + }, + { + "x": 1.765089887743631, + "y": 4.178521763746107, + "heading": -0.13944160626704527, + "angularVelocity": -1.619681245622387, + "velocityX": 2.021812736254959, + "velocityY": 1.1411882157451263, + "timestamp": 0.8530441356351606 + }, + { + "x": 1.9880237520646193, + "y": 4.245061751353186, + "heading": -0.20350198247198673, + "angularVelocity": -0.7440920447226632, + "velocityX": 2.589483933248167, + "velocityY": 0.7728939223830609, + "timestamp": 0.9391361408639195 + }, + { + "x": 2.2451970133288186, + "y": 4.2634626429072915, + "heading": -0.20350197796884295, + "angularVelocity": 5.2306178076003237e-8, + "velocityX": 2.98719097761578, + "velocityY": 0.21373519533215735, + "timestamp": 1.0252281460926784 + }, + { + "x": 2.4993613384299262, + "y": 4.220136155857836, + "heading": -0.20350195456811618, + "angularVelocity": 2.718106834118565e-7, + "velocityX": 2.9522407385651688, + "velocityY": -0.5032579614603062, + "timestamp": 1.1113201513214372 + }, + { + "x": 2.732767913883175, + "y": 4.145066843585559, + "heading": -0.1654270851112631, + "angularVelocity": 0.4422579002043526, + "velocityX": 2.711129504221143, + "velocityY": -0.8719661259231624, + "timestamp": 1.197412156550196 + }, + { + "x": 2.9079624706558325, + "y": 4.088324361045792, + "heading": -0.09227635108510589, + "angularVelocity": 0.8496809178945844, + "velocityX": 2.034968941740179, + "velocityY": -0.6590911942286967, + "timestamp": 1.2835041617789549 + }, + { + "x": 3.0244537137790224, + "y": 4.050518775847017, + "heading": -0.03280274151338361, + "angularVelocity": 0.690814546759508, + "velocityX": 1.3531017521738042, + "velocityY": -0.4391300341805365, + "timestamp": 1.3695961670077137 + }, + { + "x": 3.082585573196411, + "y": 4.031636714935303, + "heading": 1.5144082138928652e-26, + "angularVelocity": 0.3810196013697437, + "velocityX": 0.6752294741297277, + "velocityY": -0.21932420857827498, + "timestamp": 1.4556881722364725 + }, + { + "x": 3.082585573196411, + "y": 4.031636714935303, + "heading": 7.518751599458815e-27, + "angularVelocity": -2.1309887488531084e-27, + "velocityX": 7.927389728497897e-27, + "velocityY": -1.297368142908954e-26, + "timestamp": 1.5417801774652313 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Amp 3 Piece 6.1.traj b/src/main/deploy/choreo/Amp 3 Piece 6.1.traj new file mode 100644 index 0000000..d220e69 --- /dev/null +++ b/src/main/deploy/choreo/Amp 3 Piece 6.1.traj @@ -0,0 +1,185 @@ +{ + "samples": [ + { + "x": 2.57999587059021, + "y": 4.08027458190918, + "heading": 1.0971166550765098e-23, + "angularVelocity": 1.1911120416893396e-24, + "velocityX": 1.392755277940113e-23, + "velocityY": 2.3707203164248613e-24, + "timestamp": 0 + }, + { + "x": 2.579662827473817, + "y": 4.073976766895919, + "heading": 0.05900902390512706, + "angularVelocity": 1.4321204231514015, + "velocityX": -0.00808279509153697, + "velocityY": -0.15284491938419753, + "timestamp": 0.0412039539072258 + }, + { + "x": 2.579370687354804, + "y": 4.061287185375009, + "heading": 0.17643622955934568, + "angularVelocity": 2.84990139341035, + "velocityX": -0.0070900991606351405, + "velocityY": -0.3079699960222945, + "timestamp": 0.0824079078144516 + }, + { + "x": 2.579579590471969, + "y": 4.042200777159572, + "heading": 0.3514246001504735, + "angularVelocity": 4.246882980820942, + "velocityX": 0.005069977450094339, + "velocityY": -0.4632178809444451, + "timestamp": 0.1236118617216774 + }, + { + "x": 2.5750606099681796, + "y": 4.01194105258541, + "heading": 0.5521013175478051, + "angularVelocity": 4.870326713042449, + "velocityX": -0.10967346759885296, + "velocityY": -0.7343888560377853, + "timestamp": 0.1648158156289032 + }, + { + "x": 2.56423762146791, + "y": 3.9690933804161306, + "heading": 0.747472147114967, + "angularVelocity": 4.741555385850978, + "velocityX": -0.2626686876856264, + "velocityY": -1.039892245917828, + "timestamp": 0.20601976953612902 + }, + { + "x": 2.5467043297801593, + "y": 3.9154287676491246, + "heading": 0.9189990053029754, + "angularVelocity": 4.162873751733042, + "velocityX": -0.4255244952275232, + "velocityY": -1.3024141539386382, + "timestamp": 0.24722372344335483 + }, + { + "x": 2.523298157292756, + "y": 3.8503922530083323, + "heading": 1.0672567739768406, + "angularVelocity": 3.5981442219763733, + "velocityX": -0.5680564671075964, + "velocityY": -1.5784047032774475, + "timestamp": 0.2884276773505806 + }, + { + "x": 2.4960522044580586, + "y": 3.773392408577416, + "heading": 1.1901950139788295, + "angularVelocity": 2.9836515272003945, + "velocityX": -0.6612460759480528, + "velocityY": -1.8687489216274693, + "timestamp": 0.3296316312578064 + }, + { + "x": 2.469923707904784, + "y": 3.684349647681216, + "heading": 1.2853707724318124, + "angularVelocity": 2.3098695495893162, + "velocityX": -0.634125953351582, + "velocityY": -2.1610246700277287, + "timestamp": 0.37083558516503223 + }, + { + "x": 2.4521201557482595, + "y": 3.5926952586845426, + "heading": 1.3789369657166912, + "angularVelocity": 2.2708061827161288, + "velocityX": -0.43208358587652174, + "velocityY": -2.2244076188183466, + "timestamp": 0.41203953907225804 + }, + { + "x": 2.4455284878881836, + "y": 3.507277328616976, + "heading": 1.4937827841922569, + "angularVelocity": 2.7872523771420314, + "velocityX": -0.15997658561888023, + "velocityY": -2.0730517818724965, + "timestamp": 0.45324349297948385 + }, + { + "x": 2.4410469344735444, + "y": 3.434231880209267, + "heading": 1.6336884732647516, + "angularVelocity": 3.3954432962308463, + "velocityX": -0.1087651302768112, + "velocityY": -1.772777645858383, + "timestamp": 0.49444744688670966 + }, + { + "x": 2.435553400376216, + "y": 3.374006755150976, + "heading": 1.7961672522867937, + "angularVelocity": 3.943281253733004, + "velocityX": -0.13332541118985913, + "velocityY": -1.4616346089963215, + "timestamp": 0.5356514007939355 + }, + { + "x": 2.427518633441147, + "y": 3.3263479617338527, + "heading": 1.9814413179082384, + "angularVelocity": 4.4965118162835775, + "velocityX": -0.19499990105705994, + "velocityY": -1.1566558278467975, + "timestamp": 0.5768553547011612 + }, + { + "x": 2.418456777536987, + "y": 3.292689475234703, + "heading": 2.160196350293327, + "angularVelocity": 4.3382980377943925, + "velocityX": -0.21992685276183732, + "velocityY": -0.8168751614210406, + "timestamp": 0.618059308608387 + }, + { + "x": 2.40957775522149, + "y": 3.271735169772442, + "heading": 2.3172806828535286, + "angularVelocity": 3.8123606514532646, + "velocityX": -0.21548957013905015, + "velocityY": -0.5085508422187158, + "timestamp": 0.6592632625156127 + }, + { + "x": 2.404195229874686, + "y": 3.2593131748644586, + "heading": 2.4299841661174084, + "angularVelocity": 2.735258939412498, + "velocityX": -0.13063128259300513, + "velocityY": -0.30147579855934886, + "timestamp": 0.7004672164228385 + }, + { + "x": 2.4016571044921875, + "y": 3.2534329891204834, + "heading": 2.487823343289701, + "angularVelocity": 1.4037288096798257, + "velocityX": -0.06159907343390637, + "velocityY": -0.14270925933989237, + "timestamp": 0.7416711703300642 + }, + { + "x": 2.4016571044921875, + "y": 3.2534329891204834, + "heading": 2.487823343289701, + "angularVelocity": -7.318967086218513e-25, + "velocityX": 1.3814485584668107e-23, + "velocityY": -6.25634806032e-24, + "timestamp": 0.78287512423729 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Amp 3 Piece 6.traj b/src/main/deploy/choreo/Amp 3 Piece 6.traj new file mode 100644 index 0000000..d220e69 --- /dev/null +++ b/src/main/deploy/choreo/Amp 3 Piece 6.traj @@ -0,0 +1,185 @@ +{ + "samples": [ + { + "x": 2.57999587059021, + "y": 4.08027458190918, + "heading": 1.0971166550765098e-23, + "angularVelocity": 1.1911120416893396e-24, + "velocityX": 1.392755277940113e-23, + "velocityY": 2.3707203164248613e-24, + "timestamp": 0 + }, + { + "x": 2.579662827473817, + "y": 4.073976766895919, + "heading": 0.05900902390512706, + "angularVelocity": 1.4321204231514015, + "velocityX": -0.00808279509153697, + "velocityY": -0.15284491938419753, + "timestamp": 0.0412039539072258 + }, + { + "x": 2.579370687354804, + "y": 4.061287185375009, + "heading": 0.17643622955934568, + "angularVelocity": 2.84990139341035, + "velocityX": -0.0070900991606351405, + "velocityY": -0.3079699960222945, + "timestamp": 0.0824079078144516 + }, + { + "x": 2.579579590471969, + "y": 4.042200777159572, + "heading": 0.3514246001504735, + "angularVelocity": 4.246882980820942, + "velocityX": 0.005069977450094339, + "velocityY": -0.4632178809444451, + "timestamp": 0.1236118617216774 + }, + { + "x": 2.5750606099681796, + "y": 4.01194105258541, + "heading": 0.5521013175478051, + "angularVelocity": 4.870326713042449, + "velocityX": -0.10967346759885296, + "velocityY": -0.7343888560377853, + "timestamp": 0.1648158156289032 + }, + { + "x": 2.56423762146791, + "y": 3.9690933804161306, + "heading": 0.747472147114967, + "angularVelocity": 4.741555385850978, + "velocityX": -0.2626686876856264, + "velocityY": -1.039892245917828, + "timestamp": 0.20601976953612902 + }, + { + "x": 2.5467043297801593, + "y": 3.9154287676491246, + "heading": 0.9189990053029754, + "angularVelocity": 4.162873751733042, + "velocityX": -0.4255244952275232, + "velocityY": -1.3024141539386382, + "timestamp": 0.24722372344335483 + }, + { + "x": 2.523298157292756, + "y": 3.8503922530083323, + "heading": 1.0672567739768406, + "angularVelocity": 3.5981442219763733, + "velocityX": -0.5680564671075964, + "velocityY": -1.5784047032774475, + "timestamp": 0.2884276773505806 + }, + { + "x": 2.4960522044580586, + "y": 3.773392408577416, + "heading": 1.1901950139788295, + "angularVelocity": 2.9836515272003945, + "velocityX": -0.6612460759480528, + "velocityY": -1.8687489216274693, + "timestamp": 0.3296316312578064 + }, + { + "x": 2.469923707904784, + "y": 3.684349647681216, + "heading": 1.2853707724318124, + "angularVelocity": 2.3098695495893162, + "velocityX": -0.634125953351582, + "velocityY": -2.1610246700277287, + "timestamp": 0.37083558516503223 + }, + { + "x": 2.4521201557482595, + "y": 3.5926952586845426, + "heading": 1.3789369657166912, + "angularVelocity": 2.2708061827161288, + "velocityX": -0.43208358587652174, + "velocityY": -2.2244076188183466, + "timestamp": 0.41203953907225804 + }, + { + "x": 2.4455284878881836, + "y": 3.507277328616976, + "heading": 1.4937827841922569, + "angularVelocity": 2.7872523771420314, + "velocityX": -0.15997658561888023, + "velocityY": -2.0730517818724965, + "timestamp": 0.45324349297948385 + }, + { + "x": 2.4410469344735444, + "y": 3.434231880209267, + "heading": 1.6336884732647516, + "angularVelocity": 3.3954432962308463, + "velocityX": -0.1087651302768112, + "velocityY": -1.772777645858383, + "timestamp": 0.49444744688670966 + }, + { + "x": 2.435553400376216, + "y": 3.374006755150976, + "heading": 1.7961672522867937, + "angularVelocity": 3.943281253733004, + "velocityX": -0.13332541118985913, + "velocityY": -1.4616346089963215, + "timestamp": 0.5356514007939355 + }, + { + "x": 2.427518633441147, + "y": 3.3263479617338527, + "heading": 1.9814413179082384, + "angularVelocity": 4.4965118162835775, + "velocityX": -0.19499990105705994, + "velocityY": -1.1566558278467975, + "timestamp": 0.5768553547011612 + }, + { + "x": 2.418456777536987, + "y": 3.292689475234703, + "heading": 2.160196350293327, + "angularVelocity": 4.3382980377943925, + "velocityX": -0.21992685276183732, + "velocityY": -0.8168751614210406, + "timestamp": 0.618059308608387 + }, + { + "x": 2.40957775522149, + "y": 3.271735169772442, + "heading": 2.3172806828535286, + "angularVelocity": 3.8123606514532646, + "velocityX": -0.21548957013905015, + "velocityY": -0.5085508422187158, + "timestamp": 0.6592632625156127 + }, + { + "x": 2.404195229874686, + "y": 3.2593131748644586, + "heading": 2.4299841661174084, + "angularVelocity": 2.735258939412498, + "velocityX": -0.13063128259300513, + "velocityY": -0.30147579855934886, + "timestamp": 0.7004672164228385 + }, + { + "x": 2.4016571044921875, + "y": 3.2534329891204834, + "heading": 2.487823343289701, + "angularVelocity": 1.4037288096798257, + "velocityX": -0.06159907343390637, + "velocityY": -0.14270925933989237, + "timestamp": 0.7416711703300642 + }, + { + "x": 2.4016571044921875, + "y": 3.2534329891204834, + "heading": 2.487823343289701, + "angularVelocity": -7.318967086218513e-25, + "velocityX": 1.3814485584668107e-23, + "velocityY": -6.25634806032e-24, + "timestamp": 0.78287512423729 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Choreo 2 Piece 2.1.traj b/src/main/deploy/choreo/Choreo 2 Piece 2.1.traj index 9f57e3c..6c58d99 100644 --- a/src/main/deploy/choreo/Choreo 2 Piece 2.1.traj +++ b/src/main/deploy/choreo/Choreo 2 Piece 2.1.traj @@ -4,415 +4,487 @@ "x": 2.578, "y": 3.434, "heading": 2.456, - "angularVelocity": 1.0219812008109311e-25, - "velocityX": -7.726426952121863e-25, - "velocityY": -8.236726072553901e-26, + "angularVelocity": -2.2243193051899215e-21, + "velocityX": 4.420947543039026e-20, + "velocityY": 1.7808811974639867e-21, "timestamp": 0 }, { - "x": 2.584286650145692, - "y": 3.421951310408596, - "heading": 2.4350954049954208, - "angularVelocity": -0.3464845678617885, - "velocityX": 0.10419849121933693, - "velocityY": -0.1997017883132429, - "timestamp": 0.06033340859474565 - }, - { - "x": 2.5967989346479854, - "y": 3.3977100915163163, - "heading": 2.3938477021537534, - "angularVelocity": -0.6836627301919046, - "velocityX": 0.2073856722788015, - "velocityY": -0.401787657234916, - "timestamp": 0.1206668171894913 - }, - { - "x": 2.615462449887643, - "y": 3.361117228577582, - "heading": 2.332876835959309, - "angularVelocity": -1.0105655823953221, - "velocityX": 0.3093396457179988, - "velocityY": -0.6065107838432474, - "timestamp": 0.18100022578423694 - }, - { - "x": 2.640198059989427, - "y": 3.3119715556696296, - "heading": 2.2531378381258196, - "angularVelocity": -1.3216391994208312, - "velocityX": 0.4099819764524013, - "velocityY": -0.8145681481061332, - "timestamp": 0.2413336343789826 - }, - { - "x": 2.6709458792631127, - "y": 3.2500095590805107, - "heading": 2.1562840802735845, - "angularVelocity": -1.605308901122988, - "velocityX": 0.5096317279240128, - "velocityY": -1.026993137505499, - "timestamp": 0.30166704297372826 - }, - { - "x": 2.7077008608268662, - "y": 3.1749016535376815, - "heading": 2.045096498973365, - "angularVelocity": -1.8428857889839654, - "velocityX": 0.6091978295248277, - "velocityY": -1.2448808594144314, - "timestamp": 0.36200045156847394 - }, - { - "x": 2.7505245327527716, - "y": 3.0862787536737564, - "heading": 1.9239140602650735, - "angularVelocity": -2.0085462023580276, - "velocityX": 0.7097837321532509, - "velocityY": -1.4688860107208206, - "timestamp": 0.4223338601632196 - }, - { - "x": 2.7994482297249395, - "y": 2.9838047667261183, - "heading": 1.7994067052659932, - "angularVelocity": -2.0636552434056146, - "velocityX": 0.8108889935389533, - "velocityY": -1.6984617533537163, - "timestamp": 0.4826672687579653 - }, - { - "x": 2.8540407096263034, - "y": 2.867709288903639, - "heading": 1.6856485603266385, - "angularVelocity": -1.885491763004119, - "velocityX": 0.9048466044419411, - "velocityY": -1.924232038708154, - "timestamp": 0.543000677352711 - }, - { - "x": 2.9119156128261765, - "y": 2.7403356565757657, - "heading": 1.604080673099594, - "angularVelocity": -1.3519522454786057, - "velocityX": 0.95925134262865, - "velocityY": -2.1111625431845082, - "timestamp": 0.6033340859474566 - }, - { - "x": 2.9732896898216743, - "y": 2.6016323913890385, - "heading": 1.553622517502379, - "angularVelocity": -0.8363219776979306, - "velocityX": 1.0172486260098852, - "velocityY": -2.298946278974311, - "timestamp": 0.6636674945422023 - }, - { - "x": 3.038399585143699, - "y": 2.4516460837393157, - "heading": 1.5337765355615411, - "angularVelocity": -0.32893851686951064, - "velocityX": 1.0791681895409253, - "velocityY": -2.4859577992214517, - "timestamp": 0.724000903136948 - }, - { - "x": 3.1118123187513027, - "y": 2.2970091829036123, - "heading": 1.5337764978118669, - "angularVelocity": -6.256844289379484e-7, - "velocityX": 1.2167841220560034, - "velocityY": -2.5630393580841155, - "timestamp": 0.7843343117316937 - }, - { - "x": 3.1852252486361183, - "y": 2.1423723752490966, - "heading": 1.5337764600625183, - "angularVelocity": -6.256790302738932e-7, - "velocityX": 1.216787375265398, - "velocityY": -2.563037813646468, - "timestamp": 0.8446677203264393 - }, - { - "x": 3.2586383526710114, - "y": 1.9877356502714374, - "heading": 1.5337764223131956, - "angularVelocity": -6.256786018158767e-7, - "velocityX": 1.2167902617271742, - "velocityY": -2.5630364433135364, - "timestamp": 0.905001128921185 - }, - { - "x": 3.339262039402322, - "y": 1.8367329293476955, - "heading": 1.5337763853100257, - "angularVelocity": -6.133114449160005e-7, - "velocityX": 1.3363025330269804, - "velocityY": -2.5028044070576922, - "timestamp": 0.9653345375159307 - }, - { - "x": 3.432942751765767, - "y": 1.6934642146806478, - "heading": 1.533776348204764, - "angularVelocity": -6.150035711868799e-7, - "velocityX": 1.5527170525486216, - "velocityY": -2.3746166179565877, - "timestamp": 1.0256679461106764 - }, - { - "x": 3.5389443424843976, - "y": 1.559055647892523, - "heading": 1.533776308684474, - "angularVelocity": -6.550316132036273e-7, - "velocityX": 1.7569302512084846, - "velocityY": -2.2277635213839058, - "timestamp": 1.086001354705422 - }, - { - "x": 3.6564336903688766, - "y": 1.4345636707390452, - "heading": 1.533776263619502, - "angularVelocity": -7.469323051999795e-7, - "velocityX": 1.9473348285962937, - "velocityY": -2.063400362304737, - "timestamp": 1.1463347633001675 - }, - { - "x": 3.7844873612800467, - "y": 1.3209667734788668, - "heading": 1.533776207514778, - "angularVelocity": -9.29911391627009e-7, - "velocityX": 2.1224338868585284, - "velocityY": -1.8828191528709874, - "timestamp": 1.206668171894913 + "x": 2.5808143020901366, + "y": 3.4305511164394358, + "heading": 2.421420914475003, + "angularVelocity": -0.7062096950549269, + "velocityX": 0.05747657552803998, + "velocityY": -0.07043665182601658, + "timestamp": 0.04896433136946255 + }, + { + "x": 2.5865263986145903, + "y": 3.423301716588506, + "heading": 2.353190948627095, + "angularVelocity": -1.3934626275824327, + "velocityX": 0.11665831769034016, + "velocityY": -0.14805470938110385, + "timestamp": 0.0979286627389251 + }, + { + "x": 2.5951072938917, + "y": 3.411803141318997, + "heading": 2.252339287405858, + "angularVelocity": -2.059696485187491, + "velocityX": 0.1752478801857445, + "velocityY": -0.23483574569237411, + "timestamp": 0.14689299410838766 + }, + { + "x": 2.6063817008248584, + "y": 3.395552329101315, + "heading": 2.1199335438440907, + "angularVelocity": -2.704126449980364, + "velocityX": 0.23025754907213325, + "velocityY": -0.3318908226296179, + "timestamp": 0.1958573254778502 + }, + { + "x": 2.6200490589295313, + "y": 3.3739990762642234, + "heading": 1.957267690055127, + "angularVelocity": -3.322129583707805, + "velocityX": 0.2791288622216491, + "velocityY": -0.44018272555292703, + "timestamp": 0.24482165684731275 + }, + { + "x": 2.6358077196258662, + "y": 3.346424151831386, + "heading": 1.7667703729621236, + "angularVelocity": -3.890532388885235, + "velocityX": 0.3218395974292316, + "velocityY": -0.5631635041587543, + "timestamp": 0.2937859882167753 + }, + { + "x": 2.6542369913783963, + "y": 3.311395651079251, + "heading": 1.5565531821810574, + "angularVelocity": -4.293271957394105, + "velocityX": 0.37638156668515743, + "velocityY": -0.7153881156437778, + "timestamp": 0.34275031958623786 + }, + { + "x": 2.677454625002408, + "y": 3.268157202317915, + "heading": 1.3372511935604507, + "angularVelocity": -4.478811054640037, + "velocityX": 0.4741744240049492, + "velocityY": -0.8830601287108768, + "timestamp": 0.3917146509557004 + }, + { + "x": 2.706331728536566, + "y": 3.2188789886399363, + "heading": 1.1357091046403869, + "angularVelocity": -4.116100093337721, + "velocityX": 0.5897579467859876, + "velocityY": -1.0064104277491477, + "timestamp": 0.44067898232516295 + }, + { + "x": 2.7406445911847674, + "y": 3.1628572349258164, + "heading": 0.9501369190074549, + "angularVelocity": -3.7899462821760768, + "velocityX": 0.7007726173006585, + "velocityY": -1.1441339470436553, + "timestamp": 0.4896433136946255 + }, + { + "x": 2.77961503012768, + "y": 3.099262425694046, + "heading": 0.7791196669307836, + "angularVelocity": -3.4926904400318053, + "velocityX": 0.7958944368897106, + "velocityY": -1.2987986857600298, + "timestamp": 0.5386076450640881 + }, + { + "x": 2.821598359020251, + "y": 3.0272467328347097, + "heading": 0.6221812057707179, + "angularVelocity": -3.2051588732189473, + "velocityX": 0.8574267781913745, + "velocityY": -1.4707786432530845, + "timestamp": 0.5875719764335506 + }, + { + "x": 2.8636245721731495, + "y": 2.9463007656727807, + "heading": 0.4788804629728746, + "angularVelocity": -2.926635344339965, + "velocityX": 0.8583026047225649, + "velocityY": -1.6531619016942172, + "timestamp": 0.6365363078030132 + }, + { + "x": 2.9010385562385563, + "y": 2.8582501044070647, + "heading": 0.35102943849270457, + "angularVelocity": -2.6111052863249458, + "velocityX": 0.7641069125006744, + "velocityY": -1.7982612812846699, + "timestamp": 0.6855006391724757 + }, + { + "x": 2.9381959956467134, + "y": 2.7627981012239995, + "heading": 0.24579862310441236, + "angularVelocity": -2.1491320813567003, + "velocityX": 0.7588674933153383, + "velocityY": -1.949419108019954, + "timestamp": 0.7344649705419383 + }, + { + "x": 2.9765902696547557, + "y": 2.659953900667316, + "heading": 0.16360236786662388, + "angularVelocity": -1.678696572359438, + "velocityX": 0.7841274032359897, + "velocityY": -2.1003901754668526, + "timestamp": 0.7834293019114008 + }, + { + "x": 3.017865715538054, + "y": 2.5502233979386646, + "heading": 0.10367998080393974, + "angularVelocity": -1.2237966982646422, + "velocityX": 0.8429696623825639, + "velocityY": -2.241029330119635, + "timestamp": 0.8323936332808634 + }, + { + "x": 3.063560421246788, + "y": 2.434583512531678, + "heading": 0.0641258476586635, + "angularVelocity": -0.8078152409928518, + "velocityX": 0.9332243376096464, + "velocityY": -2.3617168288158923, + "timestamp": 0.8813579646503259 + }, + { + "x": 3.115401381748359, + "y": 2.3142782546228218, + "heading": 0.04299697728371769, + "angularVelocity": -0.4315155498707209, + "velocityX": 1.0587494825651373, + "velocityY": -2.456997870573845, + "timestamp": 0.9303222960197884 + }, + { + "x": 3.173044544543568, + "y": 2.188964351893496, + "heading": 0.04038455549354976, + "angularVelocity": -0.0533535681403628, + "velocityX": 1.1772480330663169, + "velocityY": -2.559289573134488, + "timestamp": 0.979286627389251 + }, + { + "x": 3.232092719253376, + "y": 2.0632166415863358, + "heading": 0.04038346790054724, + "angularVelocity": -0.000022211944329746854, + "velocityX": 1.2059426333065189, + "velocityY": -2.568149238234756, + "timestamp": 1.0282509587587136 + }, + { + "x": 3.2911425700668717, + "y": 1.9374697182380494, + "heading": 0.04038238058608858, + "angularVelocity": -0.000022206255619953, + "velocityX": 1.2059768644246023, + "velocityY": -2.568133166149746, + "timestamp": 1.0772152901281762 + }, + { + "x": 3.3583393467395415, + "y": 1.8158809793568123, + "heading": 0.0403818264403618, + "angularVelocity": -0.000011317334706426395, + "velocityX": 1.3723617742393555, + "velocityY": -2.483210440755192, + "timestamp": 1.1261796214976387 + }, + { + "x": 3.4341046175380407, + "y": 1.6994385677916184, + "heading": 0.040381131756193525, + "angularVelocity": -0.000014187555488713555, + "velocityX": 1.5473563853405372, + "velocityY": -2.3781068444822635, + "timestamp": 1.1751439528671013 + }, + { + "x": 3.5178049582052053, + "y": 1.5890648238390772, + "heading": 0.039385078873634086, + "angularVelocity": -0.020342417729421827, + "velocityX": 1.7094145539434111, + "velocityY": -2.2541662648206686, + "timestamp": 1.2241082842365638 + }, + { + "x": 3.608774844587042, + "y": 1.4855271513306618, + "heading": 0.036685479739839225, + "angularVelocity": -0.05513399362946303, + "velocityX": 1.8578807029022373, + "velocityY": -2.1145529738200697, + "timestamp": 1.2730726156060264 + }, + { + "x": 3.7066552250984595, + "y": 1.3891998671498478, + "heading": 0.032788252293559854, + "angularVelocity": -0.07959319237656216, + "velocityX": 1.9990139306283103, + "velocityY": -1.9672949979443013, + "timestamp": 1.322036946975489 + }, + { + "x": 3.8111775959241823, + "y": 1.300340998111743, + "heading": 0.028541553024262294, + "angularVelocity": -0.0867304658416327, + "velocityX": 2.1346634969251537, + "velocityY": -1.8147673327263893, + "timestamp": 1.3710012783449514 }, { "x": 3.922098875045776, "y": 1.2191578149795532, - "heading": 1.5337761150741231, - "angularVelocity": -0.000001532163640308562, - "velocityX": 2.2808509741270218, - "velocityY": -1.6874391961368493, - "timestamp": 1.2670015804896586 - }, - { - "x": 4.133215899282255, - "y": 1.100654492548557, - "heading": 1.5337761164554422, - "angularVelocity": 1.6187741152567537e-8, - "velocityX": 2.474089843952036, - "velocityY": -1.3887457326638517, - "timestamp": 1.3523327679372603 - }, - { - "x": 4.357502974678636, - "y": 1.0095021810718636, - "heading": 1.533776117552903, - "angularVelocity": 1.2861193573885216e-8, - "velocityX": 2.628430262195824, - "velocityY": -1.0682180127009988, - "timestamp": 1.437663955384862 - }, - { - "x": 4.59143382498898, - "y": 0.9471339698744419, - "heading": 1.5337761185021443, - "angularVelocity": 1.1124200677480839e-8, - "velocityX": 2.7414460914890086, - "velocityY": -0.730895854879782, - "timestamp": 1.5229951428324637 - }, - { - "x": 4.831330550647783, - "y": 0.9145303241420727, - "heading": 1.5337761193835278, - "angularVelocity": 1.0328972095127946e-8, - "velocityX": 2.811360451372056, - "velocityY": -0.38208358171963364, - "timestamp": 1.6083263302800654 - }, - { - "x": 5.0730890918673595, - "y": 0.9016367408729034, - "heading": 1.5337761202500422, - "angularVelocity": 1.0154720062447253e-8, - "velocityX": 2.8331791511518682, - "velocityY": -0.1511004786741872, - "timestamp": 1.6936575177276671 - }, - { - "x": 5.314847643606767, - "y": 0.8887433548552026, - "heading": 1.5337761211165564, - "angularVelocity": 1.0154719461608446e-8, - "velocityX": 2.833179274434229, - "velocityY": -0.1510981670754085, - "timestamp": 1.7789887051752689 - }, - { - "x": 5.556606195346236, - "y": 0.8758499688386339, - "heading": 1.5337761219830706, - "angularVelocity": 1.0154719791329154e-8, - "velocityX": 2.8331792744349373, - "velocityY": -0.15109816706214324, - "timestamp": 1.8643198926228706 - }, - { - "x": 5.798364747085703, - "y": 0.862956582822065, - "heading": 1.533776122849585, - "angularVelocity": 1.0154719990167722e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 1.9496510800704723 - }, - { - "x": 6.040123298825171, - "y": 0.8500631968054962, - "heading": 1.5337761237160994, - "angularVelocity": 1.0154719824488932e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.0349822675180738 - }, - { - "x": 6.281881850564639, - "y": 0.8371698107889274, - "heading": 1.5337761245826136, - "angularVelocity": 1.0154719065955154e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.1203134549656752 - }, - { - "x": 6.523640402304108, - "y": 0.8242764247723586, - "heading": 1.533776125449128, - "angularVelocity": 1.015472011167056e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.2056446424132767 - }, - { - "x": 6.765398954043575, - "y": 0.8113830387557898, - "heading": 1.5337761263156422, - "angularVelocity": 1.0154720167050709e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.1510981670621432, - "timestamp": 2.290975829860878 - }, - { - "x": 7.007157505783043, - "y": 0.798489652739221, - "heading": 1.5337761271821566, - "angularVelocity": 1.0154720142992157e-8, - "velocityX": 2.8331792744349364, - "velocityY": -0.15109816706214316, - "timestamp": 2.3763070173084797 - }, - { - "x": 7.248916057522512, - "y": 0.7855962667226521, - "heading": 1.533776128048671, - "angularVelocity": 1.0154720015095924e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.461638204756081 - }, - { - "x": 7.490674609261979, - "y": 0.7727028807060851, - "heading": 1.5337761289151852, - "angularVelocity": 1.015471927674143e-8, - "velocityX": 2.8331792744349378, - "velocityY": -0.15109816706212442, - "timestamp": 2.5469693922036827 - }, - { - "x": 7.7324331610160755, - "y": 0.7598094949690721, - "heading": 1.5337761297818946, - "angularVelocity": 1.0157005131011138e-8, - "velocityX": 2.833179274606365, - "velocityY": -0.15109816378601534, - "timestamp": 2.632300579651284 - }, - { - "x": 7.944587343206802, - "y": 0.7484949682004244, - "heading": 1.546215447760533, - "angularVelocity": 0.1457769234288085, - "velocityX": 2.486244344378793, - "velocityY": -0.1325954449607942, - "timestamp": 2.7176317670988857 - }, - { - "x": 8.12643379054156, - "y": 0.738796802526807, - "heading": 1.5568779338247372, - "angularVelocity": 0.12495415079922173, - "velocityX": 2.131066644846836, - "velocityY": -0.11365323703684094, - "timestamp": 2.802962954546487 - }, - { - "x": 8.277972498710973, - "y": 0.7307149979607698, - "heading": 1.5657634145409067, - "angularVelocity": 0.10412934569351617, - "velocityX": 1.7758888948131386, - "velocityY": -0.09471102896581571, - "timestamp": 2.8882941419940886 - }, - { - "x": 8.399203466259046, - "y": 0.7242495544154388, - "heading": 1.5728718351258109, - "angularVelocity": 0.08330389858068031, - "velocityX": 1.420711127716517, - "velocityY": -0.07576882191286877, - "timestamp": 2.97362532944169 - }, - { - "x": 8.490126692448452, - "y": 0.719400471808881, - "heading": 1.5782031697100374, - "angularVelocity": 0.06247814830304947, - "velocityX": 1.0655333519791725, - "velocityY": -0.05682661582010127, - "timestamp": 3.0589565168892916 - }, - { - "x": 8.550742176835362, - "y": 0.7161677500844638, - "heading": 1.5817574030517292, - "angularVelocity": 0.04165221940541114, - "velocityX": 0.7103555710405419, - "velocityY": -0.03788441039101151, - "timestamp": 3.144287704336893 + "heading": 0.02492521296459199, + "angularVelocity": -0.07385662090191833, + "velocityX": 2.2653485919093206, + "velocityY": -1.6580065705303566, + "timestamp": 1.419965609714414 + }, + { + "x": 4.133754076857049, + "y": 1.1017463293287075, + "heading": 0.024925206123710186, + "angularVelocity": -8.018916008040494e-8, + "velocityX": 2.481032906181765, + "velocityY": -1.3763033318839852, + "timestamp": 1.505274918302749 + }, + { + "x": 4.358435567651369, + "y": 1.011737381091072, + "heading": 0.024925205866123235, + "angularVelocity": -3.0194471762044385e-9, + "velocityX": 2.6337277198966884, + "velocityY": -1.0550894120123064, + "timestamp": 1.590584226891084 + }, + { + "x": 4.5926126795284254, + "y": 0.9505453575452277, + "heading": 0.024925205642497325, + "angularVelocity": -2.6213541510789247e-9, + "velocityX": 2.745035867153646, + "velocityY": -0.7172959734217197, + "timestamp": 1.6758935354794189 + }, + { + "x": 4.832605528624095, + "y": 0.9191317291658319, + "heading": 0.024925205434181426, + "angularVelocity": -2.44188946073925e-9, + "velocityX": 2.813208230930211, + "velocityY": -0.3682321296375535, + "timestamp": 1.7612028440677538 + }, + { + "x": 5.074285887787392, + "y": 0.9059412894376224, + "heading": 0.024925205229021138, + "angularVelocity": -2.404899218132483e-9, + "velocityX": 2.8329893086994424, + "velocityY": -0.154618996994628, + "timestamp": 1.8465121526560888 + }, + { + "x": 5.315966255946622, + "y": 0.892751014537456, + "heading": 0.024925205023860926, + "angularVelocity": -2.404898293722967e-9, + "velocityX": 2.832989414150284, + "velocityY": -0.15461706487131047, + "timestamp": 1.9318214612444238 + }, + { + "x": 5.55764662410591, + "y": 0.8795607396382881, + "heading": 0.024925204818700676, + "angularVelocity": -2.4048987883106987e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.1546170648602097, + "timestamp": 2.0171307698327587 + }, + { + "x": 5.799326992265192, + "y": 0.8663704647390634, + "heading": 0.024925204613540374, + "angularVelocity": -2.4048993550096436e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020768, + "timestamp": 2.1024400784210937 + }, + { + "x": 6.04100736042448, + "y": 0.8531801898398956, + "heading": 0.02492520440838013, + "angularVelocity": -2.4048986846119357e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020765, + "timestamp": 2.1877493870094287 + }, + { + "x": 6.282687728583764, + "y": 0.8399899149407278, + "heading": 0.024925204203219874, + "angularVelocity": -2.4048988174777923e-9, + "velocityX": 2.832989414150889, + "velocityY": -0.15461706486020768, + "timestamp": 2.2730586955977636 + }, + { + "x": 6.52436809674305, + "y": 0.8267996400415316, + "heading": 0.024925203998059656, + "angularVelocity": -2.4048983926523396e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020763, + "timestamp": 2.3583680041860986 + }, + { + "x": 6.766048464902335, + "y": 0.8136093651422786, + "heading": 0.02492520379289938, + "angularVelocity": -2.4048990449979503e-9, + "velocityX": 2.832989414150889, + "velocityY": -0.15461706486020768, + "timestamp": 2.4436773127744336 + }, + { + "x": 7.007728833061619, + "y": 0.8004190902431392, + "heading": 0.024925203587739076, + "angularVelocity": -2.404899369928152e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020763, + "timestamp": 2.5289866213627685 + }, + { + "x": 7.249409201220906, + "y": 0.7872288153439714, + "heading": 0.024925203382578868, + "angularVelocity": -2.4048982983592985e-9, + "velocityX": 2.832989414150889, + "velocityY": -0.15461706486020768, + "timestamp": 2.6142959299511035 + }, + { + "x": 7.491089569380192, + "y": 0.7740385404447467, + "heading": 0.024925203177418673, + "angularVelocity": -2.4048980880486034e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020768, + "timestamp": 2.6996052385394385 + }, + { + "x": 7.732769937539386, + "y": 0.7608482655471763, + "heading": 0.024925202972228687, + "angularVelocity": -2.405247335081803e-9, + "velocityX": 2.8329894141498406, + "velocityY": -0.15461706484157828, + "timestamp": 2.7849145471277734 + }, + { + "x": 7.944839923818755, + "y": 0.7492740469626908, + "heading": 0.02198121203200045, + "angularVelocity": -0.03450960966563021, + "velocityX": 2.485895030549647, + "velocityY": -0.13567357157148477, + "timestamp": 2.8702238557161084 + }, + { + "x": 8.126614204583387, + "y": 0.7393532878041089, + "heading": 0.019457756648722437, + "angularVelocity": -0.029580070745328552, + "velocityX": 2.1307672488800957, + "velocityY": -0.11629163713479268, + "timestamp": 2.9555331643044433 + }, + { + "x": 8.278092774424247, + "y": 0.7310859883666867, + "heading": 0.017354864228594055, + "angularVelocity": -0.02465021056818093, + "velocityX": 1.775639403805618, + "velocityY": -0.09690969923711115, + "timestamp": 3.0408424728927783 + }, + { + "x": 8.399275631538378, + "y": 0.7244721487488996, + "heading": 0.015672543952851302, + "angularVelocity": -0.019720242768123807, + "velocityX": 1.4205115375967707, + "velocityY": -0.07752776018500543, + "timestamp": 3.1261517814811133 + }, + { + "x": 8.49016277502432, + "y": 0.7195117690000139, + "heading": 0.01441080042435244, + "angularVelocity": -0.014790221013130912, + "velocityX": 1.0653836608209173, + "velocityY": -0.05814582055535578, + "timestamp": 3.2114610900694482 + }, + { + "x": 8.550754204341196, + "y": 0.7162048491495833, + "heading": 0.013569636407469346, + "angularVelocity": -0.009860166854032092, + "velocityX": 0.7102557777048953, + "velocityY": -0.03876388057911283, + "timestamp": 3.296770398657783 }, { "x": 8.581049919128418, "y": 0.7145513892173767, - "heading": 1.5835345242998375, - "angularVelocity": 0.020826163343848954, - "velocityX": 0.35517778668750655, - "velocityY": -0.018942205252676, - "timestamp": 3.2296188917844946 + "heading": 0.013149053743484958, + "angularVelocity": -0.004930091111322124, + "velocityX": 0.3551278903620608, + "velocityY": -0.0193819403719191, + "timestamp": 3.382079707246118 }, { "x": 8.581049919128418, "y": 0.7145513892173767, - "heading": 1.5835345242998375, - "angularVelocity": -1.33415825335105e-27, - "velocityX": -1.0225334762057861e-26, - "velocityY": 2.2811900613103305e-26, - "timestamp": 3.314950079232096 + "heading": 0.013149053743484958, + "angularVelocity": 2.9756992123058065e-23, + "velocityX": -2.4082246897494348e-21, + "velocityY": -1.7635906049154447e-20, + "timestamp": 3.467389015834453 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Choreo 2 Piece 2.traj b/src/main/deploy/choreo/Choreo 2 Piece 2.traj index 9f57e3c..6c58d99 100644 --- a/src/main/deploy/choreo/Choreo 2 Piece 2.traj +++ b/src/main/deploy/choreo/Choreo 2 Piece 2.traj @@ -4,415 +4,487 @@ "x": 2.578, "y": 3.434, "heading": 2.456, - "angularVelocity": 1.0219812008109311e-25, - "velocityX": -7.726426952121863e-25, - "velocityY": -8.236726072553901e-26, + "angularVelocity": -2.2243193051899215e-21, + "velocityX": 4.420947543039026e-20, + "velocityY": 1.7808811974639867e-21, "timestamp": 0 }, { - "x": 2.584286650145692, - "y": 3.421951310408596, - "heading": 2.4350954049954208, - "angularVelocity": -0.3464845678617885, - "velocityX": 0.10419849121933693, - "velocityY": -0.1997017883132429, - "timestamp": 0.06033340859474565 - }, - { - "x": 2.5967989346479854, - "y": 3.3977100915163163, - "heading": 2.3938477021537534, - "angularVelocity": -0.6836627301919046, - "velocityX": 0.2073856722788015, - "velocityY": -0.401787657234916, - "timestamp": 0.1206668171894913 - }, - { - "x": 2.615462449887643, - "y": 3.361117228577582, - "heading": 2.332876835959309, - "angularVelocity": -1.0105655823953221, - "velocityX": 0.3093396457179988, - "velocityY": -0.6065107838432474, - "timestamp": 0.18100022578423694 - }, - { - "x": 2.640198059989427, - "y": 3.3119715556696296, - "heading": 2.2531378381258196, - "angularVelocity": -1.3216391994208312, - "velocityX": 0.4099819764524013, - "velocityY": -0.8145681481061332, - "timestamp": 0.2413336343789826 - }, - { - "x": 2.6709458792631127, - "y": 3.2500095590805107, - "heading": 2.1562840802735845, - "angularVelocity": -1.605308901122988, - "velocityX": 0.5096317279240128, - "velocityY": -1.026993137505499, - "timestamp": 0.30166704297372826 - }, - { - "x": 2.7077008608268662, - "y": 3.1749016535376815, - "heading": 2.045096498973365, - "angularVelocity": -1.8428857889839654, - "velocityX": 0.6091978295248277, - "velocityY": -1.2448808594144314, - "timestamp": 0.36200045156847394 - }, - { - "x": 2.7505245327527716, - "y": 3.0862787536737564, - "heading": 1.9239140602650735, - "angularVelocity": -2.0085462023580276, - "velocityX": 0.7097837321532509, - "velocityY": -1.4688860107208206, - "timestamp": 0.4223338601632196 - }, - { - "x": 2.7994482297249395, - "y": 2.9838047667261183, - "heading": 1.7994067052659932, - "angularVelocity": -2.0636552434056146, - "velocityX": 0.8108889935389533, - "velocityY": -1.6984617533537163, - "timestamp": 0.4826672687579653 - }, - { - "x": 2.8540407096263034, - "y": 2.867709288903639, - "heading": 1.6856485603266385, - "angularVelocity": -1.885491763004119, - "velocityX": 0.9048466044419411, - "velocityY": -1.924232038708154, - "timestamp": 0.543000677352711 - }, - { - "x": 2.9119156128261765, - "y": 2.7403356565757657, - "heading": 1.604080673099594, - "angularVelocity": -1.3519522454786057, - "velocityX": 0.95925134262865, - "velocityY": -2.1111625431845082, - "timestamp": 0.6033340859474566 - }, - { - "x": 2.9732896898216743, - "y": 2.6016323913890385, - "heading": 1.553622517502379, - "angularVelocity": -0.8363219776979306, - "velocityX": 1.0172486260098852, - "velocityY": -2.298946278974311, - "timestamp": 0.6636674945422023 - }, - { - "x": 3.038399585143699, - "y": 2.4516460837393157, - "heading": 1.5337765355615411, - "angularVelocity": -0.32893851686951064, - "velocityX": 1.0791681895409253, - "velocityY": -2.4859577992214517, - "timestamp": 0.724000903136948 - }, - { - "x": 3.1118123187513027, - "y": 2.2970091829036123, - "heading": 1.5337764978118669, - "angularVelocity": -6.256844289379484e-7, - "velocityX": 1.2167841220560034, - "velocityY": -2.5630393580841155, - "timestamp": 0.7843343117316937 - }, - { - "x": 3.1852252486361183, - "y": 2.1423723752490966, - "heading": 1.5337764600625183, - "angularVelocity": -6.256790302738932e-7, - "velocityX": 1.216787375265398, - "velocityY": -2.563037813646468, - "timestamp": 0.8446677203264393 - }, - { - "x": 3.2586383526710114, - "y": 1.9877356502714374, - "heading": 1.5337764223131956, - "angularVelocity": -6.256786018158767e-7, - "velocityX": 1.2167902617271742, - "velocityY": -2.5630364433135364, - "timestamp": 0.905001128921185 - }, - { - "x": 3.339262039402322, - "y": 1.8367329293476955, - "heading": 1.5337763853100257, - "angularVelocity": -6.133114449160005e-7, - "velocityX": 1.3363025330269804, - "velocityY": -2.5028044070576922, - "timestamp": 0.9653345375159307 - }, - { - "x": 3.432942751765767, - "y": 1.6934642146806478, - "heading": 1.533776348204764, - "angularVelocity": -6.150035711868799e-7, - "velocityX": 1.5527170525486216, - "velocityY": -2.3746166179565877, - "timestamp": 1.0256679461106764 - }, - { - "x": 3.5389443424843976, - "y": 1.559055647892523, - "heading": 1.533776308684474, - "angularVelocity": -6.550316132036273e-7, - "velocityX": 1.7569302512084846, - "velocityY": -2.2277635213839058, - "timestamp": 1.086001354705422 - }, - { - "x": 3.6564336903688766, - "y": 1.4345636707390452, - "heading": 1.533776263619502, - "angularVelocity": -7.469323051999795e-7, - "velocityX": 1.9473348285962937, - "velocityY": -2.063400362304737, - "timestamp": 1.1463347633001675 - }, - { - "x": 3.7844873612800467, - "y": 1.3209667734788668, - "heading": 1.533776207514778, - "angularVelocity": -9.29911391627009e-7, - "velocityX": 2.1224338868585284, - "velocityY": -1.8828191528709874, - "timestamp": 1.206668171894913 + "x": 2.5808143020901366, + "y": 3.4305511164394358, + "heading": 2.421420914475003, + "angularVelocity": -0.7062096950549269, + "velocityX": 0.05747657552803998, + "velocityY": -0.07043665182601658, + "timestamp": 0.04896433136946255 + }, + { + "x": 2.5865263986145903, + "y": 3.423301716588506, + "heading": 2.353190948627095, + "angularVelocity": -1.3934626275824327, + "velocityX": 0.11665831769034016, + "velocityY": -0.14805470938110385, + "timestamp": 0.0979286627389251 + }, + { + "x": 2.5951072938917, + "y": 3.411803141318997, + "heading": 2.252339287405858, + "angularVelocity": -2.059696485187491, + "velocityX": 0.1752478801857445, + "velocityY": -0.23483574569237411, + "timestamp": 0.14689299410838766 + }, + { + "x": 2.6063817008248584, + "y": 3.395552329101315, + "heading": 2.1199335438440907, + "angularVelocity": -2.704126449980364, + "velocityX": 0.23025754907213325, + "velocityY": -0.3318908226296179, + "timestamp": 0.1958573254778502 + }, + { + "x": 2.6200490589295313, + "y": 3.3739990762642234, + "heading": 1.957267690055127, + "angularVelocity": -3.322129583707805, + "velocityX": 0.2791288622216491, + "velocityY": -0.44018272555292703, + "timestamp": 0.24482165684731275 + }, + { + "x": 2.6358077196258662, + "y": 3.346424151831386, + "heading": 1.7667703729621236, + "angularVelocity": -3.890532388885235, + "velocityX": 0.3218395974292316, + "velocityY": -0.5631635041587543, + "timestamp": 0.2937859882167753 + }, + { + "x": 2.6542369913783963, + "y": 3.311395651079251, + "heading": 1.5565531821810574, + "angularVelocity": -4.293271957394105, + "velocityX": 0.37638156668515743, + "velocityY": -0.7153881156437778, + "timestamp": 0.34275031958623786 + }, + { + "x": 2.677454625002408, + "y": 3.268157202317915, + "heading": 1.3372511935604507, + "angularVelocity": -4.478811054640037, + "velocityX": 0.4741744240049492, + "velocityY": -0.8830601287108768, + "timestamp": 0.3917146509557004 + }, + { + "x": 2.706331728536566, + "y": 3.2188789886399363, + "heading": 1.1357091046403869, + "angularVelocity": -4.116100093337721, + "velocityX": 0.5897579467859876, + "velocityY": -1.0064104277491477, + "timestamp": 0.44067898232516295 + }, + { + "x": 2.7406445911847674, + "y": 3.1628572349258164, + "heading": 0.9501369190074549, + "angularVelocity": -3.7899462821760768, + "velocityX": 0.7007726173006585, + "velocityY": -1.1441339470436553, + "timestamp": 0.4896433136946255 + }, + { + "x": 2.77961503012768, + "y": 3.099262425694046, + "heading": 0.7791196669307836, + "angularVelocity": -3.4926904400318053, + "velocityX": 0.7958944368897106, + "velocityY": -1.2987986857600298, + "timestamp": 0.5386076450640881 + }, + { + "x": 2.821598359020251, + "y": 3.0272467328347097, + "heading": 0.6221812057707179, + "angularVelocity": -3.2051588732189473, + "velocityX": 0.8574267781913745, + "velocityY": -1.4707786432530845, + "timestamp": 0.5875719764335506 + }, + { + "x": 2.8636245721731495, + "y": 2.9463007656727807, + "heading": 0.4788804629728746, + "angularVelocity": -2.926635344339965, + "velocityX": 0.8583026047225649, + "velocityY": -1.6531619016942172, + "timestamp": 0.6365363078030132 + }, + { + "x": 2.9010385562385563, + "y": 2.8582501044070647, + "heading": 0.35102943849270457, + "angularVelocity": -2.6111052863249458, + "velocityX": 0.7641069125006744, + "velocityY": -1.7982612812846699, + "timestamp": 0.6855006391724757 + }, + { + "x": 2.9381959956467134, + "y": 2.7627981012239995, + "heading": 0.24579862310441236, + "angularVelocity": -2.1491320813567003, + "velocityX": 0.7588674933153383, + "velocityY": -1.949419108019954, + "timestamp": 0.7344649705419383 + }, + { + "x": 2.9765902696547557, + "y": 2.659953900667316, + "heading": 0.16360236786662388, + "angularVelocity": -1.678696572359438, + "velocityX": 0.7841274032359897, + "velocityY": -2.1003901754668526, + "timestamp": 0.7834293019114008 + }, + { + "x": 3.017865715538054, + "y": 2.5502233979386646, + "heading": 0.10367998080393974, + "angularVelocity": -1.2237966982646422, + "velocityX": 0.8429696623825639, + "velocityY": -2.241029330119635, + "timestamp": 0.8323936332808634 + }, + { + "x": 3.063560421246788, + "y": 2.434583512531678, + "heading": 0.0641258476586635, + "angularVelocity": -0.8078152409928518, + "velocityX": 0.9332243376096464, + "velocityY": -2.3617168288158923, + "timestamp": 0.8813579646503259 + }, + { + "x": 3.115401381748359, + "y": 2.3142782546228218, + "heading": 0.04299697728371769, + "angularVelocity": -0.4315155498707209, + "velocityX": 1.0587494825651373, + "velocityY": -2.456997870573845, + "timestamp": 0.9303222960197884 + }, + { + "x": 3.173044544543568, + "y": 2.188964351893496, + "heading": 0.04038455549354976, + "angularVelocity": -0.0533535681403628, + "velocityX": 1.1772480330663169, + "velocityY": -2.559289573134488, + "timestamp": 0.979286627389251 + }, + { + "x": 3.232092719253376, + "y": 2.0632166415863358, + "heading": 0.04038346790054724, + "angularVelocity": -0.000022211944329746854, + "velocityX": 1.2059426333065189, + "velocityY": -2.568149238234756, + "timestamp": 1.0282509587587136 + }, + { + "x": 3.2911425700668717, + "y": 1.9374697182380494, + "heading": 0.04038238058608858, + "angularVelocity": -0.000022206255619953, + "velocityX": 1.2059768644246023, + "velocityY": -2.568133166149746, + "timestamp": 1.0772152901281762 + }, + { + "x": 3.3583393467395415, + "y": 1.8158809793568123, + "heading": 0.0403818264403618, + "angularVelocity": -0.000011317334706426395, + "velocityX": 1.3723617742393555, + "velocityY": -2.483210440755192, + "timestamp": 1.1261796214976387 + }, + { + "x": 3.4341046175380407, + "y": 1.6994385677916184, + "heading": 0.040381131756193525, + "angularVelocity": -0.000014187555488713555, + "velocityX": 1.5473563853405372, + "velocityY": -2.3781068444822635, + "timestamp": 1.1751439528671013 + }, + { + "x": 3.5178049582052053, + "y": 1.5890648238390772, + "heading": 0.039385078873634086, + "angularVelocity": -0.020342417729421827, + "velocityX": 1.7094145539434111, + "velocityY": -2.2541662648206686, + "timestamp": 1.2241082842365638 + }, + { + "x": 3.608774844587042, + "y": 1.4855271513306618, + "heading": 0.036685479739839225, + "angularVelocity": -0.05513399362946303, + "velocityX": 1.8578807029022373, + "velocityY": -2.1145529738200697, + "timestamp": 1.2730726156060264 + }, + { + "x": 3.7066552250984595, + "y": 1.3891998671498478, + "heading": 0.032788252293559854, + "angularVelocity": -0.07959319237656216, + "velocityX": 1.9990139306283103, + "velocityY": -1.9672949979443013, + "timestamp": 1.322036946975489 + }, + { + "x": 3.8111775959241823, + "y": 1.300340998111743, + "heading": 0.028541553024262294, + "angularVelocity": -0.0867304658416327, + "velocityX": 2.1346634969251537, + "velocityY": -1.8147673327263893, + "timestamp": 1.3710012783449514 }, { "x": 3.922098875045776, "y": 1.2191578149795532, - "heading": 1.5337761150741231, - "angularVelocity": -0.000001532163640308562, - "velocityX": 2.2808509741270218, - "velocityY": -1.6874391961368493, - "timestamp": 1.2670015804896586 - }, - { - "x": 4.133215899282255, - "y": 1.100654492548557, - "heading": 1.5337761164554422, - "angularVelocity": 1.6187741152567537e-8, - "velocityX": 2.474089843952036, - "velocityY": -1.3887457326638517, - "timestamp": 1.3523327679372603 - }, - { - "x": 4.357502974678636, - "y": 1.0095021810718636, - "heading": 1.533776117552903, - "angularVelocity": 1.2861193573885216e-8, - "velocityX": 2.628430262195824, - "velocityY": -1.0682180127009988, - "timestamp": 1.437663955384862 - }, - { - "x": 4.59143382498898, - "y": 0.9471339698744419, - "heading": 1.5337761185021443, - "angularVelocity": 1.1124200677480839e-8, - "velocityX": 2.7414460914890086, - "velocityY": -0.730895854879782, - "timestamp": 1.5229951428324637 - }, - { - "x": 4.831330550647783, - "y": 0.9145303241420727, - "heading": 1.5337761193835278, - "angularVelocity": 1.0328972095127946e-8, - "velocityX": 2.811360451372056, - "velocityY": -0.38208358171963364, - "timestamp": 1.6083263302800654 - }, - { - "x": 5.0730890918673595, - "y": 0.9016367408729034, - "heading": 1.5337761202500422, - "angularVelocity": 1.0154720062447253e-8, - "velocityX": 2.8331791511518682, - "velocityY": -0.1511004786741872, - "timestamp": 1.6936575177276671 - }, - { - "x": 5.314847643606767, - "y": 0.8887433548552026, - "heading": 1.5337761211165564, - "angularVelocity": 1.0154719461608446e-8, - "velocityX": 2.833179274434229, - "velocityY": -0.1510981670754085, - "timestamp": 1.7789887051752689 - }, - { - "x": 5.556606195346236, - "y": 0.8758499688386339, - "heading": 1.5337761219830706, - "angularVelocity": 1.0154719791329154e-8, - "velocityX": 2.8331792744349373, - "velocityY": -0.15109816706214324, - "timestamp": 1.8643198926228706 - }, - { - "x": 5.798364747085703, - "y": 0.862956582822065, - "heading": 1.533776122849585, - "angularVelocity": 1.0154719990167722e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 1.9496510800704723 - }, - { - "x": 6.040123298825171, - "y": 0.8500631968054962, - "heading": 1.5337761237160994, - "angularVelocity": 1.0154719824488932e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.0349822675180738 - }, - { - "x": 6.281881850564639, - "y": 0.8371698107889274, - "heading": 1.5337761245826136, - "angularVelocity": 1.0154719065955154e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.1203134549656752 - }, - { - "x": 6.523640402304108, - "y": 0.8242764247723586, - "heading": 1.533776125449128, - "angularVelocity": 1.015472011167056e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.2056446424132767 - }, - { - "x": 6.765398954043575, - "y": 0.8113830387557898, - "heading": 1.5337761263156422, - "angularVelocity": 1.0154720167050709e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.1510981670621432, - "timestamp": 2.290975829860878 - }, - { - "x": 7.007157505783043, - "y": 0.798489652739221, - "heading": 1.5337761271821566, - "angularVelocity": 1.0154720142992157e-8, - "velocityX": 2.8331792744349364, - "velocityY": -0.15109816706214316, - "timestamp": 2.3763070173084797 - }, - { - "x": 7.248916057522512, - "y": 0.7855962667226521, - "heading": 1.533776128048671, - "angularVelocity": 1.0154720015095924e-8, - "velocityX": 2.833179274434937, - "velocityY": -0.15109816706214318, - "timestamp": 2.461638204756081 - }, - { - "x": 7.490674609261979, - "y": 0.7727028807060851, - "heading": 1.5337761289151852, - "angularVelocity": 1.015471927674143e-8, - "velocityX": 2.8331792744349378, - "velocityY": -0.15109816706212442, - "timestamp": 2.5469693922036827 - }, - { - "x": 7.7324331610160755, - "y": 0.7598094949690721, - "heading": 1.5337761297818946, - "angularVelocity": 1.0157005131011138e-8, - "velocityX": 2.833179274606365, - "velocityY": -0.15109816378601534, - "timestamp": 2.632300579651284 - }, - { - "x": 7.944587343206802, - "y": 0.7484949682004244, - "heading": 1.546215447760533, - "angularVelocity": 0.1457769234288085, - "velocityX": 2.486244344378793, - "velocityY": -0.1325954449607942, - "timestamp": 2.7176317670988857 - }, - { - "x": 8.12643379054156, - "y": 0.738796802526807, - "heading": 1.5568779338247372, - "angularVelocity": 0.12495415079922173, - "velocityX": 2.131066644846836, - "velocityY": -0.11365323703684094, - "timestamp": 2.802962954546487 - }, - { - "x": 8.277972498710973, - "y": 0.7307149979607698, - "heading": 1.5657634145409067, - "angularVelocity": 0.10412934569351617, - "velocityX": 1.7758888948131386, - "velocityY": -0.09471102896581571, - "timestamp": 2.8882941419940886 - }, - { - "x": 8.399203466259046, - "y": 0.7242495544154388, - "heading": 1.5728718351258109, - "angularVelocity": 0.08330389858068031, - "velocityX": 1.420711127716517, - "velocityY": -0.07576882191286877, - "timestamp": 2.97362532944169 - }, - { - "x": 8.490126692448452, - "y": 0.719400471808881, - "heading": 1.5782031697100374, - "angularVelocity": 0.06247814830304947, - "velocityX": 1.0655333519791725, - "velocityY": -0.05682661582010127, - "timestamp": 3.0589565168892916 - }, - { - "x": 8.550742176835362, - "y": 0.7161677500844638, - "heading": 1.5817574030517292, - "angularVelocity": 0.04165221940541114, - "velocityX": 0.7103555710405419, - "velocityY": -0.03788441039101151, - "timestamp": 3.144287704336893 + "heading": 0.02492521296459199, + "angularVelocity": -0.07385662090191833, + "velocityX": 2.2653485919093206, + "velocityY": -1.6580065705303566, + "timestamp": 1.419965609714414 + }, + { + "x": 4.133754076857049, + "y": 1.1017463293287075, + "heading": 0.024925206123710186, + "angularVelocity": -8.018916008040494e-8, + "velocityX": 2.481032906181765, + "velocityY": -1.3763033318839852, + "timestamp": 1.505274918302749 + }, + { + "x": 4.358435567651369, + "y": 1.011737381091072, + "heading": 0.024925205866123235, + "angularVelocity": -3.0194471762044385e-9, + "velocityX": 2.6337277198966884, + "velocityY": -1.0550894120123064, + "timestamp": 1.590584226891084 + }, + { + "x": 4.5926126795284254, + "y": 0.9505453575452277, + "heading": 0.024925205642497325, + "angularVelocity": -2.6213541510789247e-9, + "velocityX": 2.745035867153646, + "velocityY": -0.7172959734217197, + "timestamp": 1.6758935354794189 + }, + { + "x": 4.832605528624095, + "y": 0.9191317291658319, + "heading": 0.024925205434181426, + "angularVelocity": -2.44188946073925e-9, + "velocityX": 2.813208230930211, + "velocityY": -0.3682321296375535, + "timestamp": 1.7612028440677538 + }, + { + "x": 5.074285887787392, + "y": 0.9059412894376224, + "heading": 0.024925205229021138, + "angularVelocity": -2.404899218132483e-9, + "velocityX": 2.8329893086994424, + "velocityY": -0.154618996994628, + "timestamp": 1.8465121526560888 + }, + { + "x": 5.315966255946622, + "y": 0.892751014537456, + "heading": 0.024925205023860926, + "angularVelocity": -2.404898293722967e-9, + "velocityX": 2.832989414150284, + "velocityY": -0.15461706487131047, + "timestamp": 1.9318214612444238 + }, + { + "x": 5.55764662410591, + "y": 0.8795607396382881, + "heading": 0.024925204818700676, + "angularVelocity": -2.4048987883106987e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.1546170648602097, + "timestamp": 2.0171307698327587 + }, + { + "x": 5.799326992265192, + "y": 0.8663704647390634, + "heading": 0.024925204613540374, + "angularVelocity": -2.4048993550096436e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020768, + "timestamp": 2.1024400784210937 + }, + { + "x": 6.04100736042448, + "y": 0.8531801898398956, + "heading": 0.02492520440838013, + "angularVelocity": -2.4048986846119357e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020765, + "timestamp": 2.1877493870094287 + }, + { + "x": 6.282687728583764, + "y": 0.8399899149407278, + "heading": 0.024925204203219874, + "angularVelocity": -2.4048988174777923e-9, + "velocityX": 2.832989414150889, + "velocityY": -0.15461706486020768, + "timestamp": 2.2730586955977636 + }, + { + "x": 6.52436809674305, + "y": 0.8267996400415316, + "heading": 0.024925203998059656, + "angularVelocity": -2.4048983926523396e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020763, + "timestamp": 2.3583680041860986 + }, + { + "x": 6.766048464902335, + "y": 0.8136093651422786, + "heading": 0.02492520379289938, + "angularVelocity": -2.4048990449979503e-9, + "velocityX": 2.832989414150889, + "velocityY": -0.15461706486020768, + "timestamp": 2.4436773127744336 + }, + { + "x": 7.007728833061619, + "y": 0.8004190902431392, + "heading": 0.024925203587739076, + "angularVelocity": -2.404899369928152e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020763, + "timestamp": 2.5289866213627685 + }, + { + "x": 7.249409201220906, + "y": 0.7872288153439714, + "heading": 0.024925203382578868, + "angularVelocity": -2.4048982983592985e-9, + "velocityX": 2.832989414150889, + "velocityY": -0.15461706486020768, + "timestamp": 2.6142959299511035 + }, + { + "x": 7.491089569380192, + "y": 0.7740385404447467, + "heading": 0.024925203177418673, + "angularVelocity": -2.4048980880486034e-9, + "velocityX": 2.8329894141508896, + "velocityY": -0.15461706486020768, + "timestamp": 2.6996052385394385 + }, + { + "x": 7.732769937539386, + "y": 0.7608482655471763, + "heading": 0.024925202972228687, + "angularVelocity": -2.405247335081803e-9, + "velocityX": 2.8329894141498406, + "velocityY": -0.15461706484157828, + "timestamp": 2.7849145471277734 + }, + { + "x": 7.944839923818755, + "y": 0.7492740469626908, + "heading": 0.02198121203200045, + "angularVelocity": -0.03450960966563021, + "velocityX": 2.485895030549647, + "velocityY": -0.13567357157148477, + "timestamp": 2.8702238557161084 + }, + { + "x": 8.126614204583387, + "y": 0.7393532878041089, + "heading": 0.019457756648722437, + "angularVelocity": -0.029580070745328552, + "velocityX": 2.1307672488800957, + "velocityY": -0.11629163713479268, + "timestamp": 2.9555331643044433 + }, + { + "x": 8.278092774424247, + "y": 0.7310859883666867, + "heading": 0.017354864228594055, + "angularVelocity": -0.02465021056818093, + "velocityX": 1.775639403805618, + "velocityY": -0.09690969923711115, + "timestamp": 3.0408424728927783 + }, + { + "x": 8.399275631538378, + "y": 0.7244721487488996, + "heading": 0.015672543952851302, + "angularVelocity": -0.019720242768123807, + "velocityX": 1.4205115375967707, + "velocityY": -0.07752776018500543, + "timestamp": 3.1261517814811133 + }, + { + "x": 8.49016277502432, + "y": 0.7195117690000139, + "heading": 0.01441080042435244, + "angularVelocity": -0.014790221013130912, + "velocityX": 1.0653836608209173, + "velocityY": -0.05814582055535578, + "timestamp": 3.2114610900694482 + }, + { + "x": 8.550754204341196, + "y": 0.7162048491495833, + "heading": 0.013569636407469346, + "angularVelocity": -0.009860166854032092, + "velocityX": 0.7102557777048953, + "velocityY": -0.03876388057911283, + "timestamp": 3.296770398657783 }, { "x": 8.581049919128418, "y": 0.7145513892173767, - "heading": 1.5835345242998375, - "angularVelocity": 0.020826163343848954, - "velocityX": 0.35517778668750655, - "velocityY": -0.018942205252676, - "timestamp": 3.2296188917844946 + "heading": 0.013149053743484958, + "angularVelocity": -0.004930091111322124, + "velocityX": 0.3551278903620608, + "velocityY": -0.0193819403719191, + "timestamp": 3.382079707246118 }, { "x": 8.581049919128418, "y": 0.7145513892173767, - "heading": 1.5835345242998375, - "angularVelocity": -1.33415825335105e-27, - "velocityX": -1.0225334762057861e-26, - "velocityY": 2.2811900613103305e-26, - "timestamp": 3.314950079232096 + "heading": 0.013149053743484958, + "angularVelocity": 2.9756992123058065e-23, + "velocityX": -2.4082246897494348e-21, + "velocityY": -1.7635906049154447e-20, + "timestamp": 3.467389015834453 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Choreo 2 Piece 3.1.traj b/src/main/deploy/choreo/Choreo 2 Piece 3.1.traj index 01b34a0..f84e595 100644 --- a/src/main/deploy/choreo/Choreo 2 Piece 3.1.traj +++ b/src/main/deploy/choreo/Choreo 2 Piece 3.1.traj @@ -3,398 +3,470 @@ { "x": 8.59085464477539, "y": 0.6655267477035522, - "heading": 1.584, - "angularVelocity": -1.194765989428476e-25, - "velocityX": 1.0490028843079857e-24, - "velocityY": 4.196119292892119e-25, + "heading": 0.003579741058468549, + "angularVelocity": -3.0917568952683027e-22, + "velocityX": 1.6244082062603867e-21, + "velocityY": -5.90354802461058e-22, "timestamp": 0 }, { - "x": 8.569338797659704, - "y": 0.6716822291946352, - "heading": 1.601438244624625, - "angularVelocity": 0.23422359204075424, - "velocityX": -0.28899233298513016, - "velocityY": 0.08267798833065461, - "timestamp": 0.07445127313046465 - }, - { - "x": 8.526241242230943, - "y": 0.6840212818502882, - "heading": 1.635395030064886, - "angularVelocity": 0.45609408694404635, - "velocityX": -0.5788692874766389, - "velocityY": 0.16573326602529256, - "timestamp": 0.1489025462609293 - }, - { - "x": 8.46148078388417, - "y": 0.702580492867376, - "heading": 1.6846695662056992, - "angularVelocity": 0.6618360448244704, - "velocityX": -0.8698368157289821, - "velocityY": 0.24927996845084846, - "timestamp": 0.22335381939139395 - }, - { - "x": 8.374959318473937, - "y": 0.7274054321169395, - "heading": 1.747704005496551, - "angularVelocity": 0.8466536116903378, - "velocityX": -1.1621220399900463, - "velocityY": 0.3334387473275515, - "timestamp": 0.2978050925218586 - }, - { - "x": 8.266556813740802, - "y": 0.7585470442654593, - "heading": 1.8223949877693462, - "angularVelocity": 1.0032196782170628, - "velocityX": -1.4560194899981764, - "velocityY": 0.41828179477802546, - "timestamp": 0.37225636565232323 - }, - { - "x": 8.13612233451273, - "y": 0.7960531508424938, - "heading": 1.905548533586374, - "angularVelocity": 1.116885478523836, - "velocityX": -1.7519442414303088, - "velocityY": 0.5037671620646551, - "timestamp": 0.44670763878278785 - }, - { - "x": 7.983495243498111, - "y": 0.8399498616480997, - "heading": 1.9911506980893818, - "angularVelocity": 1.1497743544694357, - "velocityX": -2.050026609312157, - "velocityY": 0.5896032258398517, - "timestamp": 0.5211589119132525 - }, - { - "x": 7.809039943700515, - "y": 0.8900668498836193, - "heading": 2.063896942952919, - "angularVelocity": 0.9770987359216822, - "velocityX": -2.343214460441654, - "velocityY": 0.673151527546038, - "timestamp": 0.5956101850437171 - }, - { - "x": 7.618770984995591, - "y": 0.9491126693527949, - "heading": 2.0933181978593507, - "angularVelocity": 0.39517463797933094, - "velocityX": -2.5556172608560517, - "velocityY": 0.7930800507025142, - "timestamp": 0.6700614581741817 - }, - { - "x": 7.415983670640095, - "y": 1.0082473892448494, - "heading": 2.093318235466261, - "angularVelocity": 5.051211193561299e-7, - "velocityX": -2.723758853661807, - "velocityY": 0.794274125956581, - "timestamp": 0.7445127313046463 - }, - { - "x": 7.213196316699656, - "y": 1.0673819733908265, - "heading": 2.0933182730728315, - "angularVelocity": 5.051165512148015e-7, - "velocityX": -2.723759385351071, - "velocityY": 0.7942723026690597, - "timestamp": 0.818964004435111 - }, - { - "x": 7.010408962758315, - "y": 1.1265165575337097, - "heading": 2.0933183106794018, - "angularVelocity": 5.051165509169277e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274991, - "timestamp": 0.8934152775655756 - }, - { - "x": 6.807621608816973, - "y": 1.1856511416765925, - "heading": 2.093318348285972, - "angularVelocity": 5.051165516307808e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274983, - "timestamp": 0.9678665506960402 - }, - { - "x": 6.604834254875632, - "y": 1.2447857258194757, - "heading": 2.0933183858925424, - "angularVelocity": 5.051165517874018e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274995, - "timestamp": 1.042317823826505 - }, - { - "x": 6.40204690093429, - "y": 1.3039203099623589, - "heading": 2.0933184234991127, - "angularVelocity": 5.051165514872098e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274996, - "timestamp": 1.1167690969569697 - }, - { - "x": 6.199259546992949, - "y": 1.3630548941052418, - "heading": 2.093318461105683, - "angularVelocity": 5.051165527045066e-7, - "velocityX": -2.7237593853631896, - "velocityY": 0.7942723026275, - "timestamp": 1.1912203700874344 - }, - { - "x": 5.996472193051607, - "y": 1.422189478248125, - "heading": 2.0933184987122533, - "angularVelocity": 5.05116552107724e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275003, - "timestamp": 1.2656716432178992 - }, - { - "x": 5.793684839110266, - "y": 1.481324062391008, - "heading": 2.0933185363188236, - "angularVelocity": 5.051165528043117e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275005, - "timestamp": 1.340122916348364 - }, - { - "x": 5.590897485168925, - "y": 1.5404586465338912, - "heading": 2.093318573925394, - "angularVelocity": 5.051165529619199e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275014, - "timestamp": 1.4145741894788286 - }, - { - "x": 5.388110131227584, - "y": 1.5995932306767744, - "heading": 2.093318611531964, - "angularVelocity": 5.051165529309538e-7, - "velocityX": -2.7237593853631896, - "velocityY": 0.7942723026275016, - "timestamp": 1.4890254626092934 - }, - { - "x": 5.185322777286242, - "y": 1.6587278148196576, - "heading": 2.0933186491385345, - "angularVelocity": 5.051165538065676e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275019, - "timestamp": 1.563476735739758 - }, - { - "x": 4.982535423344901, - "y": 1.7178623989625408, - "heading": 2.093318686745105, - "angularVelocity": 5.051165537812963e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275026, - "timestamp": 1.6379280088702228 - }, - { - "x": 4.779748069403955, - "y": 1.7769969831067813, - "heading": 2.093318724351675, - "angularVelocity": 5.051165541155275e-7, - "velocityX": -2.7237593853578734, - "velocityY": 0.7942723026457309, - "timestamp": 1.7123792820006876 - }, - { - "x": 4.5769607328234585, - "y": 1.8361316267843304, - "heading": 2.093318761958246, - "angularVelocity": 5.051165582796277e-7, - "velocityX": -2.7237591521792095, - "velocityY": 0.7942731022735431, - "timestamp": 1.7868305551311523 - }, - { - "x": 4.374941076875769, - "y": 1.897837935676309, - "heading": 2.0933187996353646, - "angularVelocity": 5.060641290697099e-7, - "velocityX": -2.7134479701063015, - "velocityY": 0.8288146904331337, - "timestamp": 1.861281828261617 - }, - { - "x": 4.18087007463253, - "y": 1.9812409077974285, - "heading": 2.093318838788321, - "angularVelocity": 5.258869968320627e-7, - "velocityX": -2.6066848031350367, - "velocityY": 1.1202356738073298, - "timestamp": 1.9357331013920818 + "x": 8.581278259975488, + "y": 0.6665691805773499, + "heading": 0.049210209005701194, + "angularVelocity": 0.7412190640469178, + "velocityX": -0.1555583209566295, + "velocityY": 0.01693322803398738, + "timestamp": 0.0615613793014157 + }, + { + "x": 8.562140843080153, + "y": 0.6681624484519715, + "heading": 0.14036624615251878, + "angularVelocity": 1.4807341580262696, + "velocityX": -0.3108672533413248, + "velocityY": 0.02588096453818382, + "timestamp": 0.1231227586028314 + }, + { + "x": 8.53345789975939, + "y": 0.6696003342516827, + "heading": 0.27652291893725567, + "angularVelocity": 2.2117222571977972, + "velocityX": -0.4659243123895012, + "velocityY": 0.023356945799909313, + "timestamp": 0.1846841379042471 + }, + { + "x": 8.494774393681595, + "y": 0.6700913705803325, + "heading": 0.45437781249309583, + "angularVelocity": 2.8890660926395153, + "velocityX": -0.6283729590981868, + "velocityY": 0.007976369831580163, + "timestamp": 0.2462455172056628 + }, + { + "x": 8.445611037120612, + "y": 0.676397849256214, + "heading": 0.6654299357693332, + "angularVelocity": 3.4283202499880296, + "velocityX": -0.7986071319206547, + "velocityY": 0.10244212763661113, + "timestamp": 0.3078068965070785 + }, + { + "x": 8.384281821724839, + "y": 0.6873224721600781, + "heading": 0.905533699151053, + "angularVelocity": 3.9002336547095253, + "velocityX": -0.9962287410016354, + "velocityY": 0.17745903402155838, + "timestamp": 0.3693682758084942 + }, + { + "x": 8.308431678971848, + "y": 0.7019055174308684, + "heading": 1.16123585616331, + "angularVelocity": 4.153613189208983, + "velocityX": -1.2321059666583078, + "velocityY": 0.23688626597186843, + "timestamp": 0.4309296551099099 + }, + { + "x": 8.217894206979377, + "y": 0.7176144971385626, + "heading": 1.398998972446434, + "angularVelocity": 3.8622122990291166, + "velocityX": -1.4706862162587828, + "velocityY": 0.2551758892662216, + "timestamp": 0.4924910344113256 + }, + { + "x": 8.116714631429112, + "y": 0.7341860694138748, + "heading": 1.595116572105417, + "angularVelocity": 3.185724587143436, + "velocityX": -1.6435560199987134, + "velocityY": 0.26918780026312733, + "timestamp": 0.5540524137127413 + }, + { + "x": 8.00449036583329, + "y": 0.7525893238791683, + "heading": 1.7539844890971241, + "angularVelocity": 2.5806425845961125, + "velocityX": -1.8229654187302906, + "velocityY": 0.2989415551459339, + "timestamp": 0.615613793014157 + }, + { + "x": 7.883611717181969, + "y": 0.7794643101857551, + "heading": 1.8813239739827363, + "angularVelocity": 2.0684962931407838, + "velocityX": -1.9635467889612548, + "velocityY": 0.43655594808884984, + "timestamp": 0.6771751723155727 + }, + { + "x": 7.75310035066067, + "y": 0.8143312926954029, + "heading": 1.9786852945004982, + "angularVelocity": 1.5815324741354986, + "velocityX": -2.120020181521479, + "velocityY": 0.5663775390563021, + "timestamp": 0.7387365516169884 + }, + { + "x": 7.6123058950143605, + "y": 0.8564531019763303, + "heading": 2.0464927345736803, + "angularVelocity": 1.1014607021909668, + "velocityX": -2.2870581725752865, + "velocityY": 0.6842245862408529, + "timestamp": 0.8002979309184041 + }, + { + "x": 7.460836017732183, + "y": 0.9050424052451691, + "heading": 2.0846731899989006, + "angularVelocity": 0.6202014291830935, + "velocityX": -2.4604691935271816, + "velocityY": 0.7892822386408368, + "timestamp": 0.8618593102198198 + }, + { + "x": 7.298037440831554, + "y": 0.9578569112638236, + "heading": 2.093276903536618, + "angularVelocity": 0.13975829709064802, + "velocityX": -2.6444920297113224, + "velocityY": 0.8579162230928108, + "timestamp": 0.9234206895212355 + }, + { + "x": 7.131708118089986, + "y": 1.0111620444693228, + "heading": 2.0932771548444187, + "angularVelocity": 0.000004082231488927161, + "velocityX": -2.701845290489505, + "velocityY": 0.8658859468451401, + "timestamp": 0.9849820688226512 + }, + { + "x": 6.965378792094487, + "y": 1.0644671675235107, + "heading": 2.093277406150717, + "angularVelocity": 0.000004082207080043569, + "velocityX": -2.7018453433462057, + "velocityY": 0.8658857819477386, + "timestamp": 1.046543448124067 + }, + { + "x": 6.799049466098943, + "y": 1.1177722905775571, + "heading": 2.0932776574570204, + "angularVelocity": 0.000004082207158569437, + "velocityX": -2.7018453433469287, + "velocityY": 0.8658857819454437, + "timestamp": 1.1081048274254828 + }, + { + "x": 6.632720140103406, + "y": 1.1710774136316247, + "heading": 2.0932779087633286, + "angularVelocity": 0.0000040822072370843174, + "velocityX": -2.7018453433468057, + "velocityY": 0.8658857819457861, + "timestamp": 1.1696662067268986 + }, + { + "x": 6.466390814107878, + "y": 1.2243825366857128, + "heading": 2.0932781600696417, + "angularVelocity": 0.000004082207315896948, + "velocityX": -2.7018453433466827, + "velocityY": 0.8658857819461279, + "timestamp": 1.2312275860283144 + }, + { + "x": 6.300061488112355, + "y": 1.2776876597398226, + "heading": 2.0932784113759593, + "angularVelocity": 0.00000408220739475046, + "velocityX": -2.7018453433465606, + "velocityY": 0.8658857819464707, + "timestamp": 1.2927889653297302 + }, + { + "x": 6.133732162116841, + "y": 1.3309927827939534, + "heading": 2.093278662682282, + "angularVelocity": 0.000004082207473083141, + "velocityX": -2.7018453433464376, + "velocityY": 0.8658857819468134, + "timestamp": 1.354350344631146 + }, + { + "x": 5.967402836121334, + "y": 1.384297905848105, + "heading": 2.09327891398861, + "angularVelocity": 0.0000040822075518289365, + "velocityX": -2.701845343346315, + "velocityY": 0.8658857819471559, + "timestamp": 1.4159117239325618 + }, + { + "x": 5.801073510125836, + "y": 1.4376030289022779, + "heading": 2.093279165294942, + "angularVelocity": 0.000004082207630794512, + "velocityX": -2.701845343346192, + "velocityY": 0.8658857819474983, + "timestamp": 1.4774731032339776 + }, + { + "x": 5.634744184130344, + "y": 1.4909081519564717, + "heading": 2.093279416601279, + "angularVelocity": 0.000004082207709261434, + "velocityX": -2.701845343346069, + "velocityY": 0.8658857819478408, + "timestamp": 1.5390344825353934 + }, + { + "x": 5.468414858134862, + "y": 1.544213275010687, + "heading": 2.093279667907621, + "angularVelocity": 0.00000408220778743359, + "velocityX": -2.701845343345947, + "velocityY": 0.8658857819481832, + "timestamp": 1.6005958618368092 + }, + { + "x": 5.302085532139386, + "y": 1.597518398064923, + "heading": 2.0932799192139675, + "angularVelocity": 0.00000408220786728094, + "velocityX": -2.7018453433458234, + "velocityY": 0.8658857819485258, + "timestamp": 1.662157241138225 + }, + { + "x": 5.135756206143918, + "y": 1.65082352111918, + "heading": 2.0932801705203192, + "angularVelocity": 0.000004082207945695806, + "velocityX": -2.701845343345701, + "velocityY": 0.8658857819488687, + "timestamp": 1.7237186204396409 + }, + { + "x": 4.969426880148456, + "y": 1.7041286441734584, + "heading": 2.093280421826676, + "angularVelocity": 0.000004082208024460346, + "velocityX": -2.701845343345578, + "velocityY": 0.8658857819492114, + "timestamp": 1.7852799997410567 + }, + { + "x": 4.803097554153003, + "y": 1.7574337672277578, + "heading": 2.0932806731330373, + "angularVelocity": 0.000004082208102945811, + "velocityX": -2.7018453433454552, + "velocityY": 0.8658857819495545, + "timestamp": 1.8468413790424725 + }, + { + "x": 4.6367682281581555, + "y": 1.810738890283948, + "heading": 2.0932809244394037, + "angularVelocity": 0.0000040822081820467654, + "velocityX": -2.7018453433355996, + "velocityY": 0.8658857819802678, + "timestamp": 1.9084027583438883 + }, + { + "x": 4.470438939607256, + "y": 1.8640441301771067, + "heading": 2.0932811757459198, + "angularVelocity": 0.000004082210616647047, + "velocityX": -2.701844735097984, + "velocityY": 0.8658876798742076, + "timestamp": 1.969964137645304 + }, + { + "x": 4.306176669660201, + "y": 1.9234140327969524, + "heading": 2.093281442887588, + "angularVelocity": 0.000004339436045796883, + "velocityX": -2.668268187150199, + "velocityY": 0.9644017611944702, + "timestamp": 2.03152551694672 + }, + { + "x": 4.147951507665768, + "y": 1.9973846826411954, + "heading": 2.093281853657193, + "angularVelocity": 0.000006672521138398165, + "velocityX": -2.570201704216772, + "velocityY": 1.2015755768250211, + "timestamp": 2.0930868962481357 }, { "x": 3.9970710277557377, "y": 2.0853443145751953, "heading": 2.0933188819049127, - "angularVelocity": 5.791249741848488e-7, - "velocityX": -2.468715968828528, - "velocityY": 1.3982757097429517, - "timestamp": 2.0101843745225465 - }, - { - "x": 3.8417885319373366, - "y": 2.194746303912347, - "heading": 2.093318907323053, - "angularVelocity": 3.796579913258463e-7, - "velocityX": -2.3193766038751393, - "velocityY": 1.634082535501843, - "timestamp": 2.0771344746308946 - }, - { - "x": 3.6980071143049322, - "y": 2.31887669174007, - "heading": 2.093318931169399, - "angularVelocity": 3.5618088315780296e-7, - "velocityX": -2.147590778799685, - "velocityY": 1.854073222098814, - "timestamp": 2.1440845747392427 - }, - { - "x": 3.5671177985502105, - "y": 2.456533643810284, - "heading": 2.093318954469134, - "angularVelocity": 3.480164309169473e-7, - "velocityX": -1.9550279318910289, - "velocityY": 2.0561127145058484, - "timestamp": 2.211034674847591 - }, - { - "x": 3.440627773307266, - "y": 2.598243639821922, - "heading": 2.093318977734502, - "angularVelocity": 3.4750310659854874e-7, - "velocityX": -1.8893179403502105, - "velocityY": 2.116650995029174, - "timestamp": 2.277984774955939 - }, - { - "x": 3.314137927462374, - "y": 2.7399537959635722, - "heading": 2.093319001000051, - "angularVelocity": 3.475058166249576e-7, - "velocityX": -1.8893152607716497, - "velocityY": 2.116653386810687, - "timestamp": 2.344934875064287 - }, - { - "x": 3.1930707757983563, - "y": 2.8770243686628967, - "heading": 2.114175460284759, - "angularVelocity": 0.3115224510636341, - "velocityX": -1.8083192029300867, - "velocityY": 2.047354260523847, - "timestamp": 2.4118849751726352 - }, - { - "x": 3.082167243806988, - "y": 3.0018001495332065, - "heading": 2.1671051603213085, - "angularVelocity": 0.7905843299844287, - "velocityX": -1.6565103235378043, - "velocityY": 1.8637131336380355, - "timestamp": 2.4788350752809833 - }, - { - "x": 2.9836145948825417, - "y": 3.1125735219481654, - "heading": 2.2223290697280427, - "angularVelocity": 0.8248517824075432, - "velocityX": -1.4720313900196498, - "velocityY": 1.6545661953557886, - "timestamp": 2.5457851753893315 - }, - { - "x": 2.897439709795232, - "y": 3.2093792657888156, - "heading": 2.2747244573880905, - "angularVelocity": 0.7826035745316955, - "velocityX": -1.2871509519455429, - "velocityY": 1.4459387466782787, - "timestamp": 2.6127352754976796 - }, - { - "x": 2.8236207245224336, - "y": 3.2922706416025598, - "heading": 2.3220108807788065, - "angularVelocity": 0.7062935427160024, - "velocityX": -1.1025970857897722, - "velocityY": 1.2381068240315942, - "timestamp": 2.6796853756060277 - }, - { - "x": 2.762136351273778, - "y": 3.3612886907695043, - "heading": 2.3628840100451693, - "angularVelocity": 0.6105013913379634, - "velocityX": -0.9183611846607055, - "velocityY": 1.0308879158544917, - "timestamp": 2.746635475714376 - }, - { - "x": 2.712969884575379, - "y": 3.4164643641886956, - "heading": 2.3965008699236137, - "angularVelocity": 0.5021181420795616, - "velocityX": -0.7343748047998556, - "velocityY": 0.8241313057022762, - "timestamp": 2.813585575822724 - }, - { - "x": 2.676108490863902, - "y": 3.4578214702193035, - "heading": 2.42227370118923, - "angularVelocity": 0.3849558286530869, - "velocityX": -0.5505801134251128, - "velocityY": 0.6177303090462575, - "timestamp": 2.880535675931072 - }, - { - "x": 2.651542090312647, - "y": 3.4853787426726646, - "heading": 2.4397690611795695, - "angularVelocity": 0.2613194000012874, - "velocityX": -0.36693597935624045, - "velocityY": 0.4116091299156203, - "timestamp": 2.94748577603942 + "angularVelocity": 0.0006014850242100183, + "velocityX": -2.450895051770885, + "velocityY": 1.4288119098718277, + "timestamp": 2.1546482755495515 + }, + { + "x": 3.843205910355169, + "y": 2.1966367718038953, + "heading": 2.093318891906276, + "angularVelocity": 1.4942876205203166e-7, + "velocityX": -2.2988740829855514, + "velocityY": 1.662802783874456, + "timestamp": 2.2215789171794507 + }, + { + "x": 3.701017218385049, + "y": 2.3225052008470026, + "heading": 2.0933189013422897, + "angularVelocity": 1.409819682458666e-7, + "velocityX": -2.124418480198765, + "velocityY": 1.8805800449233931, + "timestamp": 2.28850955880935 + }, + { + "x": 3.5718797746002595, + "y": 2.4617316574789556, + "heading": 2.093318910626833, + "angularVelocity": 1.387188756626878e-7, + "velocityX": -1.9294218707609325, + "velocityY": 2.0801601963092122, + "timestamp": 2.355440200439249 + }, + { + "x": 3.4447158253880024, + "y": 2.6027629151415783, + "heading": 2.0933189199075177, + "angularVelocity": 1.3866122154045785e-7, + "velocityX": -1.8999362043385877, + "velocityY": 2.107125439532927, + "timestamp": 2.422370842069148 + }, + { + "x": 3.317551912654746, + "y": 2.7437942056962745, + "heading": 2.0933189291882313, + "angularVelocity": 1.3866165525269637e-7, + "velocityX": -1.8999356593116823, + "velocityY": 2.1071259309681434, + "timestamp": 2.4893014836990472 + }, + { + "x": 3.1958691275778026, + "y": 2.880185747480273, + "heading": 2.114236602845924, + "angularVelocity": 0.3125276128885705, + "velocityX": -1.8180430086088528, + "velocityY": 2.0378041874780046, + "timestamp": 2.5562321253289464 + }, + { + "x": 3.084402838552189, + "y": 3.004327638614834, + "heading": 2.167146744456709, + "angularVelocity": 0.7905219541052365, + "velocityX": -1.6653999769190886, + "velocityY": 1.8547841184762293, + "timestamp": 2.6231627669588455 + }, + { + "x": 2.9853512622212666, + "y": 3.1145382871506517, + "heading": 2.222354458529675, + "angularVelocity": 0.8248496163871107, + "velocityX": -1.4799137423280575, + "velocityY": 1.6466396534077914, + "timestamp": 2.6900934085887447 + }, + { + "x": 2.8987408213697994, + "y": 3.210852203380145, + "heading": 2.2747381958649533, + "angularVelocity": 0.7826570321100548, + "velocityX": -1.294032729140559, + "velocityY": 1.439010801092748, + "timestamp": 2.757024050218644 + }, + { + "x": 2.8245492222018562, + "y": 3.2933223707153383, + "heading": 2.322017090666629, + "angularVelocity": 0.7063863971767974, + "velocityX": -1.1084848039884865, + "velocityY": 1.2321735654533488, + "timestamp": 2.823954691848543 + }, + { + "x": 2.762754840052126, + "y": 3.361989636833625, + "heading": 2.36288597520057, + "angularVelocity": 0.6106154601046585, + "velocityX": -0.9232599694984354, + "velocityY": 1.0259466284215513, + "timestamp": 2.890885333478442 + }, + { + "x": 2.713340706240203, + "y": 3.416884822801194, + "heading": 2.39650092018746, + "angularVelocity": 0.5022355107959039, + "velocityX": -0.738288661345317, + "velocityY": 0.8201801840047823, + "timestamp": 2.9578159751083413 + }, + { + "x": 2.6762937800618425, + "y": 3.4580316490257124, + "heading": 2.422273262441906, + "angularVelocity": 0.38506043908793497, + "velocityX": -0.5535121922663854, + "velocityY": 0.6147681424009704, + "timestamp": 3.0247466167382404 + }, + { + "x": 2.6516038167671496, + "y": 3.4854487858585643, + "heading": 2.439768796706212, + "angularVelocity": 0.261397976147448, + "velocityX": -0.3688887883552444, + "velocityY": 0.40963505152779395, + "timestamp": 3.0916772583681396 }, { "x": 2.6392624378204346, "y": 3.4991512298583984, "heading": 2.448651867948598, - "angularVelocity": 0.13267802071472284, - "velocityX": -0.18341499822016766, - "velocityY": 0.20571271982335043, - "timestamp": 3.0144358761477683 + "angularVelocity": 0.13272054512051312, + "velocityX": -0.18439056680433819, + "velocityY": 0.20472602183620905, + "timestamp": 3.1586078999980387 }, { "x": 2.6392624378204346, "y": 3.4991512298583984, "heading": 2.448651867948598, - "angularVelocity": 2.0995005926462733e-27, - "velocityX": 2.2917468406814997e-27, - "velocityY": -2.2636416821297325e-27, - "timestamp": 3.0813859762561164 + "angularVelocity": -1.6679097586736474e-24, + "velocityX": 3.3994819825962006e-22, + "velocityY": 2.9924033131311345e-22, + "timestamp": 3.225538541627938 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Choreo 2 Piece 3.traj b/src/main/deploy/choreo/Choreo 2 Piece 3.traj index 01b34a0..f84e595 100644 --- a/src/main/deploy/choreo/Choreo 2 Piece 3.traj +++ b/src/main/deploy/choreo/Choreo 2 Piece 3.traj @@ -3,398 +3,470 @@ { "x": 8.59085464477539, "y": 0.6655267477035522, - "heading": 1.584, - "angularVelocity": -1.194765989428476e-25, - "velocityX": 1.0490028843079857e-24, - "velocityY": 4.196119292892119e-25, + "heading": 0.003579741058468549, + "angularVelocity": -3.0917568952683027e-22, + "velocityX": 1.6244082062603867e-21, + "velocityY": -5.90354802461058e-22, "timestamp": 0 }, { - "x": 8.569338797659704, - "y": 0.6716822291946352, - "heading": 1.601438244624625, - "angularVelocity": 0.23422359204075424, - "velocityX": -0.28899233298513016, - "velocityY": 0.08267798833065461, - "timestamp": 0.07445127313046465 - }, - { - "x": 8.526241242230943, - "y": 0.6840212818502882, - "heading": 1.635395030064886, - "angularVelocity": 0.45609408694404635, - "velocityX": -0.5788692874766389, - "velocityY": 0.16573326602529256, - "timestamp": 0.1489025462609293 - }, - { - "x": 8.46148078388417, - "y": 0.702580492867376, - "heading": 1.6846695662056992, - "angularVelocity": 0.6618360448244704, - "velocityX": -0.8698368157289821, - "velocityY": 0.24927996845084846, - "timestamp": 0.22335381939139395 - }, - { - "x": 8.374959318473937, - "y": 0.7274054321169395, - "heading": 1.747704005496551, - "angularVelocity": 0.8466536116903378, - "velocityX": -1.1621220399900463, - "velocityY": 0.3334387473275515, - "timestamp": 0.2978050925218586 - }, - { - "x": 8.266556813740802, - "y": 0.7585470442654593, - "heading": 1.8223949877693462, - "angularVelocity": 1.0032196782170628, - "velocityX": -1.4560194899981764, - "velocityY": 0.41828179477802546, - "timestamp": 0.37225636565232323 - }, - { - "x": 8.13612233451273, - "y": 0.7960531508424938, - "heading": 1.905548533586374, - "angularVelocity": 1.116885478523836, - "velocityX": -1.7519442414303088, - "velocityY": 0.5037671620646551, - "timestamp": 0.44670763878278785 - }, - { - "x": 7.983495243498111, - "y": 0.8399498616480997, - "heading": 1.9911506980893818, - "angularVelocity": 1.1497743544694357, - "velocityX": -2.050026609312157, - "velocityY": 0.5896032258398517, - "timestamp": 0.5211589119132525 - }, - { - "x": 7.809039943700515, - "y": 0.8900668498836193, - "heading": 2.063896942952919, - "angularVelocity": 0.9770987359216822, - "velocityX": -2.343214460441654, - "velocityY": 0.673151527546038, - "timestamp": 0.5956101850437171 - }, - { - "x": 7.618770984995591, - "y": 0.9491126693527949, - "heading": 2.0933181978593507, - "angularVelocity": 0.39517463797933094, - "velocityX": -2.5556172608560517, - "velocityY": 0.7930800507025142, - "timestamp": 0.6700614581741817 - }, - { - "x": 7.415983670640095, - "y": 1.0082473892448494, - "heading": 2.093318235466261, - "angularVelocity": 5.051211193561299e-7, - "velocityX": -2.723758853661807, - "velocityY": 0.794274125956581, - "timestamp": 0.7445127313046463 - }, - { - "x": 7.213196316699656, - "y": 1.0673819733908265, - "heading": 2.0933182730728315, - "angularVelocity": 5.051165512148015e-7, - "velocityX": -2.723759385351071, - "velocityY": 0.7942723026690597, - "timestamp": 0.818964004435111 - }, - { - "x": 7.010408962758315, - "y": 1.1265165575337097, - "heading": 2.0933183106794018, - "angularVelocity": 5.051165509169277e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274991, - "timestamp": 0.8934152775655756 - }, - { - "x": 6.807621608816973, - "y": 1.1856511416765925, - "heading": 2.093318348285972, - "angularVelocity": 5.051165516307808e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274983, - "timestamp": 0.9678665506960402 - }, - { - "x": 6.604834254875632, - "y": 1.2447857258194757, - "heading": 2.0933183858925424, - "angularVelocity": 5.051165517874018e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274995, - "timestamp": 1.042317823826505 - }, - { - "x": 6.40204690093429, - "y": 1.3039203099623589, - "heading": 2.0933184234991127, - "angularVelocity": 5.051165514872098e-7, - "velocityX": -2.72375938536319, - "velocityY": 0.7942723026274996, - "timestamp": 1.1167690969569697 - }, - { - "x": 6.199259546992949, - "y": 1.3630548941052418, - "heading": 2.093318461105683, - "angularVelocity": 5.051165527045066e-7, - "velocityX": -2.7237593853631896, - "velocityY": 0.7942723026275, - "timestamp": 1.1912203700874344 - }, - { - "x": 5.996472193051607, - "y": 1.422189478248125, - "heading": 2.0933184987122533, - "angularVelocity": 5.05116552107724e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275003, - "timestamp": 1.2656716432178992 - }, - { - "x": 5.793684839110266, - "y": 1.481324062391008, - "heading": 2.0933185363188236, - "angularVelocity": 5.051165528043117e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275005, - "timestamp": 1.340122916348364 - }, - { - "x": 5.590897485168925, - "y": 1.5404586465338912, - "heading": 2.093318573925394, - "angularVelocity": 5.051165529619199e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275014, - "timestamp": 1.4145741894788286 - }, - { - "x": 5.388110131227584, - "y": 1.5995932306767744, - "heading": 2.093318611531964, - "angularVelocity": 5.051165529309538e-7, - "velocityX": -2.7237593853631896, - "velocityY": 0.7942723026275016, - "timestamp": 1.4890254626092934 - }, - { - "x": 5.185322777286242, - "y": 1.6587278148196576, - "heading": 2.0933186491385345, - "angularVelocity": 5.051165538065676e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275019, - "timestamp": 1.563476735739758 - }, - { - "x": 4.982535423344901, - "y": 1.7178623989625408, - "heading": 2.093318686745105, - "angularVelocity": 5.051165537812963e-7, - "velocityX": -2.723759385363189, - "velocityY": 0.7942723026275026, - "timestamp": 1.6379280088702228 - }, - { - "x": 4.779748069403955, - "y": 1.7769969831067813, - "heading": 2.093318724351675, - "angularVelocity": 5.051165541155275e-7, - "velocityX": -2.7237593853578734, - "velocityY": 0.7942723026457309, - "timestamp": 1.7123792820006876 - }, - { - "x": 4.5769607328234585, - "y": 1.8361316267843304, - "heading": 2.093318761958246, - "angularVelocity": 5.051165582796277e-7, - "velocityX": -2.7237591521792095, - "velocityY": 0.7942731022735431, - "timestamp": 1.7868305551311523 - }, - { - "x": 4.374941076875769, - "y": 1.897837935676309, - "heading": 2.0933187996353646, - "angularVelocity": 5.060641290697099e-7, - "velocityX": -2.7134479701063015, - "velocityY": 0.8288146904331337, - "timestamp": 1.861281828261617 - }, - { - "x": 4.18087007463253, - "y": 1.9812409077974285, - "heading": 2.093318838788321, - "angularVelocity": 5.258869968320627e-7, - "velocityX": -2.6066848031350367, - "velocityY": 1.1202356738073298, - "timestamp": 1.9357331013920818 + "x": 8.581278259975488, + "y": 0.6665691805773499, + "heading": 0.049210209005701194, + "angularVelocity": 0.7412190640469178, + "velocityX": -0.1555583209566295, + "velocityY": 0.01693322803398738, + "timestamp": 0.0615613793014157 + }, + { + "x": 8.562140843080153, + "y": 0.6681624484519715, + "heading": 0.14036624615251878, + "angularVelocity": 1.4807341580262696, + "velocityX": -0.3108672533413248, + "velocityY": 0.02588096453818382, + "timestamp": 0.1231227586028314 + }, + { + "x": 8.53345789975939, + "y": 0.6696003342516827, + "heading": 0.27652291893725567, + "angularVelocity": 2.2117222571977972, + "velocityX": -0.4659243123895012, + "velocityY": 0.023356945799909313, + "timestamp": 0.1846841379042471 + }, + { + "x": 8.494774393681595, + "y": 0.6700913705803325, + "heading": 0.45437781249309583, + "angularVelocity": 2.8890660926395153, + "velocityX": -0.6283729590981868, + "velocityY": 0.007976369831580163, + "timestamp": 0.2462455172056628 + }, + { + "x": 8.445611037120612, + "y": 0.676397849256214, + "heading": 0.6654299357693332, + "angularVelocity": 3.4283202499880296, + "velocityX": -0.7986071319206547, + "velocityY": 0.10244212763661113, + "timestamp": 0.3078068965070785 + }, + { + "x": 8.384281821724839, + "y": 0.6873224721600781, + "heading": 0.905533699151053, + "angularVelocity": 3.9002336547095253, + "velocityX": -0.9962287410016354, + "velocityY": 0.17745903402155838, + "timestamp": 0.3693682758084942 + }, + { + "x": 8.308431678971848, + "y": 0.7019055174308684, + "heading": 1.16123585616331, + "angularVelocity": 4.153613189208983, + "velocityX": -1.2321059666583078, + "velocityY": 0.23688626597186843, + "timestamp": 0.4309296551099099 + }, + { + "x": 8.217894206979377, + "y": 0.7176144971385626, + "heading": 1.398998972446434, + "angularVelocity": 3.8622122990291166, + "velocityX": -1.4706862162587828, + "velocityY": 0.2551758892662216, + "timestamp": 0.4924910344113256 + }, + { + "x": 8.116714631429112, + "y": 0.7341860694138748, + "heading": 1.595116572105417, + "angularVelocity": 3.185724587143436, + "velocityX": -1.6435560199987134, + "velocityY": 0.26918780026312733, + "timestamp": 0.5540524137127413 + }, + { + "x": 8.00449036583329, + "y": 0.7525893238791683, + "heading": 1.7539844890971241, + "angularVelocity": 2.5806425845961125, + "velocityX": -1.8229654187302906, + "velocityY": 0.2989415551459339, + "timestamp": 0.615613793014157 + }, + { + "x": 7.883611717181969, + "y": 0.7794643101857551, + "heading": 1.8813239739827363, + "angularVelocity": 2.0684962931407838, + "velocityX": -1.9635467889612548, + "velocityY": 0.43655594808884984, + "timestamp": 0.6771751723155727 + }, + { + "x": 7.75310035066067, + "y": 0.8143312926954029, + "heading": 1.9786852945004982, + "angularVelocity": 1.5815324741354986, + "velocityX": -2.120020181521479, + "velocityY": 0.5663775390563021, + "timestamp": 0.7387365516169884 + }, + { + "x": 7.6123058950143605, + "y": 0.8564531019763303, + "heading": 2.0464927345736803, + "angularVelocity": 1.1014607021909668, + "velocityX": -2.2870581725752865, + "velocityY": 0.6842245862408529, + "timestamp": 0.8002979309184041 + }, + { + "x": 7.460836017732183, + "y": 0.9050424052451691, + "heading": 2.0846731899989006, + "angularVelocity": 0.6202014291830935, + "velocityX": -2.4604691935271816, + "velocityY": 0.7892822386408368, + "timestamp": 0.8618593102198198 + }, + { + "x": 7.298037440831554, + "y": 0.9578569112638236, + "heading": 2.093276903536618, + "angularVelocity": 0.13975829709064802, + "velocityX": -2.6444920297113224, + "velocityY": 0.8579162230928108, + "timestamp": 0.9234206895212355 + }, + { + "x": 7.131708118089986, + "y": 1.0111620444693228, + "heading": 2.0932771548444187, + "angularVelocity": 0.000004082231488927161, + "velocityX": -2.701845290489505, + "velocityY": 0.8658859468451401, + "timestamp": 0.9849820688226512 + }, + { + "x": 6.965378792094487, + "y": 1.0644671675235107, + "heading": 2.093277406150717, + "angularVelocity": 0.000004082207080043569, + "velocityX": -2.7018453433462057, + "velocityY": 0.8658857819477386, + "timestamp": 1.046543448124067 + }, + { + "x": 6.799049466098943, + "y": 1.1177722905775571, + "heading": 2.0932776574570204, + "angularVelocity": 0.000004082207158569437, + "velocityX": -2.7018453433469287, + "velocityY": 0.8658857819454437, + "timestamp": 1.1081048274254828 + }, + { + "x": 6.632720140103406, + "y": 1.1710774136316247, + "heading": 2.0932779087633286, + "angularVelocity": 0.0000040822072370843174, + "velocityX": -2.7018453433468057, + "velocityY": 0.8658857819457861, + "timestamp": 1.1696662067268986 + }, + { + "x": 6.466390814107878, + "y": 1.2243825366857128, + "heading": 2.0932781600696417, + "angularVelocity": 0.000004082207315896948, + "velocityX": -2.7018453433466827, + "velocityY": 0.8658857819461279, + "timestamp": 1.2312275860283144 + }, + { + "x": 6.300061488112355, + "y": 1.2776876597398226, + "heading": 2.0932784113759593, + "angularVelocity": 0.00000408220739475046, + "velocityX": -2.7018453433465606, + "velocityY": 0.8658857819464707, + "timestamp": 1.2927889653297302 + }, + { + "x": 6.133732162116841, + "y": 1.3309927827939534, + "heading": 2.093278662682282, + "angularVelocity": 0.000004082207473083141, + "velocityX": -2.7018453433464376, + "velocityY": 0.8658857819468134, + "timestamp": 1.354350344631146 + }, + { + "x": 5.967402836121334, + "y": 1.384297905848105, + "heading": 2.09327891398861, + "angularVelocity": 0.0000040822075518289365, + "velocityX": -2.701845343346315, + "velocityY": 0.8658857819471559, + "timestamp": 1.4159117239325618 + }, + { + "x": 5.801073510125836, + "y": 1.4376030289022779, + "heading": 2.093279165294942, + "angularVelocity": 0.000004082207630794512, + "velocityX": -2.701845343346192, + "velocityY": 0.8658857819474983, + "timestamp": 1.4774731032339776 + }, + { + "x": 5.634744184130344, + "y": 1.4909081519564717, + "heading": 2.093279416601279, + "angularVelocity": 0.000004082207709261434, + "velocityX": -2.701845343346069, + "velocityY": 0.8658857819478408, + "timestamp": 1.5390344825353934 + }, + { + "x": 5.468414858134862, + "y": 1.544213275010687, + "heading": 2.093279667907621, + "angularVelocity": 0.00000408220778743359, + "velocityX": -2.701845343345947, + "velocityY": 0.8658857819481832, + "timestamp": 1.6005958618368092 + }, + { + "x": 5.302085532139386, + "y": 1.597518398064923, + "heading": 2.0932799192139675, + "angularVelocity": 0.00000408220786728094, + "velocityX": -2.7018453433458234, + "velocityY": 0.8658857819485258, + "timestamp": 1.662157241138225 + }, + { + "x": 5.135756206143918, + "y": 1.65082352111918, + "heading": 2.0932801705203192, + "angularVelocity": 0.000004082207945695806, + "velocityX": -2.701845343345701, + "velocityY": 0.8658857819488687, + "timestamp": 1.7237186204396409 + }, + { + "x": 4.969426880148456, + "y": 1.7041286441734584, + "heading": 2.093280421826676, + "angularVelocity": 0.000004082208024460346, + "velocityX": -2.701845343345578, + "velocityY": 0.8658857819492114, + "timestamp": 1.7852799997410567 + }, + { + "x": 4.803097554153003, + "y": 1.7574337672277578, + "heading": 2.0932806731330373, + "angularVelocity": 0.000004082208102945811, + "velocityX": -2.7018453433454552, + "velocityY": 0.8658857819495545, + "timestamp": 1.8468413790424725 + }, + { + "x": 4.6367682281581555, + "y": 1.810738890283948, + "heading": 2.0932809244394037, + "angularVelocity": 0.0000040822081820467654, + "velocityX": -2.7018453433355996, + "velocityY": 0.8658857819802678, + "timestamp": 1.9084027583438883 + }, + { + "x": 4.470438939607256, + "y": 1.8640441301771067, + "heading": 2.0932811757459198, + "angularVelocity": 0.000004082210616647047, + "velocityX": -2.701844735097984, + "velocityY": 0.8658876798742076, + "timestamp": 1.969964137645304 + }, + { + "x": 4.306176669660201, + "y": 1.9234140327969524, + "heading": 2.093281442887588, + "angularVelocity": 0.000004339436045796883, + "velocityX": -2.668268187150199, + "velocityY": 0.9644017611944702, + "timestamp": 2.03152551694672 + }, + { + "x": 4.147951507665768, + "y": 1.9973846826411954, + "heading": 2.093281853657193, + "angularVelocity": 0.000006672521138398165, + "velocityX": -2.570201704216772, + "velocityY": 1.2015755768250211, + "timestamp": 2.0930868962481357 }, { "x": 3.9970710277557377, "y": 2.0853443145751953, "heading": 2.0933188819049127, - "angularVelocity": 5.791249741848488e-7, - "velocityX": -2.468715968828528, - "velocityY": 1.3982757097429517, - "timestamp": 2.0101843745225465 - }, - { - "x": 3.8417885319373366, - "y": 2.194746303912347, - "heading": 2.093318907323053, - "angularVelocity": 3.796579913258463e-7, - "velocityX": -2.3193766038751393, - "velocityY": 1.634082535501843, - "timestamp": 2.0771344746308946 - }, - { - "x": 3.6980071143049322, - "y": 2.31887669174007, - "heading": 2.093318931169399, - "angularVelocity": 3.5618088315780296e-7, - "velocityX": -2.147590778799685, - "velocityY": 1.854073222098814, - "timestamp": 2.1440845747392427 - }, - { - "x": 3.5671177985502105, - "y": 2.456533643810284, - "heading": 2.093318954469134, - "angularVelocity": 3.480164309169473e-7, - "velocityX": -1.9550279318910289, - "velocityY": 2.0561127145058484, - "timestamp": 2.211034674847591 - }, - { - "x": 3.440627773307266, - "y": 2.598243639821922, - "heading": 2.093318977734502, - "angularVelocity": 3.4750310659854874e-7, - "velocityX": -1.8893179403502105, - "velocityY": 2.116650995029174, - "timestamp": 2.277984774955939 - }, - { - "x": 3.314137927462374, - "y": 2.7399537959635722, - "heading": 2.093319001000051, - "angularVelocity": 3.475058166249576e-7, - "velocityX": -1.8893152607716497, - "velocityY": 2.116653386810687, - "timestamp": 2.344934875064287 - }, - { - "x": 3.1930707757983563, - "y": 2.8770243686628967, - "heading": 2.114175460284759, - "angularVelocity": 0.3115224510636341, - "velocityX": -1.8083192029300867, - "velocityY": 2.047354260523847, - "timestamp": 2.4118849751726352 - }, - { - "x": 3.082167243806988, - "y": 3.0018001495332065, - "heading": 2.1671051603213085, - "angularVelocity": 0.7905843299844287, - "velocityX": -1.6565103235378043, - "velocityY": 1.8637131336380355, - "timestamp": 2.4788350752809833 - }, - { - "x": 2.9836145948825417, - "y": 3.1125735219481654, - "heading": 2.2223290697280427, - "angularVelocity": 0.8248517824075432, - "velocityX": -1.4720313900196498, - "velocityY": 1.6545661953557886, - "timestamp": 2.5457851753893315 - }, - { - "x": 2.897439709795232, - "y": 3.2093792657888156, - "heading": 2.2747244573880905, - "angularVelocity": 0.7826035745316955, - "velocityX": -1.2871509519455429, - "velocityY": 1.4459387466782787, - "timestamp": 2.6127352754976796 - }, - { - "x": 2.8236207245224336, - "y": 3.2922706416025598, - "heading": 2.3220108807788065, - "angularVelocity": 0.7062935427160024, - "velocityX": -1.1025970857897722, - "velocityY": 1.2381068240315942, - "timestamp": 2.6796853756060277 - }, - { - "x": 2.762136351273778, - "y": 3.3612886907695043, - "heading": 2.3628840100451693, - "angularVelocity": 0.6105013913379634, - "velocityX": -0.9183611846607055, - "velocityY": 1.0308879158544917, - "timestamp": 2.746635475714376 - }, - { - "x": 2.712969884575379, - "y": 3.4164643641886956, - "heading": 2.3965008699236137, - "angularVelocity": 0.5021181420795616, - "velocityX": -0.7343748047998556, - "velocityY": 0.8241313057022762, - "timestamp": 2.813585575822724 - }, - { - "x": 2.676108490863902, - "y": 3.4578214702193035, - "heading": 2.42227370118923, - "angularVelocity": 0.3849558286530869, - "velocityX": -0.5505801134251128, - "velocityY": 0.6177303090462575, - "timestamp": 2.880535675931072 - }, - { - "x": 2.651542090312647, - "y": 3.4853787426726646, - "heading": 2.4397690611795695, - "angularVelocity": 0.2613194000012874, - "velocityX": -0.36693597935624045, - "velocityY": 0.4116091299156203, - "timestamp": 2.94748577603942 + "angularVelocity": 0.0006014850242100183, + "velocityX": -2.450895051770885, + "velocityY": 1.4288119098718277, + "timestamp": 2.1546482755495515 + }, + { + "x": 3.843205910355169, + "y": 2.1966367718038953, + "heading": 2.093318891906276, + "angularVelocity": 1.4942876205203166e-7, + "velocityX": -2.2988740829855514, + "velocityY": 1.662802783874456, + "timestamp": 2.2215789171794507 + }, + { + "x": 3.701017218385049, + "y": 2.3225052008470026, + "heading": 2.0933189013422897, + "angularVelocity": 1.409819682458666e-7, + "velocityX": -2.124418480198765, + "velocityY": 1.8805800449233931, + "timestamp": 2.28850955880935 + }, + { + "x": 3.5718797746002595, + "y": 2.4617316574789556, + "heading": 2.093318910626833, + "angularVelocity": 1.387188756626878e-7, + "velocityX": -1.9294218707609325, + "velocityY": 2.0801601963092122, + "timestamp": 2.355440200439249 + }, + { + "x": 3.4447158253880024, + "y": 2.6027629151415783, + "heading": 2.0933189199075177, + "angularVelocity": 1.3866122154045785e-7, + "velocityX": -1.8999362043385877, + "velocityY": 2.107125439532927, + "timestamp": 2.422370842069148 + }, + { + "x": 3.317551912654746, + "y": 2.7437942056962745, + "heading": 2.0933189291882313, + "angularVelocity": 1.3866165525269637e-7, + "velocityX": -1.8999356593116823, + "velocityY": 2.1071259309681434, + "timestamp": 2.4893014836990472 + }, + { + "x": 3.1958691275778026, + "y": 2.880185747480273, + "heading": 2.114236602845924, + "angularVelocity": 0.3125276128885705, + "velocityX": -1.8180430086088528, + "velocityY": 2.0378041874780046, + "timestamp": 2.5562321253289464 + }, + { + "x": 3.084402838552189, + "y": 3.004327638614834, + "heading": 2.167146744456709, + "angularVelocity": 0.7905219541052365, + "velocityX": -1.6653999769190886, + "velocityY": 1.8547841184762293, + "timestamp": 2.6231627669588455 + }, + { + "x": 2.9853512622212666, + "y": 3.1145382871506517, + "heading": 2.222354458529675, + "angularVelocity": 0.8248496163871107, + "velocityX": -1.4799137423280575, + "velocityY": 1.6466396534077914, + "timestamp": 2.6900934085887447 + }, + { + "x": 2.8987408213697994, + "y": 3.210852203380145, + "heading": 2.2747381958649533, + "angularVelocity": 0.7826570321100548, + "velocityX": -1.294032729140559, + "velocityY": 1.439010801092748, + "timestamp": 2.757024050218644 + }, + { + "x": 2.8245492222018562, + "y": 3.2933223707153383, + "heading": 2.322017090666629, + "angularVelocity": 0.7063863971767974, + "velocityX": -1.1084848039884865, + "velocityY": 1.2321735654533488, + "timestamp": 2.823954691848543 + }, + { + "x": 2.762754840052126, + "y": 3.361989636833625, + "heading": 2.36288597520057, + "angularVelocity": 0.6106154601046585, + "velocityX": -0.9232599694984354, + "velocityY": 1.0259466284215513, + "timestamp": 2.890885333478442 + }, + { + "x": 2.713340706240203, + "y": 3.416884822801194, + "heading": 2.39650092018746, + "angularVelocity": 0.5022355107959039, + "velocityX": -0.738288661345317, + "velocityY": 0.8201801840047823, + "timestamp": 2.9578159751083413 + }, + { + "x": 2.6762937800618425, + "y": 3.4580316490257124, + "heading": 2.422273262441906, + "angularVelocity": 0.38506043908793497, + "velocityX": -0.5535121922663854, + "velocityY": 0.6147681424009704, + "timestamp": 3.0247466167382404 + }, + { + "x": 2.6516038167671496, + "y": 3.4854487858585643, + "heading": 2.439768796706212, + "angularVelocity": 0.261397976147448, + "velocityX": -0.3688887883552444, + "velocityY": 0.40963505152779395, + "timestamp": 3.0916772583681396 }, { "x": 2.6392624378204346, "y": 3.4991512298583984, "heading": 2.448651867948598, - "angularVelocity": 0.13267802071472284, - "velocityX": -0.18341499822016766, - "velocityY": 0.20571271982335043, - "timestamp": 3.0144358761477683 + "angularVelocity": 0.13272054512051312, + "velocityX": -0.18439056680433819, + "velocityY": 0.20472602183620905, + "timestamp": 3.1586078999980387 }, { "x": 2.6392624378204346, "y": 3.4991512298583984, "heading": 2.448651867948598, - "angularVelocity": 2.0995005926462733e-27, - "velocityX": 2.2917468406814997e-27, - "velocityY": -2.2636416821297325e-27, - "timestamp": 3.0813859762561164 + "angularVelocity": -1.6679097586736474e-24, + "velocityX": 3.3994819825962006e-22, + "velocityY": 2.9924033131311345e-22, + "timestamp": 3.225538541627938 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 1.1.traj b/src/main/deploy/choreo/Close 2 Note 1.1.traj index 3112e79..e5c32bd 100644 --- a/src/main/deploy/choreo/Close 2 Note 1.1.traj +++ b/src/main/deploy/choreo/Close 2 Note 1.1.traj @@ -4,127 +4,127 @@ "x": 0.4625682830810547, "y": 4.048226833343506, "heading": -3.141592530780722, - "angularVelocity": 2.456306710221323e-28, - "velocityX": 1.788801844274686e-27, - "velocityY": -7.82277930953085e-28, + "angularVelocity": 5.759379414997178e-23, + "velocityX": 3.901586197109837e-23, + "velocityY": 3.074737368166392e-23, "timestamp": 0 }, { - "x": 0.5005081965427643, - "y": 4.031977412921586, - "heading": -3.1694044453423356, - "angularVelocity": -0.39190233323436413, - "velocityX": 0.534617657314254, - "velocityY": -0.22897329714388057, - "timestamp": 0.07096644291980092 + "x": 0.49719645689563025, + "y": 4.035311227037971, + "heading": -3.1685878765684166, + "angularVelocity": -0.40141935074427276, + "velocityX": 0.5149190960333256, + "velocityY": -0.19205437627695554, + "timestamp": 0.06724973705837163 }, { - "x": 0.5764839525466269, - "y": 3.9994312658249234, - "heading": -3.2229174521355954, - "angularVelocity": -0.7540607164675686, - "velocityX": 1.0705870673231095, - "velocityY": -0.4586131945973854, - "timestamp": 0.14193288583960184 + "x": 0.5665241731222844, + "y": 4.009400966165659, + "heading": -3.2208545680350382, + "angularVelocity": -0.7772029119051421, + "velocityX": 1.0308994393015785, + "velocityY": -0.38528419597866803, + "timestamp": 0.13449947411674326 }, { - "x": 0.6906355955542217, - "y": 3.9505270752114026, - "heading": -3.298712638550308, - "angularVelocity": -1.0680426310836606, - "velocityX": 1.6085298672303114, - "velocityY": -0.6891171179142679, - "timestamp": 0.21289932875940276 + "x": 0.6706558658845477, + "y": 3.9703839131162932, + "heading": -3.295610335882003, + "angularVelocity": -1.1116142771246529, + "velocityX": 1.5484327124116424, + "velocityY": -0.5801814959588587, + "timestamp": 0.2017492111751149 }, { - "x": 0.843174414098898, - "y": 3.8851948830384924, - "heading": -3.390518020683689, - "angularVelocity": -1.2936449729787134, - "velocityX": 2.1494499691503526, - "velocityY": -0.9206068316928638, - "timestamp": 0.2838657716792037 + "x": 0.8097504058175827, + "y": 3.918094176827053, + "heading": -3.387768258841931, + "angularVelocity": -1.370383394658279, + "velocityX": 2.06832838338539, + "velocityY": -0.7775455871872714, + "timestamp": 0.2689989482334865 }, { - "x": 1.0343457963046487, - "y": 3.803412500195947, - "heading": -3.484313484917418, - "angularVelocity": -1.3216875522382732, - "velocityX": 2.6938278761103085, - "velocityY": -1.1524092159299486, - "timestamp": 0.3548322145990046 + "x": 0.9839944627129134, + "y": 3.8522600039621713, + "heading": -3.486174940623861, + "angularVelocity": -1.4633021047578973, + "velocityX": 2.5909998241939562, + "velocityY": -0.9789506360112324, + "timestamp": 0.33624868529185814 }, { - "x": 1.2602014683101048, - "y": 3.7073597002170535, - "heading": -3.5049392152200776, - "angularVelocity": -0.29064061060477225, - "velocityX": 3.1825699966489127, - "velocityY": -1.353496047243663, - "timestamp": 0.4257986575188055 + "x": 1.1913257283544783, + "y": 3.772870322561364, + "heading": -3.5398918687364964, + "angularVelocity": -0.7987678534119754, + "velocityX": 3.0830048519238766, + "velocityY": -1.180520324293636, + "timestamp": 0.40349842235022976 }, { - "x": 1.486615288654731, - "y": 3.610382149167226, - "heading": -3.5229483465773357, - "angularVelocity": -0.2537696778406977, - "velocityX": 3.190434958118108, - "velocityY": -1.3665268690361414, - "timestamp": 0.49676510043860644 + "x": 1.3968116539650544, + "y": 3.6932929076649863, + "heading": -3.596464396112961, + "angularVelocity": -0.8412304620218959, + "velocityX": 3.055564744174655, + "velocityY": -1.1833119113507133, + "timestamp": 0.4707481594086014 }, { - "x": 1.6779759070617781, - "y": 3.5282783831779434, - "heading": -3.6088796447422054, - "angularVelocity": -1.2108722746887648, - "velocityX": 2.6964944350290114, - "velocityY": -1.156937879528049, - "timestamp": 0.5677315433584074 + "x": 1.5702772156145808, + "y": 3.627313135982868, + "heading": -3.7079920004982503, + "angularVelocity": -1.6584095234228897, + "velocityX": 2.5794236414480194, + "velocityY": -0.9811156826509139, + "timestamp": 0.537997896466973 }, { - "x": 1.8307040868272946, - "y": 3.4626308105116546, - "heading": -3.6935584190600492, - "angularVelocity": -1.193222751963764, - "velocityX": 2.1521182897403275, - "velocityY": -0.9250509108999159, - "timestamp": 0.6386979862782083 + "x": 1.7086059487553384, + "y": 3.5748707707048286, + "heading": -3.812251230661302, + "angularVelocity": -1.5503291867529019, + "velocityX": 2.0569408772660527, + "velocityY": -0.7798151721027226, + "timestamp": 0.6052476335253447 }, { - "x": 1.9450099109679613, - "y": 3.4134373887939176, - "heading": -3.763746700694216, - "angularVelocity": -0.9890347993556098, - "velocityX": 1.6107024593277632, - "velocityY": -0.6931927217111649, - "timestamp": 0.7096644291980092 + "x": 1.8120769868060278, + "y": 3.5356871260751257, + "heading": -3.8965156601796274, + "angularVelocity": -1.2530075685676696, + "velocityX": 1.5386088121188892, + "velocityY": -0.5826587038652028, + "timestamp": 0.6724973705837164 }, { - "x": 2.0210874791239872, - "y": 3.380664133786014, - "heading": -3.8135102476904286, - "angularVelocity": -0.701226452232503, - "velocityX": 1.0720217193639106, - "velocityY": -0.461813410106245, - "timestamp": 0.7806308721178101 + "x": 1.880917107973145, + "y": 3.5096285360361232, + "heading": -3.9553366407492576, + "angularVelocity": -0.8746648409728991, + "velocityX": 1.023648926796031, + "velocityY": -0.38748984276893983, + "timestamp": 0.7397471076420881 }, { - "x": 2.059075355529785, - "y": 3.3642866611480713, - "heading": -3.839495466368024, - "angularVelocity": -0.36616205643788113, - "velocityX": 0.5352935111701761, - "velocityY": -0.23077770230714698, - "timestamp": 0.851597315037611 + "x": 1.9152796268463137, + "y": 3.496621608734131, + "heading": -3.985746630194921, + "angularVelocity": -0.4521949196510347, + "velocityX": 0.5109688212363194, + "velocityY": -0.19341231461921185, + "timestamp": 0.8069968447004597 }, { - "x": 2.059075355529785, - "y": 3.3642866611480713, - "heading": -3.839495466368024, - "angularVelocity": -2.0248765755018437e-27, - "velocityX": 6.656997760587202e-28, - "velocityY": -7.813409565467552e-28, - "timestamp": 0.922563757957412 + "x": 1.9152796268463137, + "y": 3.496621608734131, + "heading": -3.985746630194921, + "angularVelocity": -7.373947357741041e-23, + "velocityX": 1.569986147768196e-24, + "velocityY": 8.224120381678207e-23, + "timestamp": 0.8742465817588314 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 1.traj b/src/main/deploy/choreo/Close 2 Note 1.traj index 3112e79..e5c32bd 100644 --- a/src/main/deploy/choreo/Close 2 Note 1.traj +++ b/src/main/deploy/choreo/Close 2 Note 1.traj @@ -4,127 +4,127 @@ "x": 0.4625682830810547, "y": 4.048226833343506, "heading": -3.141592530780722, - "angularVelocity": 2.456306710221323e-28, - "velocityX": 1.788801844274686e-27, - "velocityY": -7.82277930953085e-28, + "angularVelocity": 5.759379414997178e-23, + "velocityX": 3.901586197109837e-23, + "velocityY": 3.074737368166392e-23, "timestamp": 0 }, { - "x": 0.5005081965427643, - "y": 4.031977412921586, - "heading": -3.1694044453423356, - "angularVelocity": -0.39190233323436413, - "velocityX": 0.534617657314254, - "velocityY": -0.22897329714388057, - "timestamp": 0.07096644291980092 + "x": 0.49719645689563025, + "y": 4.035311227037971, + "heading": -3.1685878765684166, + "angularVelocity": -0.40141935074427276, + "velocityX": 0.5149190960333256, + "velocityY": -0.19205437627695554, + "timestamp": 0.06724973705837163 }, { - "x": 0.5764839525466269, - "y": 3.9994312658249234, - "heading": -3.2229174521355954, - "angularVelocity": -0.7540607164675686, - "velocityX": 1.0705870673231095, - "velocityY": -0.4586131945973854, - "timestamp": 0.14193288583960184 + "x": 0.5665241731222844, + "y": 4.009400966165659, + "heading": -3.2208545680350382, + "angularVelocity": -0.7772029119051421, + "velocityX": 1.0308994393015785, + "velocityY": -0.38528419597866803, + "timestamp": 0.13449947411674326 }, { - "x": 0.6906355955542217, - "y": 3.9505270752114026, - "heading": -3.298712638550308, - "angularVelocity": -1.0680426310836606, - "velocityX": 1.6085298672303114, - "velocityY": -0.6891171179142679, - "timestamp": 0.21289932875940276 + "x": 0.6706558658845477, + "y": 3.9703839131162932, + "heading": -3.295610335882003, + "angularVelocity": -1.1116142771246529, + "velocityX": 1.5484327124116424, + "velocityY": -0.5801814959588587, + "timestamp": 0.2017492111751149 }, { - "x": 0.843174414098898, - "y": 3.8851948830384924, - "heading": -3.390518020683689, - "angularVelocity": -1.2936449729787134, - "velocityX": 2.1494499691503526, - "velocityY": -0.9206068316928638, - "timestamp": 0.2838657716792037 + "x": 0.8097504058175827, + "y": 3.918094176827053, + "heading": -3.387768258841931, + "angularVelocity": -1.370383394658279, + "velocityX": 2.06832838338539, + "velocityY": -0.7775455871872714, + "timestamp": 0.2689989482334865 }, { - "x": 1.0343457963046487, - "y": 3.803412500195947, - "heading": -3.484313484917418, - "angularVelocity": -1.3216875522382732, - "velocityX": 2.6938278761103085, - "velocityY": -1.1524092159299486, - "timestamp": 0.3548322145990046 + "x": 0.9839944627129134, + "y": 3.8522600039621713, + "heading": -3.486174940623861, + "angularVelocity": -1.4633021047578973, + "velocityX": 2.5909998241939562, + "velocityY": -0.9789506360112324, + "timestamp": 0.33624868529185814 }, { - "x": 1.2602014683101048, - "y": 3.7073597002170535, - "heading": -3.5049392152200776, - "angularVelocity": -0.29064061060477225, - "velocityX": 3.1825699966489127, - "velocityY": -1.353496047243663, - "timestamp": 0.4257986575188055 + "x": 1.1913257283544783, + "y": 3.772870322561364, + "heading": -3.5398918687364964, + "angularVelocity": -0.7987678534119754, + "velocityX": 3.0830048519238766, + "velocityY": -1.180520324293636, + "timestamp": 0.40349842235022976 }, { - "x": 1.486615288654731, - "y": 3.610382149167226, - "heading": -3.5229483465773357, - "angularVelocity": -0.2537696778406977, - "velocityX": 3.190434958118108, - "velocityY": -1.3665268690361414, - "timestamp": 0.49676510043860644 + "x": 1.3968116539650544, + "y": 3.6932929076649863, + "heading": -3.596464396112961, + "angularVelocity": -0.8412304620218959, + "velocityX": 3.055564744174655, + "velocityY": -1.1833119113507133, + "timestamp": 0.4707481594086014 }, { - "x": 1.6779759070617781, - "y": 3.5282783831779434, - "heading": -3.6088796447422054, - "angularVelocity": -1.2108722746887648, - "velocityX": 2.6964944350290114, - "velocityY": -1.156937879528049, - "timestamp": 0.5677315433584074 + "x": 1.5702772156145808, + "y": 3.627313135982868, + "heading": -3.7079920004982503, + "angularVelocity": -1.6584095234228897, + "velocityX": 2.5794236414480194, + "velocityY": -0.9811156826509139, + "timestamp": 0.537997896466973 }, { - "x": 1.8307040868272946, - "y": 3.4626308105116546, - "heading": -3.6935584190600492, - "angularVelocity": -1.193222751963764, - "velocityX": 2.1521182897403275, - "velocityY": -0.9250509108999159, - "timestamp": 0.6386979862782083 + "x": 1.7086059487553384, + "y": 3.5748707707048286, + "heading": -3.812251230661302, + "angularVelocity": -1.5503291867529019, + "velocityX": 2.0569408772660527, + "velocityY": -0.7798151721027226, + "timestamp": 0.6052476335253447 }, { - "x": 1.9450099109679613, - "y": 3.4134373887939176, - "heading": -3.763746700694216, - "angularVelocity": -0.9890347993556098, - "velocityX": 1.6107024593277632, - "velocityY": -0.6931927217111649, - "timestamp": 0.7096644291980092 + "x": 1.8120769868060278, + "y": 3.5356871260751257, + "heading": -3.8965156601796274, + "angularVelocity": -1.2530075685676696, + "velocityX": 1.5386088121188892, + "velocityY": -0.5826587038652028, + "timestamp": 0.6724973705837164 }, { - "x": 2.0210874791239872, - "y": 3.380664133786014, - "heading": -3.8135102476904286, - "angularVelocity": -0.701226452232503, - "velocityX": 1.0720217193639106, - "velocityY": -0.461813410106245, - "timestamp": 0.7806308721178101 + "x": 1.880917107973145, + "y": 3.5096285360361232, + "heading": -3.9553366407492576, + "angularVelocity": -0.8746648409728991, + "velocityX": 1.023648926796031, + "velocityY": -0.38748984276893983, + "timestamp": 0.7397471076420881 }, { - "x": 2.059075355529785, - "y": 3.3642866611480713, - "heading": -3.839495466368024, - "angularVelocity": -0.36616205643788113, - "velocityX": 0.5352935111701761, - "velocityY": -0.23077770230714698, - "timestamp": 0.851597315037611 + "x": 1.9152796268463137, + "y": 3.496621608734131, + "heading": -3.985746630194921, + "angularVelocity": -0.4521949196510347, + "velocityX": 0.5109688212363194, + "velocityY": -0.19341231461921185, + "timestamp": 0.8069968447004597 }, { - "x": 2.059075355529785, - "y": 3.3642866611480713, - "heading": -3.839495466368024, - "angularVelocity": -2.0248765755018437e-27, - "velocityX": 6.656997760587202e-28, - "velocityY": -7.813409565467552e-28, - "timestamp": 0.922563757957412 + "x": 1.9152796268463137, + "y": 3.496621608734131, + "heading": -3.985746630194921, + "angularVelocity": -7.373947357741041e-23, + "velocityX": 1.569986147768196e-24, + "velocityY": 8.224120381678207e-23, + "timestamp": 0.8742465817588314 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 2.1.traj b/src/main/deploy/choreo/Close 2 Note 2.1.traj index f101170..6b4e36c 100644 --- a/src/main/deploy/choreo/Close 2 Note 2.1.traj +++ b/src/main/deploy/choreo/Close 2 Note 2.1.traj @@ -1,193 +1,274 @@ { "samples": [ { - "x": 2.162928342819214, - "y": 3.453303337097168, + "x": 2.36685562133789, + "y": 4.003089904785156, "heading": 2.647, - "angularVelocity": 7.022113809915831e-24, - "velocityX": -3.4078579548884074e-24, - "velocityY": 1.7716860021550897e-23, + "angularVelocity": 1.515129418425611e-22, + "velocityX": -1.1699452732974461e-22, + "velocityY": 8.049673997142983e-22, "timestamp": 0 }, { - "x": 2.1425389520968783, - "y": 3.467250631324219, - "heading": 2.6339713166813827, - "angularVelocity": -0.23809760264625404, - "velocityX": -0.37261363498404976, - "velocityY": 0.2548851052444871, - "timestamp": 0.05471992650834813 - }, - { - "x": 2.1020164250363758, - "y": 3.495473507452917, - "heading": 2.607355879948924, - "angularVelocity": -0.48639386839085036, - "velocityX": -0.7405442522719768, - "velocityY": 0.5157696278044568, - "timestamp": 0.10943985301669626 - }, - { - "x": 2.0417796658114202, - "y": 3.538482116702539, - "heading": 2.566266686397403, - "angularVelocity": -0.7509000134576662, - "velocityX": -1.1008194467469847, - "velocityY": 0.7859771018343888, - "timestamp": 0.1641597795250444 - }, - { - "x": 1.9626298042618509, - "y": 3.597171847629263, - "heading": 2.50909305220371, - "angularVelocity": -1.044841209444435, - "velocityX": -1.446454090860199, - "velocityY": 1.0725476928001763, - "timestamp": 0.21887970603339252 - }, - { - "x": 1.8666391340457336, - "y": 3.673472522298835, - "heading": 2.432100025693096, - "angularVelocity": -1.4070381929125568, - "velocityX": -1.7542178204766536, - "velocityY": 1.3943855472454167, - "timestamp": 0.27359963254174063 - }, - { - "x": 1.7650455993955068, - "y": 3.7727148333401828, - "heading": 2.326049396431328, - "angularVelocity": -1.9380623481939154, - "velocityX": -1.8566094863948293, - "velocityY": 1.8136411609801288, - "timestamp": 0.32831955905008875 - }, - { - "x": 1.6882967509494158, - "y": 3.87295238025058, - "heading": 2.211959011327005, - "angularVelocity": -2.0849879081419744, - "velocityX": -1.4025758684888243, - "velocityY": 1.8318289754118107, - "timestamp": 0.38303948555843687 - }, - { - "x": 1.6354914799537714, - "y": 3.9661527572411814, - "heading": 2.1004292198963976, - "angularVelocity": -2.0381933702632526, - "velocityX": -0.9650099034323164, - "velocityY": 1.7032255512328316, - "timestamp": 0.437759412066785 - }, - { - "x": 1.6058693049320312, - "y": 4.050214103514873, - "heading": 1.9943710354808801, - "angularVelocity": -1.938200417709569, - "velocityX": -0.5413416448434165, - "velocityY": 1.536210876687981, - "timestamp": 0.4924793385751331 - }, - { - "x": 1.5990118210425874, - "y": 4.124201694067412, - "heading": 1.895096592298551, - "angularVelocity": -1.814228371947535, - "velocityX": -0.12531968383396322, - "velocityY": 1.3521142164043614, - "timestamp": 0.5471992650834813 - }, - { - "x": 1.6146607398986816, - "y": 4.187590599060059, - "heading": 1.8033532796155989, - "angularVelocity": -1.6765978782693645, - "velocityX": 0.28598208832950334, - "velocityY": 1.158424527177966, - "timestamp": 0.6019191915918294 - }, - { - "x": 1.6905120212342348, - "y": 4.258849164477337, - "heading": 1.6820774903470974, - "angularVelocity": -1.4527350214804762, - "velocityX": 0.9086052004688632, - "velocityY": 0.8535901038990373, - "timestamp": 0.6854002032527928 - }, - { - "x": 1.8179066252511533, - "y": 4.303857525516893, - "heading": 1.5806423784124122, - "angularVelocity": -1.2150680725652636, - "velocityX": 1.5260309078943428, - "velocityY": 0.5391448922821541, - "timestamp": 0.7688812149137563 - }, - { - "x": 1.9959545196143786, - "y": 4.321064540954123, - "heading": 1.501233745672766, - "angularVelocity": -0.9512179016486254, - "velocityX": 2.1327951209590146, - "velocityY": 0.20611891368915536, - "timestamp": 0.8523622265747197 - }, - { - "x": 2.221863436317024, - "y": 4.306254552246914, - "heading": 1.4497152819618502, - "angularVelocity": -0.6171279274878088, - "velocityX": 2.706111392374063, - "velocityY": -0.1774054771563637, - "timestamp": 0.9358432382356832 - }, - { - "x": 2.443298757309218, - "y": 4.237785772933149, - "heading": 1.481002566027846, - "angularVelocity": 0.37478324044587696, - "velocityX": 2.652523209607188, - "velocityY": -0.8201718924039002, - "timestamp": 1.0193242498966466 - }, - { - "x": 2.6077641513687433, - "y": 4.180765687739868, - "heading": 1.5117621140320452, - "angularVelocity": 0.3684616105171448, - "velocityX": 1.9700934474471774, - "velocityY": -0.6830305965247938, - "timestamp": 1.10280526155761 - }, - { - "x": 2.7168458549001153, - "y": 4.14118234012958, - "heading": 1.5343226948830555, - "angularVelocity": 0.2702480528462504, - "velocityX": 1.3066648494196378, - "velocityY": -0.47415989364199385, - "timestamp": 1.1862862732185735 - }, - { - "x": 2.771209239959717, - "y": 4.120928764343262, - "heading": 1.5462110292578024, - "angularVelocity": 0.14240764622053664, - "velocityX": 0.651206591510705, - "velocityY": -0.24261296531207407, - "timestamp": 1.269767284879537 - }, - { - "x": 2.771209239959717, - "y": 4.120928764343262, - "heading": 1.5462110292578024, - "angularVelocity": 1.3345092530168404e-23, - "velocityX": -1.0052627512755276e-23, - "velocityY": 2.977034014839026e-23, - "timestamp": 1.3532482965405004 + "x": 2.3582182665685285, + "y": 4.005140044275413, + "heading": 2.626572128421542, + "angularVelocity": -0.5800138561279072, + "velocityX": -0.24524265424721656, + "velocityY": 0.05821014229392081, + "timestamp": 0.0352196268462121 + }, + { + "x": 2.340949746064586, + "y": 4.0093581184378255, + "heading": 2.5859880369259955, + "angularVelocity": -1.1523146361759788, + "velocityX": -0.4903095816263469, + "velocityY": 0.11976487374015862, + "timestamp": 0.0704392536924242 + }, + { + "x": 2.315025310616676, + "y": 4.0158942358801255, + "heading": 2.5258878520188563, + "angularVelocity": -1.7064401383231338, + "velocityX": -0.7360792197199094, + "velocityY": 0.18558167782148904, + "timestamp": 0.10565888053863629 + }, + { + "x": 2.2803832616948174, + "y": 4.024926514000347, + "heading": 2.4473235520751233, + "angularVelocity": -2.230696545616096, + "velocityX": -0.9836006801903231, + "velocityY": 0.25645581537904877, + "timestamp": 0.1408785073848484 + }, + { + "x": 2.2369349430382, + "y": 4.036673464094862, + "heading": 2.351697628937878, + "angularVelocity": -2.7151316382424904, + "velocityX": -1.2336393808559734, + "velocityY": 0.3335342008518269, + "timestamp": 0.1760981342310605 + }, + { + "x": 2.184628025978604, + "y": 4.05150435046694, + "heading": 2.240519075284163, + "angularVelocity": -3.1567215104004607, + "velocityX": -1.485163862980013, + "velocityY": 0.42109720346804624, + "timestamp": 0.2113177610772726 + }, + { + "x": 2.1236673593002187, + "y": 4.070437803424587, + "heading": 2.115442529811484, + "angularVelocity": -3.5513307968545753, + "velocityX": -1.7308720204371333, + "velocityY": 0.5375824406184673, + "timestamp": 0.2465373879234847 + }, + { + "x": 2.056940171048295, + "y": 4.097882582880828, + "heading": 1.9863138442222075, + "angularVelocity": -3.6663842621934117, + "velocityX": -1.8946023631436484, + "velocityY": 0.779246741484578, + "timestamp": 0.2817570147696968 + }, + { + "x": 1.9961028754422039, + "y": 4.13376425540664, + "heading": 1.8534458383236918, + "angularVelocity": -3.7725557536054866, + "velocityX": -1.727369113586192, + "velocityY": 1.0187976346956282, + "timestamp": 0.3169766416159089 + }, + { + "x": 1.9435464476369881, + "y": 4.17533041611823, + "heading": 1.7117204090724802, + "angularVelocity": -4.0240468722187535, + "velocityX": -1.4922482862928892, + "velocityY": 1.1801987821502666, + "timestamp": 0.352196268462121 + }, + { + "x": 1.8999454238017455, + "y": 4.221481056301366, + "heading": 1.5613962576553257, + "angularVelocity": -4.268192620936928, + "velocityX": -1.2379751786044981, + "velocityY": 1.3103670968625691, + "timestamp": 0.3874158953083331 + }, + { + "x": 1.8656935910383048, + "y": 4.271628463138376, + "heading": 1.4043136517982262, + "angularVelocity": -4.460087170798457, + "velocityX": -0.9725211715890388, + "velocityY": 1.4238483291143482, + "timestamp": 0.4226355221545452 + }, + { + "x": 1.8411062322404879, + "y": 4.325262090117391, + "heading": 1.2429884596951064, + "angularVelocity": -4.580548022486237, + "velocityX": -0.6981152555995893, + "velocityY": 1.5228334818326028, + "timestamp": 0.4578551490007573 + }, + { + "x": 1.8264926299504727, + "y": 4.381633415364751, + "heading": 1.0814768977873637, + "angularVelocity": -4.585839668687841, + "velocityX": -0.4149278001666941, + "velocityY": 1.6005656588446922, + "timestamp": 0.4930747758469694 + }, + { + "x": 1.8219761515922521, + "y": 4.439993109865742, + "heading": 0.9236480211125673, + "angularVelocity": -4.48127623168645, + "velocityX": -0.1282375414691572, + "velocityY": 1.657021942787722, + "timestamp": 0.5282944026931815 + }, + { + "x": 1.8275461196899416, + "y": 4.499822616577148, + "heading": 0.7720656380191331, + "angularVelocity": -4.303917919270544, + "velocityX": 0.1581495488866161, + "velocityY": 1.6987547021055334, + "timestamp": 0.5635140295393936 + }, + { + "x": 1.863631380068701, + "y": 4.6009476633877675, + "heading": 0.5458535834271052, + "angularVelocity": -3.9160123080463913, + "velocityX": 0.624680784749141, + "velocityY": 1.750600465020928, + "timestamp": 0.6212799497657474 + }, + { + "x": 1.9264611646703016, + "y": 4.704317253907965, + "heading": 0.34642581420132385, + "angularVelocity": -3.452342980849784, + "velocityX": 1.0876617970494085, + "velocityY": 1.7894563111806223, + "timestamp": 0.6790458699921013 + }, + { + "x": 2.015600664927885, + "y": 4.808735070353454, + "heading": 0.18046889819718334, + "angularVelocity": -2.8729208390317975, + "velocityX": 1.5431157317030553, + "velocityY": 1.8076024070304682, + "timestamp": 0.7368117902184551 + }, + { + "x": 2.1302379264615894, + "y": 4.912412090690155, + "heading": 0.05682984585261984, + "angularVelocity": -2.1403459316511855, + "velocityX": 1.98451372512552, + "velocityY": 1.7947783040671845, + "timestamp": 0.794577710444809 + }, + { + "x": 2.2701009625494035, + "y": 5.012833103004185, + "heading": -0.02381934322313901, + "angularVelocity": -1.3961378743684443, + "velocityX": 2.4212032897555655, + "velocityY": 1.7384127513329013, + "timestamp": 0.8523436306711628 + }, + { + "x": 2.432555238928894, + "y": 5.102059929533658, + "heading": -0.06105728015599246, + "angularVelocity": -0.6446350510290115, + "velocityX": 2.8122857862028177, + "velocityY": 1.5446274581939572, + "timestamp": 0.9101095508975167 + }, + { + "x": 2.5850691468596714, + "y": 5.167502593301118, + "heading": -0.059583445184643466, + "angularVelocity": 0.025513918337556375, + "velocityX": 2.6402056321984575, + "velocityY": 1.132893988549338, + "timestamp": 0.9678754711238705 + }, + { + "x": 2.714659135727784, + "y": 5.217422693936502, + "heading": -0.0482224195291295, + "angularVelocity": 0.1966734990284264, + "velocityX": 2.2433640520283893, + "velocityY": 0.8641790945216953, + "timestamp": 1.0256413913502245 + }, + { + "x": 2.8192112721940457, + "y": 5.255223857981348, + "heading": -0.034645348058129244, + "angularVelocity": 0.23503601116019512, + "velocityX": 1.8099276538238398, + "velocityY": 0.6543852135776276, + "timestamp": 1.0834073115765783 + }, + { + "x": 2.8980093999029637, + "y": 5.282500252165217, + "heading": -0.022245861805103868, + "angularVelocity": 0.2146505448963401, + "velocityX": 1.3640936974626836, + "velocityY": 0.4721883435253398, + "timestamp": 1.1411732318029322 + }, + { + "x": 2.950712826385929, + "y": 5.300160015762531, + "heading": -0.012907289054399772, + "angularVelocity": 0.1616623212113855, + "velocityX": 0.9123619302949761, + "velocityY": 0.3057124949811315, + "timestamp": 1.198939152029286 + }, + { + "x": 2.9771268367767334, + "y": 5.308786869049072, + "heading": -0.007825065024634177, + "angularVelocity": 0.08797962552749206, + "velocityX": 0.4572594063643587, + "velocityY": 0.14934157116754843, + "timestamp": 1.2567050722556399 + }, + { + "x": 2.9771268367767334, + "y": 5.308786869049072, + "heading": -0.007825065024634177, + "angularVelocity": -9.033896437363216e-23, + "velocityX": 2.001984212704135e-22, + "velocityY": -4.919331709829794e-22, + "timestamp": 1.3144709924819937 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 2.traj b/src/main/deploy/choreo/Close 2 Note 2.traj index f101170..6b4e36c 100644 --- a/src/main/deploy/choreo/Close 2 Note 2.traj +++ b/src/main/deploy/choreo/Close 2 Note 2.traj @@ -1,193 +1,274 @@ { "samples": [ { - "x": 2.162928342819214, - "y": 3.453303337097168, + "x": 2.36685562133789, + "y": 4.003089904785156, "heading": 2.647, - "angularVelocity": 7.022113809915831e-24, - "velocityX": -3.4078579548884074e-24, - "velocityY": 1.7716860021550897e-23, + "angularVelocity": 1.515129418425611e-22, + "velocityX": -1.1699452732974461e-22, + "velocityY": 8.049673997142983e-22, "timestamp": 0 }, { - "x": 2.1425389520968783, - "y": 3.467250631324219, - "heading": 2.6339713166813827, - "angularVelocity": -0.23809760264625404, - "velocityX": -0.37261363498404976, - "velocityY": 0.2548851052444871, - "timestamp": 0.05471992650834813 - }, - { - "x": 2.1020164250363758, - "y": 3.495473507452917, - "heading": 2.607355879948924, - "angularVelocity": -0.48639386839085036, - "velocityX": -0.7405442522719768, - "velocityY": 0.5157696278044568, - "timestamp": 0.10943985301669626 - }, - { - "x": 2.0417796658114202, - "y": 3.538482116702539, - "heading": 2.566266686397403, - "angularVelocity": -0.7509000134576662, - "velocityX": -1.1008194467469847, - "velocityY": 0.7859771018343888, - "timestamp": 0.1641597795250444 - }, - { - "x": 1.9626298042618509, - "y": 3.597171847629263, - "heading": 2.50909305220371, - "angularVelocity": -1.044841209444435, - "velocityX": -1.446454090860199, - "velocityY": 1.0725476928001763, - "timestamp": 0.21887970603339252 - }, - { - "x": 1.8666391340457336, - "y": 3.673472522298835, - "heading": 2.432100025693096, - "angularVelocity": -1.4070381929125568, - "velocityX": -1.7542178204766536, - "velocityY": 1.3943855472454167, - "timestamp": 0.27359963254174063 - }, - { - "x": 1.7650455993955068, - "y": 3.7727148333401828, - "heading": 2.326049396431328, - "angularVelocity": -1.9380623481939154, - "velocityX": -1.8566094863948293, - "velocityY": 1.8136411609801288, - "timestamp": 0.32831955905008875 - }, - { - "x": 1.6882967509494158, - "y": 3.87295238025058, - "heading": 2.211959011327005, - "angularVelocity": -2.0849879081419744, - "velocityX": -1.4025758684888243, - "velocityY": 1.8318289754118107, - "timestamp": 0.38303948555843687 - }, - { - "x": 1.6354914799537714, - "y": 3.9661527572411814, - "heading": 2.1004292198963976, - "angularVelocity": -2.0381933702632526, - "velocityX": -0.9650099034323164, - "velocityY": 1.7032255512328316, - "timestamp": 0.437759412066785 - }, - { - "x": 1.6058693049320312, - "y": 4.050214103514873, - "heading": 1.9943710354808801, - "angularVelocity": -1.938200417709569, - "velocityX": -0.5413416448434165, - "velocityY": 1.536210876687981, - "timestamp": 0.4924793385751331 - }, - { - "x": 1.5990118210425874, - "y": 4.124201694067412, - "heading": 1.895096592298551, - "angularVelocity": -1.814228371947535, - "velocityX": -0.12531968383396322, - "velocityY": 1.3521142164043614, - "timestamp": 0.5471992650834813 - }, - { - "x": 1.6146607398986816, - "y": 4.187590599060059, - "heading": 1.8033532796155989, - "angularVelocity": -1.6765978782693645, - "velocityX": 0.28598208832950334, - "velocityY": 1.158424527177966, - "timestamp": 0.6019191915918294 - }, - { - "x": 1.6905120212342348, - "y": 4.258849164477337, - "heading": 1.6820774903470974, - "angularVelocity": -1.4527350214804762, - "velocityX": 0.9086052004688632, - "velocityY": 0.8535901038990373, - "timestamp": 0.6854002032527928 - }, - { - "x": 1.8179066252511533, - "y": 4.303857525516893, - "heading": 1.5806423784124122, - "angularVelocity": -1.2150680725652636, - "velocityX": 1.5260309078943428, - "velocityY": 0.5391448922821541, - "timestamp": 0.7688812149137563 - }, - { - "x": 1.9959545196143786, - "y": 4.321064540954123, - "heading": 1.501233745672766, - "angularVelocity": -0.9512179016486254, - "velocityX": 2.1327951209590146, - "velocityY": 0.20611891368915536, - "timestamp": 0.8523622265747197 - }, - { - "x": 2.221863436317024, - "y": 4.306254552246914, - "heading": 1.4497152819618502, - "angularVelocity": -0.6171279274878088, - "velocityX": 2.706111392374063, - "velocityY": -0.1774054771563637, - "timestamp": 0.9358432382356832 - }, - { - "x": 2.443298757309218, - "y": 4.237785772933149, - "heading": 1.481002566027846, - "angularVelocity": 0.37478324044587696, - "velocityX": 2.652523209607188, - "velocityY": -0.8201718924039002, - "timestamp": 1.0193242498966466 - }, - { - "x": 2.6077641513687433, - "y": 4.180765687739868, - "heading": 1.5117621140320452, - "angularVelocity": 0.3684616105171448, - "velocityX": 1.9700934474471774, - "velocityY": -0.6830305965247938, - "timestamp": 1.10280526155761 - }, - { - "x": 2.7168458549001153, - "y": 4.14118234012958, - "heading": 1.5343226948830555, - "angularVelocity": 0.2702480528462504, - "velocityX": 1.3066648494196378, - "velocityY": -0.47415989364199385, - "timestamp": 1.1862862732185735 - }, - { - "x": 2.771209239959717, - "y": 4.120928764343262, - "heading": 1.5462110292578024, - "angularVelocity": 0.14240764622053664, - "velocityX": 0.651206591510705, - "velocityY": -0.24261296531207407, - "timestamp": 1.269767284879537 - }, - { - "x": 2.771209239959717, - "y": 4.120928764343262, - "heading": 1.5462110292578024, - "angularVelocity": 1.3345092530168404e-23, - "velocityX": -1.0052627512755276e-23, - "velocityY": 2.977034014839026e-23, - "timestamp": 1.3532482965405004 + "x": 2.3582182665685285, + "y": 4.005140044275413, + "heading": 2.626572128421542, + "angularVelocity": -0.5800138561279072, + "velocityX": -0.24524265424721656, + "velocityY": 0.05821014229392081, + "timestamp": 0.0352196268462121 + }, + { + "x": 2.340949746064586, + "y": 4.0093581184378255, + "heading": 2.5859880369259955, + "angularVelocity": -1.1523146361759788, + "velocityX": -0.4903095816263469, + "velocityY": 0.11976487374015862, + "timestamp": 0.0704392536924242 + }, + { + "x": 2.315025310616676, + "y": 4.0158942358801255, + "heading": 2.5258878520188563, + "angularVelocity": -1.7064401383231338, + "velocityX": -0.7360792197199094, + "velocityY": 0.18558167782148904, + "timestamp": 0.10565888053863629 + }, + { + "x": 2.2803832616948174, + "y": 4.024926514000347, + "heading": 2.4473235520751233, + "angularVelocity": -2.230696545616096, + "velocityX": -0.9836006801903231, + "velocityY": 0.25645581537904877, + "timestamp": 0.1408785073848484 + }, + { + "x": 2.2369349430382, + "y": 4.036673464094862, + "heading": 2.351697628937878, + "angularVelocity": -2.7151316382424904, + "velocityX": -1.2336393808559734, + "velocityY": 0.3335342008518269, + "timestamp": 0.1760981342310605 + }, + { + "x": 2.184628025978604, + "y": 4.05150435046694, + "heading": 2.240519075284163, + "angularVelocity": -3.1567215104004607, + "velocityX": -1.485163862980013, + "velocityY": 0.42109720346804624, + "timestamp": 0.2113177610772726 + }, + { + "x": 2.1236673593002187, + "y": 4.070437803424587, + "heading": 2.115442529811484, + "angularVelocity": -3.5513307968545753, + "velocityX": -1.7308720204371333, + "velocityY": 0.5375824406184673, + "timestamp": 0.2465373879234847 + }, + { + "x": 2.056940171048295, + "y": 4.097882582880828, + "heading": 1.9863138442222075, + "angularVelocity": -3.6663842621934117, + "velocityX": -1.8946023631436484, + "velocityY": 0.779246741484578, + "timestamp": 0.2817570147696968 + }, + { + "x": 1.9961028754422039, + "y": 4.13376425540664, + "heading": 1.8534458383236918, + "angularVelocity": -3.7725557536054866, + "velocityX": -1.727369113586192, + "velocityY": 1.0187976346956282, + "timestamp": 0.3169766416159089 + }, + { + "x": 1.9435464476369881, + "y": 4.17533041611823, + "heading": 1.7117204090724802, + "angularVelocity": -4.0240468722187535, + "velocityX": -1.4922482862928892, + "velocityY": 1.1801987821502666, + "timestamp": 0.352196268462121 + }, + { + "x": 1.8999454238017455, + "y": 4.221481056301366, + "heading": 1.5613962576553257, + "angularVelocity": -4.268192620936928, + "velocityX": -1.2379751786044981, + "velocityY": 1.3103670968625691, + "timestamp": 0.3874158953083331 + }, + { + "x": 1.8656935910383048, + "y": 4.271628463138376, + "heading": 1.4043136517982262, + "angularVelocity": -4.460087170798457, + "velocityX": -0.9725211715890388, + "velocityY": 1.4238483291143482, + "timestamp": 0.4226355221545452 + }, + { + "x": 1.8411062322404879, + "y": 4.325262090117391, + "heading": 1.2429884596951064, + "angularVelocity": -4.580548022486237, + "velocityX": -0.6981152555995893, + "velocityY": 1.5228334818326028, + "timestamp": 0.4578551490007573 + }, + { + "x": 1.8264926299504727, + "y": 4.381633415364751, + "heading": 1.0814768977873637, + "angularVelocity": -4.585839668687841, + "velocityX": -0.4149278001666941, + "velocityY": 1.6005656588446922, + "timestamp": 0.4930747758469694 + }, + { + "x": 1.8219761515922521, + "y": 4.439993109865742, + "heading": 0.9236480211125673, + "angularVelocity": -4.48127623168645, + "velocityX": -0.1282375414691572, + "velocityY": 1.657021942787722, + "timestamp": 0.5282944026931815 + }, + { + "x": 1.8275461196899416, + "y": 4.499822616577148, + "heading": 0.7720656380191331, + "angularVelocity": -4.303917919270544, + "velocityX": 0.1581495488866161, + "velocityY": 1.6987547021055334, + "timestamp": 0.5635140295393936 + }, + { + "x": 1.863631380068701, + "y": 4.6009476633877675, + "heading": 0.5458535834271052, + "angularVelocity": -3.9160123080463913, + "velocityX": 0.624680784749141, + "velocityY": 1.750600465020928, + "timestamp": 0.6212799497657474 + }, + { + "x": 1.9264611646703016, + "y": 4.704317253907965, + "heading": 0.34642581420132385, + "angularVelocity": -3.452342980849784, + "velocityX": 1.0876617970494085, + "velocityY": 1.7894563111806223, + "timestamp": 0.6790458699921013 + }, + { + "x": 2.015600664927885, + "y": 4.808735070353454, + "heading": 0.18046889819718334, + "angularVelocity": -2.8729208390317975, + "velocityX": 1.5431157317030553, + "velocityY": 1.8076024070304682, + "timestamp": 0.7368117902184551 + }, + { + "x": 2.1302379264615894, + "y": 4.912412090690155, + "heading": 0.05682984585261984, + "angularVelocity": -2.1403459316511855, + "velocityX": 1.98451372512552, + "velocityY": 1.7947783040671845, + "timestamp": 0.794577710444809 + }, + { + "x": 2.2701009625494035, + "y": 5.012833103004185, + "heading": -0.02381934322313901, + "angularVelocity": -1.3961378743684443, + "velocityX": 2.4212032897555655, + "velocityY": 1.7384127513329013, + "timestamp": 0.8523436306711628 + }, + { + "x": 2.432555238928894, + "y": 5.102059929533658, + "heading": -0.06105728015599246, + "angularVelocity": -0.6446350510290115, + "velocityX": 2.8122857862028177, + "velocityY": 1.5446274581939572, + "timestamp": 0.9101095508975167 + }, + { + "x": 2.5850691468596714, + "y": 5.167502593301118, + "heading": -0.059583445184643466, + "angularVelocity": 0.025513918337556375, + "velocityX": 2.6402056321984575, + "velocityY": 1.132893988549338, + "timestamp": 0.9678754711238705 + }, + { + "x": 2.714659135727784, + "y": 5.217422693936502, + "heading": -0.0482224195291295, + "angularVelocity": 0.1966734990284264, + "velocityX": 2.2433640520283893, + "velocityY": 0.8641790945216953, + "timestamp": 1.0256413913502245 + }, + { + "x": 2.8192112721940457, + "y": 5.255223857981348, + "heading": -0.034645348058129244, + "angularVelocity": 0.23503601116019512, + "velocityX": 1.8099276538238398, + "velocityY": 0.6543852135776276, + "timestamp": 1.0834073115765783 + }, + { + "x": 2.8980093999029637, + "y": 5.282500252165217, + "heading": -0.022245861805103868, + "angularVelocity": 0.2146505448963401, + "velocityX": 1.3640936974626836, + "velocityY": 0.4721883435253398, + "timestamp": 1.1411732318029322 + }, + { + "x": 2.950712826385929, + "y": 5.300160015762531, + "heading": -0.012907289054399772, + "angularVelocity": 0.1616623212113855, + "velocityX": 0.9123619302949761, + "velocityY": 0.3057124949811315, + "timestamp": 1.198939152029286 + }, + { + "x": 2.9771268367767334, + "y": 5.308786869049072, + "heading": -0.007825065024634177, + "angularVelocity": 0.08797962552749206, + "velocityX": 0.4572594063643587, + "velocityY": 0.14934157116754843, + "timestamp": 1.2567050722556399 + }, + { + "x": 2.9771268367767334, + "y": 5.308786869049072, + "heading": -0.007825065024634177, + "angularVelocity": -9.033896437363216e-23, + "velocityX": 2.001984212704135e-22, + "velocityY": -4.919331709829794e-22, + "timestamp": 1.3144709924819937 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 3.1.traj b/src/main/deploy/choreo/Close 2 Note 3.1.traj index eeaf513..f68d978 100644 --- a/src/main/deploy/choreo/Close 2 Note 3.1.traj +++ b/src/main/deploy/choreo/Close 2 Note 3.1.traj @@ -1,103 +1,202 @@ { "samples": [ { - "x": 2.629, - "y": 4.107, - "heading": 1.546, - "angularVelocity": -8.203167337623884e-22, - "velocityX": 4.329369978962889e-21, - "velocityY": 1.525295443830001e-20, + "x": 3.048088550567627, + "y": 5.33717155456543, + "heading": -0.01821858883733709, + "angularVelocity": 1.98280707847529e-26, + "velocityX": -3.686176979860547e-26, + "velocityY": -9.942929150065618e-26, "timestamp": 0 }, { - "x": 2.6242456552823756, - "y": 4.0804929655510955, - "heading": 1.58312669994352, - "angularVelocity": 0.6327197789954236, - "velocityX": -0.0810243825489381, - "velocityY": -0.451737563636128, - "timestamp": 0.05867795061261784 - }, - { - "x": 2.614797212256109, - "y": 4.027426968793948, - "heading": 1.656708910709224, - "angularVelocity": 1.254001034417197, - "velocityX": -0.1610220351532649, - "velocityY": -0.904360091024302, - "timestamp": 0.11735590122523568 - }, - { - "x": 2.600686059565033, - "y": 3.947699552686434, - "heading": 1.7652887549030123, - "angularVelocity": 1.8504368857497178, - "velocityX": -0.2404847569444782, - "velocityY": -1.3587287094237785, - "timestamp": 0.17603385183785353 - }, - { - "x": 2.58182712139046, - "y": 3.841172258879563, - "heading": 1.9066425962383542, - "angularVelocity": 2.408977134674257, - "velocityX": -0.3213973558666907, - "velocityY": -1.8154569594658583, - "timestamp": 0.23471180245047135 - }, - { - "x": 2.5554465223661467, - "y": 3.707426020625259, - "heading": 2.066347870244412, - "angularVelocity": 2.721725492091665, - "velocityX": -0.4495828287950398, - "velocityY": -2.2793270190582233, - "timestamp": 0.2933897530630892 - }, - { - "x": 2.5341409307456813, - "y": 3.6005266015036144, - "heading": 2.1949908224625454, - "angularVelocity": 2.1923559169169495, - "velocityX": -0.363093656101234, - "velocityY": -1.8217987848174317, - "timestamp": 0.352067703675707 - }, - { - "x": 2.518082931719197, - "y": 3.5204124576524585, - "heading": 2.2921355834233315, - "angularVelocity": 1.6555581772465078, - "velocityX": -0.2736632561095736, - "velocityY": -1.3653193919476883, - "timestamp": 0.4107456542883248 - }, - { - "x": 2.5073565100987056, - "y": 3.467021273525448, - "heading": 2.3571203843486037, - "angularVelocity": 1.1074824571548374, - "velocityX": -0.18280157211529632, - "velocityY": -0.9099019916269808, - "timestamp": 0.46942360490094265 - }, - { - "x": 2.501993417739868, - "y": 3.440321445465088, - "heading": 2.3895683761463378, - "angularVelocity": 0.5529844082652147, - "velocityX": -0.09139876738784368, - "velocityY": -0.45502318642019196, - "timestamp": 0.5281015555135605 - }, - { - "x": 2.501993417739868, - "y": 3.440321445465088, - "heading": 2.3895683761463378, - "angularVelocity": 6.196323349296843e-22, - "velocityX": -4.670139220367797e-21, - "velocityY": -1.5443840632789222e-20, - "timestamp": 0.5867795061261784 + "x": 3.0461249759665745, + "y": 5.329789651433795, + "heading": 0.022783459542432594, + "angularVelocity": 1.066293792007848, + "velocityX": -0.051064458727859946, + "velocityY": -0.19197278656813405, + "timestamp": 0.038452862322833355 + }, + { + "x": 3.0423993082753173, + "y": 5.314779984880276, + "heading": 0.10329419369255852, + "angularVelocity": 2.093751395518834, + "velocityX": -0.09688921620393233, + "velocityY": -0.3903393829958584, + "timestamp": 0.07690572464566671 + }, + { + "x": 3.032907918028394, + "y": 5.290887968080895, + "heading": 0.20545892573589697, + "angularVelocity": 2.656882371606259, + "velocityX": -0.2468318266462912, + "velocityY": -0.6213325967464687, + "timestamp": 0.11535858696850007 + }, + { + "x": 3.0165602178325317, + "y": 5.259789389738606, + "heading": 0.33465802025849095, + "angularVelocity": 3.3599343902645034, + "velocityX": -0.42513610712811417, + "velocityY": -0.8087454733850095, + "timestamp": 0.15381144929133342 + }, + { + "x": 2.9936431691568814, + "y": 5.220807962711857, + "heading": 0.48978809219771535, + "angularVelocity": 4.034291924403973, + "velocityX": -0.5959777059832116, + "velocityY": -1.0137457830701675, + "timestamp": 0.19226431161416677 + }, + { + "x": 2.9642904690085223, + "y": 5.172884357745661, + "heading": 0.6658699909928164, + "angularVelocity": 4.579162334309335, + "velocityX": -0.7633423983350427, + "velocityY": -1.246294867826739, + "timestamp": 0.23071717393700011 + }, + { + "x": 2.928089447111606, + "y": 5.114902261857335, + "heading": 0.8491068027757267, + "angularVelocity": 4.7652320455245825, + "velocityX": -0.9414389387450126, + "velocityY": -1.507874638863897, + "timestamp": 0.2691700362598335 + }, + { + "x": 2.8859836301958794, + "y": 5.047128174361195, + "heading": 1.0148826733109657, + "angularVelocity": 4.311145140339815, + "velocityX": -1.0949982490828563, + "velocityY": -1.7625238643391223, + "timestamp": 0.30762289858266684 + }, + { + "x": 2.8416687526149524, + "y": 4.969882073543266, + "heading": 1.150055375667919, + "angularVelocity": 3.5152832374896503, + "velocityX": -1.1524467855963136, + "velocityY": -2.008851777259247, + "timestamp": 0.3460757609055002 + }, + { + "x": 2.794651136420651, + "y": 4.883085920156056, + "heading": 1.2554742450273566, + "angularVelocity": 2.7415090318735467, + "velocityX": -1.222733844871212, + "velocityY": -2.2572091684230795, + "timestamp": 0.38452862322833353 + }, + { + "x": 2.7509500334287784, + "y": 4.798093290887787, + "heading": 1.3652052247880782, + "angularVelocity": 2.8536491988416444, + "velocityX": -1.1364850456378852, + "velocityY": -2.2103069611491293, + "timestamp": 0.4229814855511669 + }, + { + "x": 2.7108561849571107, + "y": 4.723392024777697, + "heading": 1.4968704984881265, + "angularVelocity": 3.424069516454839, + "velocityX": -1.0426752665395174, + "velocityY": -1.9426711458546686, + "timestamp": 0.46143434787400023 + }, + { + "x": 2.6740931092193048, + "y": 4.658985356507111, + "heading": 1.650582325216758, + "angularVelocity": 3.9974092289446306, + "velocityX": -0.9560556358369164, + "velocityY": -1.674951209869765, + "timestamp": 0.4998872101968336 + }, + { + "x": 2.640223792764239, + "y": 4.60465818865713, + "heading": 1.8276925817226235, + "angularVelocity": 4.605905667539798, + "velocityX": -0.8808009185561768, + "velocityY": -1.4128250686223396, + "timestamp": 0.538340072519667 + }, + { + "x": 2.6122072946516184, + "y": 4.558661386609658, + "heading": 2.0303012950028507, + "angularVelocity": 5.269015127644162, + "velocityX": -0.7285933067194444, + "velocityY": -1.1961866885565553, + "timestamp": 0.5767929348425004 + }, + { + "x": 2.590171635106383, + "y": 4.523029597001796, + "heading": 2.222212243469772, + "angularVelocity": 4.990810485204496, + "velocityX": -0.573056418017296, + "velocityY": -0.9266355598892387, + "timestamp": 0.6152457971653338 + }, + { + "x": 2.5738089300274605, + "y": 4.495952446465408, + "heading": 2.387016152370131, + "angularVelocity": 4.285868435923901, + "velocityX": -0.42552632211221264, + "velocityY": -0.7041647591552526, + "timestamp": 0.6536986594881672 + }, + { + "x": 2.562290479492898, + "y": 4.476281411764592, + "heading": 2.515465208376393, + "angularVelocity": 3.340428988819089, + "velocityX": -0.29954728565739847, + "velocityY": -0.5115623002435097, + "timestamp": 0.6921515218110006 + }, + { + "x": 2.5549432582798937, + "y": 4.463492222573091, + "heading": 2.6032428246599313, + "angularVelocity": 2.282732961374791, + "velocityX": -0.19107085322595763, + "velocityY": -0.332593945390306, + "timestamp": 0.730604384133834 + }, + { + "x": 2.551356077194214, + "y": 4.457245349884033, + "heading": 2.647920421723555, + "angularVelocity": 1.1618796200014927, + "velocityX": -0.09328775204205611, + "velocityY": -0.16245533652637065, + "timestamp": 0.7690572464566674 + }, + { + "x": 2.551356077194214, + "y": 4.457245349884033, + "heading": 2.647920421723555, + "angularVelocity": -2.0111425344083367e-26, + "velocityX": 8.099203026682335e-26, + "velocityY": 2.8915510028596647e-26, + "timestamp": 0.8075101087795008 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 3.traj b/src/main/deploy/choreo/Close 2 Note 3.traj index eeaf513..f68d978 100644 --- a/src/main/deploy/choreo/Close 2 Note 3.traj +++ b/src/main/deploy/choreo/Close 2 Note 3.traj @@ -1,103 +1,202 @@ { "samples": [ { - "x": 2.629, - "y": 4.107, - "heading": 1.546, - "angularVelocity": -8.203167337623884e-22, - "velocityX": 4.329369978962889e-21, - "velocityY": 1.525295443830001e-20, + "x": 3.048088550567627, + "y": 5.33717155456543, + "heading": -0.01821858883733709, + "angularVelocity": 1.98280707847529e-26, + "velocityX": -3.686176979860547e-26, + "velocityY": -9.942929150065618e-26, "timestamp": 0 }, { - "x": 2.6242456552823756, - "y": 4.0804929655510955, - "heading": 1.58312669994352, - "angularVelocity": 0.6327197789954236, - "velocityX": -0.0810243825489381, - "velocityY": -0.451737563636128, - "timestamp": 0.05867795061261784 - }, - { - "x": 2.614797212256109, - "y": 4.027426968793948, - "heading": 1.656708910709224, - "angularVelocity": 1.254001034417197, - "velocityX": -0.1610220351532649, - "velocityY": -0.904360091024302, - "timestamp": 0.11735590122523568 - }, - { - "x": 2.600686059565033, - "y": 3.947699552686434, - "heading": 1.7652887549030123, - "angularVelocity": 1.8504368857497178, - "velocityX": -0.2404847569444782, - "velocityY": -1.3587287094237785, - "timestamp": 0.17603385183785353 - }, - { - "x": 2.58182712139046, - "y": 3.841172258879563, - "heading": 1.9066425962383542, - "angularVelocity": 2.408977134674257, - "velocityX": -0.3213973558666907, - "velocityY": -1.8154569594658583, - "timestamp": 0.23471180245047135 - }, - { - "x": 2.5554465223661467, - "y": 3.707426020625259, - "heading": 2.066347870244412, - "angularVelocity": 2.721725492091665, - "velocityX": -0.4495828287950398, - "velocityY": -2.2793270190582233, - "timestamp": 0.2933897530630892 - }, - { - "x": 2.5341409307456813, - "y": 3.6005266015036144, - "heading": 2.1949908224625454, - "angularVelocity": 2.1923559169169495, - "velocityX": -0.363093656101234, - "velocityY": -1.8217987848174317, - "timestamp": 0.352067703675707 - }, - { - "x": 2.518082931719197, - "y": 3.5204124576524585, - "heading": 2.2921355834233315, - "angularVelocity": 1.6555581772465078, - "velocityX": -0.2736632561095736, - "velocityY": -1.3653193919476883, - "timestamp": 0.4107456542883248 - }, - { - "x": 2.5073565100987056, - "y": 3.467021273525448, - "heading": 2.3571203843486037, - "angularVelocity": 1.1074824571548374, - "velocityX": -0.18280157211529632, - "velocityY": -0.9099019916269808, - "timestamp": 0.46942360490094265 - }, - { - "x": 2.501993417739868, - "y": 3.440321445465088, - "heading": 2.3895683761463378, - "angularVelocity": 0.5529844082652147, - "velocityX": -0.09139876738784368, - "velocityY": -0.45502318642019196, - "timestamp": 0.5281015555135605 - }, - { - "x": 2.501993417739868, - "y": 3.440321445465088, - "heading": 2.3895683761463378, - "angularVelocity": 6.196323349296843e-22, - "velocityX": -4.670139220367797e-21, - "velocityY": -1.5443840632789222e-20, - "timestamp": 0.5867795061261784 + "x": 3.0461249759665745, + "y": 5.329789651433795, + "heading": 0.022783459542432594, + "angularVelocity": 1.066293792007848, + "velocityX": -0.051064458727859946, + "velocityY": -0.19197278656813405, + "timestamp": 0.038452862322833355 + }, + { + "x": 3.0423993082753173, + "y": 5.314779984880276, + "heading": 0.10329419369255852, + "angularVelocity": 2.093751395518834, + "velocityX": -0.09688921620393233, + "velocityY": -0.3903393829958584, + "timestamp": 0.07690572464566671 + }, + { + "x": 3.032907918028394, + "y": 5.290887968080895, + "heading": 0.20545892573589697, + "angularVelocity": 2.656882371606259, + "velocityX": -0.2468318266462912, + "velocityY": -0.6213325967464687, + "timestamp": 0.11535858696850007 + }, + { + "x": 3.0165602178325317, + "y": 5.259789389738606, + "heading": 0.33465802025849095, + "angularVelocity": 3.3599343902645034, + "velocityX": -0.42513610712811417, + "velocityY": -0.8087454733850095, + "timestamp": 0.15381144929133342 + }, + { + "x": 2.9936431691568814, + "y": 5.220807962711857, + "heading": 0.48978809219771535, + "angularVelocity": 4.034291924403973, + "velocityX": -0.5959777059832116, + "velocityY": -1.0137457830701675, + "timestamp": 0.19226431161416677 + }, + { + "x": 2.9642904690085223, + "y": 5.172884357745661, + "heading": 0.6658699909928164, + "angularVelocity": 4.579162334309335, + "velocityX": -0.7633423983350427, + "velocityY": -1.246294867826739, + "timestamp": 0.23071717393700011 + }, + { + "x": 2.928089447111606, + "y": 5.114902261857335, + "heading": 0.8491068027757267, + "angularVelocity": 4.7652320455245825, + "velocityX": -0.9414389387450126, + "velocityY": -1.507874638863897, + "timestamp": 0.2691700362598335 + }, + { + "x": 2.8859836301958794, + "y": 5.047128174361195, + "heading": 1.0148826733109657, + "angularVelocity": 4.311145140339815, + "velocityX": -1.0949982490828563, + "velocityY": -1.7625238643391223, + "timestamp": 0.30762289858266684 + }, + { + "x": 2.8416687526149524, + "y": 4.969882073543266, + "heading": 1.150055375667919, + "angularVelocity": 3.5152832374896503, + "velocityX": -1.1524467855963136, + "velocityY": -2.008851777259247, + "timestamp": 0.3460757609055002 + }, + { + "x": 2.794651136420651, + "y": 4.883085920156056, + "heading": 1.2554742450273566, + "angularVelocity": 2.7415090318735467, + "velocityX": -1.222733844871212, + "velocityY": -2.2572091684230795, + "timestamp": 0.38452862322833353 + }, + { + "x": 2.7509500334287784, + "y": 4.798093290887787, + "heading": 1.3652052247880782, + "angularVelocity": 2.8536491988416444, + "velocityX": -1.1364850456378852, + "velocityY": -2.2103069611491293, + "timestamp": 0.4229814855511669 + }, + { + "x": 2.7108561849571107, + "y": 4.723392024777697, + "heading": 1.4968704984881265, + "angularVelocity": 3.424069516454839, + "velocityX": -1.0426752665395174, + "velocityY": -1.9426711458546686, + "timestamp": 0.46143434787400023 + }, + { + "x": 2.6740931092193048, + "y": 4.658985356507111, + "heading": 1.650582325216758, + "angularVelocity": 3.9974092289446306, + "velocityX": -0.9560556358369164, + "velocityY": -1.674951209869765, + "timestamp": 0.4998872101968336 + }, + { + "x": 2.640223792764239, + "y": 4.60465818865713, + "heading": 1.8276925817226235, + "angularVelocity": 4.605905667539798, + "velocityX": -0.8808009185561768, + "velocityY": -1.4128250686223396, + "timestamp": 0.538340072519667 + }, + { + "x": 2.6122072946516184, + "y": 4.558661386609658, + "heading": 2.0303012950028507, + "angularVelocity": 5.269015127644162, + "velocityX": -0.7285933067194444, + "velocityY": -1.1961866885565553, + "timestamp": 0.5767929348425004 + }, + { + "x": 2.590171635106383, + "y": 4.523029597001796, + "heading": 2.222212243469772, + "angularVelocity": 4.990810485204496, + "velocityX": -0.573056418017296, + "velocityY": -0.9266355598892387, + "timestamp": 0.6152457971653338 + }, + { + "x": 2.5738089300274605, + "y": 4.495952446465408, + "heading": 2.387016152370131, + "angularVelocity": 4.285868435923901, + "velocityX": -0.42552632211221264, + "velocityY": -0.7041647591552526, + "timestamp": 0.6536986594881672 + }, + { + "x": 2.562290479492898, + "y": 4.476281411764592, + "heading": 2.515465208376393, + "angularVelocity": 3.340428988819089, + "velocityX": -0.29954728565739847, + "velocityY": -0.5115623002435097, + "timestamp": 0.6921515218110006 + }, + { + "x": 2.5549432582798937, + "y": 4.463492222573091, + "heading": 2.6032428246599313, + "angularVelocity": 2.282732961374791, + "velocityX": -0.19107085322595763, + "velocityY": -0.332593945390306, + "timestamp": 0.730604384133834 + }, + { + "x": 2.551356077194214, + "y": 4.457245349884033, + "heading": 2.647920421723555, + "angularVelocity": 1.1618796200014927, + "velocityX": -0.09328775204205611, + "velocityY": -0.16245533652637065, + "timestamp": 0.7690572464566674 + }, + { + "x": 2.551356077194214, + "y": 4.457245349884033, + "heading": 2.647920421723555, + "angularVelocity": -2.0111425344083367e-26, + "velocityX": 8.099203026682335e-26, + "velocityY": 2.8915510028596647e-26, + "timestamp": 0.8075101087795008 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 4.1.traj b/src/main/deploy/choreo/Close 2 Note 4.1.traj index 7ed32ee..7d72777 100644 --- a/src/main/deploy/choreo/Close 2 Note 4.1.traj +++ b/src/main/deploy/choreo/Close 2 Note 4.1.traj @@ -3,317 +3,389 @@ { "x": 2.502, "y": 3.44, - "heading": 2.39, - "angularVelocity": 1.6143473252561374e-27, - "velocityX": -4.497600413655855e-27, - "velocityY": 7.73733480239798e-27, + "heading": 2.5104836654523037, + "angularVelocity": 1.9281904905287756e-29, + "velocityX": -4.71586515216861e-27, + "velocityY": 2.3810934236992728e-27, "timestamp": 0 }, { - "x": 2.525916739364106, - "y": 3.420694982582511, - "heading": 2.32354752179907, - "angularVelocity": -1.023433961876349, - "velocityX": 0.3683414672446169, - "velocityY": -0.29731638299377017, - "timestamp": 0.06493089019549111 - }, - { - "x": 2.5746652987077048, - "y": 3.3812894837618575, - "heading": 2.2018139650877506, - "angularVelocity": -1.8748173072140173, - "velocityX": 0.7507760820285803, - "velocityY": -0.6068836989915484, - "timestamp": 0.12986178039098223 - }, - { - "x": 2.649625766646246, - "y": 3.321219957909989, - "heading": 2.0401525298056455, - "angularVelocity": -2.4897461715892337, - "velocityX": 1.1544654279781719, - "velocityY": -0.925130175653128, - "timestamp": 0.19479267058647334 - }, - { - "x": 2.7520459663591446, - "y": 3.239983238174216, - "heading": 1.8578167867140025, - "angularVelocity": -2.8081509824164503, - "velocityX": 1.5773724864164937, - "velocityY": -1.2511259200542133, - "timestamp": 0.25972356078196446 - }, - { - "x": 2.8818046504752663, - "y": 3.1376357565206274, - "heading": 1.6986985692029641, - "angularVelocity": -2.4505780997607194, - "velocityX": 1.9984122152869, - "velocityY": -1.5762525563017087, - "timestamp": 0.3246544509774556 - }, - { - "x": 3.0370045621229718, - "y": 3.020948186403271, - "heading": 1.6099501280880217, - "angularVelocity": -1.3668138669859953, - "velocityX": 2.390232309774843, - "velocityY": -1.7971041174091287, - "timestamp": 0.38958534117294674 - }, - { - "x": 3.217315269794052, - "y": 2.8895687443365436, - "heading": 1.5922226066527227, - "angularVelocity": -0.2730213829184488, - "velocityX": 2.776963431861306, - "velocityY": -2.0233734925114266, - "timestamp": 0.4545162313684379 - }, - { - "x": 3.400066011553934, - "y": 2.749461510644502, - "heading": 1.5922225170352793, - "angularVelocity": -0.0000013801973627609237, - "velocityX": 2.814542372816138, - "velocityY": -2.1577901253195986, - "timestamp": 0.519447121563929 - }, - { - "x": 3.582816703850075, - "y": 2.6093542124337037, - "heading": 1.5922224274179715, - "angularVelocity": -0.0000013801952753049937, - "velocityX": 2.814541611025554, - "velocityY": -2.15779111897233, - "timestamp": 0.5843780117594202 - }, - { - "x": 3.7655673961455407, - "y": 2.4692469142220252, - "heading": 1.5922223378006626, - "angularVelocity": -0.0000013801952903393593, - "velocityX": 2.8145416110151595, - "velocityY": -2.1577911189858847, - "timestamp": 0.6493089019549113 - }, - { - "x": 3.948318088441007, - "y": 2.3291396160103477, - "heading": 1.592222248183353, - "angularVelocity": -0.0000013801953035277997, - "velocityX": 2.8145416110151644, - "velocityY": -2.157791118985876, - "timestamp": 0.7142397921504025 - }, - { - "x": 4.131068780736474, - "y": 2.1890323177986706, - "heading": 1.5922221585660423, - "angularVelocity": -0.0000013801953177064064, - "velocityX": 2.81454161101517, - "velocityY": -2.157791118985867, - "timestamp": 0.7791706823458936 - }, - { - "x": 4.31381947303194, - "y": 2.0489250195869935, - "heading": 1.5922220689487308, - "angularVelocity": -0.000001380195332153733, - "velocityX": 2.814541611015174, - "velocityY": -2.1577911189858576, - "timestamp": 0.8441015725413847 - }, - { - "x": 4.496570165327408, - "y": 1.9088177213753177, - "heading": 1.5922219793314183, - "angularVelocity": -0.0000013801953476150256, - "velocityX": 2.8145416110151804, - "velocityY": -2.157791118985848, - "timestamp": 0.9090324627368759 - }, - { - "x": 4.679320857625259, - "y": 1.7687104231667514, - "heading": 1.5922218897141047, - "angularVelocity": -0.0000013801953620228687, - "velocityX": 2.8145416110518964, - "velocityY": -2.1577911189379546, - "timestamp": 0.973963352932367 - }, - { - "x": 4.86207172453282, - "y": 1.6286033527128316, - "heading": 1.5922218000967605, - "angularVelocity": -0.0000013801958387075775, - "velocityX": 2.8145443002142994, - "velocityY": -2.1577876112908903, - "timestamp": 1.038894243127858 - }, - { - "x": 5.054351274988585, - "y": 1.5018893260974921, - "heading": 1.592221707127701, - "angularVelocity": -0.000001431815567947987, - "velocityX": 2.9612954616340366, - "velocityY": -1.9515214751227716, - "timestamp": 1.103825133323349 + "x": 2.5078330623275535, + "y": 3.4353964642834374, + "heading": 2.421550371026829, + "angularVelocity": -1.8125731387547213, + "velocityX": 0.11888519547047234, + "velocityY": -0.09382588643594957, + "timestamp": 0.04906466532245639 + }, + { + "x": 2.5190564983313566, + "y": 3.4257216835023843, + "heading": 2.2438316854082814, + "angularVelocity": -3.622131822372945, + "velocityX": 0.22874783573966653, + "velocityY": -0.19718428154904508, + "timestamp": 0.09812933064491278 + }, + { + "x": 2.5349008514646854, + "y": 3.4113020995219303, + "heading": 1.9758859051359892, + "angularVelocity": -5.461074247859102, + "velocityX": 0.32292797737839174, + "velocityY": -0.2938893781438674, + "timestamp": 0.14719399596736915 + }, + { + "x": 2.5604353108293316, + "y": 3.394955070383565, + "heading": 1.6293166979187397, + "angularVelocity": -7.063519234047003, + "velocityX": 0.5204246110073755, + "velocityY": -0.3331731507986746, + "timestamp": 0.19625866128982555 + }, + { + "x": 2.603992889740609, + "y": 3.3729904224492855, + "heading": 1.3083335355675372, + "angularVelocity": -6.54204324520872, + "velocityX": 0.8877586064230485, + "velocityY": -0.44766733432149164, + "timestamp": 0.24532332661228196 + }, + { + "x": 2.6607200682182177, + "y": 3.3376856037523086, + "heading": 1.014097923240798, + "angularVelocity": -5.996894310661298, + "velocityX": 1.1561717196029637, + "velocityY": -0.7195569044433685, + "timestamp": 0.29438799193473836 + }, + { + "x": 2.71502692076517, + "y": 3.2838347448517657, + "heading": 0.745483337859882, + "angularVelocity": -5.474705342746399, + "velocityX": 1.1068424127637233, + "velocityY": -1.0975486849167542, + "timestamp": 0.34345265725719476 + }, + { + "x": 2.7744606723157283, + "y": 3.214460375697132, + "heading": 0.5232634756481663, + "angularVelocity": -4.529122144241102, + "velocityX": 1.2113351056194037, + "velocityY": -1.4139374781974092, + "timestamp": 0.39251732257965116 + }, + { + "x": 2.84873633367785, + "y": 3.1410738657590347, + "heading": 0.34689629796158034, + "angularVelocity": -3.594586379576598, + "velocityX": 1.513832018907633, + "velocityY": -1.4957099871322082, + "timestamp": 0.44158198790210756 + }, + { + "x": 2.940226570979376, + "y": 3.0662458182374484, + "heading": 0.20973540275964336, + "angularVelocity": -2.7955127035006972, + "velocityX": 1.8646868719117038, + "velocityY": -1.5250903482131433, + "timestamp": 0.49064665322456397 + }, + { + "x": 3.0480227715570303, + "y": 2.9863881956756497, + "heading": 0.11214926277131038, + "angularVelocity": -1.9889290866857052, + "velocityX": 2.197023048444546, + "velocityY": -1.6275994554730684, + "timestamp": 0.5397113185470204 + }, + { + "x": 3.169811200590146, + "y": 2.8977558044744653, + "heading": 0.054667190153068854, + "angularVelocity": -1.1715574179598567, + "velocityX": 2.4822023799146242, + "velocityY": -1.8064403500703765, + "timestamp": 0.5887759838694767 + }, + { + "x": 3.303879745950196, + "y": 2.7983821340483717, + "heading": 0.037123590068076556, + "angularVelocity": -0.3575607816683257, + "velocityX": 2.7324866985017113, + "velocityY": -2.02536122019794, + "timestamp": 0.6378406491919331 + }, + { + "x": 3.4431520829592457, + "y": 2.6940705143750954, + "heading": 0.037115475659100644, + "angularVelocity": -0.00016538192857494248, + "velocityX": 2.838546560824209, + "velocityY": -2.126002877788589, + "timestamp": 0.6869053145143894 + }, + { + "x": 3.5824244012585216, + "y": 2.5897588683797643, + "heading": 0.03710736322260343, + "angularVelocity": -0.0001653417269616524, + "velocityX": 2.838546179495335, + "velocityY": -2.126003414265387, + "timestamp": 0.7359699798368458 + }, + { + "x": 3.7216967195717463, + "y": 2.485447222406277, + "heading": 0.03709925077439075, + "angularVelocity": -0.00016534196573771254, + "velocityX": 2.8385461797796308, + "velocityY": -2.12600341382019, + "timestamp": 0.7850346451593021 + }, + { + "x": 3.8609690378995296, + "y": 2.381135576455448, + "heading": 0.03709113831446088, + "angularVelocity": -0.00016534220454895188, + "velocityX": 2.8385461800763494, + "velocityY": -2.1260034133583896, + "timestamp": 0.8340993104817584 + }, + { + "x": 4.000241356241871, + "y": 2.276823930527277, + "heading": 0.03708302584281285, + "angularVelocity": -0.0001653424433797745, + "velocityX": 2.8385461803730676, + "velocityY": -2.126003412896584, + "timestamp": 0.8831639758042148 + }, + { + "x": 4.139513674598771, + "y": 2.172512284621764, + "heading": 0.03707491335944571, + "angularVelocity": -0.00016534268223024974, + "velocityX": 2.838546180669785, + "velocityY": -2.1260034124347746, + "timestamp": 0.9322286411266711 + }, + { + "x": 4.278785992970229, + "y": 2.0682006387389102, + "heading": 0.03706680086435853, + "angularVelocity": -0.00016534292109930652, + "velocityX": 2.838546180966501, + "velocityY": -2.1260034119729605, + "timestamp": 0.9812933064491275 + }, + { + "x": 4.418058311356245, + "y": 1.9638889928787158, + "heading": 0.03705868835755037, + "angularVelocity": -0.00016534315998779618, + "velocityX": 2.8385461812632156, + "velocityY": -2.1260034115111432, + "timestamp": 1.0303579717715838 + }, + { + "x": 4.5573306297568195, + "y": 1.85957734704118, + "heading": 0.03705057583902021, + "angularVelocity": -0.00016534339889699094, + "velocityX": 2.8385461815599298, + "velocityY": -2.1260034110493207, + "timestamp": 1.0794226370940403 + }, + { + "x": 4.696602948173282, + "y": 1.7552657012280795, + "heading": 0.03704246330876568, + "angularVelocity": -0.00016534363785453005, + "velocityX": 2.838546181883735, + "velocityY": -2.126003410551304, + "timestamp": 1.1284873024164968 + }, + { + "x": 4.835875308437814, + "y": 1.650954112123811, + "heading": 0.03703434953912002, + "angularVelocity": -0.00016536889821494152, + "velocityX": 2.8385470348003916, + "velocityY": -2.1260022547535105, + "timestamp": 1.1775519677389532 + }, + { + "x": 4.972500711386871, + "y": 1.5511534257066393, + "heading": 0.025156273343944498, + "angularVelocity": -0.2420902316791928, + "velocityX": 2.7845986934007563, + "velocityY": -2.034064346740661, + "timestamp": 1.2266166330614097 + }, + { + "x": 5.112272239054231, + "y": 1.4704127227705035, + "heading": -0.006717749301178638, + "angularVelocity": -0.64963293717882, + "velocityX": 2.8487206984654128, + "velocityY": -1.6455977515693263, + "timestamp": 1.2756812983838661 }, { "x": 5.263677597045898, "y": 1.4059187173843384, - "heading": 1.5922215933347457, - "angularVelocity": -0.0000017525241831714588, - "velocityX": 3.2238326230717362, - "velocityY": -1.4780423989908298, - "timestamp": 1.1687560235188401 - }, - { - "x": 5.549175877605322, - "y": 1.3375763530891083, - "heading": 1.5922215924257537, - "angularVelocity": -1.0981400650486354e-8, - "velocityX": 3.449063323110087, - "velocityY": -0.8256341917136142, - "timestamp": 1.2515316219389887 - }, - { - "x": 5.84250473577961, - "y": 1.325823529497918, - "heading": 1.5922215915939637, - "angularVelocity": -1.0048733277896019e-8, - "velocityX": 3.5436633956473873, - "velocityY": -0.14198415735439168, - "timestamp": 1.3343072203591373 - }, - { - "x": 6.135970278596253, - "y": 1.3182128061287934, - "heading": 1.5922215907646338, - "angularVelocity": -1.0019015771647788e-8, - "velocityX": 3.5453146629890213, - "velocityY": -0.0919440452788315, - "timestamp": 1.4170828187792859 - }, - { - "x": 6.4294358220976315, - "y": 1.3106021091627869, - "heading": 1.5922215899353038, - "angularVelocity": -1.0019015824110795e-8, - "velocityX": 3.54531467126122, - "velocityY": -0.09194372630659559, - "timestamp": 1.4998584171994345 - }, - { - "x": 6.722901365599015, - "y": 1.3029914121969473, - "heading": 1.5922215891059737, - "angularVelocity": -1.0019016426747296e-8, - "velocityX": 3.545314671261272, - "velocityY": -0.09194372630457344, - "timestamp": 1.582634015619583 - }, - { - "x": 7.016366909100399, - "y": 1.2953807152311076, - "heading": 1.5922215882766437, - "angularVelocity": -1.0019015739011674e-8, - "velocityX": 3.545314671261273, - "velocityY": -0.09194372630457337, - "timestamp": 1.6654096140397316 - }, - { - "x": 7.309832452601783, - "y": 1.2877700182652683, - "heading": 1.5922215874473136, - "angularVelocity": -1.00190160592321e-8, - "velocityX": 3.5453146712612726, - "velocityY": -0.09194372630457343, - "timestamp": 1.7481852124598802 - }, - { - "x": 7.603297996103167, - "y": 1.2801593212994287, - "heading": 1.5922215866179836, - "angularVelocity": -1.0019015458013803e-8, - "velocityX": 3.545314671261272, - "velocityY": -0.0919437263045734, - "timestamp": 1.8309608108800288 - }, - { - "x": 7.89676353960455, - "y": 1.272548624333589, - "heading": 1.5922215857886535, - "angularVelocity": -1.0019015102150109e-8, - "velocityX": 3.545314671261272, - "velocityY": -0.09194372630457341, - "timestamp": 1.9137364093001774 - }, - { - "x": 8.190229083105928, - "y": 1.264937927367751, - "heading": 1.5922215849593204, - "angularVelocity": -1.0019051996053813e-8, - "velocityX": 3.545314671261198, - "velocityY": -0.09194372630455969, - "timestamp": 1.996512007720326 - }, - { - "x": 8.475782151148142, - "y": 1.2575324316094862, - "heading": 1.5850799971897327, - "angularVelocity": -0.08627648613712059, - "velocityX": 3.44972520274412, - "velocityY": -0.08946472027512459, - "timestamp": 2.0792876061404746 - }, - { - "x": 8.70422462444052, - "y": 1.2516080345083516, - "heading": 1.5793665822224103, - "angularVelocity": -0.06902293763341408, - "velocityX": 2.7597803900234106, - "velocityY": -0.07157178219436212, - "timestamp": 2.162063204560623 - }, - { - "x": 8.875556485627149, - "y": 1.2471647365187968, - "heading": 1.5750814725763596, - "angularVelocity": -0.051767788186808314, - "velocityX": 2.0698353676283796, - "velocityY": -0.0536788386234512, - "timestamp": 2.2448388029807713 - }, - { - "x": 8.989777728924334, - "y": 1.2442025377932433, - "heading": 1.572224713236977, - "angularVelocity": -0.034512095278156824, - "velocityX": 1.3798902753614153, - "velocityY": -0.03578589321116078, - "timestamp": 2.3276144014009197 - }, - { - "x": 9.04688835144043, - "y": 1.242721438407898, - "heading": 1.5707963267948966, - "angularVelocity": -0.017256129455330296, - "velocityX": 0.6899451481608816, - "velocityY": -0.017892946878229225, - "timestamp": 2.410389999821068 - }, - { - "x": 9.04688835144043, - "y": 1.242721438407898, - "heading": 1.5707963267948966, - "angularVelocity": -5.163840199570749e-28, - "velocityX": -6.169448749202353e-27, - "velocityY": 2.9824086172169885e-27, - "timestamp": 2.4931655982412164 + "heading": -0.031824505216569196, + "angularVelocity": -0.5117074731965909, + "velocityX": 3.0858328900568646, + "velocityY": -1.31446948557187, + "timestamp": 1.3247459637063226 + }, + { + "x": 5.548529116436538, + "y": 1.343358066124809, + "heading": -0.03182450881816663, + "angularVelocity": -4.379736737746259e-8, + "velocityX": 3.4639481169894566, + "velocityY": -0.7607712628377938, + "timestamp": 1.4069791582491455 + }, + { + "x": 5.839915359315492, + "y": 1.3311809328886355, + "heading": -0.03182450708796022, + "angularVelocity": 2.1040243198202156e-8, + "velocityX": 3.5434138792603314, + "velocityY": -0.14808050816792953, + "timestamp": 1.4892123527919685 + }, + { + "x": 6.131301674589235, + "y": 1.3190055321081195, + "heading": -0.031824505357755536, + "angularVelocity": 2.1040222208336333e-8, + "velocityX": 3.5434147596200054, + "velocityY": -0.1480594405726954, + "timestamp": 1.5714455473347915 + }, + { + "x": 6.4226879898634435, + "y": 1.3068301313387687, + "heading": -0.03182450362755091, + "angularVelocity": 2.10402215194868e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.14805944043692082, + "timestamp": 1.6536787418776144 + }, + { + "x": 6.714074305137652, + "y": 1.294654730569418, + "heading": -0.03182450189734621, + "angularVelocity": 2.1040222382135106e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.1480594404369199, + "timestamp": 1.7359119364204374 + }, + { + "x": 7.005460620411861, + "y": 1.2824793298000672, + "heading": -0.031824500167141584, + "angularVelocity": 2.1040221454378006e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.14805944043691993, + "timestamp": 1.8181451309632604 + }, + { + "x": 7.2968469356860695, + "y": 1.2703039290307165, + "heading": -0.03182449843693696, + "angularVelocity": 2.104022148411524e-8, + "velocityX": 3.543414759625678, + "velocityY": -0.1480594404369199, + "timestamp": 1.9003783255060833 + }, + { + "x": 7.588233250960278, + "y": 1.2581285282613657, + "heading": -0.03182449670673225, + "angularVelocity": 2.104022241858639e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.14805944043691993, + "timestamp": 1.9826115200489063 + }, + { + "x": 7.879619566234481, + "y": 1.2459531274920435, + "heading": -0.03182449497652102, + "angularVelocity": 2.104030178948416e-8, + "velocityX": 3.5434147596255965, + "velocityY": -0.14805944043657504, + "timestamp": 2.0648447145917292 + }, + { + "x": 8.161268192201323, + "y": 1.2341846133473202, + "heading": -0.017069904161984, + "angularVelocity": 0.1794237825316825, + "velocityX": 3.424999205402077, + "velocityY": -0.14311147961781526, + "timestamp": 2.1470779091345524 + }, + { + "x": 8.386587111141134, + "y": 1.2247698014142092, + "heading": -0.005266011713750806, + "angularVelocity": 0.14354169887059787, + "velocityX": 2.739999585234113, + "velocityY": -0.11448919120134828, + "timestamp": 2.2293111036773756 + }, + { + "x": 8.555576306335714, + "y": 1.217708692286378, + "heading": 0.0035869801538412527, + "angularVelocity": 0.10765715617410258, + "velocityX": 2.0549997617638245, + "velocityY": -0.08586689556556354, + "timestamp": 2.311544298220199 + }, + { + "x": 8.668235772211057, + "y": 1.213001286124164, + "heading": 0.00948900448730265, + "angularVelocity": 0.07177179928706179, + "velocityX": 1.3699998705106224, + "velocityY": -0.05724459797998689, + "timestamp": 2.393777492763022 + }, + { + "x": 8.724565505981445, + "y": 1.2106475830078125, + "heading": 0.012440027539999704, + "angularVelocity": 0.03588603202275334, + "velocityX": 0.6849999453816004, + "velocityY": -0.028622299418584626, + "timestamp": 2.476010687305845 + }, + { + "x": 8.724565505981445, + "y": 1.2106475830078125, + "heading": 0.012440027539999704, + "angularVelocity": 1.1793246737780391e-29, + "velocityX": -2.6822762319390616e-28, + "velocityY": 3.69381070917653e-27, + "timestamp": 2.5582438818486684 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 4.traj b/src/main/deploy/choreo/Close 2 Note 4.traj index 7ed32ee..7d72777 100644 --- a/src/main/deploy/choreo/Close 2 Note 4.traj +++ b/src/main/deploy/choreo/Close 2 Note 4.traj @@ -3,317 +3,389 @@ { "x": 2.502, "y": 3.44, - "heading": 2.39, - "angularVelocity": 1.6143473252561374e-27, - "velocityX": -4.497600413655855e-27, - "velocityY": 7.73733480239798e-27, + "heading": 2.5104836654523037, + "angularVelocity": 1.9281904905287756e-29, + "velocityX": -4.71586515216861e-27, + "velocityY": 2.3810934236992728e-27, "timestamp": 0 }, { - "x": 2.525916739364106, - "y": 3.420694982582511, - "heading": 2.32354752179907, - "angularVelocity": -1.023433961876349, - "velocityX": 0.3683414672446169, - "velocityY": -0.29731638299377017, - "timestamp": 0.06493089019549111 - }, - { - "x": 2.5746652987077048, - "y": 3.3812894837618575, - "heading": 2.2018139650877506, - "angularVelocity": -1.8748173072140173, - "velocityX": 0.7507760820285803, - "velocityY": -0.6068836989915484, - "timestamp": 0.12986178039098223 - }, - { - "x": 2.649625766646246, - "y": 3.321219957909989, - "heading": 2.0401525298056455, - "angularVelocity": -2.4897461715892337, - "velocityX": 1.1544654279781719, - "velocityY": -0.925130175653128, - "timestamp": 0.19479267058647334 - }, - { - "x": 2.7520459663591446, - "y": 3.239983238174216, - "heading": 1.8578167867140025, - "angularVelocity": -2.8081509824164503, - "velocityX": 1.5773724864164937, - "velocityY": -1.2511259200542133, - "timestamp": 0.25972356078196446 - }, - { - "x": 2.8818046504752663, - "y": 3.1376357565206274, - "heading": 1.6986985692029641, - "angularVelocity": -2.4505780997607194, - "velocityX": 1.9984122152869, - "velocityY": -1.5762525563017087, - "timestamp": 0.3246544509774556 - }, - { - "x": 3.0370045621229718, - "y": 3.020948186403271, - "heading": 1.6099501280880217, - "angularVelocity": -1.3668138669859953, - "velocityX": 2.390232309774843, - "velocityY": -1.7971041174091287, - "timestamp": 0.38958534117294674 - }, - { - "x": 3.217315269794052, - "y": 2.8895687443365436, - "heading": 1.5922226066527227, - "angularVelocity": -0.2730213829184488, - "velocityX": 2.776963431861306, - "velocityY": -2.0233734925114266, - "timestamp": 0.4545162313684379 - }, - { - "x": 3.400066011553934, - "y": 2.749461510644502, - "heading": 1.5922225170352793, - "angularVelocity": -0.0000013801973627609237, - "velocityX": 2.814542372816138, - "velocityY": -2.1577901253195986, - "timestamp": 0.519447121563929 - }, - { - "x": 3.582816703850075, - "y": 2.6093542124337037, - "heading": 1.5922224274179715, - "angularVelocity": -0.0000013801952753049937, - "velocityX": 2.814541611025554, - "velocityY": -2.15779111897233, - "timestamp": 0.5843780117594202 - }, - { - "x": 3.7655673961455407, - "y": 2.4692469142220252, - "heading": 1.5922223378006626, - "angularVelocity": -0.0000013801952903393593, - "velocityX": 2.8145416110151595, - "velocityY": -2.1577911189858847, - "timestamp": 0.6493089019549113 - }, - { - "x": 3.948318088441007, - "y": 2.3291396160103477, - "heading": 1.592222248183353, - "angularVelocity": -0.0000013801953035277997, - "velocityX": 2.8145416110151644, - "velocityY": -2.157791118985876, - "timestamp": 0.7142397921504025 - }, - { - "x": 4.131068780736474, - "y": 2.1890323177986706, - "heading": 1.5922221585660423, - "angularVelocity": -0.0000013801953177064064, - "velocityX": 2.81454161101517, - "velocityY": -2.157791118985867, - "timestamp": 0.7791706823458936 - }, - { - "x": 4.31381947303194, - "y": 2.0489250195869935, - "heading": 1.5922220689487308, - "angularVelocity": -0.000001380195332153733, - "velocityX": 2.814541611015174, - "velocityY": -2.1577911189858576, - "timestamp": 0.8441015725413847 - }, - { - "x": 4.496570165327408, - "y": 1.9088177213753177, - "heading": 1.5922219793314183, - "angularVelocity": -0.0000013801953476150256, - "velocityX": 2.8145416110151804, - "velocityY": -2.157791118985848, - "timestamp": 0.9090324627368759 - }, - { - "x": 4.679320857625259, - "y": 1.7687104231667514, - "heading": 1.5922218897141047, - "angularVelocity": -0.0000013801953620228687, - "velocityX": 2.8145416110518964, - "velocityY": -2.1577911189379546, - "timestamp": 0.973963352932367 - }, - { - "x": 4.86207172453282, - "y": 1.6286033527128316, - "heading": 1.5922218000967605, - "angularVelocity": -0.0000013801958387075775, - "velocityX": 2.8145443002142994, - "velocityY": -2.1577876112908903, - "timestamp": 1.038894243127858 - }, - { - "x": 5.054351274988585, - "y": 1.5018893260974921, - "heading": 1.592221707127701, - "angularVelocity": -0.000001431815567947987, - "velocityX": 2.9612954616340366, - "velocityY": -1.9515214751227716, - "timestamp": 1.103825133323349 + "x": 2.5078330623275535, + "y": 3.4353964642834374, + "heading": 2.421550371026829, + "angularVelocity": -1.8125731387547213, + "velocityX": 0.11888519547047234, + "velocityY": -0.09382588643594957, + "timestamp": 0.04906466532245639 + }, + { + "x": 2.5190564983313566, + "y": 3.4257216835023843, + "heading": 2.2438316854082814, + "angularVelocity": -3.622131822372945, + "velocityX": 0.22874783573966653, + "velocityY": -0.19718428154904508, + "timestamp": 0.09812933064491278 + }, + { + "x": 2.5349008514646854, + "y": 3.4113020995219303, + "heading": 1.9758859051359892, + "angularVelocity": -5.461074247859102, + "velocityX": 0.32292797737839174, + "velocityY": -0.2938893781438674, + "timestamp": 0.14719399596736915 + }, + { + "x": 2.5604353108293316, + "y": 3.394955070383565, + "heading": 1.6293166979187397, + "angularVelocity": -7.063519234047003, + "velocityX": 0.5204246110073755, + "velocityY": -0.3331731507986746, + "timestamp": 0.19625866128982555 + }, + { + "x": 2.603992889740609, + "y": 3.3729904224492855, + "heading": 1.3083335355675372, + "angularVelocity": -6.54204324520872, + "velocityX": 0.8877586064230485, + "velocityY": -0.44766733432149164, + "timestamp": 0.24532332661228196 + }, + { + "x": 2.6607200682182177, + "y": 3.3376856037523086, + "heading": 1.014097923240798, + "angularVelocity": -5.996894310661298, + "velocityX": 1.1561717196029637, + "velocityY": -0.7195569044433685, + "timestamp": 0.29438799193473836 + }, + { + "x": 2.71502692076517, + "y": 3.2838347448517657, + "heading": 0.745483337859882, + "angularVelocity": -5.474705342746399, + "velocityX": 1.1068424127637233, + "velocityY": -1.0975486849167542, + "timestamp": 0.34345265725719476 + }, + { + "x": 2.7744606723157283, + "y": 3.214460375697132, + "heading": 0.5232634756481663, + "angularVelocity": -4.529122144241102, + "velocityX": 1.2113351056194037, + "velocityY": -1.4139374781974092, + "timestamp": 0.39251732257965116 + }, + { + "x": 2.84873633367785, + "y": 3.1410738657590347, + "heading": 0.34689629796158034, + "angularVelocity": -3.594586379576598, + "velocityX": 1.513832018907633, + "velocityY": -1.4957099871322082, + "timestamp": 0.44158198790210756 + }, + { + "x": 2.940226570979376, + "y": 3.0662458182374484, + "heading": 0.20973540275964336, + "angularVelocity": -2.7955127035006972, + "velocityX": 1.8646868719117038, + "velocityY": -1.5250903482131433, + "timestamp": 0.49064665322456397 + }, + { + "x": 3.0480227715570303, + "y": 2.9863881956756497, + "heading": 0.11214926277131038, + "angularVelocity": -1.9889290866857052, + "velocityX": 2.197023048444546, + "velocityY": -1.6275994554730684, + "timestamp": 0.5397113185470204 + }, + { + "x": 3.169811200590146, + "y": 2.8977558044744653, + "heading": 0.054667190153068854, + "angularVelocity": -1.1715574179598567, + "velocityX": 2.4822023799146242, + "velocityY": -1.8064403500703765, + "timestamp": 0.5887759838694767 + }, + { + "x": 3.303879745950196, + "y": 2.7983821340483717, + "heading": 0.037123590068076556, + "angularVelocity": -0.3575607816683257, + "velocityX": 2.7324866985017113, + "velocityY": -2.02536122019794, + "timestamp": 0.6378406491919331 + }, + { + "x": 3.4431520829592457, + "y": 2.6940705143750954, + "heading": 0.037115475659100644, + "angularVelocity": -0.00016538192857494248, + "velocityX": 2.838546560824209, + "velocityY": -2.126002877788589, + "timestamp": 0.6869053145143894 + }, + { + "x": 3.5824244012585216, + "y": 2.5897588683797643, + "heading": 0.03710736322260343, + "angularVelocity": -0.0001653417269616524, + "velocityX": 2.838546179495335, + "velocityY": -2.126003414265387, + "timestamp": 0.7359699798368458 + }, + { + "x": 3.7216967195717463, + "y": 2.485447222406277, + "heading": 0.03709925077439075, + "angularVelocity": -0.00016534196573771254, + "velocityX": 2.8385461797796308, + "velocityY": -2.12600341382019, + "timestamp": 0.7850346451593021 + }, + { + "x": 3.8609690378995296, + "y": 2.381135576455448, + "heading": 0.03709113831446088, + "angularVelocity": -0.00016534220454895188, + "velocityX": 2.8385461800763494, + "velocityY": -2.1260034133583896, + "timestamp": 0.8340993104817584 + }, + { + "x": 4.000241356241871, + "y": 2.276823930527277, + "heading": 0.03708302584281285, + "angularVelocity": -0.0001653424433797745, + "velocityX": 2.8385461803730676, + "velocityY": -2.126003412896584, + "timestamp": 0.8831639758042148 + }, + { + "x": 4.139513674598771, + "y": 2.172512284621764, + "heading": 0.03707491335944571, + "angularVelocity": -0.00016534268223024974, + "velocityX": 2.838546180669785, + "velocityY": -2.1260034124347746, + "timestamp": 0.9322286411266711 + }, + { + "x": 4.278785992970229, + "y": 2.0682006387389102, + "heading": 0.03706680086435853, + "angularVelocity": -0.00016534292109930652, + "velocityX": 2.838546180966501, + "velocityY": -2.1260034119729605, + "timestamp": 0.9812933064491275 + }, + { + "x": 4.418058311356245, + "y": 1.9638889928787158, + "heading": 0.03705868835755037, + "angularVelocity": -0.00016534315998779618, + "velocityX": 2.8385461812632156, + "velocityY": -2.1260034115111432, + "timestamp": 1.0303579717715838 + }, + { + "x": 4.5573306297568195, + "y": 1.85957734704118, + "heading": 0.03705057583902021, + "angularVelocity": -0.00016534339889699094, + "velocityX": 2.8385461815599298, + "velocityY": -2.1260034110493207, + "timestamp": 1.0794226370940403 + }, + { + "x": 4.696602948173282, + "y": 1.7552657012280795, + "heading": 0.03704246330876568, + "angularVelocity": -0.00016534363785453005, + "velocityX": 2.838546181883735, + "velocityY": -2.126003410551304, + "timestamp": 1.1284873024164968 + }, + { + "x": 4.835875308437814, + "y": 1.650954112123811, + "heading": 0.03703434953912002, + "angularVelocity": -0.00016536889821494152, + "velocityX": 2.8385470348003916, + "velocityY": -2.1260022547535105, + "timestamp": 1.1775519677389532 + }, + { + "x": 4.972500711386871, + "y": 1.5511534257066393, + "heading": 0.025156273343944498, + "angularVelocity": -0.2420902316791928, + "velocityX": 2.7845986934007563, + "velocityY": -2.034064346740661, + "timestamp": 1.2266166330614097 + }, + { + "x": 5.112272239054231, + "y": 1.4704127227705035, + "heading": -0.006717749301178638, + "angularVelocity": -0.64963293717882, + "velocityX": 2.8487206984654128, + "velocityY": -1.6455977515693263, + "timestamp": 1.2756812983838661 }, { "x": 5.263677597045898, "y": 1.4059187173843384, - "heading": 1.5922215933347457, - "angularVelocity": -0.0000017525241831714588, - "velocityX": 3.2238326230717362, - "velocityY": -1.4780423989908298, - "timestamp": 1.1687560235188401 - }, - { - "x": 5.549175877605322, - "y": 1.3375763530891083, - "heading": 1.5922215924257537, - "angularVelocity": -1.0981400650486354e-8, - "velocityX": 3.449063323110087, - "velocityY": -0.8256341917136142, - "timestamp": 1.2515316219389887 - }, - { - "x": 5.84250473577961, - "y": 1.325823529497918, - "heading": 1.5922215915939637, - "angularVelocity": -1.0048733277896019e-8, - "velocityX": 3.5436633956473873, - "velocityY": -0.14198415735439168, - "timestamp": 1.3343072203591373 - }, - { - "x": 6.135970278596253, - "y": 1.3182128061287934, - "heading": 1.5922215907646338, - "angularVelocity": -1.0019015771647788e-8, - "velocityX": 3.5453146629890213, - "velocityY": -0.0919440452788315, - "timestamp": 1.4170828187792859 - }, - { - "x": 6.4294358220976315, - "y": 1.3106021091627869, - "heading": 1.5922215899353038, - "angularVelocity": -1.0019015824110795e-8, - "velocityX": 3.54531467126122, - "velocityY": -0.09194372630659559, - "timestamp": 1.4998584171994345 - }, - { - "x": 6.722901365599015, - "y": 1.3029914121969473, - "heading": 1.5922215891059737, - "angularVelocity": -1.0019016426747296e-8, - "velocityX": 3.545314671261272, - "velocityY": -0.09194372630457344, - "timestamp": 1.582634015619583 - }, - { - "x": 7.016366909100399, - "y": 1.2953807152311076, - "heading": 1.5922215882766437, - "angularVelocity": -1.0019015739011674e-8, - "velocityX": 3.545314671261273, - "velocityY": -0.09194372630457337, - "timestamp": 1.6654096140397316 - }, - { - "x": 7.309832452601783, - "y": 1.2877700182652683, - "heading": 1.5922215874473136, - "angularVelocity": -1.00190160592321e-8, - "velocityX": 3.5453146712612726, - "velocityY": -0.09194372630457343, - "timestamp": 1.7481852124598802 - }, - { - "x": 7.603297996103167, - "y": 1.2801593212994287, - "heading": 1.5922215866179836, - "angularVelocity": -1.0019015458013803e-8, - "velocityX": 3.545314671261272, - "velocityY": -0.0919437263045734, - "timestamp": 1.8309608108800288 - }, - { - "x": 7.89676353960455, - "y": 1.272548624333589, - "heading": 1.5922215857886535, - "angularVelocity": -1.0019015102150109e-8, - "velocityX": 3.545314671261272, - "velocityY": -0.09194372630457341, - "timestamp": 1.9137364093001774 - }, - { - "x": 8.190229083105928, - "y": 1.264937927367751, - "heading": 1.5922215849593204, - "angularVelocity": -1.0019051996053813e-8, - "velocityX": 3.545314671261198, - "velocityY": -0.09194372630455969, - "timestamp": 1.996512007720326 - }, - { - "x": 8.475782151148142, - "y": 1.2575324316094862, - "heading": 1.5850799971897327, - "angularVelocity": -0.08627648613712059, - "velocityX": 3.44972520274412, - "velocityY": -0.08946472027512459, - "timestamp": 2.0792876061404746 - }, - { - "x": 8.70422462444052, - "y": 1.2516080345083516, - "heading": 1.5793665822224103, - "angularVelocity": -0.06902293763341408, - "velocityX": 2.7597803900234106, - "velocityY": -0.07157178219436212, - "timestamp": 2.162063204560623 - }, - { - "x": 8.875556485627149, - "y": 1.2471647365187968, - "heading": 1.5750814725763596, - "angularVelocity": -0.051767788186808314, - "velocityX": 2.0698353676283796, - "velocityY": -0.0536788386234512, - "timestamp": 2.2448388029807713 - }, - { - "x": 8.989777728924334, - "y": 1.2442025377932433, - "heading": 1.572224713236977, - "angularVelocity": -0.034512095278156824, - "velocityX": 1.3798902753614153, - "velocityY": -0.03578589321116078, - "timestamp": 2.3276144014009197 - }, - { - "x": 9.04688835144043, - "y": 1.242721438407898, - "heading": 1.5707963267948966, - "angularVelocity": -0.017256129455330296, - "velocityX": 0.6899451481608816, - "velocityY": -0.017892946878229225, - "timestamp": 2.410389999821068 - }, - { - "x": 9.04688835144043, - "y": 1.242721438407898, - "heading": 1.5707963267948966, - "angularVelocity": -5.163840199570749e-28, - "velocityX": -6.169448749202353e-27, - "velocityY": 2.9824086172169885e-27, - "timestamp": 2.4931655982412164 + "heading": -0.031824505216569196, + "angularVelocity": -0.5117074731965909, + "velocityX": 3.0858328900568646, + "velocityY": -1.31446948557187, + "timestamp": 1.3247459637063226 + }, + { + "x": 5.548529116436538, + "y": 1.343358066124809, + "heading": -0.03182450881816663, + "angularVelocity": -4.379736737746259e-8, + "velocityX": 3.4639481169894566, + "velocityY": -0.7607712628377938, + "timestamp": 1.4069791582491455 + }, + { + "x": 5.839915359315492, + "y": 1.3311809328886355, + "heading": -0.03182450708796022, + "angularVelocity": 2.1040243198202156e-8, + "velocityX": 3.5434138792603314, + "velocityY": -0.14808050816792953, + "timestamp": 1.4892123527919685 + }, + { + "x": 6.131301674589235, + "y": 1.3190055321081195, + "heading": -0.031824505357755536, + "angularVelocity": 2.1040222208336333e-8, + "velocityX": 3.5434147596200054, + "velocityY": -0.1480594405726954, + "timestamp": 1.5714455473347915 + }, + { + "x": 6.4226879898634435, + "y": 1.3068301313387687, + "heading": -0.03182450362755091, + "angularVelocity": 2.10402215194868e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.14805944043692082, + "timestamp": 1.6536787418776144 + }, + { + "x": 6.714074305137652, + "y": 1.294654730569418, + "heading": -0.03182450189734621, + "angularVelocity": 2.1040222382135106e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.1480594404369199, + "timestamp": 1.7359119364204374 + }, + { + "x": 7.005460620411861, + "y": 1.2824793298000672, + "heading": -0.031824500167141584, + "angularVelocity": 2.1040221454378006e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.14805944043691993, + "timestamp": 1.8181451309632604 + }, + { + "x": 7.2968469356860695, + "y": 1.2703039290307165, + "heading": -0.03182449843693696, + "angularVelocity": 2.104022148411524e-8, + "velocityX": 3.543414759625678, + "velocityY": -0.1480594404369199, + "timestamp": 1.9003783255060833 + }, + { + "x": 7.588233250960278, + "y": 1.2581285282613657, + "heading": -0.03182449670673225, + "angularVelocity": 2.104022241858639e-8, + "velocityX": 3.5434147596256786, + "velocityY": -0.14805944043691993, + "timestamp": 1.9826115200489063 + }, + { + "x": 7.879619566234481, + "y": 1.2459531274920435, + "heading": -0.03182449497652102, + "angularVelocity": 2.104030178948416e-8, + "velocityX": 3.5434147596255965, + "velocityY": -0.14805944043657504, + "timestamp": 2.0648447145917292 + }, + { + "x": 8.161268192201323, + "y": 1.2341846133473202, + "heading": -0.017069904161984, + "angularVelocity": 0.1794237825316825, + "velocityX": 3.424999205402077, + "velocityY": -0.14311147961781526, + "timestamp": 2.1470779091345524 + }, + { + "x": 8.386587111141134, + "y": 1.2247698014142092, + "heading": -0.005266011713750806, + "angularVelocity": 0.14354169887059787, + "velocityX": 2.739999585234113, + "velocityY": -0.11448919120134828, + "timestamp": 2.2293111036773756 + }, + { + "x": 8.555576306335714, + "y": 1.217708692286378, + "heading": 0.0035869801538412527, + "angularVelocity": 0.10765715617410258, + "velocityX": 2.0549997617638245, + "velocityY": -0.08586689556556354, + "timestamp": 2.311544298220199 + }, + { + "x": 8.668235772211057, + "y": 1.213001286124164, + "heading": 0.00948900448730265, + "angularVelocity": 0.07177179928706179, + "velocityX": 1.3699998705106224, + "velocityY": -0.05724459797998689, + "timestamp": 2.393777492763022 + }, + { + "x": 8.724565505981445, + "y": 1.2106475830078125, + "heading": 0.012440027539999704, + "angularVelocity": 0.03588603202275334, + "velocityX": 0.6849999453816004, + "velocityY": -0.028622299418584626, + "timestamp": 2.476010687305845 + }, + { + "x": 8.724565505981445, + "y": 1.2106475830078125, + "heading": 0.012440027539999704, + "angularVelocity": 1.1793246737780391e-29, + "velocityX": -2.6822762319390616e-28, + "velocityY": 3.69381070917653e-27, + "timestamp": 2.5582438818486684 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 5.1.traj b/src/main/deploy/choreo/Close 2 Note 5.1.traj index 2dd6fa1..873633c 100644 --- a/src/main/deploy/choreo/Close 2 Note 5.1.traj +++ b/src/main/deploy/choreo/Close 2 Note 5.1.traj @@ -3,308 +3,389 @@ { "x": 8.972707748413086, "y": 1.1833769083023071, - "heading": 1.571, - "angularVelocity": -9.767399252006736e-24, - "velocityX": 5.907264225115041e-23, - "velocityY": 1.299820018625526e-22, + "heading": 0.009898490937098424, + "angularVelocity": -3.594444463136562e-23, + "velocityX": 2.661376445930665e-23, + "velocityY": 1.6196216041750108e-24, "timestamp": 0 }, { - "x": 8.935090507110717, - "y": 1.1863475460806576, - "heading": 1.6136708412192649, - "angularVelocity": 0.6202915722960445, - "velocityX": -0.5468291012353428, - "velocityY": 0.04318315565391617, - "timestamp": 0.06879158628790716 - }, - { - "x": 8.859516182153305, - "y": 1.192305430427452, - "heading": 1.6942785937843952, - "angularVelocity": 1.1717676087271771, - "velocityX": -1.0985983756955169, - "velocityY": 0.08660774766640515, - "timestamp": 0.13758317257581432 - }, - { - "x": 8.745545936468519, - "y": 1.2013296641436748, - "heading": 1.8058392839348794, - "angularVelocity": 1.6217199830743718, - "velocityX": -1.6567468761048345, - "velocityY": 0.13118223031598755, - "timestamp": 0.20637475886372147 - }, - { - "x": 8.592647322978339, - "y": 1.2135389725367793, - "heading": 1.9374414012084449, - "angularVelocity": 1.9130554239988473, - "velocityX": -2.2226353794236933, - "velocityY": 0.17748258256475968, - "timestamp": 0.27516634515162863 - }, - { - "x": 8.40044069338619, - "y": 1.228982218641201, - "heading": 2.064842015198071, - "angularVelocity": 1.8519795932082177, - "velocityX": -2.7940427014972795, - "velocityY": 0.2244932402022014, - "timestamp": 0.3439579314395358 - }, - { - "x": 8.175541171514828, - "y": 1.2426911633087852, - "heading": 2.111215871319779, - "angularVelocity": 0.6741210462515563, - "velocityX": -3.2692882081554697, - "velocityY": 0.1992822873746392, - "timestamp": 0.41274951772744295 - }, - { - "x": 7.932395229110654, - "y": 1.262724401465717, - "heading": 2.1112158916774773, - "angularVelocity": 2.9593296428696675e-7, - "velocityX": -3.534530245989049, - "velocityY": 0.2912164006960001, - "timestamp": 0.4815411040153501 - }, - { - "x": 7.68924928943743, - "y": 1.2827576727686798, - "heading": 2.1112159120351404, - "angularVelocity": 2.9593245340453995e-7, - "velocityX": -3.534530206290155, - "velocityY": 0.2912168825286064, - "timestamp": 0.5503326903032573 - }, - { - "x": 7.446103349764219, - "y": 1.3027909440717906, - "heading": 2.111215932392803, - "angularVelocity": 2.95932453993162e-7, - "velocityX": -3.5345302062899764, - "velocityY": 0.291216882530778, - "timestamp": 0.6191242765911644 - }, - { - "x": 7.202957410091008, - "y": 1.3228242153749026, - "heading": 2.111215952750466, - "angularVelocity": 2.959324524383632e-7, - "velocityX": -3.5345302062899755, - "velocityY": 0.29121688253078115, - "timestamp": 0.6879158628790716 - }, - { - "x": 6.9598114704177965, - "y": 1.342857486678014, - "heading": 2.111215973108129, - "angularVelocity": 2.9593245270626844e-7, - "velocityX": -3.534530206289975, - "velocityY": 0.2912168825307812, - "timestamp": 0.7567074491669787 - }, - { - "x": 6.716665530744585, - "y": 1.3628907579811254, - "heading": 2.1112159934657915, - "angularVelocity": 2.9593245074455596e-7, - "velocityX": -3.5345302062899755, - "velocityY": 0.29121688253078176, - "timestamp": 0.8254990354548859 - }, - { - "x": 6.4735195910713745, - "y": 1.3829240292842375, - "heading": 2.111216013823454, - "angularVelocity": 2.959324507471835e-7, - "velocityX": -3.5345302062899755, - "velocityY": 0.2912168825307816, - "timestamp": 0.8942906217427931 - }, - { - "x": 6.230373651398163, - "y": 1.402957300587349, - "heading": 2.1112160341811164, - "angularVelocity": 2.9593244991278353e-7, - "velocityX": -3.534530206289976, - "velocityY": 0.29121688253078176, - "timestamp": 0.9630822080307002 - }, - { - "x": 5.987227711724952, - "y": 1.4229905718904616, - "heading": 2.1112160545387795, - "angularVelocity": 2.9593244925906865e-7, - "velocityX": -3.534530206289975, - "velocityY": 0.29121688253078837, - "timestamp": 1.0318737943186074 - }, - { - "x": 5.744081772051767, - "y": 1.4430238431938993, - "heading": 2.111216074896442, - "angularVelocity": 2.9593244949024157e-7, - "velocityX": -3.534530206289585, - "velocityY": 0.291216882535523, - "timestamp": 1.1006653806065145 - }, - { - "x": 5.500935838264892, - "y": 1.4630571859399728, - "heading": 2.111216095254104, - "angularVelocity": 2.959324407133209e-7, - "velocityX": -3.5345301207222937, - "velocityY": 0.29121792107293887, - "timestamp": 1.1694569668944217 - }, - { - "x": 5.259315238985184, - "y": 1.4968324118293073, - "heading": 2.111216115741956, - "angularVelocity": 2.9782496007778795e-7, - "velocityX": -3.512356849404178, - "velocityY": 0.49097902391693365, - "timestamp": 1.2382485531823288 - }, - { - "x": 5.026299476623535, - "y": 1.5691161155700684, - "heading": 2.111216138366583, - "angularVelocity": 3.2888654369729736e-7, - "velocityX": -3.3872712483533762, - "velocityY": 1.0507637291316205, - "timestamp": 1.307040139470236 - }, - { - "x": 4.776317244480285, - "y": 1.702059385073433, - "heading": 2.111216140017271, - "angularVelocity": 2.06763249457757e-8, - "velocityX": -3.1312480899349673, - "velocityY": 1.6652317852077385, - "timestamp": 1.3868748331653122 - }, - { - "x": 4.555581127297633, - "y": 1.8793743483839014, - "heading": 2.1112161415313353, - "angularVelocity": 1.8964991666679164e-8, - "velocityX": -2.7649146876637363, - "velocityY": 2.2210264122476873, - "timestamp": 1.4667095268603885 - }, - { - "x": 4.3382681622702, - "y": 2.0608684388916934, - "heading": 2.111216143039461, - "angularVelocity": 1.889060440086873e-8, - "velocityX": -2.7220366856725855, - "velocityY": 2.2733736688587687, - "timestamp": 1.5465442205554647 - }, - { - "x": 4.120955206951703, - "y": 2.2423625410245305, - "heading": 2.111216144547586, - "angularVelocity": 1.8890603633646163e-8, - "velocityX": -2.7220365640596182, - "velocityY": 2.273373814472728, - "timestamp": 1.626378914250541 - }, - { - "x": 3.903642251633233, - "y": 2.423856643157401, - "heading": 2.1112161460557117, - "angularVelocity": 1.8890603742361353e-8, - "velocityX": -2.722036564059269, - "velocityY": 2.2733738144731452, - "timestamp": 1.7062136079456172 - }, - { - "x": 3.6863292963147622, - "y": 2.605350745290272, - "heading": 2.1112161475638374, - "angularVelocity": 1.8890604477781255e-8, - "velocityX": -2.7220365640592696, - "velocityY": 2.273373814473146, - "timestamp": 1.7860483016406934 - }, - { - "x": 3.4690163409962946, - "y": 2.786844847423142, - "heading": 2.111216149071963, - "angularVelocity": 1.8890603977643177e-8, - "velocityX": -2.722036564059269, - "velocityY": 2.273373814473146, - "timestamp": 1.8658829953357696 - }, - { - "x": 3.251703385681568, - "y": 2.968338949560493, - "heading": 2.1112161505800917, - "angularVelocity": 1.8890642534870236e-8, - "velocityX": -2.7220365640123627, - "velocityY": 2.273373814529255, - "timestamp": 1.9457176890308459 - }, - { - "x": 3.0478649217251967, - "y": 3.1385799156225445, - "heading": 2.1419916080333983, - "angularVelocity": 0.385489766777984, - "velocityX": -2.5532566672694883, - "velocityY": 2.1324183532572523, - "timestamp": 2.0255523827259223 - }, - { - "x": 2.8847941463462403, - "y": 3.274772687216185, - "heading": 2.1666127065045155, - "angularVelocity": 0.308400988737502, - "velocityX": -2.042605386597918, - "velocityY": 1.705934666873273, - "timestamp": 2.1053870764209988 - }, - { - "x": 2.7624910633351236, - "y": 3.376917264599957, - "heading": 2.1850788550552847, - "angularVelocity": 0.23130480867502362, - "velocityX": -1.5319540584478795, - "velocityY": 1.2794509837277892, - "timestamp": 2.185221770116075 - }, - { - "x": 2.680955673869497, - "y": 3.445013649295216, - "heading": 2.197389707457713, - "angularVelocity": 0.15420429180136633, - "velocityX": -1.02130271554678, - "velocityY": 0.8529673196386245, - "timestamp": 2.2650564638111517 - }, - { - "x": 2.640187978744507, - "y": 3.4790618419647217, + "x": 8.9642414023468, + "y": 1.1838504232385874, + "heading": 0.11269422494821685, + "angularVelocity": 1.9483306196990626, + "velocityX": -0.1604662045229367, + "velocityY": 0.008974726997325759, + "timestamp": 0.05276092926517583 + }, + { + "x": 8.948083227066952, + "y": 1.1844896520896235, + "heading": 0.3198575365997856, + "angularVelocity": 3.926453050331398, + "velocityX": -0.30625266660175565, + "velocityY": 0.012115572260365658, + "timestamp": 0.10552185853035166 + }, + { + "x": 8.924965236199132, + "y": 1.188814526065175, + "heading": 0.6300158546364878, + "angularVelocity": 5.878560562833341, + "velocityX": -0.4381649677099328, + "velocityY": 0.08197114864703657, + "timestamp": 0.15828278779552749 + }, + { + "x": 8.884295423196336, + "y": 1.2001880729953864, + "heading": 0.9938364812875791, + "angularVelocity": 6.895644783330729, + "velocityX": -0.7708320071162389, + "velocityY": 0.21556760065858568, + "timestamp": 0.21104371706070332 + }, + { + "x": 8.822221884064051, + "y": 1.2155938935187947, + "heading": 1.32752447859539, + "angularVelocity": 6.324528433354513, + "velocityX": -1.1765057969374584, + "velocityY": 0.2919929716548175, + "timestamp": 0.26380464632587913 + }, + { + "x": 8.74361650376665, + "y": 1.2194863977685344, + "heading": 1.619938928469106, + "angularVelocity": 5.542253594587851, + "velocityX": -1.489840709634433, + "velocityY": 0.0737762640641897, + "timestamp": 0.31656557559105497 + }, + { + "x": 8.646843774366385, + "y": 1.2218814186876117, + "heading": 1.8543200673475446, + "angularVelocity": 4.442323934448573, + "velocityX": -1.8341740895784047, + "velocityY": 0.04539383502970571, + "timestamp": 0.3693265048562308 + }, + { + "x": 8.535183983723305, + "y": 1.2361352435496769, + "heading": 2.0387764035405076, + "angularVelocity": 3.496078229894084, + "velocityX": -2.116334799220068, + "velocityY": 0.2701587151815607, + "timestamp": 0.42208743412140665 + }, + { + "x": 8.40747247720944, + "y": 1.2621673609702526, + "heading": 2.1777614957327995, + "angularVelocity": 2.6342426892019786, + "velocityX": -2.4205696960337497, + "velocityY": 0.4933976293279214, + "timestamp": 0.4748483633865825 + }, + { + "x": 8.261948828020005, + "y": 1.2964340038650055, + "heading": 2.270600320754398, + "angularVelocity": 1.7596131515233795, + "velocityX": -2.758170699724305, + "velocityY": 0.6494700410322398, + "timestamp": 0.5276092926517583 + }, + { + "x": 8.09782277606961, + "y": 1.3349380681436944, + "heading": 2.3167167289348747, + "angularVelocity": 0.8740636077256377, + "velocityX": -3.1107498339443698, + "velocityY": 0.7297836640664107, + "timestamp": 0.5803702219169341 + }, + { + "x": 7.914829584256359, + "y": 1.3739987658766613, + "heading": 2.3167202209384494, + "angularVelocity": 0.00006618540695237648, + "velocityX": -3.4683466413855033, + "velocityY": 0.7403337711632821, + "timestamp": 0.6331311511821099 + }, + { + "x": 7.731792737090714, + "y": 1.412856687245149, + "heading": 2.3167225237878513, + "angularVelocity": 0.00004364686964296262, + "velocityX": -3.4691740595716793, + "velocityY": 0.7364904657609108, + "timestamp": 0.6858920804472858 + }, + { + "x": 7.548755888844399, + "y": 1.451714603522748, + "heading": 2.3167248266383997, + "angularVelocity": 0.00004364689137005159, + "velocityX": -3.469174080054087, + "velocityY": 0.7364903692711618, + "timestamp": 0.7386530097124616 + }, + { + "x": 7.365719040598746, + "y": 1.4905725198022486, + "heading": 2.316727129490484, + "angularVelocity": 0.00004364692048352743, + "velocityX": -3.469174080041539, + "velocityY": 0.7364903693072045, + "timestamp": 0.7914139389776375 + }, + { + "x": 7.182682192353782, + "y": 1.529430436083779, + "heading": 2.3167294323441046, + "angularVelocity": 0.00004364694959712505, + "velocityX": -3.4691740800284765, + "velocityY": 0.7364903693456704, + "timestamp": 0.8441748682428133 + }, + { + "x": 6.999645344109507, + "y": 1.5682883523673385, + "heading": 2.3167317351992613, + "angularVelocity": 0.00004364697871207036, + "velocityX": -3.469174080015414, + "velocityY": 0.7364903693841357, + "timestamp": 0.8969357975079891 + }, + { + "x": 6.816608495865921, + "y": 1.6071462686529279, + "heading": 2.316734038055954, + "angularVelocity": 0.00004364700782609749, + "velocityX": -3.4691740800023507, + "velocityY": 0.7364903694226017, + "timestamp": 0.949696726773165 + }, + { + "x": 6.633571647623024, + "y": 1.6460041849405465, + "heading": 2.316736340914183, + "angularVelocity": 0.00004364703694171292, + "velocityX": -3.469174079989288, + "velocityY": 0.7364903694610679, + "timestamp": 1.0024576560383407 + }, + { + "x": 6.450534799380817, + "y": 1.6848621012301948, + "heading": 2.316738643773948, + "angularVelocity": 0.000043647066056982235, + "velocityX": -3.469174079976225, + "velocityY": 0.736490369499534, + "timestamp": 1.0552185853035165 + }, + { + "x": 6.267497951139299, + "y": 1.7237200175218723, + "heading": 2.3167409466352487, + "angularVelocity": 0.00004364709517405661, + "velocityX": -3.4691740799631625, + "velocityY": 0.7364903695379998, + "timestamp": 1.1079795145686924 + }, + { + "x": 6.084461102898469, + "y": 1.7625779338155794, + "heading": 2.316743249498086, + "angularVelocity": 0.000043647124290962285, + "velocityX": -3.4691740799500987, + "velocityY": 0.7364903695764661, + "timestamp": 1.1607404438338682 + }, + { + "x": 5.90142425465833, + "y": 1.8014358501113161, + "heading": 2.31674555236246, + "angularVelocity": 0.000043647153406607097, + "velocityX": -3.469174079937036, + "velocityY": 0.7364903696149331, + "timestamp": 1.213501373099044 + }, + { + "x": 5.718387406418881, + "y": 1.8402937664090826, + "heading": 2.31674785522837, + "angularVelocity": 0.000043647182524781595, + "velocityX": -3.4691740799239703, + "velocityY": 0.7364903696534086, + "timestamp": 1.2662623023642199 + }, + { + "x": 5.535350558184445, + "y": 1.879151682729246, + "heading": 2.316750158095817, + "angularVelocity": 0.00004364721167212726, + "velocityX": -3.469174079828943, + "velocityY": 0.7364903700779036, + "timestamp": 1.3190232316293957 + }, + { + "x": 5.352313882264269, + "y": 1.9180104105293494, + "heading": 2.316752461060097, + "angularVelocity": 0.0000436490469716496, + "velocityX": -3.4691708138845505, + "velocityY": 0.7365057503972241, + "timestamp": 1.3717841608945716 + }, + { + "x": 5.174009799957275, + "y": 1.9726389646530151, + "heading": 2.3183527865287403, + "angularVelocity": 0.030331639168071065, + "velocityX": -3.3794719841047485, + "velocityY": 1.0353978765063663, + "timestamp": 1.4245450901597474 + }, + { + "x": 4.918462925408197, + "y": 2.1088992111590095, + "heading": 2.318352780187111, + "angularVelocity": -7.76596778188732e-8, + "velocityX": -3.1294303624461395, + "velocityY": 1.6686447578851502, + "timestamp": 1.506204320419573 + }, + { + "x": 4.6706807831887005, + "y": 2.2588160882042634, + "heading": 2.318352773860442, + "angularVelocity": -7.747647131185673e-8, + "velocityX": -3.03434334895268, + "velocityY": 1.8358840337858229, + "timestamp": 1.5878635506793985 + }, + { + "x": 4.42289869878705, + "y": 2.4087330608106337, + "heading": 2.318352767533773, + "angularVelocity": -7.747646978575146e-8, + "velocityX": -3.0343426409145584, + "velocityY": 1.8358852040285008, + "timestamp": 1.6695227809392241 + }, + { + "x": 4.17511661438578, + "y": 2.5586500334176288, + "heading": 2.318352761207104, + "angularVelocity": -7.747646959889163e-8, + "velocityX": -3.034342640909924, + "velocityY": 1.835885204036159, + "timestamp": 1.7511820111990497 + }, + { + "x": 3.9273345299845093, + "y": 2.7085670060246243, + "heading": 2.318352754880435, + "angularVelocity": -7.747646933531489e-8, + "velocityX": -3.0343426409099234, + "velocityY": 1.835885204036159, + "timestamp": 1.8328412414588753 + }, + { + "x": 3.679552445583239, + "y": 2.85848397863162, + "heading": 2.318352748553766, + "angularVelocity": -7.747646958961607e-8, + "velocityX": -3.034342640909924, + "velocityY": 1.8358852040361588, + "timestamp": 1.9145004717187009 + }, + { + "x": 3.431770361181968, + "y": 3.0084009512386154, + "heading": 2.318352742227097, + "angularVelocity": -7.747647015947937e-8, + "velocityX": -3.034342640909924, + "velocityY": 1.8358852040361597, + "timestamp": 1.9961597019785264 + }, + { + "x": 3.1839882767807, + "y": 3.158317923845615, + "heading": 2.318352735900428, + "angularVelocity": -7.747647018934798e-8, + "velocityX": -3.034342640909895, + "velocityY": 1.835885204036207, + "timestamp": 2.077818932238352 + }, + { + "x": 2.936206192742126, + "y": 3.308234897052054, + "heading": 2.3183527295737423, + "angularVelocity": -7.74766777065616e-8, + "velocityX": -3.0343426364683324, + "velocityY": 1.8358852113769544, + "timestamp": 2.1594781624981776 + }, + { + "x": 2.6985520828578236, + "y": 3.452139832457651, + "heading": 2.2861229229565243, + "angularVelocity": -0.39468663266440823, + "velocityX": -2.91031533273248, + "velocityY": 1.7622617179676583, + "timestamp": 2.241137392758003 + }, + { + "x": 2.5084457239359947, + "y": 3.567225663105015, + "heading": 2.2549856266803583, + "angularVelocity": -0.3813077367627909, + "velocityX": -2.328044953607123, + "velocityY": 1.4093425848024874, + "timestamp": 2.3227966230178287 + }, + { + "x": 2.3658771899178763, + "y": 3.653524329460914, + "heading": 2.2299036649035693, + "angularVelocity": -0.3071540314179127, + "velocityX": -1.7458961291269777, + "velocityY": 1.0568145950104009, + "timestamp": 2.4044558532776543 + }, + { + "x": 2.270836799349362, + "y": 3.7110500271012747, + "heading": 2.2124904722719037, + "angularVelocity": -0.213242184334339, + "velocityX": -1.1638658638602495, + "velocityY": 0.7044604444264757, + "timestamp": 2.48611508353748 + }, + { + "x": 2.223318576812744, + "y": 3.7398104667663574, "heading": 2.203545115911647, - "angularVelocity": 0.07710192360033451, - "velocityX": -0.5106513626857504, - "velocityY": 0.42648366385108344, - "timestamp": 2.344891157506228 + "angularVelocity": -0.10954495078871034, + "velocityX": -0.5819087736367691, + "velocityY": 0.35220071966845595, + "timestamp": 2.5677743137973055 }, { - "x": 2.640187978744507, - "y": 3.4790618419647217, + "x": 2.223318576812744, + "y": 3.7398104667663574, "heading": 2.203545115911647, - "angularVelocity": -2.5642698021713453e-24, - "velocityX": -3.130979253679926e-22, - "velocityY": -1.883332935459204e-22, - "timestamp": 2.4247258512013046 + "angularVelocity": 4.719095338364679e-26, + "velocityX": 4.055221098284943e-24, + "velocityY": 5.8881582869771646e-24, + "timestamp": 2.649433544057131 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 5.traj b/src/main/deploy/choreo/Close 2 Note 5.traj index 2dd6fa1..873633c 100644 --- a/src/main/deploy/choreo/Close 2 Note 5.traj +++ b/src/main/deploy/choreo/Close 2 Note 5.traj @@ -3,308 +3,389 @@ { "x": 8.972707748413086, "y": 1.1833769083023071, - "heading": 1.571, - "angularVelocity": -9.767399252006736e-24, - "velocityX": 5.907264225115041e-23, - "velocityY": 1.299820018625526e-22, + "heading": 0.009898490937098424, + "angularVelocity": -3.594444463136562e-23, + "velocityX": 2.661376445930665e-23, + "velocityY": 1.6196216041750108e-24, "timestamp": 0 }, { - "x": 8.935090507110717, - "y": 1.1863475460806576, - "heading": 1.6136708412192649, - "angularVelocity": 0.6202915722960445, - "velocityX": -0.5468291012353428, - "velocityY": 0.04318315565391617, - "timestamp": 0.06879158628790716 - }, - { - "x": 8.859516182153305, - "y": 1.192305430427452, - "heading": 1.6942785937843952, - "angularVelocity": 1.1717676087271771, - "velocityX": -1.0985983756955169, - "velocityY": 0.08660774766640515, - "timestamp": 0.13758317257581432 - }, - { - "x": 8.745545936468519, - "y": 1.2013296641436748, - "heading": 1.8058392839348794, - "angularVelocity": 1.6217199830743718, - "velocityX": -1.6567468761048345, - "velocityY": 0.13118223031598755, - "timestamp": 0.20637475886372147 - }, - { - "x": 8.592647322978339, - "y": 1.2135389725367793, - "heading": 1.9374414012084449, - "angularVelocity": 1.9130554239988473, - "velocityX": -2.2226353794236933, - "velocityY": 0.17748258256475968, - "timestamp": 0.27516634515162863 - }, - { - "x": 8.40044069338619, - "y": 1.228982218641201, - "heading": 2.064842015198071, - "angularVelocity": 1.8519795932082177, - "velocityX": -2.7940427014972795, - "velocityY": 0.2244932402022014, - "timestamp": 0.3439579314395358 - }, - { - "x": 8.175541171514828, - "y": 1.2426911633087852, - "heading": 2.111215871319779, - "angularVelocity": 0.6741210462515563, - "velocityX": -3.2692882081554697, - "velocityY": 0.1992822873746392, - "timestamp": 0.41274951772744295 - }, - { - "x": 7.932395229110654, - "y": 1.262724401465717, - "heading": 2.1112158916774773, - "angularVelocity": 2.9593296428696675e-7, - "velocityX": -3.534530245989049, - "velocityY": 0.2912164006960001, - "timestamp": 0.4815411040153501 - }, - { - "x": 7.68924928943743, - "y": 1.2827576727686798, - "heading": 2.1112159120351404, - "angularVelocity": 2.9593245340453995e-7, - "velocityX": -3.534530206290155, - "velocityY": 0.2912168825286064, - "timestamp": 0.5503326903032573 - }, - { - "x": 7.446103349764219, - "y": 1.3027909440717906, - "heading": 2.111215932392803, - "angularVelocity": 2.95932453993162e-7, - "velocityX": -3.5345302062899764, - "velocityY": 0.291216882530778, - "timestamp": 0.6191242765911644 - }, - { - "x": 7.202957410091008, - "y": 1.3228242153749026, - "heading": 2.111215952750466, - "angularVelocity": 2.959324524383632e-7, - "velocityX": -3.5345302062899755, - "velocityY": 0.29121688253078115, - "timestamp": 0.6879158628790716 - }, - { - "x": 6.9598114704177965, - "y": 1.342857486678014, - "heading": 2.111215973108129, - "angularVelocity": 2.9593245270626844e-7, - "velocityX": -3.534530206289975, - "velocityY": 0.2912168825307812, - "timestamp": 0.7567074491669787 - }, - { - "x": 6.716665530744585, - "y": 1.3628907579811254, - "heading": 2.1112159934657915, - "angularVelocity": 2.9593245074455596e-7, - "velocityX": -3.5345302062899755, - "velocityY": 0.29121688253078176, - "timestamp": 0.8254990354548859 - }, - { - "x": 6.4735195910713745, - "y": 1.3829240292842375, - "heading": 2.111216013823454, - "angularVelocity": 2.959324507471835e-7, - "velocityX": -3.5345302062899755, - "velocityY": 0.2912168825307816, - "timestamp": 0.8942906217427931 - }, - { - "x": 6.230373651398163, - "y": 1.402957300587349, - "heading": 2.1112160341811164, - "angularVelocity": 2.9593244991278353e-7, - "velocityX": -3.534530206289976, - "velocityY": 0.29121688253078176, - "timestamp": 0.9630822080307002 - }, - { - "x": 5.987227711724952, - "y": 1.4229905718904616, - "heading": 2.1112160545387795, - "angularVelocity": 2.9593244925906865e-7, - "velocityX": -3.534530206289975, - "velocityY": 0.29121688253078837, - "timestamp": 1.0318737943186074 - }, - { - "x": 5.744081772051767, - "y": 1.4430238431938993, - "heading": 2.111216074896442, - "angularVelocity": 2.9593244949024157e-7, - "velocityX": -3.534530206289585, - "velocityY": 0.291216882535523, - "timestamp": 1.1006653806065145 - }, - { - "x": 5.500935838264892, - "y": 1.4630571859399728, - "heading": 2.111216095254104, - "angularVelocity": 2.959324407133209e-7, - "velocityX": -3.5345301207222937, - "velocityY": 0.29121792107293887, - "timestamp": 1.1694569668944217 - }, - { - "x": 5.259315238985184, - "y": 1.4968324118293073, - "heading": 2.111216115741956, - "angularVelocity": 2.9782496007778795e-7, - "velocityX": -3.512356849404178, - "velocityY": 0.49097902391693365, - "timestamp": 1.2382485531823288 - }, - { - "x": 5.026299476623535, - "y": 1.5691161155700684, - "heading": 2.111216138366583, - "angularVelocity": 3.2888654369729736e-7, - "velocityX": -3.3872712483533762, - "velocityY": 1.0507637291316205, - "timestamp": 1.307040139470236 - }, - { - "x": 4.776317244480285, - "y": 1.702059385073433, - "heading": 2.111216140017271, - "angularVelocity": 2.06763249457757e-8, - "velocityX": -3.1312480899349673, - "velocityY": 1.6652317852077385, - "timestamp": 1.3868748331653122 - }, - { - "x": 4.555581127297633, - "y": 1.8793743483839014, - "heading": 2.1112161415313353, - "angularVelocity": 1.8964991666679164e-8, - "velocityX": -2.7649146876637363, - "velocityY": 2.2210264122476873, - "timestamp": 1.4667095268603885 - }, - { - "x": 4.3382681622702, - "y": 2.0608684388916934, - "heading": 2.111216143039461, - "angularVelocity": 1.889060440086873e-8, - "velocityX": -2.7220366856725855, - "velocityY": 2.2733736688587687, - "timestamp": 1.5465442205554647 - }, - { - "x": 4.120955206951703, - "y": 2.2423625410245305, - "heading": 2.111216144547586, - "angularVelocity": 1.8890603633646163e-8, - "velocityX": -2.7220365640596182, - "velocityY": 2.273373814472728, - "timestamp": 1.626378914250541 - }, - { - "x": 3.903642251633233, - "y": 2.423856643157401, - "heading": 2.1112161460557117, - "angularVelocity": 1.8890603742361353e-8, - "velocityX": -2.722036564059269, - "velocityY": 2.2733738144731452, - "timestamp": 1.7062136079456172 - }, - { - "x": 3.6863292963147622, - "y": 2.605350745290272, - "heading": 2.1112161475638374, - "angularVelocity": 1.8890604477781255e-8, - "velocityX": -2.7220365640592696, - "velocityY": 2.273373814473146, - "timestamp": 1.7860483016406934 - }, - { - "x": 3.4690163409962946, - "y": 2.786844847423142, - "heading": 2.111216149071963, - "angularVelocity": 1.8890603977643177e-8, - "velocityX": -2.722036564059269, - "velocityY": 2.273373814473146, - "timestamp": 1.8658829953357696 - }, - { - "x": 3.251703385681568, - "y": 2.968338949560493, - "heading": 2.1112161505800917, - "angularVelocity": 1.8890642534870236e-8, - "velocityX": -2.7220365640123627, - "velocityY": 2.273373814529255, - "timestamp": 1.9457176890308459 - }, - { - "x": 3.0478649217251967, - "y": 3.1385799156225445, - "heading": 2.1419916080333983, - "angularVelocity": 0.385489766777984, - "velocityX": -2.5532566672694883, - "velocityY": 2.1324183532572523, - "timestamp": 2.0255523827259223 - }, - { - "x": 2.8847941463462403, - "y": 3.274772687216185, - "heading": 2.1666127065045155, - "angularVelocity": 0.308400988737502, - "velocityX": -2.042605386597918, - "velocityY": 1.705934666873273, - "timestamp": 2.1053870764209988 - }, - { - "x": 2.7624910633351236, - "y": 3.376917264599957, - "heading": 2.1850788550552847, - "angularVelocity": 0.23130480867502362, - "velocityX": -1.5319540584478795, - "velocityY": 1.2794509837277892, - "timestamp": 2.185221770116075 - }, - { - "x": 2.680955673869497, - "y": 3.445013649295216, - "heading": 2.197389707457713, - "angularVelocity": 0.15420429180136633, - "velocityX": -1.02130271554678, - "velocityY": 0.8529673196386245, - "timestamp": 2.2650564638111517 - }, - { - "x": 2.640187978744507, - "y": 3.4790618419647217, + "x": 8.9642414023468, + "y": 1.1838504232385874, + "heading": 0.11269422494821685, + "angularVelocity": 1.9483306196990626, + "velocityX": -0.1604662045229367, + "velocityY": 0.008974726997325759, + "timestamp": 0.05276092926517583 + }, + { + "x": 8.948083227066952, + "y": 1.1844896520896235, + "heading": 0.3198575365997856, + "angularVelocity": 3.926453050331398, + "velocityX": -0.30625266660175565, + "velocityY": 0.012115572260365658, + "timestamp": 0.10552185853035166 + }, + { + "x": 8.924965236199132, + "y": 1.188814526065175, + "heading": 0.6300158546364878, + "angularVelocity": 5.878560562833341, + "velocityX": -0.4381649677099328, + "velocityY": 0.08197114864703657, + "timestamp": 0.15828278779552749 + }, + { + "x": 8.884295423196336, + "y": 1.2001880729953864, + "heading": 0.9938364812875791, + "angularVelocity": 6.895644783330729, + "velocityX": -0.7708320071162389, + "velocityY": 0.21556760065858568, + "timestamp": 0.21104371706070332 + }, + { + "x": 8.822221884064051, + "y": 1.2155938935187947, + "heading": 1.32752447859539, + "angularVelocity": 6.324528433354513, + "velocityX": -1.1765057969374584, + "velocityY": 0.2919929716548175, + "timestamp": 0.26380464632587913 + }, + { + "x": 8.74361650376665, + "y": 1.2194863977685344, + "heading": 1.619938928469106, + "angularVelocity": 5.542253594587851, + "velocityX": -1.489840709634433, + "velocityY": 0.0737762640641897, + "timestamp": 0.31656557559105497 + }, + { + "x": 8.646843774366385, + "y": 1.2218814186876117, + "heading": 1.8543200673475446, + "angularVelocity": 4.442323934448573, + "velocityX": -1.8341740895784047, + "velocityY": 0.04539383502970571, + "timestamp": 0.3693265048562308 + }, + { + "x": 8.535183983723305, + "y": 1.2361352435496769, + "heading": 2.0387764035405076, + "angularVelocity": 3.496078229894084, + "velocityX": -2.116334799220068, + "velocityY": 0.2701587151815607, + "timestamp": 0.42208743412140665 + }, + { + "x": 8.40747247720944, + "y": 1.2621673609702526, + "heading": 2.1777614957327995, + "angularVelocity": 2.6342426892019786, + "velocityX": -2.4205696960337497, + "velocityY": 0.4933976293279214, + "timestamp": 0.4748483633865825 + }, + { + "x": 8.261948828020005, + "y": 1.2964340038650055, + "heading": 2.270600320754398, + "angularVelocity": 1.7596131515233795, + "velocityX": -2.758170699724305, + "velocityY": 0.6494700410322398, + "timestamp": 0.5276092926517583 + }, + { + "x": 8.09782277606961, + "y": 1.3349380681436944, + "heading": 2.3167167289348747, + "angularVelocity": 0.8740636077256377, + "velocityX": -3.1107498339443698, + "velocityY": 0.7297836640664107, + "timestamp": 0.5803702219169341 + }, + { + "x": 7.914829584256359, + "y": 1.3739987658766613, + "heading": 2.3167202209384494, + "angularVelocity": 0.00006618540695237648, + "velocityX": -3.4683466413855033, + "velocityY": 0.7403337711632821, + "timestamp": 0.6331311511821099 + }, + { + "x": 7.731792737090714, + "y": 1.412856687245149, + "heading": 2.3167225237878513, + "angularVelocity": 0.00004364686964296262, + "velocityX": -3.4691740595716793, + "velocityY": 0.7364904657609108, + "timestamp": 0.6858920804472858 + }, + { + "x": 7.548755888844399, + "y": 1.451714603522748, + "heading": 2.3167248266383997, + "angularVelocity": 0.00004364689137005159, + "velocityX": -3.469174080054087, + "velocityY": 0.7364903692711618, + "timestamp": 0.7386530097124616 + }, + { + "x": 7.365719040598746, + "y": 1.4905725198022486, + "heading": 2.316727129490484, + "angularVelocity": 0.00004364692048352743, + "velocityX": -3.469174080041539, + "velocityY": 0.7364903693072045, + "timestamp": 0.7914139389776375 + }, + { + "x": 7.182682192353782, + "y": 1.529430436083779, + "heading": 2.3167294323441046, + "angularVelocity": 0.00004364694959712505, + "velocityX": -3.4691740800284765, + "velocityY": 0.7364903693456704, + "timestamp": 0.8441748682428133 + }, + { + "x": 6.999645344109507, + "y": 1.5682883523673385, + "heading": 2.3167317351992613, + "angularVelocity": 0.00004364697871207036, + "velocityX": -3.469174080015414, + "velocityY": 0.7364903693841357, + "timestamp": 0.8969357975079891 + }, + { + "x": 6.816608495865921, + "y": 1.6071462686529279, + "heading": 2.316734038055954, + "angularVelocity": 0.00004364700782609749, + "velocityX": -3.4691740800023507, + "velocityY": 0.7364903694226017, + "timestamp": 0.949696726773165 + }, + { + "x": 6.633571647623024, + "y": 1.6460041849405465, + "heading": 2.316736340914183, + "angularVelocity": 0.00004364703694171292, + "velocityX": -3.469174079989288, + "velocityY": 0.7364903694610679, + "timestamp": 1.0024576560383407 + }, + { + "x": 6.450534799380817, + "y": 1.6848621012301948, + "heading": 2.316738643773948, + "angularVelocity": 0.000043647066056982235, + "velocityX": -3.469174079976225, + "velocityY": 0.736490369499534, + "timestamp": 1.0552185853035165 + }, + { + "x": 6.267497951139299, + "y": 1.7237200175218723, + "heading": 2.3167409466352487, + "angularVelocity": 0.00004364709517405661, + "velocityX": -3.4691740799631625, + "velocityY": 0.7364903695379998, + "timestamp": 1.1079795145686924 + }, + { + "x": 6.084461102898469, + "y": 1.7625779338155794, + "heading": 2.316743249498086, + "angularVelocity": 0.000043647124290962285, + "velocityX": -3.4691740799500987, + "velocityY": 0.7364903695764661, + "timestamp": 1.1607404438338682 + }, + { + "x": 5.90142425465833, + "y": 1.8014358501113161, + "heading": 2.31674555236246, + "angularVelocity": 0.000043647153406607097, + "velocityX": -3.469174079937036, + "velocityY": 0.7364903696149331, + "timestamp": 1.213501373099044 + }, + { + "x": 5.718387406418881, + "y": 1.8402937664090826, + "heading": 2.31674785522837, + "angularVelocity": 0.000043647182524781595, + "velocityX": -3.4691740799239703, + "velocityY": 0.7364903696534086, + "timestamp": 1.2662623023642199 + }, + { + "x": 5.535350558184445, + "y": 1.879151682729246, + "heading": 2.316750158095817, + "angularVelocity": 0.00004364721167212726, + "velocityX": -3.469174079828943, + "velocityY": 0.7364903700779036, + "timestamp": 1.3190232316293957 + }, + { + "x": 5.352313882264269, + "y": 1.9180104105293494, + "heading": 2.316752461060097, + "angularVelocity": 0.0000436490469716496, + "velocityX": -3.4691708138845505, + "velocityY": 0.7365057503972241, + "timestamp": 1.3717841608945716 + }, + { + "x": 5.174009799957275, + "y": 1.9726389646530151, + "heading": 2.3183527865287403, + "angularVelocity": 0.030331639168071065, + "velocityX": -3.3794719841047485, + "velocityY": 1.0353978765063663, + "timestamp": 1.4245450901597474 + }, + { + "x": 4.918462925408197, + "y": 2.1088992111590095, + "heading": 2.318352780187111, + "angularVelocity": -7.76596778188732e-8, + "velocityX": -3.1294303624461395, + "velocityY": 1.6686447578851502, + "timestamp": 1.506204320419573 + }, + { + "x": 4.6706807831887005, + "y": 2.2588160882042634, + "heading": 2.318352773860442, + "angularVelocity": -7.747647131185673e-8, + "velocityX": -3.03434334895268, + "velocityY": 1.8358840337858229, + "timestamp": 1.5878635506793985 + }, + { + "x": 4.42289869878705, + "y": 2.4087330608106337, + "heading": 2.318352767533773, + "angularVelocity": -7.747646978575146e-8, + "velocityX": -3.0343426409145584, + "velocityY": 1.8358852040285008, + "timestamp": 1.6695227809392241 + }, + { + "x": 4.17511661438578, + "y": 2.5586500334176288, + "heading": 2.318352761207104, + "angularVelocity": -7.747646959889163e-8, + "velocityX": -3.034342640909924, + "velocityY": 1.835885204036159, + "timestamp": 1.7511820111990497 + }, + { + "x": 3.9273345299845093, + "y": 2.7085670060246243, + "heading": 2.318352754880435, + "angularVelocity": -7.747646933531489e-8, + "velocityX": -3.0343426409099234, + "velocityY": 1.835885204036159, + "timestamp": 1.8328412414588753 + }, + { + "x": 3.679552445583239, + "y": 2.85848397863162, + "heading": 2.318352748553766, + "angularVelocity": -7.747646958961607e-8, + "velocityX": -3.034342640909924, + "velocityY": 1.8358852040361588, + "timestamp": 1.9145004717187009 + }, + { + "x": 3.431770361181968, + "y": 3.0084009512386154, + "heading": 2.318352742227097, + "angularVelocity": -7.747647015947937e-8, + "velocityX": -3.034342640909924, + "velocityY": 1.8358852040361597, + "timestamp": 1.9961597019785264 + }, + { + "x": 3.1839882767807, + "y": 3.158317923845615, + "heading": 2.318352735900428, + "angularVelocity": -7.747647018934798e-8, + "velocityX": -3.034342640909895, + "velocityY": 1.835885204036207, + "timestamp": 2.077818932238352 + }, + { + "x": 2.936206192742126, + "y": 3.308234897052054, + "heading": 2.3183527295737423, + "angularVelocity": -7.74766777065616e-8, + "velocityX": -3.0343426364683324, + "velocityY": 1.8358852113769544, + "timestamp": 2.1594781624981776 + }, + { + "x": 2.6985520828578236, + "y": 3.452139832457651, + "heading": 2.2861229229565243, + "angularVelocity": -0.39468663266440823, + "velocityX": -2.91031533273248, + "velocityY": 1.7622617179676583, + "timestamp": 2.241137392758003 + }, + { + "x": 2.5084457239359947, + "y": 3.567225663105015, + "heading": 2.2549856266803583, + "angularVelocity": -0.3813077367627909, + "velocityX": -2.328044953607123, + "velocityY": 1.4093425848024874, + "timestamp": 2.3227966230178287 + }, + { + "x": 2.3658771899178763, + "y": 3.653524329460914, + "heading": 2.2299036649035693, + "angularVelocity": -0.3071540314179127, + "velocityX": -1.7458961291269777, + "velocityY": 1.0568145950104009, + "timestamp": 2.4044558532776543 + }, + { + "x": 2.270836799349362, + "y": 3.7110500271012747, + "heading": 2.2124904722719037, + "angularVelocity": -0.213242184334339, + "velocityX": -1.1638658638602495, + "velocityY": 0.7044604444264757, + "timestamp": 2.48611508353748 + }, + { + "x": 2.223318576812744, + "y": 3.7398104667663574, "heading": 2.203545115911647, - "angularVelocity": 0.07710192360033451, - "velocityX": -0.5106513626857504, - "velocityY": 0.42648366385108344, - "timestamp": 2.344891157506228 + "angularVelocity": -0.10954495078871034, + "velocityX": -0.5819087736367691, + "velocityY": 0.35220071966845595, + "timestamp": 2.5677743137973055 }, { - "x": 2.640187978744507, - "y": 3.4790618419647217, + "x": 2.223318576812744, + "y": 3.7398104667663574, "heading": 2.203545115911647, - "angularVelocity": -2.5642698021713453e-24, - "velocityX": -3.130979253679926e-22, - "velocityY": -1.883332935459204e-22, - "timestamp": 2.4247258512013046 + "angularVelocity": 4.719095338364679e-26, + "velocityX": 4.055221098284943e-24, + "velocityY": 5.8881582869771646e-24, + "timestamp": 2.649433544057131 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Close 2 Note 6.1.traj b/src/main/deploy/choreo/Close 2 Note 6.1.traj new file mode 100644 index 0000000..6fb3f5f --- /dev/null +++ b/src/main/deploy/choreo/Close 2 Note 6.1.traj @@ -0,0 +1,401 @@ +{ + "samples": [ + { + "x": 2.7096962928771973, + "y": 3.496621608734131, + "heading": 2.3370885589294867, + "angularVelocity": -3.26643471533185e-27, + "velocityX": -8.603201911023632e-26, + "velocityY": 2.4805779170240995e-26, + "timestamp": 0 + }, + { + "x": 2.7146740314887152, + "y": 3.4937673012218937, + "heading": 2.2533967162557134, + "angularVelocity": -1.7826089205421576, + "velocityX": 0.10602420701389548, + "velocityY": -0.06079581797616154, + "timestamp": 0.04694907655253943 + }, + { + "x": 2.724452218978781, + "y": 3.488820892027521, + "heading": 2.0851306096050566, + "angularVelocity": -3.5840131266981397, + "velocityX": 0.20827220060704882, + "velocityY": -0.10535690065889125, + "timestamp": 0.09389815310507886 + }, + { + "x": 2.7406560932871677, + "y": 3.4832680280351447, + "heading": 1.8355772431800976, + "angularVelocity": -5.315405216664708, + "velocityX": 0.3451372316184537, + "velocityY": -0.11827418982697585, + "timestamp": 0.14084722965761828 + }, + { + "x": 2.767585684695567, + "y": 3.475579797799871, + "heading": 1.520977813967397, + "angularVelocity": -6.700865114154927, + "velocityX": 0.5735915035147274, + "velocityY": -0.1637567935265024, + "timestamp": 0.18779630621015772 + }, + { + "x": 2.810498710473928, + "y": 3.4589447346262188, + "heading": 1.2009849249020366, + "angularVelocity": -6.815744047857154, + "velocityX": 0.9140334364263394, + "velocityY": -0.3543214136498727, + "timestamp": 0.23474538276269716 + }, + { + "x": 2.8647081763617166, + "y": 3.432538014521098, + "heading": 0.9243397881091598, + "angularVelocity": -5.892451079059896, + "velocityX": 1.1546439220614773, + "velocityY": -0.5624545154912587, + "timestamp": 0.28169445931523657 + }, + { + "x": 2.932084724896856, + "y": 3.4027137974797674, + "heading": 0.6918755798834426, + "angularVelocity": -4.951411727248202, + "velocityX": 1.4350984829220317, + "velocityY": -0.6352460842963507, + "timestamp": 0.328643535867776 + }, + { + "x": 3.0147211413516173, + "y": 3.376471633610061, + "heading": 0.49655237322236306, + "angularVelocity": -4.1603205217998, + "velocityX": 1.7601286867119736, + "velocityY": -0.5589495214104997, + "timestamp": 0.37559261242031544 + }, + { + "x": 3.1133145878182975, + "y": 3.353150170071545, + "heading": 0.33467121013791984, + "angularVelocity": -3.448015913652453, + "velocityX": 2.100008215419251, + "velocityY": -0.49673955807026315, + "timestamp": 0.4225416889728549 + }, + { + "x": 3.2280064438607416, + "y": 3.3301514191120454, + "heading": 0.2070951624637037, + "angularVelocity": -2.7173281572737493, + "velocityX": 2.442899082670891, + "velocityY": -0.4898658855145381, + "timestamp": 0.4694907655253943 + }, + { + "x": 3.358041797859201, + "y": 3.3040376340853572, + "heading": 0.11640881453382437, + "angularVelocity": -1.9315895985386364, + "velocityX": 2.7697105789277994, + "velocityY": -0.5562150939745335, + "timestamp": 0.5164398420779337 + }, + { + "x": 3.502044068421766, + "y": 3.272023000424053, + "heading": 0.0642726455079616, + "angularVelocity": -1.1104833759087593, + "velocityX": 3.067201341040561, + "velocityY": -0.6819012430516593, + "timestamp": 0.5633889186304731 + }, + { + "x": 3.658545816104221, + "y": 3.2320987294497567, + "heading": 0.05052094344595785, + "angularVelocity": -0.2929067634933054, + "velocityX": 3.333436122163514, + "velocityY": -0.8503739350361508, + "timestamp": 0.6103379951830126 + }, + { + "x": 3.818497445143101, + "y": 3.1858458228841244, + "heading": 0.05051984068744808, + "angularVelocity": -0.000023488396167744368, + "velocityX": 3.406917468544511, + "velocityY": -0.9851718066035188, + "timestamp": 0.657287071735552 + }, + { + "x": 3.978449011059529, + "y": 3.139592697974801, + "heading": 0.05051873797026558, + "angularVelocity": -0.000023487515910291847, + "velocityX": 3.406916124056895, + "velocityY": -0.9851764572528472, + "timestamp": 0.7042361482880914 + }, + { + "x": 4.138400576973649, + "y": 3.0933395730582816, + "heading": 0.05051763525189827, + "angularVelocity": -0.000023487541146302523, + "velocityX": 3.4069161240077346, + "velocityY": -0.9851764574061228, + "timestamp": 0.7511852248406309 + }, + { + "x": 4.298352142887712, + "y": 3.047086448142349, + "heading": 0.05051653253234576, + "angularVelocity": -0.000023487566390649612, + "velocityX": 3.406916124006511, + "velocityY": -0.9851764573936154, + "timestamp": 0.7981343013931703 + }, + { + "x": 4.458303708801718, + "y": 3.0008333232270044, + "heading": 0.05051542981160801, + "angularVelocity": -0.000023487591635953872, + "velocityX": 3.4069161240052885, + "velocityY": -0.9851764573811018, + "timestamp": 0.8450833779457098 + }, + { + "x": 4.618255274715666, + "y": 2.9545801983122475, + "heading": 0.050514327089685056, + "angularVelocity": -0.000023487616880439936, + "velocityX": 3.4069161240040726, + "velocityY": -0.9851764573685653, + "timestamp": 0.8920324544982492 + }, + { + "x": 4.778206840638278, + "y": 2.908327073428245, + "heading": 0.050513224366575286, + "angularVelocity": -0.000023487642159216305, + "velocityX": 3.4069161241886405, + "velocityY": -0.985176456713516, + "timestamp": 0.9389815310507886 + }, + { + "x": 4.938158651224666, + "y": 2.8620747947569334, + "heading": 0.050512121554286644, + "angularVelocity": -0.000023489541639992042, + "velocityX": 3.4069213354471506, + "velocityY": -0.9851584326595224, + "timestamp": 0.9859306076033281 + }, + { + "x": 5.097496996112395, + "y": 2.828606048631961, + "heading": 0.040035508366877214, + "angularVelocity": -0.22314843989924574, + "velocityX": 3.393854716384416, + "velocityY": -0.7128733637075573, + "timestamp": 1.0328796841558674 + }, + { + "x": 5.255731410284399, + "y": 2.813146210355405, + "heading": 0.016867711475476607, + "angularVelocity": -0.4934665086644284, + "velocityX": 3.3703413526127197, + "velocityY": -0.3292895070951061, + "timestamp": 1.0798287607084067 + }, + { + "x": 5.4171977043151855, + "y": 2.8156931400299072, + "heading": -4.112071684642348e-22, + "angularVelocity": -0.359276746510667, + "velocityX": 3.4391793382792955, + "velocityY": 0.05424877040238669, + "timestamp": 1.126777837260946 + }, + { + "x": 5.601031770417355, + "y": 2.8403910645192147, + "heading": -1.6475843781367288e-8, + "angularVelocity": -3.15019851866737e-7, + "velocityX": 3.5149265215212115, + "velocityY": 0.4722268927335189, + "timestamp": 1.1790788071323186 + }, + { + "x": 5.7804448855599615, + "y": 2.887464289884782, + "heading": -1.4325422036977554e-8, + "angularVelocity": 4.1116288085647864e-8, + "velocityX": 3.430397477978901, + "velocityY": 0.9000449796884818, + "timestamp": 1.2313797770036912 + }, + { + "x": 5.952724706601957, + "y": 2.956200146604807, + "heading": -1.2270848156308066e-8, + "angularVelocity": 3.928366693242696e-8, + "velocityX": 3.2940081506269547, + "velocityY": 1.3142367510405981, + "timestamp": 1.2836807468750637 + }, + { + "x": 6.122871731907656, + "y": 3.0300577430139204, + "heading": -1.0225706785848107e-8, + "angularVelocity": 3.910331635321734e-8, + "velocityX": 3.2532288736547965, + "velocityY": 1.4121649481980245, + "timestamp": 1.3359817167464363 + }, + { + "x": 6.293018697456439, + "y": 3.103915477085925, + "heading": -8.180565446039309e-9, + "angularVelocity": 3.910331576716392e-8, + "velocityX": 3.253227731096333, + "velocityY": 1.4121675803268703, + "timestamp": 1.3882826866178088 + }, + { + "x": 6.463165663003702, + "y": 3.177773211161434, + "heading": -6.13542410268984e-9, + "angularVelocity": 3.9103315834861925e-8, + "velocityX": 3.2532277310672533, + "velocityY": 1.4121675803938631, + "timestamp": 1.4405836564891814 + }, + { + "x": 6.633312628550963, + "y": 3.251630945236942, + "heading": -4.090282764746313e-9, + "angularVelocity": 3.910331573149977e-8, + "velocityX": 3.253227731067253, + "velocityY": 1.4121675803938638, + "timestamp": 1.492884626360554 + }, + { + "x": 6.803459594098908, + "y": 3.3254886793108787, + "heading": -2.045141412996072e-9, + "angularVelocity": 3.910331599548557e-8, + "velocityX": 3.253227731080299, + "velocityY": 1.4121675803638085, + "timestamp": 1.5451855962319265 + }, + { + "x": 6.973606586456299, + "y": 3.399346351623535, + "heading": 4.112281238380375e-22, + "angularVelocity": 3.910331716652011e-8, + "velocityX": 3.2532282436797044, + "velocityY": 1.4121663994816895, + "timestamp": 1.597486566103299 + }, + { + "x": 7.242468138651456, + "y": 3.505475940728197, + "heading": 2.585437014143955e-17, + "angularVelocity": 3.17215609296563e-16, + "velocityX": 3.2988016026124845, + "velocityY": 1.3021588835020073, + "timestamp": 1.6789893615373233 + }, + { + "x": 7.511329714849845, + "y": 3.6116054690246653, + "heading": 3.276598422574725e-17, + "angularVelocity": 8.480217208701784e-17, + "velocityX": 3.298801897120578, + "velocityY": 1.3021581374147986, + "timestamp": 1.7604921569713476 + }, + { + "x": 7.780191291048392, + "y": 3.7177349973207363, + "heading": 3.5721707298172814e-17, + "angularVelocity": 3.6265296869230005e-17, + "velocityX": 3.2988018971225013, + "velocityY": 1.3021581374099265, + "timestamp": 1.841994952405372 + }, + { + "x": 8.049052867246928, + "y": 3.8238645256168042, + "heading": 2.8432311760247846e-17, + "angularVelocity": -8.94373683629846e-17, + "velocityX": 3.2988018971223876, + "velocityY": 1.3021581374098816, + "timestamp": 1.9234977478393964 + }, + { + "x": 8.306648971596287, + "y": 3.9255471588589534, + "heading": 1.1537732349557526e-17, + "angularVelocity": -2.0728834294140422e-16, + "velocityX": 3.1605799896505293, + "velocityY": 1.247596879354403, + "timestamp": 2.005000543273421 + }, + { + "x": 8.512725872589964, + "y": 4.006893272366167, + "heading": 4.56264194487887e-18, + "angularVelocity": -8.55809959333942e-17, + "velocityX": 2.52846420661111, + "velocityY": 0.9980775883087605, + "timestamp": 2.086503338707445 + }, + { + "x": 8.667283554110604, + "y": 4.067902859776331, + "heading": 3.1467275701042136e-19, + "angularVelocity": -5.212053359848234e-17, + "velocityX": 1.8963482258194635, + "velocityY": 0.7485582192030588, + "timestamp": 2.1680061341414696 + }, + { + "x": 8.770322010785756, + "y": 4.108575918968745, + "heading": -1.2062044578319972e-18, + "angularVelocity": -1.8660430070701055e-17, + "velocityX": 1.264232179110475, + "velocityY": 0.4990388240773634, + "timestamp": 2.249508929575494 + }, + { + "x": 8.8218412399292, + "y": 4.128912448883057, + "heading": 1.614787833327321e-28, + "angularVelocity": 1.4799547078713896e-17, + "velocityX": 0.6321160994428312, + "velocityY": 0.24951941594167767, + "timestamp": 2.3310117250095184 + }, + { + "x": 8.8218412399292, + "y": 4.128912448883057, + "heading": 7.916184096548804e-29, + "angularVelocity": -3.8711572835683144e-29, + "velocityX": -5.492029296440765e-27, + "velocityY": 6.5647493687471005e-28, + "timestamp": 2.4125145204435428 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Close 2 Note 6.traj b/src/main/deploy/choreo/Close 2 Note 6.traj new file mode 100644 index 0000000..6fb3f5f --- /dev/null +++ b/src/main/deploy/choreo/Close 2 Note 6.traj @@ -0,0 +1,401 @@ +{ + "samples": [ + { + "x": 2.7096962928771973, + "y": 3.496621608734131, + "heading": 2.3370885589294867, + "angularVelocity": -3.26643471533185e-27, + "velocityX": -8.603201911023632e-26, + "velocityY": 2.4805779170240995e-26, + "timestamp": 0 + }, + { + "x": 2.7146740314887152, + "y": 3.4937673012218937, + "heading": 2.2533967162557134, + "angularVelocity": -1.7826089205421576, + "velocityX": 0.10602420701389548, + "velocityY": -0.06079581797616154, + "timestamp": 0.04694907655253943 + }, + { + "x": 2.724452218978781, + "y": 3.488820892027521, + "heading": 2.0851306096050566, + "angularVelocity": -3.5840131266981397, + "velocityX": 0.20827220060704882, + "velocityY": -0.10535690065889125, + "timestamp": 0.09389815310507886 + }, + { + "x": 2.7406560932871677, + "y": 3.4832680280351447, + "heading": 1.8355772431800976, + "angularVelocity": -5.315405216664708, + "velocityX": 0.3451372316184537, + "velocityY": -0.11827418982697585, + "timestamp": 0.14084722965761828 + }, + { + "x": 2.767585684695567, + "y": 3.475579797799871, + "heading": 1.520977813967397, + "angularVelocity": -6.700865114154927, + "velocityX": 0.5735915035147274, + "velocityY": -0.1637567935265024, + "timestamp": 0.18779630621015772 + }, + { + "x": 2.810498710473928, + "y": 3.4589447346262188, + "heading": 1.2009849249020366, + "angularVelocity": -6.815744047857154, + "velocityX": 0.9140334364263394, + "velocityY": -0.3543214136498727, + "timestamp": 0.23474538276269716 + }, + { + "x": 2.8647081763617166, + "y": 3.432538014521098, + "heading": 0.9243397881091598, + "angularVelocity": -5.892451079059896, + "velocityX": 1.1546439220614773, + "velocityY": -0.5624545154912587, + "timestamp": 0.28169445931523657 + }, + { + "x": 2.932084724896856, + "y": 3.4027137974797674, + "heading": 0.6918755798834426, + "angularVelocity": -4.951411727248202, + "velocityX": 1.4350984829220317, + "velocityY": -0.6352460842963507, + "timestamp": 0.328643535867776 + }, + { + "x": 3.0147211413516173, + "y": 3.376471633610061, + "heading": 0.49655237322236306, + "angularVelocity": -4.1603205217998, + "velocityX": 1.7601286867119736, + "velocityY": -0.5589495214104997, + "timestamp": 0.37559261242031544 + }, + { + "x": 3.1133145878182975, + "y": 3.353150170071545, + "heading": 0.33467121013791984, + "angularVelocity": -3.448015913652453, + "velocityX": 2.100008215419251, + "velocityY": -0.49673955807026315, + "timestamp": 0.4225416889728549 + }, + { + "x": 3.2280064438607416, + "y": 3.3301514191120454, + "heading": 0.2070951624637037, + "angularVelocity": -2.7173281572737493, + "velocityX": 2.442899082670891, + "velocityY": -0.4898658855145381, + "timestamp": 0.4694907655253943 + }, + { + "x": 3.358041797859201, + "y": 3.3040376340853572, + "heading": 0.11640881453382437, + "angularVelocity": -1.9315895985386364, + "velocityX": 2.7697105789277994, + "velocityY": -0.5562150939745335, + "timestamp": 0.5164398420779337 + }, + { + "x": 3.502044068421766, + "y": 3.272023000424053, + "heading": 0.0642726455079616, + "angularVelocity": -1.1104833759087593, + "velocityX": 3.067201341040561, + "velocityY": -0.6819012430516593, + "timestamp": 0.5633889186304731 + }, + { + "x": 3.658545816104221, + "y": 3.2320987294497567, + "heading": 0.05052094344595785, + "angularVelocity": -0.2929067634933054, + "velocityX": 3.333436122163514, + "velocityY": -0.8503739350361508, + "timestamp": 0.6103379951830126 + }, + { + "x": 3.818497445143101, + "y": 3.1858458228841244, + "heading": 0.05051984068744808, + "angularVelocity": -0.000023488396167744368, + "velocityX": 3.406917468544511, + "velocityY": -0.9851718066035188, + "timestamp": 0.657287071735552 + }, + { + "x": 3.978449011059529, + "y": 3.139592697974801, + "heading": 0.05051873797026558, + "angularVelocity": -0.000023487515910291847, + "velocityX": 3.406916124056895, + "velocityY": -0.9851764572528472, + "timestamp": 0.7042361482880914 + }, + { + "x": 4.138400576973649, + "y": 3.0933395730582816, + "heading": 0.05051763525189827, + "angularVelocity": -0.000023487541146302523, + "velocityX": 3.4069161240077346, + "velocityY": -0.9851764574061228, + "timestamp": 0.7511852248406309 + }, + { + "x": 4.298352142887712, + "y": 3.047086448142349, + "heading": 0.05051653253234576, + "angularVelocity": -0.000023487566390649612, + "velocityX": 3.406916124006511, + "velocityY": -0.9851764573936154, + "timestamp": 0.7981343013931703 + }, + { + "x": 4.458303708801718, + "y": 3.0008333232270044, + "heading": 0.05051542981160801, + "angularVelocity": -0.000023487591635953872, + "velocityX": 3.4069161240052885, + "velocityY": -0.9851764573811018, + "timestamp": 0.8450833779457098 + }, + { + "x": 4.618255274715666, + "y": 2.9545801983122475, + "heading": 0.050514327089685056, + "angularVelocity": -0.000023487616880439936, + "velocityX": 3.4069161240040726, + "velocityY": -0.9851764573685653, + "timestamp": 0.8920324544982492 + }, + { + "x": 4.778206840638278, + "y": 2.908327073428245, + "heading": 0.050513224366575286, + "angularVelocity": -0.000023487642159216305, + "velocityX": 3.4069161241886405, + "velocityY": -0.985176456713516, + "timestamp": 0.9389815310507886 + }, + { + "x": 4.938158651224666, + "y": 2.8620747947569334, + "heading": 0.050512121554286644, + "angularVelocity": -0.000023489541639992042, + "velocityX": 3.4069213354471506, + "velocityY": -0.9851584326595224, + "timestamp": 0.9859306076033281 + }, + { + "x": 5.097496996112395, + "y": 2.828606048631961, + "heading": 0.040035508366877214, + "angularVelocity": -0.22314843989924574, + "velocityX": 3.393854716384416, + "velocityY": -0.7128733637075573, + "timestamp": 1.0328796841558674 + }, + { + "x": 5.255731410284399, + "y": 2.813146210355405, + "heading": 0.016867711475476607, + "angularVelocity": -0.4934665086644284, + "velocityX": 3.3703413526127197, + "velocityY": -0.3292895070951061, + "timestamp": 1.0798287607084067 + }, + { + "x": 5.4171977043151855, + "y": 2.8156931400299072, + "heading": -4.112071684642348e-22, + "angularVelocity": -0.359276746510667, + "velocityX": 3.4391793382792955, + "velocityY": 0.05424877040238669, + "timestamp": 1.126777837260946 + }, + { + "x": 5.601031770417355, + "y": 2.8403910645192147, + "heading": -1.6475843781367288e-8, + "angularVelocity": -3.15019851866737e-7, + "velocityX": 3.5149265215212115, + "velocityY": 0.4722268927335189, + "timestamp": 1.1790788071323186 + }, + { + "x": 5.7804448855599615, + "y": 2.887464289884782, + "heading": -1.4325422036977554e-8, + "angularVelocity": 4.1116288085647864e-8, + "velocityX": 3.430397477978901, + "velocityY": 0.9000449796884818, + "timestamp": 1.2313797770036912 + }, + { + "x": 5.952724706601957, + "y": 2.956200146604807, + "heading": -1.2270848156308066e-8, + "angularVelocity": 3.928366693242696e-8, + "velocityX": 3.2940081506269547, + "velocityY": 1.3142367510405981, + "timestamp": 1.2836807468750637 + }, + { + "x": 6.122871731907656, + "y": 3.0300577430139204, + "heading": -1.0225706785848107e-8, + "angularVelocity": 3.910331635321734e-8, + "velocityX": 3.2532288736547965, + "velocityY": 1.4121649481980245, + "timestamp": 1.3359817167464363 + }, + { + "x": 6.293018697456439, + "y": 3.103915477085925, + "heading": -8.180565446039309e-9, + "angularVelocity": 3.910331576716392e-8, + "velocityX": 3.253227731096333, + "velocityY": 1.4121675803268703, + "timestamp": 1.3882826866178088 + }, + { + "x": 6.463165663003702, + "y": 3.177773211161434, + "heading": -6.13542410268984e-9, + "angularVelocity": 3.9103315834861925e-8, + "velocityX": 3.2532277310672533, + "velocityY": 1.4121675803938631, + "timestamp": 1.4405836564891814 + }, + { + "x": 6.633312628550963, + "y": 3.251630945236942, + "heading": -4.090282764746313e-9, + "angularVelocity": 3.910331573149977e-8, + "velocityX": 3.253227731067253, + "velocityY": 1.4121675803938638, + "timestamp": 1.492884626360554 + }, + { + "x": 6.803459594098908, + "y": 3.3254886793108787, + "heading": -2.045141412996072e-9, + "angularVelocity": 3.910331599548557e-8, + "velocityX": 3.253227731080299, + "velocityY": 1.4121675803638085, + "timestamp": 1.5451855962319265 + }, + { + "x": 6.973606586456299, + "y": 3.399346351623535, + "heading": 4.112281238380375e-22, + "angularVelocity": 3.910331716652011e-8, + "velocityX": 3.2532282436797044, + "velocityY": 1.4121663994816895, + "timestamp": 1.597486566103299 + }, + { + "x": 7.242468138651456, + "y": 3.505475940728197, + "heading": 2.585437014143955e-17, + "angularVelocity": 3.17215609296563e-16, + "velocityX": 3.2988016026124845, + "velocityY": 1.3021588835020073, + "timestamp": 1.6789893615373233 + }, + { + "x": 7.511329714849845, + "y": 3.6116054690246653, + "heading": 3.276598422574725e-17, + "angularVelocity": 8.480217208701784e-17, + "velocityX": 3.298801897120578, + "velocityY": 1.3021581374147986, + "timestamp": 1.7604921569713476 + }, + { + "x": 7.780191291048392, + "y": 3.7177349973207363, + "heading": 3.5721707298172814e-17, + "angularVelocity": 3.6265296869230005e-17, + "velocityX": 3.2988018971225013, + "velocityY": 1.3021581374099265, + "timestamp": 1.841994952405372 + }, + { + "x": 8.049052867246928, + "y": 3.8238645256168042, + "heading": 2.8432311760247846e-17, + "angularVelocity": -8.94373683629846e-17, + "velocityX": 3.2988018971223876, + "velocityY": 1.3021581374098816, + "timestamp": 1.9234977478393964 + }, + { + "x": 8.306648971596287, + "y": 3.9255471588589534, + "heading": 1.1537732349557526e-17, + "angularVelocity": -2.0728834294140422e-16, + "velocityX": 3.1605799896505293, + "velocityY": 1.247596879354403, + "timestamp": 2.005000543273421 + }, + { + "x": 8.512725872589964, + "y": 4.006893272366167, + "heading": 4.56264194487887e-18, + "angularVelocity": -8.55809959333942e-17, + "velocityX": 2.52846420661111, + "velocityY": 0.9980775883087605, + "timestamp": 2.086503338707445 + }, + { + "x": 8.667283554110604, + "y": 4.067902859776331, + "heading": 3.1467275701042136e-19, + "angularVelocity": -5.212053359848234e-17, + "velocityX": 1.8963482258194635, + "velocityY": 0.7485582192030588, + "timestamp": 2.1680061341414696 + }, + { + "x": 8.770322010785756, + "y": 4.108575918968745, + "heading": -1.2062044578319972e-18, + "angularVelocity": -1.8660430070701055e-17, + "velocityX": 1.264232179110475, + "velocityY": 0.4990388240773634, + "timestamp": 2.249508929575494 + }, + { + "x": 8.8218412399292, + "y": 4.128912448883057, + "heading": 1.614787833327321e-28, + "angularVelocity": 1.4799547078713896e-17, + "velocityX": 0.6321160994428312, + "velocityY": 0.24951941594167767, + "timestamp": 2.3310117250095184 + }, + { + "x": 8.8218412399292, + "y": 4.128912448883057, + "heading": 7.916184096548804e-29, + "angularVelocity": -3.8711572835683144e-29, + "velocityX": -5.492029296440765e-27, + "velocityY": 6.5647493687471005e-28, + "timestamp": 2.4125145204435428 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Close 2 Note 7.1.traj b/src/main/deploy/choreo/Close 2 Note 7.1.traj new file mode 100644 index 0000000..f72e87b --- /dev/null +++ b/src/main/deploy/choreo/Close 2 Note 7.1.traj @@ -0,0 +1,401 @@ +{ + "samples": [ + { + "x": 8.465164184570312, + "y": 4.534226894378662, + "heading": -2.8449855627415714e-26, + "angularVelocity": -6.1230157259157575e-27, + "velocityX": 2.3963430003994368e-26, + "velocityY": -3.084479188733402e-26, + "timestamp": 0 + }, + { + "x": 8.460367187528206, + "y": 4.531767118727852, + "heading": 0.08450384633322554, + "angularVelocity": 1.8016904366905349, + "velocityX": -0.1022758616396483, + "velocityY": -0.05244440884966881, + "timestamp": 0.0469025336497028 + }, + { + "x": 8.449742946270945, + "y": 4.5264220063428615, + "heading": 0.2507113382987055, + "angularVelocity": 3.5436783267791148, + "velocityX": -0.22651742732301222, + "velocityY": -0.11396212462446169, + "timestamp": 0.0938050672994056 + }, + { + "x": 8.432270313112136, + "y": 4.516718694694835, + "heading": 0.49372727846173775, + "angularVelocity": 5.181296643333298, + "velocityX": -0.37253068862558697, + "velocityY": -0.20688246226733464, + "timestamp": 0.1407076009491084 + }, + { + "x": 8.406880343130178, + "y": 4.499531714826417, + "heading": 0.8023679750542415, + "angularVelocity": 6.580469594619851, + "velocityX": -0.5413347213092073, + "velocityY": -0.3664403291468338, + "timestamp": 0.1876101345988112 + }, + { + "x": 8.369319015581828, + "y": 4.46918191846546, + "heading": 1.0969671011562374, + "angularVelocity": 6.281091940624037, + "velocityX": -0.8008379212278784, + "velocityY": -0.6470822362738113, + "timestamp": 0.23451266824851402 + }, + { + "x": 8.31733458470586, + "y": 4.435074642393435, + "heading": 1.3519084583358731, + "angularVelocity": 5.435556191563035, + "velocityX": -1.1083501642836833, + "velocityY": -0.7271947465942838, + "timestamp": 0.28141520189821684 + }, + { + "x": 8.249546558176377, + "y": 4.397567790061388, + "heading": 1.5734769473555086, + "angularVelocity": 4.7240196163910095, + "velocityX": -1.4452956216772148, + "velocityY": -0.7996764655012261, + "timestamp": 0.32831773554791965 + }, + { + "x": 8.16571602730043, + "y": 4.355751955292342, + "heading": 1.7651793028229739, + "angularVelocity": 4.087249462880137, + "velocityX": -1.7873348058772967, + "velocityY": -0.8915474605562327, + "timestamp": 0.37522026919762247 + }, + { + "x": 8.066870437319741, + "y": 4.307122077538631, + "heading": 1.9259060954825424, + "angularVelocity": 3.42682537919968, + "velocityX": -2.107468025478739, + "velocityY": -1.0368283751344498, + "timestamp": 0.4221228028473253 + }, + { + "x": 7.955957471746159, + "y": 4.248022091743418, + "heading": 2.0518724582720194, + "angularVelocity": 2.6857048646939137, + "velocityX": -2.364754245515796, + "velocityY": -1.2600595574774145, + "timestamp": 0.4690253364970281 + }, + { + "x": 7.841127991815943, + "y": 4.1735531848919445, + "heading": 2.1461285868842412, + "angularVelocity": 2.00961699246751, + "velocityX": -2.448257503269065, + "velocityY": -1.5877374004494726, + "timestamp": 0.5159278701467309 + }, + { + "x": 7.720416086674602, + "y": 4.0853653777627885, + "heading": 2.2017425392035563, + "angularVelocity": 1.1857345007132167, + "velocityX": -2.5736755724732037, + "velocityY": -1.880235464203213, + "timestamp": 0.5628304037964337 + }, + { + "x": 7.594525449972563, + "y": 3.988428321875803, + "heading": 2.226983293645896, + "angularVelocity": 0.5381533251668943, + "velocityX": -2.684090323185251, + "velocityY": -2.066776533032764, + "timestamp": 0.6097329374461364 + }, + { + "x": 7.473855661362909, + "y": 3.8889244490440706, + "heading": 2.2604012205898147, + "angularVelocity": 0.7124972649346369, + "velocityX": -2.572777613910837, + "velocityY": -2.1215031489533054, + "timestamp": 0.6566354710958392 + }, + { + "x": 7.353858527255753, + "y": 3.789691370515812, + "heading": 2.2950724550213937, + "angularVelocity": 0.7392187955244643, + "velocityX": -2.5584360751888, + "velocityY": -2.1157295951087174, + "timestamp": 0.703538004745542 + }, + { + "x": 7.234194488365969, + "y": 3.690951634795317, + "heading": 2.330505605791114, + "angularVelocity": 0.7554634688683831, + "velocityX": -2.5513342154074086, + "velocityY": -2.105211126928559, + "timestamp": 0.7504405383952447 + }, + { + "x": 7.114885343623766, + "y": 3.5927409732758844, + "heading": 2.3667975931007965, + "angularVelocity": 0.7737745594029797, + "velocityX": -2.5437675847807513, + "velocityY": -2.0939308365072624, + "timestamp": 0.7973430720449475 + }, + { + "x": 6.995958632853738, + "y": 3.4951017727711147, + "heading": 2.404066159199295, + "angularVelocity": 0.7945960100331263, + "velocityX": -2.5356137827914957, + "velocityY": -2.081746824894364, + "timestamp": 0.8442456056946502 + }, + { + "x": 6.877449452771626, + "y": 3.3980853132150073, + "heading": 2.4424566069983173, + "angularVelocity": 0.8185154364100276, + "velocityX": -2.526711690400603, + "velocityY": -2.0684694835610897, + "timestamp": 0.891148139344353 + }, + { + "x": 6.759397930576002, + "y": 3.3017545649144626, + "heading": 2.482139466223676, + "angularVelocity": 0.8460706946395502, + "velocityX": -2.516954053640389, + "velocityY": -2.0538495642901125, + "timestamp": 0.9380506729940558 + }, + { + "x": 6.630091339801819, + "y": 3.2069465515769977, + "heading": 2.4987006025081633, + "angularVelocity": 0.35309683711706574, + "velocityX": -2.756921230309734, + "velocityY": -2.0213836217367276, + "timestamp": 0.9849532066437585 + }, + { + "x": 6.489136479391131, + "y": 3.1261931906999445, + "heading": 2.508890104109657, + "angularVelocity": 0.21724842580137385, + "velocityX": -3.0052717719564495, + "velocityY": -1.721727049548525, + "timestamp": 1.0318557402934614 + }, + { + "x": 6.3415058716452215, + "y": 3.0624912033101053, + "heading": 2.5228679049585003, + "angularVelocity": 0.2980180335936202, + "velocityX": -3.1476041112939677, + "velocityY": -1.3581779582656586, + "timestamp": 1.0787582739431643 + }, + { + "x": 6.188700179225271, + "y": 3.0163717053414074, + "heading": 2.5394254687500224, + "angularVelocity": 0.35302066867398807, + "velocityX": -3.257941107429268, + "velocityY": -0.9833050451633768, + "timestamp": 1.1256608075928671 + }, + { + "x": 6.031658683642738, + "y": 2.988099001678993, + "heading": 2.55600757529583, + "angularVelocity": 0.35354394007053697, + "velocityX": -3.3482518610916934, + "velocityY": -0.6027969378706126, + "timestamp": 1.17256334124257 + }, + { + "x": 5.871150970458984, + "y": 2.9778192043304443, + "heading": 2.569623693858617, + "angularVelocity": 0.29030667435753743, + "velocityX": -3.4221544273604505, + "velocityY": -0.21917360425184526, + "timestamp": 1.2194658748922729 + }, + { + "x": 5.576841136148987, + "y": 3.016980693416113, + "heading": 2.5696236883953287, + "angularVelocity": -6.525879318384462e-8, + "velocityX": -3.5155208959382085, + "velocityY": 0.4677826465415135, + "timestamp": 1.3031831512282035 + }, + { + "x": 5.2959027238957885, + "y": 3.113030268725703, + "heading": 2.5696236701163304, + "angularVelocity": -2.1834200821596536e-7, + "velocityX": -3.355799717203925, + "velocityY": 1.14730888907773, + "timestamp": 1.3869004275641341 + }, + { + "x": 5.039219321333584, + "y": 3.2622467062205915, + "heading": 2.569623654672457, + "angularVelocity": -1.8447654007901228e-7, + "velocityX": -3.0660744567491185, + "velocityY": 1.782385237858559, + "timestamp": 1.4706177039000647 + }, + { + "x": 4.799769406634373, + "y": 3.437790584146643, + "heading": 2.569623639730028, + "angularVelocity": -1.7848680691589113e-7, + "velocityX": -2.8602210341671395, + "velocityY": 2.0968656125606664, + "timestamp": 1.5543349802359954 + }, + { + "x": 4.560319652029815, + "y": 3.613334680448953, + "heading": 2.5696236247876003, + "angularVelocity": -1.7848678782979664e-7, + "velocityX": -2.860219121841975, + "velocityY": 2.096868221057592, + "timestamp": 1.638052256571926 + }, + { + "x": 4.320869897426237, + "y": 3.7888787767525987, + "heading": 2.5696236098451726, + "angularVelocity": -1.784867870189022e-7, + "velocityX": -2.860219121830273, + "velocityY": 2.0968682210735534, + "timestamp": 1.7217695329078566 + }, + { + "x": 4.081420142822659, + "y": 3.9644228730562445, + "heading": 2.569623594902745, + "angularVelocity": -1.784867875533888e-7, + "velocityX": -2.8602191218302733, + "velocityY": 2.096868221073554, + "timestamp": 1.8054868092437872 + }, + { + "x": 3.8419703882190817, + "y": 4.139966969359891, + "heading": 2.569623579960317, + "angularVelocity": -1.7848678683767884e-7, + "velocityX": -2.8602191218302733, + "velocityY": 2.0968682210735534, + "timestamp": 1.8892040855797179 + }, + { + "x": 3.6025206336155033, + "y": 4.315511065663538, + "heading": 2.5696235650178894, + "angularVelocity": -1.7848678777394938e-7, + "velocityX": -2.8602191218302737, + "velocityY": 2.096868221073554, + "timestamp": 1.9729213619156485 + }, + { + "x": 3.363070879011915, + "y": 4.491055161967169, + "heading": 2.5696235500754616, + "angularVelocity": -1.7848678692480314e-7, + "velocityX": -2.8602191218304047, + "velocityY": 2.0968682210733744, + "timestamp": 2.056638638251579 + }, + { + "x": 3.123621122611819, + "y": 4.666599255820278, + "heading": 2.5696235351330237, + "angularVelocity": -1.784869102061119e-7, + "velocityX": -2.860219143289622, + "velocityY": 2.096868191801975, + "timestamp": 2.1403559145875097 + }, + { + "x": 2.888392901577879, + "y": 4.838443251060152, + "heading": 2.5511544908862556, + "angularVelocity": -0.22061210129027164, + "velocityX": -2.809793047853641, + "velocityY": 2.0526706405296773, + "timestamp": 2.2240731909234404 + }, + { + "x": 2.7000575025773665, + "y": 4.976197095777676, + "heading": 2.5062705544048423, + "angularVelocity": -0.5361370847912993, + "velocityX": -2.2496598939122543, + "velocityY": 1.645464959523549, + "timestamp": 2.307790467259371 + }, + { + "x": 2.5589096957108692, + "y": 5.079485700111359, + "heading": 2.463997463224374, + "angularVelocity": -0.504950627046677, + "velocityX": -1.6860057212099981, + "velocityY": 1.233778842962105, + "timestamp": 2.3915077435953016 + }, + { + "x": 2.4648687186972724, + "y": 5.148321942855847, + "heading": 2.4324643694891153, + "angularVelocity": -0.37666172521818037, + "velocityX": -1.1233162511909789, + "velocityY": 0.822246563161818, + "timestamp": 2.4752250199312322 + }, + { + "x": 2.4178707599639893, + "y": 5.182730197906494, + "heading": 2.4155561764362563, + "angularVelocity": -0.20196778721051775, + "velocityX": -0.5613890082220943, + "velocityY": 0.41100542870718687, + "timestamp": 2.558942296267163 + }, + { + "x": 2.4178707599639893, + "y": 5.182730197906494, + "heading": 2.4155561764362563, + "angularVelocity": -2.09894229579262e-27, + "velocityX": 2.3772952918690395e-26, + "velocityY": 2.3276233341918227e-26, + "timestamp": 2.6426595726030935 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Close 2 Note 7.traj b/src/main/deploy/choreo/Close 2 Note 7.traj new file mode 100644 index 0000000..f72e87b --- /dev/null +++ b/src/main/deploy/choreo/Close 2 Note 7.traj @@ -0,0 +1,401 @@ +{ + "samples": [ + { + "x": 8.465164184570312, + "y": 4.534226894378662, + "heading": -2.8449855627415714e-26, + "angularVelocity": -6.1230157259157575e-27, + "velocityX": 2.3963430003994368e-26, + "velocityY": -3.084479188733402e-26, + "timestamp": 0 + }, + { + "x": 8.460367187528206, + "y": 4.531767118727852, + "heading": 0.08450384633322554, + "angularVelocity": 1.8016904366905349, + "velocityX": -0.1022758616396483, + "velocityY": -0.05244440884966881, + "timestamp": 0.0469025336497028 + }, + { + "x": 8.449742946270945, + "y": 4.5264220063428615, + "heading": 0.2507113382987055, + "angularVelocity": 3.5436783267791148, + "velocityX": -0.22651742732301222, + "velocityY": -0.11396212462446169, + "timestamp": 0.0938050672994056 + }, + { + "x": 8.432270313112136, + "y": 4.516718694694835, + "heading": 0.49372727846173775, + "angularVelocity": 5.181296643333298, + "velocityX": -0.37253068862558697, + "velocityY": -0.20688246226733464, + "timestamp": 0.1407076009491084 + }, + { + "x": 8.406880343130178, + "y": 4.499531714826417, + "heading": 0.8023679750542415, + "angularVelocity": 6.580469594619851, + "velocityX": -0.5413347213092073, + "velocityY": -0.3664403291468338, + "timestamp": 0.1876101345988112 + }, + { + "x": 8.369319015581828, + "y": 4.46918191846546, + "heading": 1.0969671011562374, + "angularVelocity": 6.281091940624037, + "velocityX": -0.8008379212278784, + "velocityY": -0.6470822362738113, + "timestamp": 0.23451266824851402 + }, + { + "x": 8.31733458470586, + "y": 4.435074642393435, + "heading": 1.3519084583358731, + "angularVelocity": 5.435556191563035, + "velocityX": -1.1083501642836833, + "velocityY": -0.7271947465942838, + "timestamp": 0.28141520189821684 + }, + { + "x": 8.249546558176377, + "y": 4.397567790061388, + "heading": 1.5734769473555086, + "angularVelocity": 4.7240196163910095, + "velocityX": -1.4452956216772148, + "velocityY": -0.7996764655012261, + "timestamp": 0.32831773554791965 + }, + { + "x": 8.16571602730043, + "y": 4.355751955292342, + "heading": 1.7651793028229739, + "angularVelocity": 4.087249462880137, + "velocityX": -1.7873348058772967, + "velocityY": -0.8915474605562327, + "timestamp": 0.37522026919762247 + }, + { + "x": 8.066870437319741, + "y": 4.307122077538631, + "heading": 1.9259060954825424, + "angularVelocity": 3.42682537919968, + "velocityX": -2.107468025478739, + "velocityY": -1.0368283751344498, + "timestamp": 0.4221228028473253 + }, + { + "x": 7.955957471746159, + "y": 4.248022091743418, + "heading": 2.0518724582720194, + "angularVelocity": 2.6857048646939137, + "velocityX": -2.364754245515796, + "velocityY": -1.2600595574774145, + "timestamp": 0.4690253364970281 + }, + { + "x": 7.841127991815943, + "y": 4.1735531848919445, + "heading": 2.1461285868842412, + "angularVelocity": 2.00961699246751, + "velocityX": -2.448257503269065, + "velocityY": -1.5877374004494726, + "timestamp": 0.5159278701467309 + }, + { + "x": 7.720416086674602, + "y": 4.0853653777627885, + "heading": 2.2017425392035563, + "angularVelocity": 1.1857345007132167, + "velocityX": -2.5736755724732037, + "velocityY": -1.880235464203213, + "timestamp": 0.5628304037964337 + }, + { + "x": 7.594525449972563, + "y": 3.988428321875803, + "heading": 2.226983293645896, + "angularVelocity": 0.5381533251668943, + "velocityX": -2.684090323185251, + "velocityY": -2.066776533032764, + "timestamp": 0.6097329374461364 + }, + { + "x": 7.473855661362909, + "y": 3.8889244490440706, + "heading": 2.2604012205898147, + "angularVelocity": 0.7124972649346369, + "velocityX": -2.572777613910837, + "velocityY": -2.1215031489533054, + "timestamp": 0.6566354710958392 + }, + { + "x": 7.353858527255753, + "y": 3.789691370515812, + "heading": 2.2950724550213937, + "angularVelocity": 0.7392187955244643, + "velocityX": -2.5584360751888, + "velocityY": -2.1157295951087174, + "timestamp": 0.703538004745542 + }, + { + "x": 7.234194488365969, + "y": 3.690951634795317, + "heading": 2.330505605791114, + "angularVelocity": 0.7554634688683831, + "velocityX": -2.5513342154074086, + "velocityY": -2.105211126928559, + "timestamp": 0.7504405383952447 + }, + { + "x": 7.114885343623766, + "y": 3.5927409732758844, + "heading": 2.3667975931007965, + "angularVelocity": 0.7737745594029797, + "velocityX": -2.5437675847807513, + "velocityY": -2.0939308365072624, + "timestamp": 0.7973430720449475 + }, + { + "x": 6.995958632853738, + "y": 3.4951017727711147, + "heading": 2.404066159199295, + "angularVelocity": 0.7945960100331263, + "velocityX": -2.5356137827914957, + "velocityY": -2.081746824894364, + "timestamp": 0.8442456056946502 + }, + { + "x": 6.877449452771626, + "y": 3.3980853132150073, + "heading": 2.4424566069983173, + "angularVelocity": 0.8185154364100276, + "velocityX": -2.526711690400603, + "velocityY": -2.0684694835610897, + "timestamp": 0.891148139344353 + }, + { + "x": 6.759397930576002, + "y": 3.3017545649144626, + "heading": 2.482139466223676, + "angularVelocity": 0.8460706946395502, + "velocityX": -2.516954053640389, + "velocityY": -2.0538495642901125, + "timestamp": 0.9380506729940558 + }, + { + "x": 6.630091339801819, + "y": 3.2069465515769977, + "heading": 2.4987006025081633, + "angularVelocity": 0.35309683711706574, + "velocityX": -2.756921230309734, + "velocityY": -2.0213836217367276, + "timestamp": 0.9849532066437585 + }, + { + "x": 6.489136479391131, + "y": 3.1261931906999445, + "heading": 2.508890104109657, + "angularVelocity": 0.21724842580137385, + "velocityX": -3.0052717719564495, + "velocityY": -1.721727049548525, + "timestamp": 1.0318557402934614 + }, + { + "x": 6.3415058716452215, + "y": 3.0624912033101053, + "heading": 2.5228679049585003, + "angularVelocity": 0.2980180335936202, + "velocityX": -3.1476041112939677, + "velocityY": -1.3581779582656586, + "timestamp": 1.0787582739431643 + }, + { + "x": 6.188700179225271, + "y": 3.0163717053414074, + "heading": 2.5394254687500224, + "angularVelocity": 0.35302066867398807, + "velocityX": -3.257941107429268, + "velocityY": -0.9833050451633768, + "timestamp": 1.1256608075928671 + }, + { + "x": 6.031658683642738, + "y": 2.988099001678993, + "heading": 2.55600757529583, + "angularVelocity": 0.35354394007053697, + "velocityX": -3.3482518610916934, + "velocityY": -0.6027969378706126, + "timestamp": 1.17256334124257 + }, + { + "x": 5.871150970458984, + "y": 2.9778192043304443, + "heading": 2.569623693858617, + "angularVelocity": 0.29030667435753743, + "velocityX": -3.4221544273604505, + "velocityY": -0.21917360425184526, + "timestamp": 1.2194658748922729 + }, + { + "x": 5.576841136148987, + "y": 3.016980693416113, + "heading": 2.5696236883953287, + "angularVelocity": -6.525879318384462e-8, + "velocityX": -3.5155208959382085, + "velocityY": 0.4677826465415135, + "timestamp": 1.3031831512282035 + }, + { + "x": 5.2959027238957885, + "y": 3.113030268725703, + "heading": 2.5696236701163304, + "angularVelocity": -2.1834200821596536e-7, + "velocityX": -3.355799717203925, + "velocityY": 1.14730888907773, + "timestamp": 1.3869004275641341 + }, + { + "x": 5.039219321333584, + "y": 3.2622467062205915, + "heading": 2.569623654672457, + "angularVelocity": -1.8447654007901228e-7, + "velocityX": -3.0660744567491185, + "velocityY": 1.782385237858559, + "timestamp": 1.4706177039000647 + }, + { + "x": 4.799769406634373, + "y": 3.437790584146643, + "heading": 2.569623639730028, + "angularVelocity": -1.7848680691589113e-7, + "velocityX": -2.8602210341671395, + "velocityY": 2.0968656125606664, + "timestamp": 1.5543349802359954 + }, + { + "x": 4.560319652029815, + "y": 3.613334680448953, + "heading": 2.5696236247876003, + "angularVelocity": -1.7848678782979664e-7, + "velocityX": -2.860219121841975, + "velocityY": 2.096868221057592, + "timestamp": 1.638052256571926 + }, + { + "x": 4.320869897426237, + "y": 3.7888787767525987, + "heading": 2.5696236098451726, + "angularVelocity": -1.784867870189022e-7, + "velocityX": -2.860219121830273, + "velocityY": 2.0968682210735534, + "timestamp": 1.7217695329078566 + }, + { + "x": 4.081420142822659, + "y": 3.9644228730562445, + "heading": 2.569623594902745, + "angularVelocity": -1.784867875533888e-7, + "velocityX": -2.8602191218302733, + "velocityY": 2.096868221073554, + "timestamp": 1.8054868092437872 + }, + { + "x": 3.8419703882190817, + "y": 4.139966969359891, + "heading": 2.569623579960317, + "angularVelocity": -1.7848678683767884e-7, + "velocityX": -2.8602191218302733, + "velocityY": 2.0968682210735534, + "timestamp": 1.8892040855797179 + }, + { + "x": 3.6025206336155033, + "y": 4.315511065663538, + "heading": 2.5696235650178894, + "angularVelocity": -1.7848678777394938e-7, + "velocityX": -2.8602191218302737, + "velocityY": 2.096868221073554, + "timestamp": 1.9729213619156485 + }, + { + "x": 3.363070879011915, + "y": 4.491055161967169, + "heading": 2.5696235500754616, + "angularVelocity": -1.7848678692480314e-7, + "velocityX": -2.8602191218304047, + "velocityY": 2.0968682210733744, + "timestamp": 2.056638638251579 + }, + { + "x": 3.123621122611819, + "y": 4.666599255820278, + "heading": 2.5696235351330237, + "angularVelocity": -1.784869102061119e-7, + "velocityX": -2.860219143289622, + "velocityY": 2.096868191801975, + "timestamp": 2.1403559145875097 + }, + { + "x": 2.888392901577879, + "y": 4.838443251060152, + "heading": 2.5511544908862556, + "angularVelocity": -0.22061210129027164, + "velocityX": -2.809793047853641, + "velocityY": 2.0526706405296773, + "timestamp": 2.2240731909234404 + }, + { + "x": 2.7000575025773665, + "y": 4.976197095777676, + "heading": 2.5062705544048423, + "angularVelocity": -0.5361370847912993, + "velocityX": -2.2496598939122543, + "velocityY": 1.645464959523549, + "timestamp": 2.307790467259371 + }, + { + "x": 2.5589096957108692, + "y": 5.079485700111359, + "heading": 2.463997463224374, + "angularVelocity": -0.504950627046677, + "velocityX": -1.6860057212099981, + "velocityY": 1.233778842962105, + "timestamp": 2.3915077435953016 + }, + { + "x": 2.4648687186972724, + "y": 5.148321942855847, + "heading": 2.4324643694891153, + "angularVelocity": -0.37666172521818037, + "velocityX": -1.1233162511909789, + "velocityY": 0.822246563161818, + "timestamp": 2.4752250199312322 + }, + { + "x": 2.4178707599639893, + "y": 5.182730197906494, + "heading": 2.4155561764362563, + "angularVelocity": -0.20196778721051775, + "velocityX": -0.5613890082220943, + "velocityY": 0.41100542870718687, + "timestamp": 2.558942296267163 + }, + { + "x": 2.4178707599639893, + "y": 5.182730197906494, + "heading": 2.4155561764362563, + "angularVelocity": -2.09894229579262e-27, + "velocityX": 2.3772952918690395e-26, + "velocityY": 2.3276233341918227e-26, + "timestamp": 2.6426595726030935 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Mid 3 1.1.traj b/src/main/deploy/choreo/Mid 3 1.1.traj index e8fa3be..c346ae3 100644 --- a/src/main/deploy/choreo/Mid 3 1.1.traj +++ b/src/main/deploy/choreo/Mid 3 1.1.traj @@ -4,244 +4,298 @@ "x": 1.3875831365585327, "y": 5.578441619873047, "heading": 3.141592653589793, - "angularVelocity": -1.1291873234278378e-20, - "velocityX": 1.3203072273457026e-19, - "velocityY": -7.78556777115951e-20, + "angularVelocity": 2.5697719393979616e-20, + "velocityX": -1.1084704093663195e-19, + "velocityY": 3.5588051044447295e-21, "timestamp": 0 }, { - "x": 1.4336237451382812, - "y": 5.57779899487797, - "heading": 3.0785239487284644, - "angularVelocity": -0.8225912321878043, - "velocityX": 0.600497521323188, - "velocityY": -0.008381616329320548, - "timestamp": 0.0766707720596395 + "x": 1.435497168845919, + "y": 5.578814866768434, + "heading": 3.202269926322516, + "angularVelocity": 0.7789200950398169, + "velocityX": 0.6150771269405634, + "velocityY": 0.004791406965653385, + "timestamp": 0.0778992262738075 }, { - "x": 1.5259556376145589, - "y": 5.576358757232951, - "heading": 2.955577173518951, - "angularVelocity": -1.6035677208764376, - "velocityX": 1.2042645456140169, - "velocityY": -0.018784702518695878, - "timestamp": 0.153341544119279 + "x": 1.5313833167159634, + "y": 5.57976146384972, + "heading": 3.322629311801515, + "angularVelocity": 1.5450652238309606, + "velocityX": 1.23089987483335, + "velocityY": 0.012151559477090946, + "timestamp": 0.155798452547615 }, { - "x": 1.657752548636916, - "y": 5.576152010354836, - "heading": 2.9483871516288307, - "angularVelocity": -0.09377787254480233, - "velocityX": 1.718998093821681, - "velocityY": -0.0026965540134869003, - "timestamp": 0.23001231617891849 + "x": 1.6638110997885283, + "y": 5.579730251476359, + "heading": 3.3230709468305517, + "angularVelocity": 0.005669312137769006, + "velocityX": 1.699988426163507, + "velocityY": -0.0004006762949166178, + "timestamp": 0.2336976788214225 }, { - "x": 1.744151163856094, - "y": 5.576992225920646, - "heading": 3.0109375917958325, - "angularVelocity": 0.8158316198817746, - "velocityX": 1.1268781166305677, - "velocityY": 0.01095874664151184, - "timestamp": 0.306683088238558 + "x": 1.7483290860755216, + "y": 5.579216882713837, + "heading": 3.262903789160066, + "angularVelocity": -0.7723716980064068, + "velocityX": 1.0849656707747208, + "velocityY": -0.006590165102761797, + "timestamp": 0.31159690509523 }, { "x": 1.7849689722061155, "y": 5.578441143035889, "heading": 3.141592653589793, - "angularVelocity": 1.7041052057272719, - "velocityX": 0.532377687787867, - "velocityY": 0.018897906937929525, - "timestamp": 0.3833538602981975 - }, - { - "x": 1.793473158716207, - "y": 5.579365807303268, - "heading": 3.2362487120966916, - "angularVelocity": 2.19466706141234, - "velocityX": 0.1971755248655801, - "velocityY": 0.02143898914124505, - "timestamp": 0.4264838913499161 - }, - { - "x": 1.7871836885671384, - "y": 5.580329664394791, - "heading": 3.348096786420172, - "angularVelocity": 2.5932759980942377, - "velocityX": -0.14582577372890423, - "velocityY": 0.022347702239475742, - "timestamp": 0.4696139224016347 - }, - { - "x": 1.7656852765643356, - "y": 5.581260377422058, - "heading": 3.4709165543645333, - "angularVelocity": 2.847662404812178, - "velocityX": -0.498455750635179, - "velocityY": 0.021579233878850173, - "timestamp": 0.5127439534533532 - }, - { - "x": 1.7286997359077372, - "y": 5.581806558753597, - "heading": 3.5910196586486482, - "angularVelocity": 2.7846746537255114, - "velocityX": -0.8575356834834563, - "velocityY": 0.01266359699311981, - "timestamp": 0.5558739845050717 - }, - { - "x": 1.678880135296367, - "y": 5.580447353738231, - "heading": 3.678953949653364, - "angularVelocity": 2.038818170551998, - "velocityX": -1.1551023589950564, - "velocityY": -0.031514120955215656, - "timestamp": 0.5990040155567903 - }, - { - "x": 1.642003204232439, - "y": 5.580155414936893, - "heading": 3.7971584641789797, - "angularVelocity": 2.740654519443145, - "velocityX": -0.8550174939523657, - "velocityY": -0.0067688057304568965, - "timestamp": 0.6421340466085088 - }, - { - "x": 1.6177973573716273, - "y": 5.580015376090363, - "heading": 3.9458939820250976, - "angularVelocity": 3.4485372307700124, - "velocityX": -0.5612295254734593, - "velocityY": -0.003246898810787891, - "timestamp": 0.6852640776602273 - }, - { - "x": 1.6085134315165317, - "y": 5.579433916920347, - "heading": 4.109633152642141, - "angularVelocity": 3.796407436403154, - "velocityX": -0.21525432810291645, - "velocityY": -0.013481538404617919, - "timestamp": 0.7283941087119459 + "angularVelocity": -1.5572829330021465, + "velocityX": 0.47034980812015426, + "velocityY": -0.009958246250379041, + "timestamp": 0.38949613136903755 + }, + { + "x": 1.7918077455407007, + "y": 5.578070537407724, + "heading": 3.07603165300874, + "angularVelocity": -1.903502222336055, + "velocityX": 0.19855737595617648, + "velocityY": -0.010760187162647133, + "timestamp": 0.42393843451517954 + }, + { + "x": 1.7893368559672658, + "y": 5.577687740411084, + "heading": 2.997989156370227, + "angularVelocity": -2.265890765416316, + "velocityX": -0.07173996358347022, + "velocityY": -0.01111415212321765, + "timestamp": 0.45838073766132154 + }, + { + "x": 1.7776291594278917, + "y": 5.577317580799147, + "heading": 2.906758179496928, + "angularVelocity": -2.6488059316532158, + "velocityX": -0.33992199910955634, + "velocityY": -0.010747237499325827, + "timestamp": 0.49282304080746353 + }, + { + "x": 1.7568034196465414, + "y": 5.577003452812707, + "heading": 2.8013208897259188, + "angularVelocity": -3.0612729155663203, + "velocityX": -0.6046558411899725, + "velocityY": -0.009120411753767155, + "timestamp": 0.5272653439536056 + }, + { + "x": 1.7271154460348663, + "y": 5.576861684230968, + "heading": 2.6797808328137562, + "angularVelocity": -3.528801671492646, + "velocityX": -0.8619624966921092, + "velocityY": -0.004116117936055084, + "timestamp": 0.5617076470997476 + }, + { + "x": 1.691069043849476, + "y": 5.578432787106803, + "heading": 2.528205165864953, + "angularVelocity": -4.400857466054248, + "velocityX": -1.0465735125912459, + "velocityY": 0.04561549990339373, + "timestamp": 0.5961499502458897 + }, + { + "x": 1.656140821204661, + "y": 5.579323072829806, + "heading": 2.3275870071573586, + "angularVelocity": -5.824760262295832, + "velocityX": -1.0141082173457174, + "velocityY": 0.02584861178490341, + "timestamp": 0.6305922533920317 + }, + { + "x": 1.631008856377759, + "y": 5.579569155022308, + "heading": 2.1226675607510446, + "angularVelocity": -5.94964412039522, + "velocityX": -0.7296830505284353, + "velocityY": 0.007144765884526293, + "timestamp": 0.6650345565381738 + }, + { + "x": 1.615749078145331, + "y": 5.579444205722283, + "heading": 1.9194967328972732, + "angularVelocity": -5.898874619147794, + "velocityX": -0.44305336282766067, + "velocityY": -0.003627785850877333, + "timestamp": 0.6994768596843158 + }, + { + "x": 1.6103138655470886, + "y": 5.579051278101532, + "heading": 1.720326094951214, + "angularVelocity": -5.782732853286816, + "velocityX": -0.15780630508883814, + "velocityY": -0.01140828530204804, + "timestamp": 0.7339191628304579 }, { "x": 1.6146607398986816, "y": 5.578441143035889, - "heading": 4.267969754957633, - "angularVelocity": 3.6711451036431124, - "velocityX": 0.14252965352096647, - "velocityY": -0.023018158351596252, - "timestamp": 0.7715241397636644 - }, - { - "x": 1.6690373330299817, - "y": 5.575429312080012, - "heading": 4.4984754181934505, - "angularVelocity": 3.129429714872091, - "velocityX": 0.7382366400451945, - "velocityY": -0.040889725472165635, - "timestamp": 0.8451815419863054 - }, - { - "x": 1.7636030315477318, - "y": 5.5708764490251905, - "heading": 4.648544403662639, - "angularVelocity": 2.0373917751753106, - "velocityX": 1.2838587251816274, - "velocityY": -0.061811344378666935, - "timestamp": 0.9188389442089462 - }, - { - "x": 1.8951140247808684, - "y": 5.5669762523657225, - "heading": 4.693072397728858, - "angularVelocity": 0.6045284346524559, - "velocityX": 1.7854416428592532, - "velocityY": -0.05295050520081145, - "timestamp": 0.992496346431587 - }, - { - "x": 2.040218953265875, - "y": 5.564513492820825, - "heading": 4.69307239826707, - "angularVelocity": 7.306959148578081e-9, - "velocityX": 1.9699979106839063, - "velocityY": -0.03343532992724096, - "timestamp": 1.0661537486542279 - }, - { - "x": 2.1853238811191047, - "y": 5.562050696052041, - "heading": 4.693072398805279, - "angularVelocity": 7.30692484830781e-9, - "velocityX": 1.9699979021066683, - "velocityY": -0.03343583529242484, - "timestamp": 1.1398111508768687 - }, - { - "x": 2.3304288085405163, - "y": 5.559587873841206, - "heading": 4.693072399343489, - "angularVelocity": 7.306924923849905e-9, - "velocityX": 1.969997896244148, - "velocityY": -0.03343618070307283, - "timestamp": 1.2134685530995095 - }, - { - "x": 2.475533735803122, - "y": 5.557125042273845, - "heading": 4.693072399881698, - "angularVelocity": 7.30692520769984e-9, - "velocityX": 1.9699978940881417, - "velocityY": -0.03343630773069961, - "timestamp": 1.2871259553221504 - }, - { - "x": 2.6206386630670626, - "y": 5.554662210785155, - "heading": 4.69307240041991, - "angularVelocity": 7.306954868223136e-9, - "velocityX": 1.9699978941062621, - "velocityY": -0.03343630666264002, - "timestamp": 1.3607833575447912 - }, - { - "x": 2.7563056000189468, - "y": 5.552359572937684, - "heading": 4.70273064510102, - "angularVelocity": 0.131123884221667, - "velocityX": 1.841864264256972, - "velocityY": -0.031261458834935876, - "timestamp": 1.434440759767432 - }, - { - "x": 2.846750227458183, - "y": 5.55082448040246, - "heading": 4.709169530239947, - "angularVelocity": 0.08741667428705875, - "velocityX": 1.2279095475815647, - "velocityY": -0.020840981203541834, - "timestamp": 1.5080981619900728 + "heading": 1.5263811612249127, + "angularVelocity": -5.631009427661504, + "velocityX": 0.12620742385167788, + "velocityY": -0.017714699944832876, + "timestamp": 0.7683614659765999 + }, + { + "x": 1.6401584005796046, + "y": 5.577198410041313, + "heading": 1.2647274815001233, + "angularVelocity": -5.390569723612346, + "velocityX": 0.5253009162114343, + "velocityY": -0.025602693079409197, + "timestamp": 0.816900617335438 + }, + { + "x": 1.684994320964394, + "y": 5.575581354594258, + "heading": 1.0153473511131832, + "angularVelocity": -5.137710969508591, + "velocityX": 0.9237063098472489, + "velocityY": -0.03331445651163336, + "timestamp": 0.865439768694276 + }, + { + "x": 1.7491124987051623, + "y": 5.573626178659196, + "heading": 0.7792584484534197, + "angularVelocity": -4.863886080628321, + "velocityX": 1.3209579472611341, + "velocityY": -0.04028038975397266, + "timestamp": 0.913978920053114 + }, + { + "x": 1.832394833509839, + "y": 5.57140698926971, + "heading": 0.55854280647037, + "angularVelocity": -4.547167303180748, + "velocityX": 1.7157764912079099, + "velocityY": -0.04571957538111955, + "timestamp": 0.962518071411952 + }, + { + "x": 1.9345711443290823, + "y": 5.569045004771247, + "heading": 0.3573383968625533, + "angularVelocity": -4.145198339385086, + "velocityX": 2.105028785194018, + "velocityY": -0.04866142963647802, + "timestamp": 1.0110572227707901 + }, + { + "x": 2.0549191287043285, + "y": 5.566727666479068, + "heading": 0.1842377602443393, + "angularVelocity": -3.5662064904786552, + "velocityX": 2.4794002574446163, + "velocityY": -0.047741631802519585, + "timestamp": 1.0595963741296281 + }, + { + "x": 2.192617094377459, + "y": 5.564252863678173, + "heading": 0.047019931683506766, + "angularVelocity": -2.8269515374592107, + "velocityX": 2.8368432866731306, + "velocityY": -0.05098570394439828, + "timestamp": 1.1081355254884662 + }, + { + "x": 2.342632262023748, + "y": 5.5613797511301, + "heading": -0.022930505967036604, + "angularVelocity": -1.4411137338066946, + "velocityX": 3.090601369135268, + "velocityY": -0.059191651844776, + "timestamp": 1.1566746768473042 + }, + { + "x": 2.479810836982564, + "y": 5.558675249403878, + "heading": -0.028156785630628813, + "angularVelocity": -0.1076714264111378, + "velocityX": 2.826142837658786, + "velocityY": -0.05571794418547554, + "timestamp": 1.2052138282061422 + }, + { + "x": 2.5975798577346256, + "y": 5.556269094299155, + "heading": -0.02249245533146242, + "angularVelocity": 0.11669611314980328, + "velocityX": 2.4262686399567257, + "velocityY": -0.04957142919403416, + "timestamp": 1.2537529795649802 + }, + { + "x": 2.69573124985327, + "y": 5.554225549899802, + "heading": -0.013355296818420646, + "angularVelocity": 0.18824306270814264, + "velocityX": 2.0221077083329138, + "velocityY": -0.04210095030804235, + "timestamp": 1.3022921309238182 + }, + { + "x": 2.7742419202395245, + "y": 5.552570833433737, + "heading": -0.0037262727394932653, + "angularVelocity": 0.1983764406539037, + "velocityX": 1.6174710143950495, + "velocityY": -0.03409034603492016, + "timestamp": 1.3508312822826563 + }, + { + "x": 2.833114272929124, + "y": 5.551319078090283, + "heading": 0.0047616375661535875, + "angularVelocity": 0.17486729924258082, + "velocityX": 1.2128838482232662, + "velocityY": -0.025788570842536836, + "timestamp": 1.3993704336414943 + }, + { + "x": 2.872355354708483, + "y": 5.550479104815246, + "heading": 0.011073016059497023, + "angularVelocity": 0.13002655210605069, + "velocityX": 0.8084418594231156, + "velocityY": -0.017305067178193873, + "timestamp": 1.4479095850003323 }, { "x": 2.891972541809082, "y": 5.5500569343566895, - "heading": 4.71238898038469, - "angularVelocity": 0.04370843998830303, - "velocityX": 0.6139547823612851, - "velocityY": -0.010420487589966523, - "timestamp": 1.5817555642127137 + "heading": 0.014491481540703788, + "angularVelocity": 0.07042697256766788, + "velocityX": 0.4041518351974077, + "velocityY": -0.0086975245083139, + "timestamp": 1.4964487363591703 }, { "x": 2.891972541809082, "y": 5.5500569343566895, - "heading": 4.71238898038469, - "angularVelocity": -7.307364922132298e-21, - "velocityX": 3.835648569289986e-21, - "velocityY": 6.13563548284752e-20, - "timestamp": 1.6554129664353545 + "heading": 0.014491481540703788, + "angularVelocity": 1.3928075939596226e-22, + "velocityX": -3.188487813654877e-20, + "velocityY": 1.8586974314537386e-21, + "timestamp": 1.5449878877180083 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Mid 3 1.traj b/src/main/deploy/choreo/Mid 3 1.traj index e8fa3be..c346ae3 100644 --- a/src/main/deploy/choreo/Mid 3 1.traj +++ b/src/main/deploy/choreo/Mid 3 1.traj @@ -4,244 +4,298 @@ "x": 1.3875831365585327, "y": 5.578441619873047, "heading": 3.141592653589793, - "angularVelocity": -1.1291873234278378e-20, - "velocityX": 1.3203072273457026e-19, - "velocityY": -7.78556777115951e-20, + "angularVelocity": 2.5697719393979616e-20, + "velocityX": -1.1084704093663195e-19, + "velocityY": 3.5588051044447295e-21, "timestamp": 0 }, { - "x": 1.4336237451382812, - "y": 5.57779899487797, - "heading": 3.0785239487284644, - "angularVelocity": -0.8225912321878043, - "velocityX": 0.600497521323188, - "velocityY": -0.008381616329320548, - "timestamp": 0.0766707720596395 + "x": 1.435497168845919, + "y": 5.578814866768434, + "heading": 3.202269926322516, + "angularVelocity": 0.7789200950398169, + "velocityX": 0.6150771269405634, + "velocityY": 0.004791406965653385, + "timestamp": 0.0778992262738075 }, { - "x": 1.5259556376145589, - "y": 5.576358757232951, - "heading": 2.955577173518951, - "angularVelocity": -1.6035677208764376, - "velocityX": 1.2042645456140169, - "velocityY": -0.018784702518695878, - "timestamp": 0.153341544119279 + "x": 1.5313833167159634, + "y": 5.57976146384972, + "heading": 3.322629311801515, + "angularVelocity": 1.5450652238309606, + "velocityX": 1.23089987483335, + "velocityY": 0.012151559477090946, + "timestamp": 0.155798452547615 }, { - "x": 1.657752548636916, - "y": 5.576152010354836, - "heading": 2.9483871516288307, - "angularVelocity": -0.09377787254480233, - "velocityX": 1.718998093821681, - "velocityY": -0.0026965540134869003, - "timestamp": 0.23001231617891849 + "x": 1.6638110997885283, + "y": 5.579730251476359, + "heading": 3.3230709468305517, + "angularVelocity": 0.005669312137769006, + "velocityX": 1.699988426163507, + "velocityY": -0.0004006762949166178, + "timestamp": 0.2336976788214225 }, { - "x": 1.744151163856094, - "y": 5.576992225920646, - "heading": 3.0109375917958325, - "angularVelocity": 0.8158316198817746, - "velocityX": 1.1268781166305677, - "velocityY": 0.01095874664151184, - "timestamp": 0.306683088238558 + "x": 1.7483290860755216, + "y": 5.579216882713837, + "heading": 3.262903789160066, + "angularVelocity": -0.7723716980064068, + "velocityX": 1.0849656707747208, + "velocityY": -0.006590165102761797, + "timestamp": 0.31159690509523 }, { "x": 1.7849689722061155, "y": 5.578441143035889, "heading": 3.141592653589793, - "angularVelocity": 1.7041052057272719, - "velocityX": 0.532377687787867, - "velocityY": 0.018897906937929525, - "timestamp": 0.3833538602981975 - }, - { - "x": 1.793473158716207, - "y": 5.579365807303268, - "heading": 3.2362487120966916, - "angularVelocity": 2.19466706141234, - "velocityX": 0.1971755248655801, - "velocityY": 0.02143898914124505, - "timestamp": 0.4264838913499161 - }, - { - "x": 1.7871836885671384, - "y": 5.580329664394791, - "heading": 3.348096786420172, - "angularVelocity": 2.5932759980942377, - "velocityX": -0.14582577372890423, - "velocityY": 0.022347702239475742, - "timestamp": 0.4696139224016347 - }, - { - "x": 1.7656852765643356, - "y": 5.581260377422058, - "heading": 3.4709165543645333, - "angularVelocity": 2.847662404812178, - "velocityX": -0.498455750635179, - "velocityY": 0.021579233878850173, - "timestamp": 0.5127439534533532 - }, - { - "x": 1.7286997359077372, - "y": 5.581806558753597, - "heading": 3.5910196586486482, - "angularVelocity": 2.7846746537255114, - "velocityX": -0.8575356834834563, - "velocityY": 0.01266359699311981, - "timestamp": 0.5558739845050717 - }, - { - "x": 1.678880135296367, - "y": 5.580447353738231, - "heading": 3.678953949653364, - "angularVelocity": 2.038818170551998, - "velocityX": -1.1551023589950564, - "velocityY": -0.031514120955215656, - "timestamp": 0.5990040155567903 - }, - { - "x": 1.642003204232439, - "y": 5.580155414936893, - "heading": 3.7971584641789797, - "angularVelocity": 2.740654519443145, - "velocityX": -0.8550174939523657, - "velocityY": -0.0067688057304568965, - "timestamp": 0.6421340466085088 - }, - { - "x": 1.6177973573716273, - "y": 5.580015376090363, - "heading": 3.9458939820250976, - "angularVelocity": 3.4485372307700124, - "velocityX": -0.5612295254734593, - "velocityY": -0.003246898810787891, - "timestamp": 0.6852640776602273 - }, - { - "x": 1.6085134315165317, - "y": 5.579433916920347, - "heading": 4.109633152642141, - "angularVelocity": 3.796407436403154, - "velocityX": -0.21525432810291645, - "velocityY": -0.013481538404617919, - "timestamp": 0.7283941087119459 + "angularVelocity": -1.5572829330021465, + "velocityX": 0.47034980812015426, + "velocityY": -0.009958246250379041, + "timestamp": 0.38949613136903755 + }, + { + "x": 1.7918077455407007, + "y": 5.578070537407724, + "heading": 3.07603165300874, + "angularVelocity": -1.903502222336055, + "velocityX": 0.19855737595617648, + "velocityY": -0.010760187162647133, + "timestamp": 0.42393843451517954 + }, + { + "x": 1.7893368559672658, + "y": 5.577687740411084, + "heading": 2.997989156370227, + "angularVelocity": -2.265890765416316, + "velocityX": -0.07173996358347022, + "velocityY": -0.01111415212321765, + "timestamp": 0.45838073766132154 + }, + { + "x": 1.7776291594278917, + "y": 5.577317580799147, + "heading": 2.906758179496928, + "angularVelocity": -2.6488059316532158, + "velocityX": -0.33992199910955634, + "velocityY": -0.010747237499325827, + "timestamp": 0.49282304080746353 + }, + { + "x": 1.7568034196465414, + "y": 5.577003452812707, + "heading": 2.8013208897259188, + "angularVelocity": -3.0612729155663203, + "velocityX": -0.6046558411899725, + "velocityY": -0.009120411753767155, + "timestamp": 0.5272653439536056 + }, + { + "x": 1.7271154460348663, + "y": 5.576861684230968, + "heading": 2.6797808328137562, + "angularVelocity": -3.528801671492646, + "velocityX": -0.8619624966921092, + "velocityY": -0.004116117936055084, + "timestamp": 0.5617076470997476 + }, + { + "x": 1.691069043849476, + "y": 5.578432787106803, + "heading": 2.528205165864953, + "angularVelocity": -4.400857466054248, + "velocityX": -1.0465735125912459, + "velocityY": 0.04561549990339373, + "timestamp": 0.5961499502458897 + }, + { + "x": 1.656140821204661, + "y": 5.579323072829806, + "heading": 2.3275870071573586, + "angularVelocity": -5.824760262295832, + "velocityX": -1.0141082173457174, + "velocityY": 0.02584861178490341, + "timestamp": 0.6305922533920317 + }, + { + "x": 1.631008856377759, + "y": 5.579569155022308, + "heading": 2.1226675607510446, + "angularVelocity": -5.94964412039522, + "velocityX": -0.7296830505284353, + "velocityY": 0.007144765884526293, + "timestamp": 0.6650345565381738 + }, + { + "x": 1.615749078145331, + "y": 5.579444205722283, + "heading": 1.9194967328972732, + "angularVelocity": -5.898874619147794, + "velocityX": -0.44305336282766067, + "velocityY": -0.003627785850877333, + "timestamp": 0.6994768596843158 + }, + { + "x": 1.6103138655470886, + "y": 5.579051278101532, + "heading": 1.720326094951214, + "angularVelocity": -5.782732853286816, + "velocityX": -0.15780630508883814, + "velocityY": -0.01140828530204804, + "timestamp": 0.7339191628304579 }, { "x": 1.6146607398986816, "y": 5.578441143035889, - "heading": 4.267969754957633, - "angularVelocity": 3.6711451036431124, - "velocityX": 0.14252965352096647, - "velocityY": -0.023018158351596252, - "timestamp": 0.7715241397636644 - }, - { - "x": 1.6690373330299817, - "y": 5.575429312080012, - "heading": 4.4984754181934505, - "angularVelocity": 3.129429714872091, - "velocityX": 0.7382366400451945, - "velocityY": -0.040889725472165635, - "timestamp": 0.8451815419863054 - }, - { - "x": 1.7636030315477318, - "y": 5.5708764490251905, - "heading": 4.648544403662639, - "angularVelocity": 2.0373917751753106, - "velocityX": 1.2838587251816274, - "velocityY": -0.061811344378666935, - "timestamp": 0.9188389442089462 - }, - { - "x": 1.8951140247808684, - "y": 5.5669762523657225, - "heading": 4.693072397728858, - "angularVelocity": 0.6045284346524559, - "velocityX": 1.7854416428592532, - "velocityY": -0.05295050520081145, - "timestamp": 0.992496346431587 - }, - { - "x": 2.040218953265875, - "y": 5.564513492820825, - "heading": 4.69307239826707, - "angularVelocity": 7.306959148578081e-9, - "velocityX": 1.9699979106839063, - "velocityY": -0.03343532992724096, - "timestamp": 1.0661537486542279 - }, - { - "x": 2.1853238811191047, - "y": 5.562050696052041, - "heading": 4.693072398805279, - "angularVelocity": 7.30692484830781e-9, - "velocityX": 1.9699979021066683, - "velocityY": -0.03343583529242484, - "timestamp": 1.1398111508768687 - }, - { - "x": 2.3304288085405163, - "y": 5.559587873841206, - "heading": 4.693072399343489, - "angularVelocity": 7.306924923849905e-9, - "velocityX": 1.969997896244148, - "velocityY": -0.03343618070307283, - "timestamp": 1.2134685530995095 - }, - { - "x": 2.475533735803122, - "y": 5.557125042273845, - "heading": 4.693072399881698, - "angularVelocity": 7.30692520769984e-9, - "velocityX": 1.9699978940881417, - "velocityY": -0.03343630773069961, - "timestamp": 1.2871259553221504 - }, - { - "x": 2.6206386630670626, - "y": 5.554662210785155, - "heading": 4.69307240041991, - "angularVelocity": 7.306954868223136e-9, - "velocityX": 1.9699978941062621, - "velocityY": -0.03343630666264002, - "timestamp": 1.3607833575447912 - }, - { - "x": 2.7563056000189468, - "y": 5.552359572937684, - "heading": 4.70273064510102, - "angularVelocity": 0.131123884221667, - "velocityX": 1.841864264256972, - "velocityY": -0.031261458834935876, - "timestamp": 1.434440759767432 - }, - { - "x": 2.846750227458183, - "y": 5.55082448040246, - "heading": 4.709169530239947, - "angularVelocity": 0.08741667428705875, - "velocityX": 1.2279095475815647, - "velocityY": -0.020840981203541834, - "timestamp": 1.5080981619900728 + "heading": 1.5263811612249127, + "angularVelocity": -5.631009427661504, + "velocityX": 0.12620742385167788, + "velocityY": -0.017714699944832876, + "timestamp": 0.7683614659765999 + }, + { + "x": 1.6401584005796046, + "y": 5.577198410041313, + "heading": 1.2647274815001233, + "angularVelocity": -5.390569723612346, + "velocityX": 0.5253009162114343, + "velocityY": -0.025602693079409197, + "timestamp": 0.816900617335438 + }, + { + "x": 1.684994320964394, + "y": 5.575581354594258, + "heading": 1.0153473511131832, + "angularVelocity": -5.137710969508591, + "velocityX": 0.9237063098472489, + "velocityY": -0.03331445651163336, + "timestamp": 0.865439768694276 + }, + { + "x": 1.7491124987051623, + "y": 5.573626178659196, + "heading": 0.7792584484534197, + "angularVelocity": -4.863886080628321, + "velocityX": 1.3209579472611341, + "velocityY": -0.04028038975397266, + "timestamp": 0.913978920053114 + }, + { + "x": 1.832394833509839, + "y": 5.57140698926971, + "heading": 0.55854280647037, + "angularVelocity": -4.547167303180748, + "velocityX": 1.7157764912079099, + "velocityY": -0.04571957538111955, + "timestamp": 0.962518071411952 + }, + { + "x": 1.9345711443290823, + "y": 5.569045004771247, + "heading": 0.3573383968625533, + "angularVelocity": -4.145198339385086, + "velocityX": 2.105028785194018, + "velocityY": -0.04866142963647802, + "timestamp": 1.0110572227707901 + }, + { + "x": 2.0549191287043285, + "y": 5.566727666479068, + "heading": 0.1842377602443393, + "angularVelocity": -3.5662064904786552, + "velocityX": 2.4794002574446163, + "velocityY": -0.047741631802519585, + "timestamp": 1.0595963741296281 + }, + { + "x": 2.192617094377459, + "y": 5.564252863678173, + "heading": 0.047019931683506766, + "angularVelocity": -2.8269515374592107, + "velocityX": 2.8368432866731306, + "velocityY": -0.05098570394439828, + "timestamp": 1.1081355254884662 + }, + { + "x": 2.342632262023748, + "y": 5.5613797511301, + "heading": -0.022930505967036604, + "angularVelocity": -1.4411137338066946, + "velocityX": 3.090601369135268, + "velocityY": -0.059191651844776, + "timestamp": 1.1566746768473042 + }, + { + "x": 2.479810836982564, + "y": 5.558675249403878, + "heading": -0.028156785630628813, + "angularVelocity": -0.1076714264111378, + "velocityX": 2.826142837658786, + "velocityY": -0.05571794418547554, + "timestamp": 1.2052138282061422 + }, + { + "x": 2.5975798577346256, + "y": 5.556269094299155, + "heading": -0.02249245533146242, + "angularVelocity": 0.11669611314980328, + "velocityX": 2.4262686399567257, + "velocityY": -0.04957142919403416, + "timestamp": 1.2537529795649802 + }, + { + "x": 2.69573124985327, + "y": 5.554225549899802, + "heading": -0.013355296818420646, + "angularVelocity": 0.18824306270814264, + "velocityX": 2.0221077083329138, + "velocityY": -0.04210095030804235, + "timestamp": 1.3022921309238182 + }, + { + "x": 2.7742419202395245, + "y": 5.552570833433737, + "heading": -0.0037262727394932653, + "angularVelocity": 0.1983764406539037, + "velocityX": 1.6174710143950495, + "velocityY": -0.03409034603492016, + "timestamp": 1.3508312822826563 + }, + { + "x": 2.833114272929124, + "y": 5.551319078090283, + "heading": 0.0047616375661535875, + "angularVelocity": 0.17486729924258082, + "velocityX": 1.2128838482232662, + "velocityY": -0.025788570842536836, + "timestamp": 1.3993704336414943 + }, + { + "x": 2.872355354708483, + "y": 5.550479104815246, + "heading": 0.011073016059497023, + "angularVelocity": 0.13002655210605069, + "velocityX": 0.8084418594231156, + "velocityY": -0.017305067178193873, + "timestamp": 1.4479095850003323 }, { "x": 2.891972541809082, "y": 5.5500569343566895, - "heading": 4.71238898038469, - "angularVelocity": 0.04370843998830303, - "velocityX": 0.6139547823612851, - "velocityY": -0.010420487589966523, - "timestamp": 1.5817555642127137 + "heading": 0.014491481540703788, + "angularVelocity": 0.07042697256766788, + "velocityX": 0.4041518351974077, + "velocityY": -0.0086975245083139, + "timestamp": 1.4964487363591703 }, { "x": 2.891972541809082, "y": 5.5500569343566895, - "heading": 4.71238898038469, - "angularVelocity": -7.307364922132298e-21, - "velocityX": 3.835648569289986e-21, - "velocityY": 6.13563548284752e-20, - "timestamp": 1.6554129664353545 + "heading": 0.014491481540703788, + "angularVelocity": 1.3928075939596226e-22, + "velocityX": -3.188487813654877e-20, + "velocityY": 1.8586974314537386e-21, + "timestamp": 1.5449878877180083 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Mid 3 2.1.traj b/src/main/deploy/choreo/Mid 3 2.1.traj index 020f1ba..b53ec6b 100644 --- a/src/main/deploy/choreo/Mid 3 2.1.traj +++ b/src/main/deploy/choreo/Mid 3 2.1.traj @@ -3,137 +3,200 @@ { "x": 2.8635878562927246, "y": 5.578441619873047, - "heading": -1.5707963267948966, - "angularVelocity": 3.07668011019845e-22, - "velocityX": 6.225343802913688e-21, - "velocityY": -7.419414269605286e-22, + "heading": 0.009614917466831609, + "angularVelocity": -1.528531437446765e-23, + "velocityX": -2.453817422824562e-23, + "velocityY": 1.50998248167019e-22, "timestamp": 0 }, { - "x": 2.8753743714707505, - "y": 5.578008878952115, - "heading": -1.6071749538697528, - "angularVelocity": -0.864093629223167, - "velocityX": 0.27996253556015577, - "velocityY": -0.010278801124406404, - "timestamp": 0.04210033015468626 - }, - { - "x": 2.8989864993914374, - "y": 5.576913727810355, - "heading": -1.6795591223435373, - "angularVelocity": -1.7193254354022516, - "velocityX": 0.5608537470833614, - "velocityY": -0.02601288725612288, - "timestamp": 0.08420066030937252 - }, - { - "x": 2.934525294888728, - "y": 5.574838164969817, - "heading": -1.786903685335108, - "angularVelocity": -2.5497320946691384, - "velocityX": 0.8441452921320711, - "velocityY": -0.04930039343896005, - "timestamp": 0.1263009904640588 - }, - { - "x": 2.9822550006530326, - "y": 5.571466907965597, - "heading": -1.9266329908587956, - "angularVelocity": -3.318959851628004, - "velocityX": 1.1337133364258365, - "velocityY": -0.08007673554654467, - "timestamp": 0.16840132061874505 - }, - { - "x": 3.0427779474067433, - "y": 5.5667931144149465, - "heading": -2.0934132586364123, - "angularVelocity": -3.9614954838792804, - "velocityX": 1.4375884115719717, - "velocityY": -0.1110156032859041, - "timestamp": 0.2105016507734313 - }, - { - "x": 3.1169264950584123, - "y": 5.561652784032428, - "heading": -2.2802480853445712, - "angularVelocity": -4.437847067272045, - "velocityX": 1.7612343508747772, - "velocityY": -0.12209715134364663, - "timestamp": 0.2526019809281176 - }, - { - "x": 3.2052828649293703, - "y": 5.558124125052269, - "heading": -2.45377086401179, - "angularVelocity": -4.12164888089134, - "velocityX": 2.0987096667967022, - "velocityY": -0.08381547050092075, - "timestamp": 0.29470231108280387 - }, - { - "x": 3.2796524843692785, - "y": 5.55600822933581, - "heading": -2.643502979834551, - "angularVelocity": -4.506665746459506, - "velocityX": 1.7664854210572272, - "velocityY": -0.05025841148224034, - "timestamp": 0.33680264123749015 - }, - { - "x": 3.3403881246799387, - "y": 5.554336823360145, - "heading": -2.812261145556071, - "angularVelocity": -4.008476064236641, - "velocityX": 1.442640475442267, - "velocityY": -0.03970054319134659, - "timestamp": 0.37890297139217644 - }, - { - "x": 3.388390555860858, - "y": 5.552788836167647, - "heading": -2.9518024547103603, - "angularVelocity": -3.3144944146894546, - "velocityX": 1.1401913240242543, - "velocityY": -0.036769003635058306, - "timestamp": 0.4210033015468627 - }, - { - "x": 3.4241306673086034, - "y": 5.551464604674301, - "heading": -3.058336722149922, - "angularVelocity": -2.5304853203794417, - "velocityX": 0.8489271061869301, - "velocityY": -0.03145418310203628, - "timestamp": 0.463103631701549 - }, - { - "x": 3.4478466303449795, - "y": 5.550522718395356, - "heading": -3.1301129064142854, - "angularVelocity": -1.7048841185007717, - "velocityX": 0.5633201200391078, - "velocityY": -0.022372420251377964, - "timestamp": 0.5052039618562353 + "x": 2.8683152245735273, + "y": 5.578274982110834, + "heading": 0.04618464340580782, + "angularVelocity": 1.0952855735453868, + "velocityX": 0.14158756036180598, + "velocityY": -0.004990902509465985, + "timestamp": 0.03338830239551295 + }, + { + "x": 2.877792114944196, + "y": 5.578133507964627, + "heading": 0.11915603160618048, + "angularVelocity": 2.18553753754726, + "velocityX": 0.28383864080319215, + "velocityY": -0.004237236878087561, + "timestamp": 0.0667766047910259 + }, + { + "x": 2.8919946410560815, + "y": 5.578297396627518, + "heading": 0.22838911975350412, + "angularVelocity": 3.271597544953453, + "velocityX": 0.4253743105487484, + "velocityY": 0.004908565311320537, + "timestamp": 0.10016490718653885 + }, + { + "x": 2.9107812374502178, + "y": 5.579092613322564, + "heading": 0.3739274892257765, + "angularVelocity": 4.3589628411845, + "velocityX": 0.5626700085439953, + "velocityY": 0.02381722453953736, + "timestamp": 0.1335532095820518 + }, + { + "x": 2.9332106538040255, + "y": 5.58025607529439, + "heading": 0.5585664198828683, + "angularVelocity": 5.530048472362744, + "velocityX": 0.6717746858795545, + "velocityY": 0.03484639494245511, + "timestamp": 0.16694151197756474 + }, + { + "x": 2.9622622193818153, + "y": 5.578513064570307, + "heading": 0.7657200090473316, + "angularVelocity": 6.204376212679287, + "velocityX": 0.8701120899668712, + "velocityY": -0.052204233187781474, + "timestamp": 0.20032981437307767 + }, + { + "x": 2.9994929664814656, + "y": 5.574221954382459, + "heading": 0.9863349814940863, + "angularVelocity": 6.6075528439086835, + "velocityX": 1.1150835600622937, + "velocityY": -0.12852136467529682, + "timestamp": 0.2337181167685906 + }, + { + "x": 3.0447312800587603, + "y": 5.5663721092988325, + "heading": 1.1953383351835791, + "angularVelocity": 6.259777787252248, + "velocityX": 1.354915055021299, + "velocityY": -0.2351076431069595, + "timestamp": 0.26710641916410355 + }, + { + "x": 3.0987733800833377, + "y": 5.5575866982672455, + "heading": 1.3938213283520493, + "angularVelocity": 5.944686579667022, + "velocityX": 1.6185938232027546, + "velocityY": -0.26312841328559256, + "timestamp": 0.3004947215596165 + }, + { + "x": 3.1585920860884475, + "y": 5.554797235578974, + "heading": 1.5779705261768464, + "angularVelocity": 5.515380675644807, + "velocityX": 1.7916066919633906, + "velocityY": -0.08354610711348699, + "timestamp": 0.3338830239551294 + }, + { + "x": 3.221965507535244, + "y": 5.559958063123297, + "heading": 1.750590432066646, + "angularVelocity": 5.170071357476323, + "velocityX": 1.898072585304906, + "velocityY": 0.15456992941021028, + "timestamp": 0.36727132635064236 + }, + { + "x": 3.2769241618398413, + "y": 5.5622903490720015, + "heading": 1.9334313224180195, + "angularVelocity": 5.476196069673357, + "velocityX": 1.6460451823384379, + "velocityY": 0.06985338521021929, + "timestamp": 0.4006596287461553 + }, + { + "x": 3.323773131257663, + "y": 5.561780428146509, + "heading": 2.129567464043533, + "angularVelocity": 5.874396946035567, + "velocityX": 1.4031551788066186, + "velocityY": -0.01527244241141609, + "timestamp": 0.43404793114166823 + }, + { + "x": 3.362968545080011, + "y": 5.558640440834612, + "heading": 2.342597979084741, + "angularVelocity": 6.380393723456773, + "velocityX": 1.1739265254654228, + "velocityY": -0.09404453316405703, + "timestamp": 0.46743623353718117 + }, + { + "x": 3.3929228094560866, + "y": 5.555144446568466, + "heading": 2.552295167463074, + "angularVelocity": 6.2805585589315465, + "velocityX": 0.8971484689831585, + "velocityY": -0.10470715835674735, + "timestamp": 0.5008245359326942 + }, + { + "x": 3.4160537194412997, + "y": 5.552868431782891, + "heading": 2.736680731758176, + "angularVelocity": 5.522459995446828, + "velocityX": 0.692784847555345, + "velocityY": -0.068168029586149, + "timestamp": 0.5342128383282071 + }, + { + "x": 3.433893136182045, + "y": 5.55152245545275, + "heading": 2.887212003334188, + "angularVelocity": 4.50850330133112, + "velocityX": 0.5343014008149213, + "velocityY": -0.040312811181968014, + "timestamp": 0.56760114072372 + }, + { + "x": 3.4469382938570345, + "y": 5.550737144467633, + "heading": 3.0014747717216315, + "angularVelocity": 3.4222395326933257, + "velocityX": 0.3907104206872714, + "velocityY": -0.0235205424891011, + "timestamp": 0.600989443119233 + }, + { + "x": 3.4554666542736996, + "y": 5.550283886264018, + "heading": 3.0782886390389157, + "angularVelocity": 2.300622128293862, + "velocityX": 0.25542959074821336, + "velocityY": -0.01357535936459554, + "timestamp": 0.6343777455147459 }, { "x": 3.4596667289733887, "y": 5.5500569343566895, - "heading": -3.166256562885956, - "angularVelocity": -0.8585124235099837, - "velocityX": 0.28076023596367544, - "velocityY": -0.011063667124691125, - "timestamp": 0.5473042920109216 + "heading": 3.1169287442936304, + "angularVelocity": 1.1572946955191077, + "velocityX": 0.12579479633122612, + "velocityY": -0.006797347904669093, + "timestamp": 0.6677660479102588 }, { "x": 3.4596667289733887, "y": 5.5500569343566895, - "heading": -3.166256562885956, - "angularVelocity": 8.614597867332237e-22, - "velocityX": -7.306610025433993e-22, - "velocityY": 6.593534096827558e-23, - "timestamp": 0.5894046221656078 + "heading": 3.1169287442936304, + "angularVelocity": 1.752573179508138e-24, + "velocityX": 1.210114208680477e-22, + "velocityY": 2.4080448202682696e-22, + "timestamp": 0.7011543503057718 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Mid 3 2.traj b/src/main/deploy/choreo/Mid 3 2.traj index 020f1ba..b53ec6b 100644 --- a/src/main/deploy/choreo/Mid 3 2.traj +++ b/src/main/deploy/choreo/Mid 3 2.traj @@ -3,137 +3,200 @@ { "x": 2.8635878562927246, "y": 5.578441619873047, - "heading": -1.5707963267948966, - "angularVelocity": 3.07668011019845e-22, - "velocityX": 6.225343802913688e-21, - "velocityY": -7.419414269605286e-22, + "heading": 0.009614917466831609, + "angularVelocity": -1.528531437446765e-23, + "velocityX": -2.453817422824562e-23, + "velocityY": 1.50998248167019e-22, "timestamp": 0 }, { - "x": 2.8753743714707505, - "y": 5.578008878952115, - "heading": -1.6071749538697528, - "angularVelocity": -0.864093629223167, - "velocityX": 0.27996253556015577, - "velocityY": -0.010278801124406404, - "timestamp": 0.04210033015468626 - }, - { - "x": 2.8989864993914374, - "y": 5.576913727810355, - "heading": -1.6795591223435373, - "angularVelocity": -1.7193254354022516, - "velocityX": 0.5608537470833614, - "velocityY": -0.02601288725612288, - "timestamp": 0.08420066030937252 - }, - { - "x": 2.934525294888728, - "y": 5.574838164969817, - "heading": -1.786903685335108, - "angularVelocity": -2.5497320946691384, - "velocityX": 0.8441452921320711, - "velocityY": -0.04930039343896005, - "timestamp": 0.1263009904640588 - }, - { - "x": 2.9822550006530326, - "y": 5.571466907965597, - "heading": -1.9266329908587956, - "angularVelocity": -3.318959851628004, - "velocityX": 1.1337133364258365, - "velocityY": -0.08007673554654467, - "timestamp": 0.16840132061874505 - }, - { - "x": 3.0427779474067433, - "y": 5.5667931144149465, - "heading": -2.0934132586364123, - "angularVelocity": -3.9614954838792804, - "velocityX": 1.4375884115719717, - "velocityY": -0.1110156032859041, - "timestamp": 0.2105016507734313 - }, - { - "x": 3.1169264950584123, - "y": 5.561652784032428, - "heading": -2.2802480853445712, - "angularVelocity": -4.437847067272045, - "velocityX": 1.7612343508747772, - "velocityY": -0.12209715134364663, - "timestamp": 0.2526019809281176 - }, - { - "x": 3.2052828649293703, - "y": 5.558124125052269, - "heading": -2.45377086401179, - "angularVelocity": -4.12164888089134, - "velocityX": 2.0987096667967022, - "velocityY": -0.08381547050092075, - "timestamp": 0.29470231108280387 - }, - { - "x": 3.2796524843692785, - "y": 5.55600822933581, - "heading": -2.643502979834551, - "angularVelocity": -4.506665746459506, - "velocityX": 1.7664854210572272, - "velocityY": -0.05025841148224034, - "timestamp": 0.33680264123749015 - }, - { - "x": 3.3403881246799387, - "y": 5.554336823360145, - "heading": -2.812261145556071, - "angularVelocity": -4.008476064236641, - "velocityX": 1.442640475442267, - "velocityY": -0.03970054319134659, - "timestamp": 0.37890297139217644 - }, - { - "x": 3.388390555860858, - "y": 5.552788836167647, - "heading": -2.9518024547103603, - "angularVelocity": -3.3144944146894546, - "velocityX": 1.1401913240242543, - "velocityY": -0.036769003635058306, - "timestamp": 0.4210033015468627 - }, - { - "x": 3.4241306673086034, - "y": 5.551464604674301, - "heading": -3.058336722149922, - "angularVelocity": -2.5304853203794417, - "velocityX": 0.8489271061869301, - "velocityY": -0.03145418310203628, - "timestamp": 0.463103631701549 - }, - { - "x": 3.4478466303449795, - "y": 5.550522718395356, - "heading": -3.1301129064142854, - "angularVelocity": -1.7048841185007717, - "velocityX": 0.5633201200391078, - "velocityY": -0.022372420251377964, - "timestamp": 0.5052039618562353 + "x": 2.8683152245735273, + "y": 5.578274982110834, + "heading": 0.04618464340580782, + "angularVelocity": 1.0952855735453868, + "velocityX": 0.14158756036180598, + "velocityY": -0.004990902509465985, + "timestamp": 0.03338830239551295 + }, + { + "x": 2.877792114944196, + "y": 5.578133507964627, + "heading": 0.11915603160618048, + "angularVelocity": 2.18553753754726, + "velocityX": 0.28383864080319215, + "velocityY": -0.004237236878087561, + "timestamp": 0.0667766047910259 + }, + { + "x": 2.8919946410560815, + "y": 5.578297396627518, + "heading": 0.22838911975350412, + "angularVelocity": 3.271597544953453, + "velocityX": 0.4253743105487484, + "velocityY": 0.004908565311320537, + "timestamp": 0.10016490718653885 + }, + { + "x": 2.9107812374502178, + "y": 5.579092613322564, + "heading": 0.3739274892257765, + "angularVelocity": 4.3589628411845, + "velocityX": 0.5626700085439953, + "velocityY": 0.02381722453953736, + "timestamp": 0.1335532095820518 + }, + { + "x": 2.9332106538040255, + "y": 5.58025607529439, + "heading": 0.5585664198828683, + "angularVelocity": 5.530048472362744, + "velocityX": 0.6717746858795545, + "velocityY": 0.03484639494245511, + "timestamp": 0.16694151197756474 + }, + { + "x": 2.9622622193818153, + "y": 5.578513064570307, + "heading": 0.7657200090473316, + "angularVelocity": 6.204376212679287, + "velocityX": 0.8701120899668712, + "velocityY": -0.052204233187781474, + "timestamp": 0.20032981437307767 + }, + { + "x": 2.9994929664814656, + "y": 5.574221954382459, + "heading": 0.9863349814940863, + "angularVelocity": 6.6075528439086835, + "velocityX": 1.1150835600622937, + "velocityY": -0.12852136467529682, + "timestamp": 0.2337181167685906 + }, + { + "x": 3.0447312800587603, + "y": 5.5663721092988325, + "heading": 1.1953383351835791, + "angularVelocity": 6.259777787252248, + "velocityX": 1.354915055021299, + "velocityY": -0.2351076431069595, + "timestamp": 0.26710641916410355 + }, + { + "x": 3.0987733800833377, + "y": 5.5575866982672455, + "heading": 1.3938213283520493, + "angularVelocity": 5.944686579667022, + "velocityX": 1.6185938232027546, + "velocityY": -0.26312841328559256, + "timestamp": 0.3004947215596165 + }, + { + "x": 3.1585920860884475, + "y": 5.554797235578974, + "heading": 1.5779705261768464, + "angularVelocity": 5.515380675644807, + "velocityX": 1.7916066919633906, + "velocityY": -0.08354610711348699, + "timestamp": 0.3338830239551294 + }, + { + "x": 3.221965507535244, + "y": 5.559958063123297, + "heading": 1.750590432066646, + "angularVelocity": 5.170071357476323, + "velocityX": 1.898072585304906, + "velocityY": 0.15456992941021028, + "timestamp": 0.36727132635064236 + }, + { + "x": 3.2769241618398413, + "y": 5.5622903490720015, + "heading": 1.9334313224180195, + "angularVelocity": 5.476196069673357, + "velocityX": 1.6460451823384379, + "velocityY": 0.06985338521021929, + "timestamp": 0.4006596287461553 + }, + { + "x": 3.323773131257663, + "y": 5.561780428146509, + "heading": 2.129567464043533, + "angularVelocity": 5.874396946035567, + "velocityX": 1.4031551788066186, + "velocityY": -0.01527244241141609, + "timestamp": 0.43404793114166823 + }, + { + "x": 3.362968545080011, + "y": 5.558640440834612, + "heading": 2.342597979084741, + "angularVelocity": 6.380393723456773, + "velocityX": 1.1739265254654228, + "velocityY": -0.09404453316405703, + "timestamp": 0.46743623353718117 + }, + { + "x": 3.3929228094560866, + "y": 5.555144446568466, + "heading": 2.552295167463074, + "angularVelocity": 6.2805585589315465, + "velocityX": 0.8971484689831585, + "velocityY": -0.10470715835674735, + "timestamp": 0.5008245359326942 + }, + { + "x": 3.4160537194412997, + "y": 5.552868431782891, + "heading": 2.736680731758176, + "angularVelocity": 5.522459995446828, + "velocityX": 0.692784847555345, + "velocityY": -0.068168029586149, + "timestamp": 0.5342128383282071 + }, + { + "x": 3.433893136182045, + "y": 5.55152245545275, + "heading": 2.887212003334188, + "angularVelocity": 4.50850330133112, + "velocityX": 0.5343014008149213, + "velocityY": -0.040312811181968014, + "timestamp": 0.56760114072372 + }, + { + "x": 3.4469382938570345, + "y": 5.550737144467633, + "heading": 3.0014747717216315, + "angularVelocity": 3.4222395326933257, + "velocityX": 0.3907104206872714, + "velocityY": -0.0235205424891011, + "timestamp": 0.600989443119233 + }, + { + "x": 3.4554666542736996, + "y": 5.550283886264018, + "heading": 3.0782886390389157, + "angularVelocity": 2.300622128293862, + "velocityX": 0.25542959074821336, + "velocityY": -0.01357535936459554, + "timestamp": 0.6343777455147459 }, { "x": 3.4596667289733887, "y": 5.5500569343566895, - "heading": -3.166256562885956, - "angularVelocity": -0.8585124235099837, - "velocityX": 0.28076023596367544, - "velocityY": -0.011063667124691125, - "timestamp": 0.5473042920109216 + "heading": 3.1169287442936304, + "angularVelocity": 1.1572946955191077, + "velocityX": 0.12579479633122612, + "velocityY": -0.006797347904669093, + "timestamp": 0.6677660479102588 }, { "x": 3.4596667289733887, "y": 5.5500569343566895, - "heading": -3.166256562885956, - "angularVelocity": 8.614597867332237e-22, - "velocityX": -7.306610025433993e-22, - "velocityY": 6.593534096827558e-23, - "timestamp": 0.5894046221656078 + "heading": 3.1169287442936304, + "angularVelocity": 1.752573179508138e-24, + "velocityX": 1.210114208680477e-22, + "velocityY": 2.4080448202682696e-22, + "timestamp": 0.7011543503057718 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Mid 3 3.1.traj b/src/main/deploy/choreo/Mid 3 3.1.traj index 9db56bb..d667f08 100644 --- a/src/main/deploy/choreo/Mid 3 3.1.traj +++ b/src/main/deploy/choreo/Mid 3 3.1.traj @@ -4,343 +4,415 @@ "x": 3.4596667289733887, "y": 5.5500569343566895, "heading": -3.0698845150991874, - "angularVelocity": 3.7072815015004976e-26, - "velocityX": 1.0418795881976682e-23, - "velocityY": 6.901816656311587e-24, + "angularVelocity": 1.2312735419469557e-26, + "velocityX": 1.812191248734851e-27, + "velocityY": 6.527407921509027e-26, "timestamp": 0 }, { - "x": 3.470351081239337, - "y": 5.533890502292575, - "heading": -3.040759961328588, - "angularVelocity": 0.5805525842398843, - "velocityX": 0.2129759091858508, - "velocityY": -0.32225262528260434, - "timestamp": 0.05016695224728367 - }, - { - "x": 3.491872536796006, - "y": 5.501531193150436, - "heading": -2.9832552756508868, - "angularVelocity": 1.1462662789289626, - "velocityX": 0.4289966719641501, - "velocityY": -0.6450323906988166, - "timestamp": 0.10033390449456733 - }, - { - "x": 3.5244383644938826, - "y": 5.4529510964403825, - "heading": -2.8984389928959944, - "angularVelocity": 1.6906803972626214, - "velocityX": 0.6491490162159438, - "velocityY": -0.9683685082281274, - "timestamp": 0.15050085674185099 - }, - { - "x": 3.568347380928225, - "y": 5.388082581800036, - "heading": -2.7884682822947835, - "angularVelocity": 2.192094709264814, - "velocityX": 0.8752578035417661, - "velocityY": -1.2930527316189402, - "timestamp": 0.20066780898913467 - }, - { - "x": 3.623990295586385, - "y": 5.306759607475579, - "heading": -2.6577142680126014, - "angularVelocity": 2.606377474112186, - "velocityX": 1.1091547755160458, - "velocityY": -1.6210467385700176, - "timestamp": 0.25083476123641835 - }, - { - "x": 3.691810783485903, - "y": 5.208713512634915, - "heading": -2.5142236168221443, - "angularVelocity": 2.860262478835876, - "velocityX": 1.3518957174280044, - "velocityY": -1.9543960804589637, - "timestamp": 0.301001713483702 - }, - { - "x": 3.7722356432776443, - "y": 5.093905970122991, - "heading": -2.373366389937741, - "angularVelocity": 2.8077692698988006, - "velocityX": 1.6031442252124417, - "velocityY": -2.288509414445041, - "timestamp": 0.3511686657309857 - }, - { - "x": 3.8638571513314606, - "y": 4.9663546159375045, - "heading": -2.2823158901507146, - "angularVelocity": 1.8149497967948816, - "velocityX": 1.826331956587555, - "velocityY": -2.542537436931775, - "timestamp": 0.4013356179782694 - }, - { - "x": 3.966502090333123, - "y": 4.825614406095303, - "heading": -2.2390967768789967, - "angularVelocity": 0.8615056593169522, - "velocityX": 2.046066870789829, - "velocityY": -2.8054367175518826, - "timestamp": 0.45150257022555307 - }, - { - "x": 4.07406031947327, - "y": 4.6692585851239965, - "heading": -2.239096739696053, - "angularVelocity": 7.411840268057862e-7, - "velocityX": 2.144005651568587, - "velocityY": -3.1167095860357494, - "timestamp": 0.5016695224728367 - }, - { - "x": 4.192225193562204, - "y": 4.520756134824619, - "heading": -2.239096700182899, - "angularVelocity": 7.876331299799558e-7, - "velocityX": 2.3554325865058465, - "velocityY": -2.960164882398619, - "timestamp": 0.5518364747201203 - }, - { - "x": 4.326063058683468, - "y": 4.386207500276538, - "heading": -2.2390966080686847, - "angularVelocity": 0.000001836153294318911, - "velocityX": 2.667849233924928, - "velocityY": -2.6820173146030837, - "timestamp": 0.6020034269674039 - }, - { - "x": 4.46665632097007, - "y": 4.270349550388192, - "heading": -2.214327295386285, - "angularVelocity": 0.4937376414717436, - "velocityX": 2.802507547071767, - "velocityY": -2.3094476482696806, - "timestamp": 0.6521703792146876 + "x": 3.470351701902805, + "y": 5.533821202631173, + "heading": -3.0410502376141126, + "angularVelocity": 0.5744467520987541, + "velocityX": 0.21286983864060433, + "velocityY": -0.32345403357157865, + "timestamp": 0.050194865546246525 + }, + { + "x": 3.4918683639146386, + "y": 5.5013221164284385, + "heading": -2.984122678919303, + "angularVelocity": 1.1341311123218276, + "velocityX": 0.4286626087684034, + "velocityY": -0.6474583774468244, + "timestamp": 0.10038973109249305 + }, + { + "x": 3.524415476543047, + "y": 5.452530110487052, + "heading": -2.900159466928913, + "angularVelocity": 1.6727450323187332, + "velocityX": 0.6484151770149017, + "velocityY": -0.9720517309969351, + "timestamp": 0.15058459663873958 + }, + { + "x": 3.568279107796025, + "y": 5.387377079170257, + "heading": -2.7912705513728184, + "angularVelocity": 2.169323781847179, + "velocityX": 0.8738668940663786, + "velocityY": -1.2980019093141477, + "timestamp": 0.2007794621849861 + }, + { + "x": 3.6238335021807875, + "y": 5.305700107177474, + "heading": -2.661703338416061, + "angularVelocity": 2.58128419205311, + "velocityX": 1.1067744435649114, + "velocityY": -1.627197744309744, + "timestamp": 0.2509743277312326 + }, + { + "x": 3.6915048154681016, + "y": 5.207234717854563, + "heading": -2.519260477416935, + "angularVelocity": 2.837797441012924, + "velocityX": 1.348172020203257, + "velocityY": -1.961662577464042, + "timestamp": 0.30116919327747915 + }, + { + "x": 3.771709210218125, + "y": 5.09191800317261, + "heading": -2.378757934963842, + "angularVelocity": 2.799141723442651, + "velocityX": 1.597860535678248, + "velocityY": -2.297380686789684, + "timestamp": 0.3513640588237257 + }, + { + "x": 3.8630702426232038, + "y": 4.963801416536227, + "heading": -2.2881483630208987, + "angularVelocity": 1.8051561839419887, + "velocityX": 1.8201270470762463, + "velocityY": -2.5523842975203284, + "timestamp": 0.4015589243699722 + }, + { + "x": 3.9654177804635915, + "y": 4.822444228754285, + "heading": -2.245465067240904, + "angularVelocity": 0.8503518301223242, + "velocityX": 2.0390041237602423, + "velocityY": -2.8161682722649046, + "timestamp": 0.45175378991621873 + }, + { + "x": 4.072604442672708, + "y": 4.665705551178546, + "heading": -2.2454649759091194, + "angularVelocity": 0.0000018195443548764102, + "velocityX": 2.1354108840148447, + "velocityY": -3.1226037936356046, + "timestamp": 0.5019486554624653 + }, + { + "x": 4.191866544117303, + "y": 4.517947153047455, + "heading": -2.245464878096673, + "angularVelocity": 0.000001948654414335729, + "velocityX": 2.375982087943118, + "velocityY": -2.9436954661220507, + "timestamp": 0.5521435210087118 + }, + { + "x": 4.326720805180037, + "y": 4.384267316197636, + "heading": -2.245464583471803, + "angularVelocity": 0.0000058696216512510084, + "velocityX": 2.686614648633487, + "velocityY": -2.6632173509191657, + "timestamp": 0.6023383865549583 + }, + { + "x": 4.4671387450953555, + "y": 4.269390431902348, + "heading": -2.217765688783718, + "angularVelocity": 0.5518272513862013, + "velocityX": 2.7974562415342206, + "velocityY": -2.288618229078576, + "timestamp": 0.6525332521012048 }, { "x": 4.60924768447876, "y": 4.173398017883301, "heading": -2.157308519530683, - "angularVelocity": 1.1365804239919575, - "velocityX": 2.84233658058051, - "velocityY": -1.9325776863421187, - "timestamp": 0.7023373314619712 - }, - { - "x": 4.757244359209621, - "y": 4.093623523820721, - "heading": -2.0655111962458994, - "angularVelocity": 1.767636272422969, - "velocityX": 2.8498030344597014, - "velocityY": -1.5361263735516977, - "timestamp": 0.7542695783370328 - }, - { - "x": 4.905332920228958, - "y": 4.0351168726459035, - "heading": -1.9470536057388323, - "angularVelocity": 2.281002606955405, - "velocityX": 2.8515723838332723, - "velocityY": -1.126595799245347, - "timestamp": 0.8062018252120944 - }, - { - "x": 5.055400824091604, - "y": 3.998698661500805, - "heading": -1.8151511335185468, - "angularVelocity": 2.539895347444063, - "velocityX": 2.889686329645598, - "velocityY": -0.7012639224471425, - "timestamp": 0.858134072087156 - }, - { - "x": 5.216908347866735, - "y": 3.980352876038938, - "heading": -1.7083842074089743, - "angularVelocity": 2.0558888269639497, - "velocityX": 3.109966032543999, - "velocityY": -0.3532638498388685, - "timestamp": 0.9100663189622176 - }, - { - "x": 5.392426431970856, - "y": 3.975108601121907, - "heading": -1.641860902012389, - "angularVelocity": 1.2809633589824587, - "velocityX": 3.3797514004426206, - "velocityY": -0.1009830159986892, - "timestamp": 0.9619985658372792 - }, - { - "x": 5.582811407311731, - "y": 3.979892618198808, - "heading": -1.6227004666535643, - "angularVelocity": 0.3689506330223126, - "velocityX": 3.6660261551729536, - "velocityY": 0.0921203561326904, - "timestamp": 1.0139308127123408 - }, - { - "x": 5.778669831046716, - "y": 3.995210464766022, - "heading": -1.6227001197359128, - "angularVelocity": 0.000006680197224880823, - "velocityX": 3.7714221032296127, - "velocityY": 0.29495828678596464, - "timestamp": 1.0658630595874024 - }, - { - "x": 5.974528240815056, - "y": 4.010528489936178, - "heading": -1.6226997728238337, - "angularVelocity": 0.000006680089923071776, - "velocityX": 3.7714218342899026, - "velocityY": 0.2949617259389604, - "timestamp": 1.117795306462464 - }, - { - "x": 6.1703866505831755, - "y": 4.025846515108401, - "heading": -1.6226994259114784, - "angularVelocity": 0.0000066800952405189, - "velocityX": 3.7714218342856256, - "velocityY": 0.2949617259787309, - "timestamp": 1.1697275533375255 - }, - { - "x": 6.366245060351238, - "y": 4.0411645402805725, - "heading": -1.6226990789988471, - "angularVelocity": 0.000006680100558868112, - "velocityX": 3.7714218342845336, - "velocityY": 0.2949617259777772, - "timestamp": 1.2216598002125871 - }, - { - "x": 6.562103470119243, - "y": 4.056482565452695, - "heading": -1.6226987320859396, - "angularVelocity": 0.000006680105877144824, - "velocityX": 3.7714218342834416, - "velocityY": 0.2949617259768231, - "timestamp": 1.2735920470876487 - }, - { - "x": 6.757961879887191, - "y": 4.071800590624768, - "heading": -1.6226983851727559, - "angularVelocity": 0.000006680111194481284, - "velocityX": 3.7714218342823496, - "velocityY": 0.29496172597586884, - "timestamp": 1.3255242939627103 - }, - { - "x": 6.953820289655174, - "y": 4.087118615795626, - "heading": -1.622698038259296, - "angularVelocity": 0.000006680116513213821, - "velocityX": 3.7714218342830135, - "velocityY": 0.2949617259524653, - "timestamp": 1.377456540837772 + "angularVelocity": 1.204449271755367, + "velocityX": 2.8311449355806007, + "velocityY": -1.9123950821345668, + "timestamp": 0.7027281176474514 + }, + { + "x": 4.722976104309171, + "y": 4.108925075993853, + "heading": -2.0891694778953918, + "angularVelocity": 1.6986564221053766, + "velocityX": 2.8351662436762037, + "velocityY": -1.6072632394614674, + "timestamp": 0.7428416101398513 + }, + { + "x": 4.836477408039613, + "y": 4.056947172457928, + "heading": -2.0033151883417575, + "angularVelocity": 2.1402845830464856, + "velocityX": 2.8295044055799075, + "velocityY": -1.2957710811586096, + "timestamp": 0.7829551026322512 + }, + { + "x": 4.949920984616205, + "y": 4.0177960509685695, + "heading": -1.903114928393686, + "angularVelocity": 2.4979191220274792, + "velocityX": 2.828065309897542, + "velocityY": -0.9760087954639355, + "timestamp": 0.8230685951246511 + }, + { + "x": 5.064065362215247, + "y": 3.991835755777838, + "heading": -1.794426344777262, + "angularVelocity": 2.7095268166195297, + "velocityX": 2.845535766317746, + "velocityY": -0.647171153088953, + "timestamp": 0.8631820876170511 + }, + { + "x": 5.180659510984554, + "y": 3.978933247316211, + "heading": -1.6868519677774367, + "angularVelocity": 2.681750461399157, + "velocityX": 2.906606768069287, + "velocityY": -0.32165008978141457, + "timestamp": 0.903295580109451 + }, + { + "x": 5.304982214386576, + "y": 3.975371793740181, + "heading": -1.599436041565328, + "angularVelocity": 2.179215041638947, + "velocityX": 3.0992739768439947, + "velocityY": -0.08878443024388993, + "timestamp": 0.9434090726018509 + }, + { + "x": 5.437286305059447, + "y": 3.9799465783185837, + "heading": -1.5362326476191075, + "angularVelocity": 1.5756143386965153, + "velocityX": 3.298244118183867, + "velocityY": 0.11404603025451941, + "timestamp": 0.9835225650942508 + }, + { + "x": 5.574519249175071, + "y": 3.9911087281656146, + "heading": -1.4896376312982285, + "angularVelocity": 1.1615796437995773, + "velocityX": 3.421116826006249, + "velocityY": 0.278264223668521, + "timestamp": 1.0236360575866508 + }, + { + "x": 5.710295358289056, + "y": 4.008878734245578, + "heading": -1.4406573828911584, + "angularVelocity": 1.2210417334354526, + "velocityX": 3.3847989959914093, + "velocityY": 0.44299324182080874, + "timestamp": 1.0637495500790508 + }, + { + "x": 5.839309895761849, + "y": 4.034487620002502, + "heading": -1.374832058502612, + "angularVelocity": 1.6409771450595625, + "velocityX": 3.216237965249132, + "velocityY": 0.6384107731775158, + "timestamp": 1.1038630425714508 + }, + { + "x": 5.9651423912202, + "y": 4.060367188933271, + "heading": -1.3030479833599253, + "angularVelocity": 1.789524438847772, + "velocityX": 3.1369119874613967, + "velocityY": 0.6451587065292861, + "timestamp": 1.1439765350638509 + }, + { + "x": 6.088325953926389, + "y": 4.085030282261382, + "heading": -1.226883005971526, + "angularVelocity": 1.8987371244932074, + "velocityX": 3.0708760332829894, + "velocityY": 0.6148328603594958, + "timestamp": 1.184090027556251 + }, + { + "x": 6.2080954642882755, + "y": 4.108236900279935, + "heading": -1.1444073412911668, + "angularVelocity": 2.056057938509982, + "velocityX": 2.9857662078309084, + "velocityY": 0.5785239972049276, + "timestamp": 1.224203520048651 + }, + { + "x": 6.31677879626179, + "y": 4.1302920762013215, + "heading": -1.0366273178562313, + "angularVelocity": 2.686877076468872, + "velocityX": 2.7093958970066208, + "velocityY": 0.5498193887147775, + "timestamp": 1.264317012541051 + }, + { + "x": 6.414746281726902, + "y": 4.147226598563455, + "heading": -0.9049619897221106, + "angularVelocity": 3.2823202357427825, + "velocityX": 2.4422576888231124, + "velocityY": 0.4221652444085389, + "timestamp": 1.304430505033451 + }, + { + "x": 6.503675902170543, + "y": 4.155946147636541, + "heading": -0.7520815391315856, + "angularVelocity": 3.8111977065943705, + "velocityX": 2.216950330627282, + "velocityY": 0.21737197464765756, + "timestamp": 1.344543997525851 + }, + { + "x": 6.586090114201575, + "y": 4.1540858277756465, + "heading": -0.580040497638246, + "angularVelocity": 4.288857209975799, + "velocityX": 2.0545259689528748, + "velocityY": -0.0463764121572527, + "timestamp": 1.3846574900182511 + }, + { + "x": 6.676170358790984, + "y": 4.141704344881373, + "heading": -0.41948306620614584, + "angularVelocity": 4.002579218514071, + "velocityX": 2.245634548187899, + "velocityY": -0.3086613038398522, + "timestamp": 1.4247709825106512 + }, + { + "x": 6.778119596986203, + "y": 4.12741918933359, + "heading": -0.2818617425003748, + "angularVelocity": 3.4307988448486575, + "velocityX": 2.54151986926928, + "velocityY": -0.3561184693775817, + "timestamp": 1.4648844750030512 + }, + { + "x": 6.891681505117521, + "y": 4.114615252299952, + "heading": -0.16923343203441635, + "angularVelocity": 2.80774132263097, + "velocityX": 2.831015228924134, + "velocityY": -0.3191927762476476, + "timestamp": 1.5049979674954512 + }, + { + "x": 7.015942193168223, + "y": 4.105678430574392, + "heading": -0.0831259035248278, + "angularVelocity": 2.1465976448174597, + "velocityX": 3.097727979538158, + "velocityY": -0.22278842280444786, + "timestamp": 1.5451114599878513 }, { "x": 7.149678707122803, "y": 4.102436542510986, - "heading": -1.6226976913440598, - "angularVelocity": 0.000006680150717856603, - "velocityX": 3.7714219825462947, - "velocityY": 0.29495983010733695, - "timestamp": 1.4293887877128335 - }, - { - "x": 7.416831851800406, - "y": 4.105946276559182, - "heading": -1.6226976909008946, - "angularVelocity": 6.274765917736025e-9, - "velocityX": 3.7826143141948623, - "velocityY": 0.04969423162037773, - "timestamp": 1.5000153762273085 - }, - { - "x": 7.683984997699984, - "y": 4.109455917591789, - "heading": -1.6226976904577277, - "angularVelocity": 6.274788999649594e-9, - "velocityX": 3.7826143314967657, - "velocityY": 0.049692914615112965, - "timestamp": 1.5706419647417835 - }, - { - "x": 7.933502359055495, - "y": 4.112733873648306, - "heading": -1.6145121736520152, - "angularVelocity": 0.11589851609546277, - "velocityX": 3.532909724280016, - "velocityY": 0.0464124931624786, - "timestamp": 1.6412685532562585 - }, - { - "x": 8.141433501977613, - "y": 4.115465503819871, - "heading": -1.60769089632527, - "angularVelocity": 0.09658228537184779, - "velocityX": 2.944091556673475, - "velocityY": 0.038677079397722, - "timestamp": 1.7118951417707335 - }, - { - "x": 8.307778419316605, - "y": 4.117650808004561, - "heading": -1.6022338692720377, - "angularVelocity": 0.07726590180855333, - "velocityX": 2.355273287834106, - "velocityY": 0.030941664189867397, - "timestamp": 1.7825217302852085 - }, - { - "x": 8.432537108690411, - "y": 4.119289786165083, - "heading": -1.5981410965811054, - "angularVelocity": 0.057949460352219886, - "velocityX": 1.7664549852671345, - "velocityY": 0.023206248453940526, - "timestamp": 1.8531483187996836 - }, - { - "x": 8.515709568908191, - "y": 4.120382438281781, - "heading": -1.5954125803793526, - "angularVelocity": 0.03863298878146346, - "velocityX": 1.1776366658391457, - "velocityY": 0.015470832439747922, - "timestamp": 1.9237749073141586 + "heading": -0.02423765959758657, + "angularVelocity": 1.4680408079251241, + "velocityX": 3.3339533818930946, + "velocityY": -0.0808178959740319, + "timestamp": 1.5852249524802513 + }, + { + "x": 7.416849021016848, + "y": 4.104072679647048, + "heading": -0.02423765672113594, + "angularVelocity": 4.072771835592131e-8, + "velocityX": 3.7828694770841707, + "velocityY": 0.023166096345521754, + "timestamp": 1.655851315750913 + }, + { + "x": 7.683995167692338, + "y": 4.108021145887048, + "heading": -0.024237655875962986, + "angularVelocity": 1.1966819636377302e-8, + "velocityX": 3.7825272929841307, + "velocityY": 0.05590640742563016, + "timestamp": 1.7264776790215746 + }, + { + "x": 7.933509607434154, + "y": 4.111709036478853, + "heading": -0.01801171480387418, + "angularVelocity": 0.08815321621798794, + "velocityX": 3.5328796243635003, + "velocityY": 0.05221691194365583, + "timestamp": 1.7971040422922362 + }, + { + "x": 8.141438328466565, + "y": 4.114782278956212, + "heading": -0.012823406858938095, + "angularVelocity": 0.07346134934136293, + "velocityX": 2.9440666544809257, + "velocityY": 0.04351409778216477, + "timestamp": 1.8677304055628978 + }, + { + "x": 8.307781312832681, + "y": 4.11724087305162, + "heading": -0.008672751470206637, + "angularVelocity": 0.05876920736842818, + "velocityX": 2.3552534303463926, + "velocityY": 0.034811279833078496, + "timestamp": 1.9383567688335595 + }, + { + "x": 8.432538554548055, + "y": 4.119084818675069, + "heading": -0.00555975571941177, + "angularVelocity": 0.044076965125117926, + "velocityX": 1.7664401214779493, + "velocityY": 0.02610846060956428, + "timestamp": 2.008983132104221 + }, + { + "x": 8.515710050620672, + "y": 4.120314115781301, + "heading": -0.0034844232543666874, + "angularVelocity": 0.029384671232352395, + "velocityX": 1.1776267702455339, + "velocityY": 0.01740564074522844, + "timestamp": 2.079609495374883 }, { "x": 8.557295799255371, "y": 4.120928764343262, - "heading": -1.5940483219094344, - "angularVelocity": 0.019316499615988192, - "velocityX": 0.5888183362935331, - "velocityY": 0.007735416264214447, - "timestamp": 1.9944014958286336 + "heading": -0.002446756286149136, + "angularVelocity": 0.014692346032895615, + "velocityX": 0.5888133935953765, + "velocityY": 0.008702820497862328, + "timestamp": 2.150235858645545 }, { "x": 8.557295799255371, "y": 4.120928764343262, - "heading": -1.5940483219094344, - "angularVelocity": 6.715491935087819e-29, - "velocityX": 4.935316924694466e-27, - "velocityY": -9.408823776589858e-28, - "timestamp": 2.065028084343109 + "heading": -0.002446756286149136, + "angularVelocity": -1.1375698231513232e-28, + "velocityX": -3.986760367825934e-26, + "velocityY": 7.675786204695289e-26, + "timestamp": 2.2208622219162066 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Mid 3 3.traj b/src/main/deploy/choreo/Mid 3 3.traj index 9db56bb..d667f08 100644 --- a/src/main/deploy/choreo/Mid 3 3.traj +++ b/src/main/deploy/choreo/Mid 3 3.traj @@ -4,343 +4,415 @@ "x": 3.4596667289733887, "y": 5.5500569343566895, "heading": -3.0698845150991874, - "angularVelocity": 3.7072815015004976e-26, - "velocityX": 1.0418795881976682e-23, - "velocityY": 6.901816656311587e-24, + "angularVelocity": 1.2312735419469557e-26, + "velocityX": 1.812191248734851e-27, + "velocityY": 6.527407921509027e-26, "timestamp": 0 }, { - "x": 3.470351081239337, - "y": 5.533890502292575, - "heading": -3.040759961328588, - "angularVelocity": 0.5805525842398843, - "velocityX": 0.2129759091858508, - "velocityY": -0.32225262528260434, - "timestamp": 0.05016695224728367 - }, - { - "x": 3.491872536796006, - "y": 5.501531193150436, - "heading": -2.9832552756508868, - "angularVelocity": 1.1462662789289626, - "velocityX": 0.4289966719641501, - "velocityY": -0.6450323906988166, - "timestamp": 0.10033390449456733 - }, - { - "x": 3.5244383644938826, - "y": 5.4529510964403825, - "heading": -2.8984389928959944, - "angularVelocity": 1.6906803972626214, - "velocityX": 0.6491490162159438, - "velocityY": -0.9683685082281274, - "timestamp": 0.15050085674185099 - }, - { - "x": 3.568347380928225, - "y": 5.388082581800036, - "heading": -2.7884682822947835, - "angularVelocity": 2.192094709264814, - "velocityX": 0.8752578035417661, - "velocityY": -1.2930527316189402, - "timestamp": 0.20066780898913467 - }, - { - "x": 3.623990295586385, - "y": 5.306759607475579, - "heading": -2.6577142680126014, - "angularVelocity": 2.606377474112186, - "velocityX": 1.1091547755160458, - "velocityY": -1.6210467385700176, - "timestamp": 0.25083476123641835 - }, - { - "x": 3.691810783485903, - "y": 5.208713512634915, - "heading": -2.5142236168221443, - "angularVelocity": 2.860262478835876, - "velocityX": 1.3518957174280044, - "velocityY": -1.9543960804589637, - "timestamp": 0.301001713483702 - }, - { - "x": 3.7722356432776443, - "y": 5.093905970122991, - "heading": -2.373366389937741, - "angularVelocity": 2.8077692698988006, - "velocityX": 1.6031442252124417, - "velocityY": -2.288509414445041, - "timestamp": 0.3511686657309857 - }, - { - "x": 3.8638571513314606, - "y": 4.9663546159375045, - "heading": -2.2823158901507146, - "angularVelocity": 1.8149497967948816, - "velocityX": 1.826331956587555, - "velocityY": -2.542537436931775, - "timestamp": 0.4013356179782694 - }, - { - "x": 3.966502090333123, - "y": 4.825614406095303, - "heading": -2.2390967768789967, - "angularVelocity": 0.8615056593169522, - "velocityX": 2.046066870789829, - "velocityY": -2.8054367175518826, - "timestamp": 0.45150257022555307 - }, - { - "x": 4.07406031947327, - "y": 4.6692585851239965, - "heading": -2.239096739696053, - "angularVelocity": 7.411840268057862e-7, - "velocityX": 2.144005651568587, - "velocityY": -3.1167095860357494, - "timestamp": 0.5016695224728367 - }, - { - "x": 4.192225193562204, - "y": 4.520756134824619, - "heading": -2.239096700182899, - "angularVelocity": 7.876331299799558e-7, - "velocityX": 2.3554325865058465, - "velocityY": -2.960164882398619, - "timestamp": 0.5518364747201203 - }, - { - "x": 4.326063058683468, - "y": 4.386207500276538, - "heading": -2.2390966080686847, - "angularVelocity": 0.000001836153294318911, - "velocityX": 2.667849233924928, - "velocityY": -2.6820173146030837, - "timestamp": 0.6020034269674039 - }, - { - "x": 4.46665632097007, - "y": 4.270349550388192, - "heading": -2.214327295386285, - "angularVelocity": 0.4937376414717436, - "velocityX": 2.802507547071767, - "velocityY": -2.3094476482696806, - "timestamp": 0.6521703792146876 + "x": 3.470351701902805, + "y": 5.533821202631173, + "heading": -3.0410502376141126, + "angularVelocity": 0.5744467520987541, + "velocityX": 0.21286983864060433, + "velocityY": -0.32345403357157865, + "timestamp": 0.050194865546246525 + }, + { + "x": 3.4918683639146386, + "y": 5.5013221164284385, + "heading": -2.984122678919303, + "angularVelocity": 1.1341311123218276, + "velocityX": 0.4286626087684034, + "velocityY": -0.6474583774468244, + "timestamp": 0.10038973109249305 + }, + { + "x": 3.524415476543047, + "y": 5.452530110487052, + "heading": -2.900159466928913, + "angularVelocity": 1.6727450323187332, + "velocityX": 0.6484151770149017, + "velocityY": -0.9720517309969351, + "timestamp": 0.15058459663873958 + }, + { + "x": 3.568279107796025, + "y": 5.387377079170257, + "heading": -2.7912705513728184, + "angularVelocity": 2.169323781847179, + "velocityX": 0.8738668940663786, + "velocityY": -1.2980019093141477, + "timestamp": 0.2007794621849861 + }, + { + "x": 3.6238335021807875, + "y": 5.305700107177474, + "heading": -2.661703338416061, + "angularVelocity": 2.58128419205311, + "velocityX": 1.1067744435649114, + "velocityY": -1.627197744309744, + "timestamp": 0.2509743277312326 + }, + { + "x": 3.6915048154681016, + "y": 5.207234717854563, + "heading": -2.519260477416935, + "angularVelocity": 2.837797441012924, + "velocityX": 1.348172020203257, + "velocityY": -1.961662577464042, + "timestamp": 0.30116919327747915 + }, + { + "x": 3.771709210218125, + "y": 5.09191800317261, + "heading": -2.378757934963842, + "angularVelocity": 2.799141723442651, + "velocityX": 1.597860535678248, + "velocityY": -2.297380686789684, + "timestamp": 0.3513640588237257 + }, + { + "x": 3.8630702426232038, + "y": 4.963801416536227, + "heading": -2.2881483630208987, + "angularVelocity": 1.8051561839419887, + "velocityX": 1.8201270470762463, + "velocityY": -2.5523842975203284, + "timestamp": 0.4015589243699722 + }, + { + "x": 3.9654177804635915, + "y": 4.822444228754285, + "heading": -2.245465067240904, + "angularVelocity": 0.8503518301223242, + "velocityX": 2.0390041237602423, + "velocityY": -2.8161682722649046, + "timestamp": 0.45175378991621873 + }, + { + "x": 4.072604442672708, + "y": 4.665705551178546, + "heading": -2.2454649759091194, + "angularVelocity": 0.0000018195443548764102, + "velocityX": 2.1354108840148447, + "velocityY": -3.1226037936356046, + "timestamp": 0.5019486554624653 + }, + { + "x": 4.191866544117303, + "y": 4.517947153047455, + "heading": -2.245464878096673, + "angularVelocity": 0.000001948654414335729, + "velocityX": 2.375982087943118, + "velocityY": -2.9436954661220507, + "timestamp": 0.5521435210087118 + }, + { + "x": 4.326720805180037, + "y": 4.384267316197636, + "heading": -2.245464583471803, + "angularVelocity": 0.0000058696216512510084, + "velocityX": 2.686614648633487, + "velocityY": -2.6632173509191657, + "timestamp": 0.6023383865549583 + }, + { + "x": 4.4671387450953555, + "y": 4.269390431902348, + "heading": -2.217765688783718, + "angularVelocity": 0.5518272513862013, + "velocityX": 2.7974562415342206, + "velocityY": -2.288618229078576, + "timestamp": 0.6525332521012048 }, { "x": 4.60924768447876, "y": 4.173398017883301, "heading": -2.157308519530683, - "angularVelocity": 1.1365804239919575, - "velocityX": 2.84233658058051, - "velocityY": -1.9325776863421187, - "timestamp": 0.7023373314619712 - }, - { - "x": 4.757244359209621, - "y": 4.093623523820721, - "heading": -2.0655111962458994, - "angularVelocity": 1.767636272422969, - "velocityX": 2.8498030344597014, - "velocityY": -1.5361263735516977, - "timestamp": 0.7542695783370328 - }, - { - "x": 4.905332920228958, - "y": 4.0351168726459035, - "heading": -1.9470536057388323, - "angularVelocity": 2.281002606955405, - "velocityX": 2.8515723838332723, - "velocityY": -1.126595799245347, - "timestamp": 0.8062018252120944 - }, - { - "x": 5.055400824091604, - "y": 3.998698661500805, - "heading": -1.8151511335185468, - "angularVelocity": 2.539895347444063, - "velocityX": 2.889686329645598, - "velocityY": -0.7012639224471425, - "timestamp": 0.858134072087156 - }, - { - "x": 5.216908347866735, - "y": 3.980352876038938, - "heading": -1.7083842074089743, - "angularVelocity": 2.0558888269639497, - "velocityX": 3.109966032543999, - "velocityY": -0.3532638498388685, - "timestamp": 0.9100663189622176 - }, - { - "x": 5.392426431970856, - "y": 3.975108601121907, - "heading": -1.641860902012389, - "angularVelocity": 1.2809633589824587, - "velocityX": 3.3797514004426206, - "velocityY": -0.1009830159986892, - "timestamp": 0.9619985658372792 - }, - { - "x": 5.582811407311731, - "y": 3.979892618198808, - "heading": -1.6227004666535643, - "angularVelocity": 0.3689506330223126, - "velocityX": 3.6660261551729536, - "velocityY": 0.0921203561326904, - "timestamp": 1.0139308127123408 - }, - { - "x": 5.778669831046716, - "y": 3.995210464766022, - "heading": -1.6227001197359128, - "angularVelocity": 0.000006680197224880823, - "velocityX": 3.7714221032296127, - "velocityY": 0.29495828678596464, - "timestamp": 1.0658630595874024 - }, - { - "x": 5.974528240815056, - "y": 4.010528489936178, - "heading": -1.6226997728238337, - "angularVelocity": 0.000006680089923071776, - "velocityX": 3.7714218342899026, - "velocityY": 0.2949617259389604, - "timestamp": 1.117795306462464 - }, - { - "x": 6.1703866505831755, - "y": 4.025846515108401, - "heading": -1.6226994259114784, - "angularVelocity": 0.0000066800952405189, - "velocityX": 3.7714218342856256, - "velocityY": 0.2949617259787309, - "timestamp": 1.1697275533375255 - }, - { - "x": 6.366245060351238, - "y": 4.0411645402805725, - "heading": -1.6226990789988471, - "angularVelocity": 0.000006680100558868112, - "velocityX": 3.7714218342845336, - "velocityY": 0.2949617259777772, - "timestamp": 1.2216598002125871 - }, - { - "x": 6.562103470119243, - "y": 4.056482565452695, - "heading": -1.6226987320859396, - "angularVelocity": 0.000006680105877144824, - "velocityX": 3.7714218342834416, - "velocityY": 0.2949617259768231, - "timestamp": 1.2735920470876487 - }, - { - "x": 6.757961879887191, - "y": 4.071800590624768, - "heading": -1.6226983851727559, - "angularVelocity": 0.000006680111194481284, - "velocityX": 3.7714218342823496, - "velocityY": 0.29496172597586884, - "timestamp": 1.3255242939627103 - }, - { - "x": 6.953820289655174, - "y": 4.087118615795626, - "heading": -1.622698038259296, - "angularVelocity": 0.000006680116513213821, - "velocityX": 3.7714218342830135, - "velocityY": 0.2949617259524653, - "timestamp": 1.377456540837772 + "angularVelocity": 1.204449271755367, + "velocityX": 2.8311449355806007, + "velocityY": -1.9123950821345668, + "timestamp": 0.7027281176474514 + }, + { + "x": 4.722976104309171, + "y": 4.108925075993853, + "heading": -2.0891694778953918, + "angularVelocity": 1.6986564221053766, + "velocityX": 2.8351662436762037, + "velocityY": -1.6072632394614674, + "timestamp": 0.7428416101398513 + }, + { + "x": 4.836477408039613, + "y": 4.056947172457928, + "heading": -2.0033151883417575, + "angularVelocity": 2.1402845830464856, + "velocityX": 2.8295044055799075, + "velocityY": -1.2957710811586096, + "timestamp": 0.7829551026322512 + }, + { + "x": 4.949920984616205, + "y": 4.0177960509685695, + "heading": -1.903114928393686, + "angularVelocity": 2.4979191220274792, + "velocityX": 2.828065309897542, + "velocityY": -0.9760087954639355, + "timestamp": 0.8230685951246511 + }, + { + "x": 5.064065362215247, + "y": 3.991835755777838, + "heading": -1.794426344777262, + "angularVelocity": 2.7095268166195297, + "velocityX": 2.845535766317746, + "velocityY": -0.647171153088953, + "timestamp": 0.8631820876170511 + }, + { + "x": 5.180659510984554, + "y": 3.978933247316211, + "heading": -1.6868519677774367, + "angularVelocity": 2.681750461399157, + "velocityX": 2.906606768069287, + "velocityY": -0.32165008978141457, + "timestamp": 0.903295580109451 + }, + { + "x": 5.304982214386576, + "y": 3.975371793740181, + "heading": -1.599436041565328, + "angularVelocity": 2.179215041638947, + "velocityX": 3.0992739768439947, + "velocityY": -0.08878443024388993, + "timestamp": 0.9434090726018509 + }, + { + "x": 5.437286305059447, + "y": 3.9799465783185837, + "heading": -1.5362326476191075, + "angularVelocity": 1.5756143386965153, + "velocityX": 3.298244118183867, + "velocityY": 0.11404603025451941, + "timestamp": 0.9835225650942508 + }, + { + "x": 5.574519249175071, + "y": 3.9911087281656146, + "heading": -1.4896376312982285, + "angularVelocity": 1.1615796437995773, + "velocityX": 3.421116826006249, + "velocityY": 0.278264223668521, + "timestamp": 1.0236360575866508 + }, + { + "x": 5.710295358289056, + "y": 4.008878734245578, + "heading": -1.4406573828911584, + "angularVelocity": 1.2210417334354526, + "velocityX": 3.3847989959914093, + "velocityY": 0.44299324182080874, + "timestamp": 1.0637495500790508 + }, + { + "x": 5.839309895761849, + "y": 4.034487620002502, + "heading": -1.374832058502612, + "angularVelocity": 1.6409771450595625, + "velocityX": 3.216237965249132, + "velocityY": 0.6384107731775158, + "timestamp": 1.1038630425714508 + }, + { + "x": 5.9651423912202, + "y": 4.060367188933271, + "heading": -1.3030479833599253, + "angularVelocity": 1.789524438847772, + "velocityX": 3.1369119874613967, + "velocityY": 0.6451587065292861, + "timestamp": 1.1439765350638509 + }, + { + "x": 6.088325953926389, + "y": 4.085030282261382, + "heading": -1.226883005971526, + "angularVelocity": 1.8987371244932074, + "velocityX": 3.0708760332829894, + "velocityY": 0.6148328603594958, + "timestamp": 1.184090027556251 + }, + { + "x": 6.2080954642882755, + "y": 4.108236900279935, + "heading": -1.1444073412911668, + "angularVelocity": 2.056057938509982, + "velocityX": 2.9857662078309084, + "velocityY": 0.5785239972049276, + "timestamp": 1.224203520048651 + }, + { + "x": 6.31677879626179, + "y": 4.1302920762013215, + "heading": -1.0366273178562313, + "angularVelocity": 2.686877076468872, + "velocityX": 2.7093958970066208, + "velocityY": 0.5498193887147775, + "timestamp": 1.264317012541051 + }, + { + "x": 6.414746281726902, + "y": 4.147226598563455, + "heading": -0.9049619897221106, + "angularVelocity": 3.2823202357427825, + "velocityX": 2.4422576888231124, + "velocityY": 0.4221652444085389, + "timestamp": 1.304430505033451 + }, + { + "x": 6.503675902170543, + "y": 4.155946147636541, + "heading": -0.7520815391315856, + "angularVelocity": 3.8111977065943705, + "velocityX": 2.216950330627282, + "velocityY": 0.21737197464765756, + "timestamp": 1.344543997525851 + }, + { + "x": 6.586090114201575, + "y": 4.1540858277756465, + "heading": -0.580040497638246, + "angularVelocity": 4.288857209975799, + "velocityX": 2.0545259689528748, + "velocityY": -0.0463764121572527, + "timestamp": 1.3846574900182511 + }, + { + "x": 6.676170358790984, + "y": 4.141704344881373, + "heading": -0.41948306620614584, + "angularVelocity": 4.002579218514071, + "velocityX": 2.245634548187899, + "velocityY": -0.3086613038398522, + "timestamp": 1.4247709825106512 + }, + { + "x": 6.778119596986203, + "y": 4.12741918933359, + "heading": -0.2818617425003748, + "angularVelocity": 3.4307988448486575, + "velocityX": 2.54151986926928, + "velocityY": -0.3561184693775817, + "timestamp": 1.4648844750030512 + }, + { + "x": 6.891681505117521, + "y": 4.114615252299952, + "heading": -0.16923343203441635, + "angularVelocity": 2.80774132263097, + "velocityX": 2.831015228924134, + "velocityY": -0.3191927762476476, + "timestamp": 1.5049979674954512 + }, + { + "x": 7.015942193168223, + "y": 4.105678430574392, + "heading": -0.0831259035248278, + "angularVelocity": 2.1465976448174597, + "velocityX": 3.097727979538158, + "velocityY": -0.22278842280444786, + "timestamp": 1.5451114599878513 }, { "x": 7.149678707122803, "y": 4.102436542510986, - "heading": -1.6226976913440598, - "angularVelocity": 0.000006680150717856603, - "velocityX": 3.7714219825462947, - "velocityY": 0.29495983010733695, - "timestamp": 1.4293887877128335 - }, - { - "x": 7.416831851800406, - "y": 4.105946276559182, - "heading": -1.6226976909008946, - "angularVelocity": 6.274765917736025e-9, - "velocityX": 3.7826143141948623, - "velocityY": 0.04969423162037773, - "timestamp": 1.5000153762273085 - }, - { - "x": 7.683984997699984, - "y": 4.109455917591789, - "heading": -1.6226976904577277, - "angularVelocity": 6.274788999649594e-9, - "velocityX": 3.7826143314967657, - "velocityY": 0.049692914615112965, - "timestamp": 1.5706419647417835 - }, - { - "x": 7.933502359055495, - "y": 4.112733873648306, - "heading": -1.6145121736520152, - "angularVelocity": 0.11589851609546277, - "velocityX": 3.532909724280016, - "velocityY": 0.0464124931624786, - "timestamp": 1.6412685532562585 - }, - { - "x": 8.141433501977613, - "y": 4.115465503819871, - "heading": -1.60769089632527, - "angularVelocity": 0.09658228537184779, - "velocityX": 2.944091556673475, - "velocityY": 0.038677079397722, - "timestamp": 1.7118951417707335 - }, - { - "x": 8.307778419316605, - "y": 4.117650808004561, - "heading": -1.6022338692720377, - "angularVelocity": 0.07726590180855333, - "velocityX": 2.355273287834106, - "velocityY": 0.030941664189867397, - "timestamp": 1.7825217302852085 - }, - { - "x": 8.432537108690411, - "y": 4.119289786165083, - "heading": -1.5981410965811054, - "angularVelocity": 0.057949460352219886, - "velocityX": 1.7664549852671345, - "velocityY": 0.023206248453940526, - "timestamp": 1.8531483187996836 - }, - { - "x": 8.515709568908191, - "y": 4.120382438281781, - "heading": -1.5954125803793526, - "angularVelocity": 0.03863298878146346, - "velocityX": 1.1776366658391457, - "velocityY": 0.015470832439747922, - "timestamp": 1.9237749073141586 + "heading": -0.02423765959758657, + "angularVelocity": 1.4680408079251241, + "velocityX": 3.3339533818930946, + "velocityY": -0.0808178959740319, + "timestamp": 1.5852249524802513 + }, + { + "x": 7.416849021016848, + "y": 4.104072679647048, + "heading": -0.02423765672113594, + "angularVelocity": 4.072771835592131e-8, + "velocityX": 3.7828694770841707, + "velocityY": 0.023166096345521754, + "timestamp": 1.655851315750913 + }, + { + "x": 7.683995167692338, + "y": 4.108021145887048, + "heading": -0.024237655875962986, + "angularVelocity": 1.1966819636377302e-8, + "velocityX": 3.7825272929841307, + "velocityY": 0.05590640742563016, + "timestamp": 1.7264776790215746 + }, + { + "x": 7.933509607434154, + "y": 4.111709036478853, + "heading": -0.01801171480387418, + "angularVelocity": 0.08815321621798794, + "velocityX": 3.5328796243635003, + "velocityY": 0.05221691194365583, + "timestamp": 1.7971040422922362 + }, + { + "x": 8.141438328466565, + "y": 4.114782278956212, + "heading": -0.012823406858938095, + "angularVelocity": 0.07346134934136293, + "velocityX": 2.9440666544809257, + "velocityY": 0.04351409778216477, + "timestamp": 1.8677304055628978 + }, + { + "x": 8.307781312832681, + "y": 4.11724087305162, + "heading": -0.008672751470206637, + "angularVelocity": 0.05876920736842818, + "velocityX": 2.3552534303463926, + "velocityY": 0.034811279833078496, + "timestamp": 1.9383567688335595 + }, + { + "x": 8.432538554548055, + "y": 4.119084818675069, + "heading": -0.00555975571941177, + "angularVelocity": 0.044076965125117926, + "velocityX": 1.7664401214779493, + "velocityY": 0.02610846060956428, + "timestamp": 2.008983132104221 + }, + { + "x": 8.515710050620672, + "y": 4.120314115781301, + "heading": -0.0034844232543666874, + "angularVelocity": 0.029384671232352395, + "velocityX": 1.1776267702455339, + "velocityY": 0.01740564074522844, + "timestamp": 2.079609495374883 }, { "x": 8.557295799255371, "y": 4.120928764343262, - "heading": -1.5940483219094344, - "angularVelocity": 0.019316499615988192, - "velocityX": 0.5888183362935331, - "velocityY": 0.007735416264214447, - "timestamp": 1.9944014958286336 + "heading": -0.002446756286149136, + "angularVelocity": 0.014692346032895615, + "velocityX": 0.5888133935953765, + "velocityY": 0.008702820497862328, + "timestamp": 2.150235858645545 }, { "x": 8.557295799255371, "y": 4.120928764343262, - "heading": -1.5940483219094344, - "angularVelocity": 6.715491935087819e-29, - "velocityX": 4.935316924694466e-27, - "velocityY": -9.408823776589858e-28, - "timestamp": 2.065028084343109 + "heading": -0.002446756286149136, + "angularVelocity": -1.1375698231513232e-28, + "velocityX": -3.986760367825934e-26, + "velocityY": 7.675786204695289e-26, + "timestamp": 2.2208622219162066 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Mid 3 4.1.traj b/src/main/deploy/choreo/Mid 3 4.1.traj index a24045e..54ff021 100644 --- a/src/main/deploy/choreo/Mid 3 4.1.traj +++ b/src/main/deploy/choreo/Mid 3 4.1.traj @@ -3,281 +3,353 @@ { "x": 8.270874977111816, "y": 3.5915117263793945, - "heading": -1.5707963267948966, - "angularVelocity": 5.354414694519943e-27, - "velocityX": 1.1934033992121382e-26, - "velocityY": 2.4414828428621674e-26, + "heading": -0.04647811321922418, + "angularVelocity": 1.3316191465946648e-27, + "velocityX": 8.288424720789902e-27, + "velocityY": -2.121860390602647e-27, "timestamp": 0 }, { - "x": 8.23848292883025, - "y": 3.5938924779782577, - "heading": -1.6238948429612086, - "angularVelocity": -0.8155689384136132, - "velocityX": -0.4975270560724381, - "velocityY": 0.0365672563811382, - "timestamp": 0.06510610405246105 - }, - { - "x": 8.17314182032488, - "y": 3.5988858589902613, - "heading": -1.7240953242981718, - "angularVelocity": -1.539033594395756, - "velocityX": -1.0036095609824784, - "velocityY": 0.07669605000446453, - "timestamp": 0.1302122081049221 - }, - { - "x": 8.074106083315115, - "y": 3.6065972666192967, - "heading": -1.8622827836674782, - "angularVelocity": -2.1224962141484913, - "velocityX": -1.5211436539032237, - "velocityY": 0.11844369650535398, - "timestamp": 0.19531831215738316 - }, - { - "x": 7.940524553703249, - "y": 3.6169600723778745, - "heading": -2.0255788872462053, - "angularVelocity": -2.508153512720506, - "velocityX": -2.0517512383205907, - "velocityY": 0.159167959892478, - "timestamp": 0.2604244162098442 - }, - { - "x": 7.771710550914322, - "y": 3.62995093841072, - "heading": -2.1923426679447573, - "angularVelocity": -2.561415448299248, - "velocityX": -2.5929059225061466, - "velocityY": 0.1995337644896925, - "timestamp": 0.3255305202623052 - }, - { - "x": 7.571402537694921, - "y": 3.6467516463163245, - "heading": -2.304001906692002, - "angularVelocity": -1.7150348707284366, - "velocityX": -3.0766395276546734, - "velocityY": 0.25805119427921946, - "timestamp": 0.39063662431476626 - }, - { - "x": 7.342572310447652, - "y": 3.66977975125525, - "heading": -2.3441463215039384, - "angularVelocity": -0.6165998625810742, - "velocityX": -3.5147276983872913, - "velocityY": 0.35370116633564747, - "timestamp": 0.4557427283672273 - }, - { - "x": 7.096978065151491, - "y": 3.688312829536684, - "heading": -2.3441463518162298, - "angularVelocity": -4.65582937531096e-7, - "velocityX": -3.772215353237325, - "velocityY": 0.28465961143213553, - "timestamp": 0.5208488324196884 - }, - { - "x": 6.85138381589539, - "y": 3.7068458553425065, - "heading": -2.3441463821284625, - "angularVelocity": -4.6558204127672377e-7, - "velocityX": -3.7722154140602266, - "velocityY": 0.28465880543073424, - "timestamp": 0.5859549364721495 - }, - { - "x": 6.605789566639263, - "y": 3.7253788811480093, - "heading": -2.344146412440695, - "angularVelocity": -4.6558204038728883e-7, - "velocityX": -3.772215414060598, - "velocityY": 0.28465880542582156, - "timestamp": 0.6510610405246106 - }, - { - "x": 6.360195317383137, - "y": 3.743911906953512, - "heading": -2.344146442752928, - "angularVelocity": -4.655820403555088e-7, - "velocityX": -3.7722154140605975, - "velocityY": 0.2846588054258209, - "timestamp": 0.7161671445770716 - }, - { - "x": 6.11460106812701, - "y": 3.762444932759015, - "heading": -2.3441464730651607, - "angularVelocity": -4.6558203882329284e-7, - "velocityX": -3.7722154140605975, - "velocityY": 0.2846588054258197, - "timestamp": 0.7812732486295327 - }, - { - "x": 5.869006818870883, - "y": 3.780977958564517, - "heading": -2.3441465033773934, - "angularVelocity": -4.655820376871189e-7, - "velocityX": -3.772215414060598, - "velocityY": 0.28465880542581967, - "timestamp": 0.8463793526819938 - }, - { - "x": 5.623412569614877, - "y": 3.799510984371621, - "heading": -2.344146533689626, - "angularVelocity": -4.65582035307202e-7, - "velocityX": -3.7722154140587425, - "velocityY": 0.2846588054504081, - "timestamp": 0.9114854567344549 - }, - { - "x": 5.3778183401842155, - "y": 3.818044272896504, - "heading": -2.344146564001866, - "angularVelocity": -4.6558214602936475e-7, - "velocityX": -3.7722151095505105, - "velocityY": 0.2846628406754266, - "timestamp": 0.976591560786916 - }, - { - "x": 5.135150406557795, - "y": 3.860142753669275, - "heading": -2.3441465956568845, - "angularVelocity": -4.862066176822936e-7, - "velocityX": -3.7272685435283313, - "velocityY": 0.6466134225885977, - "timestamp": 1.041697664839377 - }, - { - "x": 4.901006760007509, - "y": 3.936541444082641, - "heading": -2.3441466329967953, - "angularVelocity": -5.735239590740333e-7, - "velocityX": -3.5963393902608347, - "velocityY": 1.1734489649665605, - "timestamp": 1.106803768891838 + "x": 8.261714565981372, + "y": 3.5923814975091863, + "heading": -0.13089503027674879, + "angularVelocity": -1.710158870612933, + "velocityX": -0.18557605393852974, + "velocityY": 0.01762024561976423, + "timestamp": 0.04936203209428664 + }, + { + "x": 8.243251196384476, + "y": 3.595057930838583, + "heading": -0.2984548440137123, + "angularVelocity": -3.3945080181647858, + "velocityX": -0.3740399009835839, + "velocityY": 0.054220485175413155, + "timestamp": 0.09872406418857328 + }, + { + "x": 8.215818750608202, + "y": 3.6007267499258475, + "heading": -0.5482060630272272, + "angularVelocity": -5.05958139114824, + "velocityX": -0.5557397986345999, + "velocityY": 0.11484168796853474, + "timestamp": 0.14808609628285993 + }, + { + "x": 8.172797656458112, + "y": 3.6028495640275278, + "heading": -0.8441726532098716, + "angularVelocity": -5.995834807151321, + "velocityX": -0.8715422020697032, + "velocityY": 0.04300499820642603, + "timestamp": 0.19744812837714656 + }, + { + "x": 8.109804374431524, + "y": 3.604125050259272, + "heading": -1.1531707681026144, + "angularVelocity": -6.259833758515535, + "velocityX": -1.2761484759432886, + "velocityY": 0.025839419035829633, + "timestamp": 0.2468101604714332 + }, + { + "x": 8.028058626769713, + "y": 3.605873396143341, + "heading": -1.4327138691793657, + "angularVelocity": -5.66311979504399, + "velocityX": -1.6560450247604885, + "velocityY": 0.035418839336477916, + "timestamp": 0.29617219256571986 + }, + { + "x": 7.929944698522907, + "y": 3.617190837314496, + "heading": -1.6856390841183952, + "angularVelocity": -5.123881740847214, + "velocityX": -1.9876395700120058, + "velocityY": 0.22927421524172362, + "timestamp": 0.3455342246600065 + }, + { + "x": 7.81789322279875, + "y": 3.6363759065331616, + "heading": -1.8880322520166901, + "angularVelocity": -4.100179010290798, + "velocityX": -2.2699931702594154, + "velocityY": 0.38866044213942363, + "timestamp": 0.39489625675429313 + }, + { + "x": 7.690304977256709, + "y": 3.6594593247549176, + "heading": -2.0402246489251294, + "angularVelocity": -3.083187430730889, + "velocityX": -2.584744592733444, + "velocityY": 0.4676350879895761, + "timestamp": 0.44425828884857976 + }, + { + "x": 7.546813675021453, + "y": 3.6826342543625548, + "heading": -2.1470758213139596, + "angularVelocity": -2.1646429017495277, + "velocityX": -2.9069164324753056, + "velocityY": 0.46948897005234025, + "timestamp": 0.4936203209428664 + }, + { + "x": 7.387459875875891, + "y": 3.7001461214884253, + "heading": -2.213536834034917, + "angularVelocity": -1.3463994471299328, + "velocityX": -3.228266592452671, + "velocityY": 0.3547639021914809, + "timestamp": 0.542982353037153 + }, + { + "x": 7.21184251301541, + "y": 3.7140792269378826, + "heading": -2.2398648640063543, + "angularVelocity": -0.5333660073221452, + "velocityX": -3.5577417583829254, + "velocityY": 0.28226361149077894, + "timestamp": 0.5923443851314397 + }, + { + "x": 7.025484740409283, + "y": 3.725907654379097, + "heading": -2.2398670079946887, + "angularVelocity": -0.0000434339560853095, + "velocityX": -3.775326190991562, + "velocityY": 0.23962602306608316, + "timestamp": 0.6417064172257264 + }, + { + "x": 6.83912695720996, + "y": 3.7377359168228743, + "heading": -2.2398691516824063, + "angularVelocity": -0.0000434278660421668, + "velocityX": -3.775326405593678, + "velocityY": 0.2396226804679459, + "timestamp": 0.6910684493200131 + }, + { + "x": 6.652769174009924, + "y": 3.749564179259098, + "heading": -2.2398712953687108, + "angularVelocity": -0.00004342783741540177, + "velocityX": -3.775326405608112, + "velocityY": 0.2396226803149126, + "timestamp": 0.7404304814142998 + }, + { + "x": 6.46641139080954, + "y": 3.761392441693503, + "heading": -2.239873439053602, + "angularVelocity": -0.00004342780879696299, + "velocityX": -3.775326405615169, + "velocityY": 0.23962268027806155, + "timestamp": 0.7897925135085865 + }, + { + "x": 6.280053607608808, + "y": 3.773220704126089, + "heading": -2.239875582737081, + "angularVelocity": -0.00004342778017790842, + "velocityX": -3.7753264056222258, + "velocityY": 0.2396226802412151, + "timestamp": 0.8391545456028732 + }, + { + "x": 6.093695824407728, + "y": 3.7850489665568556, + "heading": -2.239877726419147, + "angularVelocity": -0.000043427751560344506, + "velocityX": -3.775326405629282, + "velocityY": 0.23962268020436814, + "timestamp": 0.8885165776971599 + }, + { + "x": 5.907338041206299, + "y": 3.796877228985804, + "heading": -2.2398798700998004, + "angularVelocity": -0.00004342772294180255, + "velocityX": -3.7753264056363385, + "velocityY": 0.23962268016752136, + "timestamp": 0.9378786097914465 + }, + { + "x": 5.720980258004522, + "y": 3.8087054914129355, + "heading": -2.2398820137790416, + "angularVelocity": -0.000043427694325368904, + "velocityX": -3.7753264056433924, + "velocityY": 0.2396226801307148, + "timestamp": 0.9872406418857332 + }, + { + "x": 5.534622474806013, + "y": 3.820533753895205, + "heading": -2.2398841574568737, + "angularVelocity": -0.000043427665782203, + "velocityX": -3.775326405577184, + "velocityY": 0.23962268124772898, + "timestamp": 1.0366026739800198 + }, + { + "x": 5.348264795875393, + "y": 3.8323636550337796, + "heading": -2.2398863017677915, + "angularVelocity": -0.0000434404911395521, + "velocityX": -3.7753242932677007, + "velocityY": 0.23965587794235257, + "timestamp": 1.0859647060743065 + }, + { + "x": 5.171853769012511, + "y": 3.8569512152531384, + "heading": -2.2615346019691334, + "angularVelocity": -0.4385617707146157, + "velocityX": -3.573820188883595, + "velocityY": 0.49810672648957277, + "timestamp": 1.1353267381685932 + }, + { + "x": 5.002868859035653, + "y": 3.900247608117125, + "heading": -2.2930504927284256, + "angularVelocity": -0.6384642086673774, + "velocityX": -3.4233783093467514, + "velocityY": 0.8771193370095877, + "timestamp": 1.1846887702628799 + }, + { + "x": 4.839142910274149, + "y": 3.9631633228826875, + "heading": -2.323008869125164, + "angularVelocity": -0.6069113268982622, + "velocityX": -3.316839721038412, + "velocityY": 1.2745770807284975, + "timestamp": 1.2340508023571666 }, { "x": 4.680209159851074, "y": 4.0456671714782715, "heading": -2.3441466852834205, - "angularVelocity": -8.030986646480626e-7, - "velocityX": -3.3913502177694665, - "velocityY": 1.6761212943674095, - "timestamp": 1.171909872944299 - }, - { - "x": 4.472372375309597, - "y": 4.189862041622909, - "heading": -2.3441466910141098, - "angularVelocity": -8.570105231495362e-8, - "velocityX": -3.108148039650104, - "velocityY": 2.1563988490122856, - "timestamp": 1.238778240181039 - }, - { - "x": 4.287987685649001, - "y": 4.363039917097588, - "heading": -2.3441466957763613, - "angularVelocity": -7.121830279054471e-8, - "velocityX": -2.7574277237516953, - "velocityY": 2.5898325715291013, - "timestamp": 1.305646607417779 - }, - { - "x": 4.131060347221029, - "y": 4.561438760621924, - "heading": -2.3441467001447087, - "angularVelocity": -6.532756518877877e-8, - "velocityX": -2.3468097833522306, - "velocityY": 2.9670059509891678, - "timestamp": 1.372514974654519 - }, - { - "x": 3.9877899693707293, - "y": 4.769913913918279, - "heading": -2.3441467044472297, - "angularVelocity": -6.434314457198755e-8, - "velocityX": -2.1425732939323447, - "velocityY": 3.117694687508523, - "timestamp": 1.4393833418912592 - }, - { - "x": 3.8445196551450387, - "y": 4.978389110939239, - "heading": -2.3441467087499444, - "angularVelocity": -6.434603866942448e-8, - "velocityX": -2.14257234244195, - "velocityY": 3.117695341399279, - "timestamp": 1.5062517091279992 - }, - { - "x": 3.7183406970943045, - "y": 5.161996657346525, - "heading": -2.4164805017111153, - "angularVelocity": -1.08173409865207, - "velocityX": -1.8869753108224054, - "velocityY": 2.7458057373712084, - "timestamp": 1.5731200763647393 - }, - { - "x": 3.61319236178323, - "y": 5.315003459544194, - "heading": -2.4767605080431827, - "angularVelocity": -0.9014726816740256, - "velocityX": -1.5724675157508774, - "velocityY": 2.2881791274484944, - "timestamp": 1.6399884436014793 - }, - { - "x": 3.5290741458237123, - "y": 5.437409251753405, - "heading": -2.5249829334569385, - "angularVelocity": -0.721154522033258, - "velocityX": -1.257967248724744, - "velocityY": 1.8305485428685142, - "timestamp": 1.7068568108382194 - }, - { - "x": 3.465985698821635, - "y": 5.529213783971836, - "heading": -2.561148047037766, - "angularVelocity": -0.5408403864982843, - "velocityX": -0.9434722217565042, - "velocityY": 1.3729142195652448, - "timestamp": 1.7737251780749594 - }, - { - "x": 3.423926804037115, - "y": 5.590416869126759, - "heading": -2.5852574378563697, - "angularVelocity": -0.36055001512518836, - "velocityX": -0.628980436079966, - "velocityY": 0.915277098635298, - "timestamp": 1.8405935453116995 + "angularVelocity": -0.42822013724802277, + "velocityX": -3.2197570415961705, + "velocityY": 1.6714030013592756, + "timestamp": 1.2834128344514533 + }, + { + "x": 4.474436093505845, + "y": 4.192482234656094, + "heading": -2.344146743629121, + "angularVelocity": -8.731670658673155e-7, + "velocityX": -3.0794773957732224, + "velocityY": 2.1971469660494822, + "timestamp": 1.35023360950493 + }, + { + "x": 4.292459307252108, + "y": 4.367929309366008, + "heading": -2.3441467553603754, + "angularVelocity": -1.7556298637551318e-7, + "velocityX": -2.723356412853638, + "velocityY": 2.6256366312647916, + "timestamp": 1.417054384558407 + }, + { + "x": 4.138225756164794, + "y": 4.5682022210646345, + "heading": -2.344146766260572, + "angularVelocity": -1.631258670814988e-7, + "velocityX": -2.3081676464225445, + "velocityY": 2.9971653507213616, + "timestamp": 1.4838751596118838 + }, + { + "x": 3.993481705313873, + "y": 4.7754373225031195, + "heading": -2.3441467770570332, + "angularVelocity": -1.6157341933246893e-7, + "velocityX": -2.1661534266114293, + "velocityY": 3.1013573439193687, + "timestamp": 1.5506959346653606 + }, + { + "x": 3.8487377220414505, + "y": 4.982672471141195, + "heading": -2.344146787854829, + "angularVelocity": -1.615933934586182e-7, + "velocityX": -2.1661524152717977, + "velocityY": 3.1013580502803935, + "timestamp": 1.6175167097188374 + }, + { + "x": 3.7213536241170644, + "y": 5.165056170282478, + "heading": -2.4164814183686705, + "angularVelocity": -1.082517083286617, + "velocityX": -1.9063546901759274, + "velocityY": 2.7294460292524283, + "timestamp": 1.6843374847723143 + }, + { + "x": 3.6152009914473524, + "y": 5.317043121214145, + "heading": -2.476761601610272, + "angularVelocity": -0.9021173907869166, + "velocityX": -1.5886172015328703, + "velocityY": 2.274546363912, + "timestamp": 1.751158259825791 + }, + { + "x": 3.530279328218681, + "y": 5.43863304450566, + "heading": -2.5249837389973546, + "angularVelocity": -0.7216638440438612, + "velocityX": -1.2708871329419387, + "velocityY": 1.8196425167802273, + "timestamp": 1.817979034879268 + }, + { + "x": 3.4665882908564125, + "y": 5.52982568005271, + "heading": -2.561148443081316, + "angularVelocity": -0.5412194643809262, + "velocityX": -0.953162206084802, + "velocityY": 1.3647347770819482, + "timestamp": 1.8847998099327448 + }, + { + "x": 3.4241276677886727, + "y": 5.590620834962734, + "heading": -2.585257539099172, + "angularVelocity": -0.3608024001302229, + "velocityX": -0.6354404454865786, + "velocityY": 0.9098241506682598, + "timestamp": 1.9516205849862216 }, { "x": 3.402897357940674, "y": 5.621018409729004, "heading": -2.59731223675972, - "angularVelocity": -0.18027655529066144, - "velocityX": -0.31449019866133116, - "velocityY": 0.45763851977875486, - "timestamp": 1.9074619125484396 + "angularVelocity": -0.18040343966229697, + "velocityX": -0.3177201975135489, + "velocityY": 0.45491203509600503, + "timestamp": 2.0184413600396987 }, { "x": 3.402897357940674, "y": 5.621018409729004, "heading": -2.59731223675972, - "angularVelocity": 2.7558604882702093e-25, - "velocityX": -2.598680883455403e-25, - "velocityY": -4.201313047715582e-25, - "timestamp": 1.9743302797851796 + "angularVelocity": 3.854629324739845e-28, + "velocityX": 2.481722314351389e-27, + "velocityY": -5.327025844101886e-28, + "timestamp": 2.0852621350931755 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Mid 3 4.traj b/src/main/deploy/choreo/Mid 3 4.traj index a24045e..54ff021 100644 --- a/src/main/deploy/choreo/Mid 3 4.traj +++ b/src/main/deploy/choreo/Mid 3 4.traj @@ -3,281 +3,353 @@ { "x": 8.270874977111816, "y": 3.5915117263793945, - "heading": -1.5707963267948966, - "angularVelocity": 5.354414694519943e-27, - "velocityX": 1.1934033992121382e-26, - "velocityY": 2.4414828428621674e-26, + "heading": -0.04647811321922418, + "angularVelocity": 1.3316191465946648e-27, + "velocityX": 8.288424720789902e-27, + "velocityY": -2.121860390602647e-27, "timestamp": 0 }, { - "x": 8.23848292883025, - "y": 3.5938924779782577, - "heading": -1.6238948429612086, - "angularVelocity": -0.8155689384136132, - "velocityX": -0.4975270560724381, - "velocityY": 0.0365672563811382, - "timestamp": 0.06510610405246105 - }, - { - "x": 8.17314182032488, - "y": 3.5988858589902613, - "heading": -1.7240953242981718, - "angularVelocity": -1.539033594395756, - "velocityX": -1.0036095609824784, - "velocityY": 0.07669605000446453, - "timestamp": 0.1302122081049221 - }, - { - "x": 8.074106083315115, - "y": 3.6065972666192967, - "heading": -1.8622827836674782, - "angularVelocity": -2.1224962141484913, - "velocityX": -1.5211436539032237, - "velocityY": 0.11844369650535398, - "timestamp": 0.19531831215738316 - }, - { - "x": 7.940524553703249, - "y": 3.6169600723778745, - "heading": -2.0255788872462053, - "angularVelocity": -2.508153512720506, - "velocityX": -2.0517512383205907, - "velocityY": 0.159167959892478, - "timestamp": 0.2604244162098442 - }, - { - "x": 7.771710550914322, - "y": 3.62995093841072, - "heading": -2.1923426679447573, - "angularVelocity": -2.561415448299248, - "velocityX": -2.5929059225061466, - "velocityY": 0.1995337644896925, - "timestamp": 0.3255305202623052 - }, - { - "x": 7.571402537694921, - "y": 3.6467516463163245, - "heading": -2.304001906692002, - "angularVelocity": -1.7150348707284366, - "velocityX": -3.0766395276546734, - "velocityY": 0.25805119427921946, - "timestamp": 0.39063662431476626 - }, - { - "x": 7.342572310447652, - "y": 3.66977975125525, - "heading": -2.3441463215039384, - "angularVelocity": -0.6165998625810742, - "velocityX": -3.5147276983872913, - "velocityY": 0.35370116633564747, - "timestamp": 0.4557427283672273 - }, - { - "x": 7.096978065151491, - "y": 3.688312829536684, - "heading": -2.3441463518162298, - "angularVelocity": -4.65582937531096e-7, - "velocityX": -3.772215353237325, - "velocityY": 0.28465961143213553, - "timestamp": 0.5208488324196884 - }, - { - "x": 6.85138381589539, - "y": 3.7068458553425065, - "heading": -2.3441463821284625, - "angularVelocity": -4.6558204127672377e-7, - "velocityX": -3.7722154140602266, - "velocityY": 0.28465880543073424, - "timestamp": 0.5859549364721495 - }, - { - "x": 6.605789566639263, - "y": 3.7253788811480093, - "heading": -2.344146412440695, - "angularVelocity": -4.6558204038728883e-7, - "velocityX": -3.772215414060598, - "velocityY": 0.28465880542582156, - "timestamp": 0.6510610405246106 - }, - { - "x": 6.360195317383137, - "y": 3.743911906953512, - "heading": -2.344146442752928, - "angularVelocity": -4.655820403555088e-7, - "velocityX": -3.7722154140605975, - "velocityY": 0.2846588054258209, - "timestamp": 0.7161671445770716 - }, - { - "x": 6.11460106812701, - "y": 3.762444932759015, - "heading": -2.3441464730651607, - "angularVelocity": -4.6558203882329284e-7, - "velocityX": -3.7722154140605975, - "velocityY": 0.2846588054258197, - "timestamp": 0.7812732486295327 - }, - { - "x": 5.869006818870883, - "y": 3.780977958564517, - "heading": -2.3441465033773934, - "angularVelocity": -4.655820376871189e-7, - "velocityX": -3.772215414060598, - "velocityY": 0.28465880542581967, - "timestamp": 0.8463793526819938 - }, - { - "x": 5.623412569614877, - "y": 3.799510984371621, - "heading": -2.344146533689626, - "angularVelocity": -4.65582035307202e-7, - "velocityX": -3.7722154140587425, - "velocityY": 0.2846588054504081, - "timestamp": 0.9114854567344549 - }, - { - "x": 5.3778183401842155, - "y": 3.818044272896504, - "heading": -2.344146564001866, - "angularVelocity": -4.6558214602936475e-7, - "velocityX": -3.7722151095505105, - "velocityY": 0.2846628406754266, - "timestamp": 0.976591560786916 - }, - { - "x": 5.135150406557795, - "y": 3.860142753669275, - "heading": -2.3441465956568845, - "angularVelocity": -4.862066176822936e-7, - "velocityX": -3.7272685435283313, - "velocityY": 0.6466134225885977, - "timestamp": 1.041697664839377 - }, - { - "x": 4.901006760007509, - "y": 3.936541444082641, - "heading": -2.3441466329967953, - "angularVelocity": -5.735239590740333e-7, - "velocityX": -3.5963393902608347, - "velocityY": 1.1734489649665605, - "timestamp": 1.106803768891838 + "x": 8.261714565981372, + "y": 3.5923814975091863, + "heading": -0.13089503027674879, + "angularVelocity": -1.710158870612933, + "velocityX": -0.18557605393852974, + "velocityY": 0.01762024561976423, + "timestamp": 0.04936203209428664 + }, + { + "x": 8.243251196384476, + "y": 3.595057930838583, + "heading": -0.2984548440137123, + "angularVelocity": -3.3945080181647858, + "velocityX": -0.3740399009835839, + "velocityY": 0.054220485175413155, + "timestamp": 0.09872406418857328 + }, + { + "x": 8.215818750608202, + "y": 3.6007267499258475, + "heading": -0.5482060630272272, + "angularVelocity": -5.05958139114824, + "velocityX": -0.5557397986345999, + "velocityY": 0.11484168796853474, + "timestamp": 0.14808609628285993 + }, + { + "x": 8.172797656458112, + "y": 3.6028495640275278, + "heading": -0.8441726532098716, + "angularVelocity": -5.995834807151321, + "velocityX": -0.8715422020697032, + "velocityY": 0.04300499820642603, + "timestamp": 0.19744812837714656 + }, + { + "x": 8.109804374431524, + "y": 3.604125050259272, + "heading": -1.1531707681026144, + "angularVelocity": -6.259833758515535, + "velocityX": -1.2761484759432886, + "velocityY": 0.025839419035829633, + "timestamp": 0.2468101604714332 + }, + { + "x": 8.028058626769713, + "y": 3.605873396143341, + "heading": -1.4327138691793657, + "angularVelocity": -5.66311979504399, + "velocityX": -1.6560450247604885, + "velocityY": 0.035418839336477916, + "timestamp": 0.29617219256571986 + }, + { + "x": 7.929944698522907, + "y": 3.617190837314496, + "heading": -1.6856390841183952, + "angularVelocity": -5.123881740847214, + "velocityX": -1.9876395700120058, + "velocityY": 0.22927421524172362, + "timestamp": 0.3455342246600065 + }, + { + "x": 7.81789322279875, + "y": 3.6363759065331616, + "heading": -1.8880322520166901, + "angularVelocity": -4.100179010290798, + "velocityX": -2.2699931702594154, + "velocityY": 0.38866044213942363, + "timestamp": 0.39489625675429313 + }, + { + "x": 7.690304977256709, + "y": 3.6594593247549176, + "heading": -2.0402246489251294, + "angularVelocity": -3.083187430730889, + "velocityX": -2.584744592733444, + "velocityY": 0.4676350879895761, + "timestamp": 0.44425828884857976 + }, + { + "x": 7.546813675021453, + "y": 3.6826342543625548, + "heading": -2.1470758213139596, + "angularVelocity": -2.1646429017495277, + "velocityX": -2.9069164324753056, + "velocityY": 0.46948897005234025, + "timestamp": 0.4936203209428664 + }, + { + "x": 7.387459875875891, + "y": 3.7001461214884253, + "heading": -2.213536834034917, + "angularVelocity": -1.3463994471299328, + "velocityX": -3.228266592452671, + "velocityY": 0.3547639021914809, + "timestamp": 0.542982353037153 + }, + { + "x": 7.21184251301541, + "y": 3.7140792269378826, + "heading": -2.2398648640063543, + "angularVelocity": -0.5333660073221452, + "velocityX": -3.5577417583829254, + "velocityY": 0.28226361149077894, + "timestamp": 0.5923443851314397 + }, + { + "x": 7.025484740409283, + "y": 3.725907654379097, + "heading": -2.2398670079946887, + "angularVelocity": -0.0000434339560853095, + "velocityX": -3.775326190991562, + "velocityY": 0.23962602306608316, + "timestamp": 0.6417064172257264 + }, + { + "x": 6.83912695720996, + "y": 3.7377359168228743, + "heading": -2.2398691516824063, + "angularVelocity": -0.0000434278660421668, + "velocityX": -3.775326405593678, + "velocityY": 0.2396226804679459, + "timestamp": 0.6910684493200131 + }, + { + "x": 6.652769174009924, + "y": 3.749564179259098, + "heading": -2.2398712953687108, + "angularVelocity": -0.00004342783741540177, + "velocityX": -3.775326405608112, + "velocityY": 0.2396226803149126, + "timestamp": 0.7404304814142998 + }, + { + "x": 6.46641139080954, + "y": 3.761392441693503, + "heading": -2.239873439053602, + "angularVelocity": -0.00004342780879696299, + "velocityX": -3.775326405615169, + "velocityY": 0.23962268027806155, + "timestamp": 0.7897925135085865 + }, + { + "x": 6.280053607608808, + "y": 3.773220704126089, + "heading": -2.239875582737081, + "angularVelocity": -0.00004342778017790842, + "velocityX": -3.7753264056222258, + "velocityY": 0.2396226802412151, + "timestamp": 0.8391545456028732 + }, + { + "x": 6.093695824407728, + "y": 3.7850489665568556, + "heading": -2.239877726419147, + "angularVelocity": -0.000043427751560344506, + "velocityX": -3.775326405629282, + "velocityY": 0.23962268020436814, + "timestamp": 0.8885165776971599 + }, + { + "x": 5.907338041206299, + "y": 3.796877228985804, + "heading": -2.2398798700998004, + "angularVelocity": -0.00004342772294180255, + "velocityX": -3.7753264056363385, + "velocityY": 0.23962268016752136, + "timestamp": 0.9378786097914465 + }, + { + "x": 5.720980258004522, + "y": 3.8087054914129355, + "heading": -2.2398820137790416, + "angularVelocity": -0.000043427694325368904, + "velocityX": -3.7753264056433924, + "velocityY": 0.2396226801307148, + "timestamp": 0.9872406418857332 + }, + { + "x": 5.534622474806013, + "y": 3.820533753895205, + "heading": -2.2398841574568737, + "angularVelocity": -0.000043427665782203, + "velocityX": -3.775326405577184, + "velocityY": 0.23962268124772898, + "timestamp": 1.0366026739800198 + }, + { + "x": 5.348264795875393, + "y": 3.8323636550337796, + "heading": -2.2398863017677915, + "angularVelocity": -0.0000434404911395521, + "velocityX": -3.7753242932677007, + "velocityY": 0.23965587794235257, + "timestamp": 1.0859647060743065 + }, + { + "x": 5.171853769012511, + "y": 3.8569512152531384, + "heading": -2.2615346019691334, + "angularVelocity": -0.4385617707146157, + "velocityX": -3.573820188883595, + "velocityY": 0.49810672648957277, + "timestamp": 1.1353267381685932 + }, + { + "x": 5.002868859035653, + "y": 3.900247608117125, + "heading": -2.2930504927284256, + "angularVelocity": -0.6384642086673774, + "velocityX": -3.4233783093467514, + "velocityY": 0.8771193370095877, + "timestamp": 1.1846887702628799 + }, + { + "x": 4.839142910274149, + "y": 3.9631633228826875, + "heading": -2.323008869125164, + "angularVelocity": -0.6069113268982622, + "velocityX": -3.316839721038412, + "velocityY": 1.2745770807284975, + "timestamp": 1.2340508023571666 }, { "x": 4.680209159851074, "y": 4.0456671714782715, "heading": -2.3441466852834205, - "angularVelocity": -8.030986646480626e-7, - "velocityX": -3.3913502177694665, - "velocityY": 1.6761212943674095, - "timestamp": 1.171909872944299 - }, - { - "x": 4.472372375309597, - "y": 4.189862041622909, - "heading": -2.3441466910141098, - "angularVelocity": -8.570105231495362e-8, - "velocityX": -3.108148039650104, - "velocityY": 2.1563988490122856, - "timestamp": 1.238778240181039 - }, - { - "x": 4.287987685649001, - "y": 4.363039917097588, - "heading": -2.3441466957763613, - "angularVelocity": -7.121830279054471e-8, - "velocityX": -2.7574277237516953, - "velocityY": 2.5898325715291013, - "timestamp": 1.305646607417779 - }, - { - "x": 4.131060347221029, - "y": 4.561438760621924, - "heading": -2.3441467001447087, - "angularVelocity": -6.532756518877877e-8, - "velocityX": -2.3468097833522306, - "velocityY": 2.9670059509891678, - "timestamp": 1.372514974654519 - }, - { - "x": 3.9877899693707293, - "y": 4.769913913918279, - "heading": -2.3441467044472297, - "angularVelocity": -6.434314457198755e-8, - "velocityX": -2.1425732939323447, - "velocityY": 3.117694687508523, - "timestamp": 1.4393833418912592 - }, - { - "x": 3.8445196551450387, - "y": 4.978389110939239, - "heading": -2.3441467087499444, - "angularVelocity": -6.434603866942448e-8, - "velocityX": -2.14257234244195, - "velocityY": 3.117695341399279, - "timestamp": 1.5062517091279992 - }, - { - "x": 3.7183406970943045, - "y": 5.161996657346525, - "heading": -2.4164805017111153, - "angularVelocity": -1.08173409865207, - "velocityX": -1.8869753108224054, - "velocityY": 2.7458057373712084, - "timestamp": 1.5731200763647393 - }, - { - "x": 3.61319236178323, - "y": 5.315003459544194, - "heading": -2.4767605080431827, - "angularVelocity": -0.9014726816740256, - "velocityX": -1.5724675157508774, - "velocityY": 2.2881791274484944, - "timestamp": 1.6399884436014793 - }, - { - "x": 3.5290741458237123, - "y": 5.437409251753405, - "heading": -2.5249829334569385, - "angularVelocity": -0.721154522033258, - "velocityX": -1.257967248724744, - "velocityY": 1.8305485428685142, - "timestamp": 1.7068568108382194 - }, - { - "x": 3.465985698821635, - "y": 5.529213783971836, - "heading": -2.561148047037766, - "angularVelocity": -0.5408403864982843, - "velocityX": -0.9434722217565042, - "velocityY": 1.3729142195652448, - "timestamp": 1.7737251780749594 - }, - { - "x": 3.423926804037115, - "y": 5.590416869126759, - "heading": -2.5852574378563697, - "angularVelocity": -0.36055001512518836, - "velocityX": -0.628980436079966, - "velocityY": 0.915277098635298, - "timestamp": 1.8405935453116995 + "angularVelocity": -0.42822013724802277, + "velocityX": -3.2197570415961705, + "velocityY": 1.6714030013592756, + "timestamp": 1.2834128344514533 + }, + { + "x": 4.474436093505845, + "y": 4.192482234656094, + "heading": -2.344146743629121, + "angularVelocity": -8.731670658673155e-7, + "velocityX": -3.0794773957732224, + "velocityY": 2.1971469660494822, + "timestamp": 1.35023360950493 + }, + { + "x": 4.292459307252108, + "y": 4.367929309366008, + "heading": -2.3441467553603754, + "angularVelocity": -1.7556298637551318e-7, + "velocityX": -2.723356412853638, + "velocityY": 2.6256366312647916, + "timestamp": 1.417054384558407 + }, + { + "x": 4.138225756164794, + "y": 4.5682022210646345, + "heading": -2.344146766260572, + "angularVelocity": -1.631258670814988e-7, + "velocityX": -2.3081676464225445, + "velocityY": 2.9971653507213616, + "timestamp": 1.4838751596118838 + }, + { + "x": 3.993481705313873, + "y": 4.7754373225031195, + "heading": -2.3441467770570332, + "angularVelocity": -1.6157341933246893e-7, + "velocityX": -2.1661534266114293, + "velocityY": 3.1013573439193687, + "timestamp": 1.5506959346653606 + }, + { + "x": 3.8487377220414505, + "y": 4.982672471141195, + "heading": -2.344146787854829, + "angularVelocity": -1.615933934586182e-7, + "velocityX": -2.1661524152717977, + "velocityY": 3.1013580502803935, + "timestamp": 1.6175167097188374 + }, + { + "x": 3.7213536241170644, + "y": 5.165056170282478, + "heading": -2.4164814183686705, + "angularVelocity": -1.082517083286617, + "velocityX": -1.9063546901759274, + "velocityY": 2.7294460292524283, + "timestamp": 1.6843374847723143 + }, + { + "x": 3.6152009914473524, + "y": 5.317043121214145, + "heading": -2.476761601610272, + "angularVelocity": -0.9021173907869166, + "velocityX": -1.5886172015328703, + "velocityY": 2.274546363912, + "timestamp": 1.751158259825791 + }, + { + "x": 3.530279328218681, + "y": 5.43863304450566, + "heading": -2.5249837389973546, + "angularVelocity": -0.7216638440438612, + "velocityX": -1.2708871329419387, + "velocityY": 1.8196425167802273, + "timestamp": 1.817979034879268 + }, + { + "x": 3.4665882908564125, + "y": 5.52982568005271, + "heading": -2.561148443081316, + "angularVelocity": -0.5412194643809262, + "velocityX": -0.953162206084802, + "velocityY": 1.3647347770819482, + "timestamp": 1.8847998099327448 + }, + { + "x": 3.4241276677886727, + "y": 5.590620834962734, + "heading": -2.585257539099172, + "angularVelocity": -0.3608024001302229, + "velocityX": -0.6354404454865786, + "velocityY": 0.9098241506682598, + "timestamp": 1.9516205849862216 }, { "x": 3.402897357940674, "y": 5.621018409729004, "heading": -2.59731223675972, - "angularVelocity": -0.18027655529066144, - "velocityX": -0.31449019866133116, - "velocityY": 0.45763851977875486, - "timestamp": 1.9074619125484396 + "angularVelocity": -0.18040343966229697, + "velocityX": -0.3177201975135489, + "velocityY": 0.45491203509600503, + "timestamp": 2.0184413600396987 }, { "x": 3.402897357940674, "y": 5.621018409729004, "heading": -2.59731223675972, - "angularVelocity": 2.7558604882702093e-25, - "velocityX": -2.598680883455403e-25, - "velocityY": -4.201313047715582e-25, - "timestamp": 1.9743302797851796 + "angularVelocity": 3.854629324739845e-28, + "velocityX": 2.481722314351389e-27, + "velocityY": -5.327025844101886e-28, + "timestamp": 2.0852621350931755 } ], "eventMarkers": [] diff --git a/src/main/deploy/limelight/apriltag0.vpr b/src/main/deploy/limelight/apriltag0.vpr index f687db4..5039dbf 100644 --- a/src/main/deploy/limelight/apriltag0.vpr +++ b/src/main/deploy/limelight/apriltag0.vpr @@ -3,6 +3,7 @@ area_min:0.21367506249999998 area_similarity:0 aspect_max:20.000000 aspect_min:0.000000 +barcode_type:qrzx black_level:0 blue_balance:1855 botfloorsnap:1 @@ -26,6 +27,7 @@ cross_a_y:0 cross_b_a:1 cross_b_x:0 cross_b_y:0 +debugpipe:0 desc:Pipeline_Name desired_contour_region:0 detector_conf:0.800000 @@ -35,9 +37,10 @@ direction_filter:0 dual_close_sort_origin:0 erosion_steps:0 exposure:405 +fiducial_backend:umich fiducial_decoder_strictness:strict fiducial_denoise:0.000000 -fiducial_idfilters:1,2,4,5,6,7,9,10,11,12,13,14,15 +fiducial_idfilters:1,2,4,5,6,7,9,10,11,12,13,14,15,16 fiducial_locfilters: fiducial_method:sqpnp fiducial_qualitythreshold:1 @@ -47,6 +50,7 @@ fiducial_size:165.1 fiducial_skip3d:0 fiducial_type:aprilClassic36h11 fiducial_vis_mode:3dbotposefieldspace +flicker:0 flicker_correction:0 force_convex:1 hue_max:85 @@ -60,11 +64,14 @@ lcgain:10 multigroup_max:7 multigroup_min:1 multigroup_rejector:0 +nnp_rotate:0 pipeline_led_enabled:0 pipeline_led_power:100 pipeline_res:4 -pipeline_type:3 +pipeline_type:4 +python_snapscript_name: red_balance:2099 +reverse_morpho:0 roi_x:0.000000 roi_y:0.000000 rsf:0.35 @@ -76,6 +83,7 @@ rsyaw:0 sat_max:255 sat_min:70 send_corners:1 +send_json:0 send_raw_contours:0 solve3d:0 solve3d_algo:0 diff --git a/src/main/deploy/pathplanner/autos/2 Centerline Notes.auto b/src/main/deploy/pathplanner/autos/2 Centerline Notes.auto new file mode 100644 index 0000000..2fa7620 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/2 Centerline Notes.auto @@ -0,0 +1,123 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.4625682830810547, + "y": 4.048226833343506 + }, + "rotation": -179.99999296355855 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Close 2 Note 1" + } + }, + { + "type": "named", + "data": { + "name": "align-launch" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Close 2 Note 4" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "index" + } + }, + { + "type": "path", + "data": { + "pathName": "Close 2 Note 5" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "align-launch" + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "path", + "data": { + "pathName": "Close 2 Note 6.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "index" + } + }, + { + "type": "path", + "data": { + "pathName": "Close 2 Note 7.1" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "intake" + } + }, + { + "type": "named", + "data": { + "name": "align-launch" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/Amp 3 Note.auto b/src/main/deploy/pathplanner/autos/Amp 3 Note.auto index b8e4a9e..d289817 100644 --- a/src/main/deploy/pathplanner/autos/Amp 3 Note.auto +++ b/src/main/deploy/pathplanner/autos/Amp 3 Note.auto @@ -2,10 +2,10 @@ "version": 1.0, "startingPose": { "position": { - "x": 0.8056966066360474, - "y": 6.671253204345703 + "x": 0.5076572895050049, + "y": 7.026061534881592 }, - "rotation": -126.86988481229311 + "rotation": 180.0 }, "command": { "type": "sequential", @@ -14,13 +14,13 @@ { "type": "path", "data": { - "pathName": "Amp 2 Piece 0" + "pathName": "Amp 2 Piece 1" } }, { "type": "named", "data": { - "name": "subwoofer-launch" + "name": "align-launch" } }, { @@ -32,13 +32,26 @@ { "type": "path", "data": { - "pathName": "Amp 2 Piece 1" + "pathName": "Amp 2 Piece 2" } }, { - "type": "named", + "type": "parallel", "data": { - "name": "index" + "commands": [ + { + "type": "named", + "data": { + "name": "index" + } + }, + { + "type": "path", + "data": { + "pathName": "Amp 2 Piece 3" + } + } + ] } }, { @@ -54,33 +67,46 @@ } }, { - "type": "path", + "type": "named", "data": { - "pathName": "Amp 2 Piece 2" + "name": "align-launch" } }, { "type": "named", "data": { - "name": "align-launch" + "name": "intake" } }, { - "type": "named", + "type": "parallel", "data": { - "name": "intake" + "commands": [ + { + "type": "path", + "data": { + "pathName": "Amp 3 Piece 4" + } + }, + { + "type": "named", + "data": { + "name": "index" + } + } + ] } }, { - "type": "path", + "type": "named", "data": { - "pathName": "Amp 3 Piece 3" + "name": "intake" } }, { "type": "named", "data": { - "name": "index" + "name": "align-launch" } }, { @@ -92,7 +118,32 @@ { "type": "path", "data": { - "pathName": "Amp 3 Piece 4" + "pathName": "Amp 3 Piece 5" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "Amp 3 Piece 6" + } + }, + { + "type": "named", + "data": { + "name": "index" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "intake" } }, { diff --git a/src/main/deploy/pathplanner/autos/Middle 1 Note.auto b/src/main/deploy/pathplanner/autos/Middle 1 Note.auto index 7167915..8aa6d25 100644 --- a/src/main/deploy/pathplanner/autos/Middle 1 Note.auto +++ b/src/main/deploy/pathplanner/autos/Middle 1 Note.auto @@ -2,8 +2,8 @@ "version": 1.0, "startingPose": { "position": { - "x": 1.328428690111538, - "y": 5.52811290895741 + "x": 1.386927239412675, + "y": 5.5768617000416905 }, "rotation": 179.056944767529 }, diff --git a/src/main/deploy/pathplanner/autos/Source 3 Note.auto b/src/main/deploy/pathplanner/autos/Source 3 Note.auto index f1b9720..9fb8155 100644 --- a/src/main/deploy/pathplanner/autos/Source 3 Note.auto +++ b/src/main/deploy/pathplanner/autos/Source 3 Note.auto @@ -32,13 +32,7 @@ { "type": "path", "data": { - "pathName": "Close 2 Note 2" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.3 + "pathName": "Close 2 Note 4" } }, { @@ -54,7 +48,7 @@ { "type": "path", "data": { - "pathName": "Close 2 Note 3" + "pathName": "Close 2 Note 5" } } ] @@ -81,7 +75,7 @@ { "type": "path", "data": { - "pathName": "Close 2 Note 4" + "pathName": "Close 2 Note 2" } }, { @@ -103,7 +97,7 @@ { "type": "path", "data": { - "pathName": "Close 2 Note 5" + "pathName": "Close 2 Note 3" } } ] diff --git a/src/main/deploy/pathplanner/paths/Middle 1 Note 0.path b/src/main/deploy/pathplanner/paths/Middle 1 Note 0.path index dbd2790..bc2f442 100644 --- a/src/main/deploy/pathplanner/paths/Middle 1 Note 0.path +++ b/src/main/deploy/pathplanner/paths/Middle 1 Note 0.path @@ -20,8 +20,8 @@ "y": 5.5768617000416905 }, "prevControl": { - "x": 1.386927239412675, - "y": 5.557362183607978 + "x": 1.4043673838437358, + "y": 5.5681397447923 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json index d166ed2..89ca4d4 100644 --- a/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json +++ b/src/main/deploy/swerves/KrakenSwerve/modules/physicalproperties.json @@ -4,7 +4,7 @@ "angle": 16.8 }, "currentLimit": { - "drive": 120, + "drive": 105, "angle": 40 }, "rampRate": { diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index feca10e..5a6dd06 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -23,8 +23,8 @@ public final class Constants { public static final boolean kLeftBackDriveInverted = false; public static final boolean kRightBackDriveInverted = false; public static final boolean kRightFrontDriveInverted = false; - public static final int driveStatorCurrentLimit = 120; - public static final int driveSupplyCurrentLimit = 120; + public static final int driveStatorCurrentLimit = 110; + public static final int driveSupplyCurrentLimit = 105; public static final double LOOP_TIME = 0.13; public static final double ROBOT_MASS = 115 * 0.453592; @@ -110,7 +110,7 @@ public static class Climb { public static class Transfer { public static final boolean motorInverted = true; - public static final double motorSpeed = 0.15; + public static final double motorSpeed = 0.3; public static final int motorID = 40; } diff --git a/src/main/java/frc/robot/LimelightHelpers.java b/src/main/java/frc/robot/LimelightHelpers.java index 40ec7a4..7b72cbc 100644 --- a/src/main/java/frc/robot/LimelightHelpers.java +++ b/src/main/java/frc/robot/LimelightHelpers.java @@ -1,5 +1,4 @@ -// LimelightHelpers v1.2.1 (March 1, 2023) -// Copied from https://github.com/LimelightVision/limelightlib-wpijava +// LimelightHelpers v1.6 (April 9, 2024) package frc.robot; @@ -293,6 +292,18 @@ public static class Results { @JsonProperty("botpose_wpiblue") public double[] botpose_wpiblue; + @JsonProperty("botpose_tagcount") + public double botpose_tagcount; + + @JsonProperty("botpose_span") + public double botpose_span; + + @JsonProperty("botpose_avgdist") + public double botpose_avgdist; + + @JsonProperty("botpose_avgarea") + public double botpose_avgarea; + @JsonProperty("t6c_rs") public double[] camerapose_robotspace; @@ -352,8 +363,111 @@ public static class LimelightResults { @JsonProperty("Results") public Results targetingResults; + public String error; + public LimelightResults() { targetingResults = new Results(); + error = ""; + } + } + + public static class RawFiducial { + public int id = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double distToCamera = 0; + public double distToRobot = 0; + public double ambiguity = 0; + + public RawFiducial( + int id, + double txnc, + double tync, + double ta, + double distToCamera, + double distToRobot, + double ambiguity) { + this.id = id; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.distToCamera = distToCamera; + this.distToRobot = distToRobot; + this.ambiguity = ambiguity; + } + } + + public static class RawDetection { + public int classId = 0; + public double txnc = 0; + public double tync = 0; + public double ta = 0; + public double corner0_X = 0; + public double corner0_Y = 0; + public double corner1_X = 0; + public double corner1_Y = 0; + public double corner2_X = 0; + public double corner2_Y = 0; + public double corner3_X = 0; + public double corner3_Y = 0; + + public RawDetection( + int classId, + double txnc, + double tync, + double ta, + double corner0_X, + double corner0_Y, + double corner1_X, + double corner1_Y, + double corner2_X, + double corner2_Y, + double corner3_X, + double corner3_Y) { + this.classId = classId; + this.txnc = txnc; + this.tync = tync; + this.ta = ta; + this.corner0_X = corner0_X; + this.corner0_Y = corner0_Y; + this.corner1_X = corner1_X; + this.corner1_Y = corner1_Y; + this.corner2_X = corner2_X; + this.corner2_Y = corner2_Y; + this.corner3_X = corner3_X; + this.corner3_Y = corner3_Y; + } + } + + public static class PoseEstimate { + public Pose2d pose; + public double timestampSeconds; + public double latency; + public int tagCount; + public double tagSpan; + public double avgTagDist; + public double avgTagArea; + public RawFiducial[] rawFiducials; + + public PoseEstimate( + Pose2d pose, + double timestampSeconds, + double latency, + int tagCount, + double tagSpan, + double avgTagDist, + double avgTagArea, + RawFiducial[] rawFiducials) { + + this.pose = pose; + this.timestampSeconds = timestampSeconds; + this.latency = latency; + this.tagCount = tagCount; + this.tagSpan = tagSpan; + this.avgTagDist = avgTagDist; + this.avgTagArea = avgTagArea; + this.rawFiducials = rawFiducials; } } @@ -371,7 +485,7 @@ static final String sanitizeName(String name) { private static Pose3d toPose3D(double[] inData) { if (inData.length < 6) { - System.err.println("Bad LL 3D Pose Data!"); + // System.err.println("Bad LL 3D Pose Data!"); return new Pose3d(); } return new Pose3d( @@ -384,7 +498,7 @@ private static Pose3d toPose3D(double[] inData) { private static Pose2d toPose2D(double[] inData) { if (inData.length < 6) { - System.err.println("Bad LL 2D Pose Data!"); + // System.err.println("Bad LL 2D Pose Data!"); return new Pose2d(); } Translation2d tran2d = new Translation2d(inData[0], inData[1]); @@ -392,6 +506,146 @@ private static Pose2d toPose2D(double[] inData) { return new Pose2d(tran2d, r2d); } + private static double extractArrayEntry(double[] inData, int position) { + if (inData.length < position + 1) { + return 0; + } + return inData[position]; + } + + private static PoseEstimate getBotPoseEstimate(String limelightName, String entryName) { + var poseEntry = LimelightHelpers.getLimelightNTTableEntry(limelightName, entryName); + var poseArray = poseEntry.getDoubleArray(new double[0]); + var pose = toPose2D(poseArray); + double latency = extractArrayEntry(poseArray, 6); + int tagCount = (int) extractArrayEntry(poseArray, 7); + double tagSpan = extractArrayEntry(poseArray, 8); + double tagDist = extractArrayEntry(poseArray, 9); + double tagArea = extractArrayEntry(poseArray, 10); + // getlastchange() in microseconds, ll latency in milliseconds + var timestamp = (poseEntry.getLastChange() / 1000000.0) - (latency / 1000.0); + + RawFiducial[] rawFiducials = new RawFiducial[tagCount]; + int valsPerFiducial = 7; + int expectedTotalVals = 11 + valsPerFiducial * tagCount; + + if (poseArray.length != expectedTotalVals) { + // Don't populate fiducials + } else { + for (int i = 0; i < tagCount; i++) { + int baseIndex = 11 + (i * valsPerFiducial); + int id = (int) poseArray[baseIndex]; + double txnc = poseArray[baseIndex + 1]; + double tync = poseArray[baseIndex + 2]; + double ta = poseArray[baseIndex + 3]; + double distToCamera = poseArray[baseIndex + 4]; + double distToRobot = poseArray[baseIndex + 5]; + double ambiguity = poseArray[baseIndex + 6]; + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + } + + return new PoseEstimate( + pose, timestamp, latency, tagCount, tagSpan, tagDist, tagArea, rawFiducials); + } + + private static RawFiducial[] getRawFiducials(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawfiducials"); + var rawFiducialArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 7; + if (rawFiducialArray.length % valsPerEntry != 0) { + return new RawFiducial[0]; + } + + int numFiducials = rawFiducialArray.length / valsPerEntry; + RawFiducial[] rawFiducials = new RawFiducial[numFiducials]; + + for (int i = 0; i < numFiducials; i++) { + int baseIndex = i * valsPerEntry; + int id = (int) extractArrayEntry(rawFiducialArray, baseIndex); + double txnc = extractArrayEntry(rawFiducialArray, baseIndex + 1); + double tync = extractArrayEntry(rawFiducialArray, baseIndex + 2); + double ta = extractArrayEntry(rawFiducialArray, baseIndex + 3); + double distToCamera = extractArrayEntry(rawFiducialArray, baseIndex + 4); + double distToRobot = extractArrayEntry(rawFiducialArray, baseIndex + 5); + double ambiguity = extractArrayEntry(rawFiducialArray, baseIndex + 6); + + rawFiducials[i] = new RawFiducial(id, txnc, tync, ta, distToCamera, distToRobot, ambiguity); + } + + return rawFiducials; + } + + public static RawDetection[] getRawDetections(String limelightName) { + var entry = LimelightHelpers.getLimelightNTTableEntry(limelightName, "rawdetections"); + var rawDetectionArray = entry.getDoubleArray(new double[0]); + int valsPerEntry = 11; + if (rawDetectionArray.length % valsPerEntry != 0) { + return new RawDetection[0]; + } + + int numDetections = rawDetectionArray.length / valsPerEntry; + RawDetection[] rawDetections = new RawDetection[numDetections]; + + for (int i = 0; i < numDetections; i++) { + int baseIndex = i * valsPerEntry; // Starting index for this detection's data + int classId = (int) extractArrayEntry(rawDetectionArray, baseIndex); + double txnc = extractArrayEntry(rawDetectionArray, baseIndex + 1); + double tync = extractArrayEntry(rawDetectionArray, baseIndex + 2); + double ta = extractArrayEntry(rawDetectionArray, baseIndex + 3); + double corner0_X = extractArrayEntry(rawDetectionArray, baseIndex + 4); + double corner0_Y = extractArrayEntry(rawDetectionArray, baseIndex + 5); + double corner1_X = extractArrayEntry(rawDetectionArray, baseIndex + 6); + double corner1_Y = extractArrayEntry(rawDetectionArray, baseIndex + 7); + double corner2_X = extractArrayEntry(rawDetectionArray, baseIndex + 8); + double corner2_Y = extractArrayEntry(rawDetectionArray, baseIndex + 9); + double corner3_X = extractArrayEntry(rawDetectionArray, baseIndex + 10); + double corner3_Y = extractArrayEntry(rawDetectionArray, baseIndex + 11); + + rawDetections[i] = + new RawDetection( + classId, txnc, tync, ta, corner0_X, corner0_Y, corner1_X, corner1_Y, corner2_X, + corner2_Y, corner3_X, corner3_Y); + } + + return rawDetections; + } + + private static void printPoseEstimate(PoseEstimate pose) { + if (pose == null) { + System.out.println("No PoseEstimate available."); + return; + } + + System.out.printf("Pose Estimate Information:%n"); + System.out.printf("Timestamp (Seconds): %.3f%n", pose.timestampSeconds); + System.out.printf("Latency: %.3f ms%n", pose.latency); + System.out.printf("Tag Count: %d%n", pose.tagCount); + System.out.printf("Tag Span: %.2f meters%n", pose.tagSpan); + System.out.printf("Average Tag Distance: %.2f meters%n", pose.avgTagDist); + System.out.printf("Average Tag Area: %.2f%% of image%n", pose.avgTagArea); + System.out.println(); + + if (pose.rawFiducials == null || pose.rawFiducials.length == 0) { + System.out.println("No RawFiducials data available."); + return; + } + + System.out.println("Raw Fiducials Details:"); + for (int i = 0; i < pose.rawFiducials.length; i++) { + RawFiducial fiducial = pose.rawFiducials[i]; + System.out.printf(" Fiducial #%d:%n", i + 1); + System.out.printf(" ID: %d%n", fiducial.id); + System.out.printf(" TXNC: %.2f%n", fiducial.txnc); + System.out.printf(" TYNC: %.2f%n", fiducial.tync); + System.out.printf(" TA: %.2f%n", fiducial.ta); + System.out.printf(" Distance to Camera: %.2f meters%n", fiducial.distToCamera); + System.out.printf(" Distance to Robot: %.2f meters%n", fiducial.distToRobot); + System.out.printf(" Ambiguity: %.2f%n", fiducial.ambiguity); + System.out.println(); + } + } + public static NetworkTable getLimelightNTTable(String tableName) { return NetworkTableInstance.getDefault().getTable(sanitizeName(tableName)); } @@ -532,8 +786,8 @@ public static double getFiducialID(String limelightName) { return getLimelightNTDouble(limelightName, "tid"); } - public static double getNeuralClassID(String limelightName) { - return getLimelightNTDouble(limelightName, "tclass"); + public static String getNeuralClassID(String limelightName) { + return getLimelightNTString(limelightName, "tclass"); } ///// @@ -591,6 +845,28 @@ public static Pose2d getBotPose2d_wpiBlue(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the BLUE alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpiblue"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the BLUE alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiBlue_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpiblue"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) * @@ -603,6 +879,28 @@ public static Pose2d getBotPose2d_wpiRed(String limelightName) { return toPose2D(result); } + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_wpired"); + } + + /** + * Gets the Pose2d and timestamp for use with WPILib pose estimator (addVisionMeasurement) when + * you are on the RED alliance + * + * @param limelightName + * @return + */ + public static PoseEstimate getBotPoseEstimate_wpiRed_MegaTag2(String limelightName) { + return getBotPoseEstimate(limelightName, "botpose_orb_wpired"); + } + /** * Gets the Pose2d for easy use with Odometry vision pose estimator (addVisionMeasurement) * @@ -626,6 +924,10 @@ public static void setPipelineIndex(String limelightName, int pipelineIndex) { setLimelightNTDouble(limelightName, "pipeline", pipelineIndex); } + public static void setPriorityTagID(String limelightName, int ID) { + setLimelightNTDouble(limelightName, "priorityid", ID); + } + /** The LEDs will be controlled by Limelight pipeline settings, and not by robot code. */ public static void setLEDMode_PipelineControl(String limelightName) { setLimelightNTDouble(limelightName, "ledMode", 0); @@ -677,6 +979,53 @@ public static void setCropWindow( setLimelightNTDoubleArray(limelightName, "crop", entries); } + public static void SetRobotOrientation( + String limelightName, + double yaw, + double yawRate, + double pitch, + double pitchRate, + double roll, + double rollRate) { + + double[] entries = new double[6]; + entries[0] = yaw; + entries[1] = yawRate; + entries[2] = pitch; + entries[3] = pitchRate; + entries[4] = roll; + entries[5] = rollRate; + setLimelightNTDoubleArray(limelightName, "robot_orientation_set", entries); + } + + public static void SetFiducialIDFiltersOverride(String limelightName, int[] validIDs) { + double[] validIDsDouble = new double[validIDs.length]; + for (int i = 0; i < validIDs.length; i++) { + validIDsDouble[i] = validIDs[i]; + } + setLimelightNTDoubleArray(limelightName, "fiducial_id_filters_set", validIDsDouble); + } + + public static void SetFiducialDownscalingOverride(String limelightName, float downscale) { + int d = 0; // pipeline + if (downscale == 1.0) { + d = 1; + } + if (downscale == 1.5) { + d = 2; + } + if (downscale == 2) { + d = 3; + } + if (downscale == 3) { + d = 4; + } + if (downscale == 4) { + d = 5; + } + setLimelightNTDouble(limelightName, "fiducial_downscale_set", d); + } + public static void setCameraPose_RobotSpace( String limelightName, double forward, @@ -751,7 +1100,7 @@ public static LimelightResults getLatestResults(String limelightName) { try { results = mapper.readValue(getJSONDump(limelightName), LimelightResults.class); } catch (JsonProcessingException e) { - System.err.println("lljson error: " + e.getMessage()); + results.error = "lljson error: " + e.getMessage(); } long end = System.nanoTime(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 94e55bc..305d944 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -109,13 +109,12 @@ public RobotContainer() { m_chooser.addOption("Source 1", m_swerve.getAutonomousCommand("Source 1 Note")); m_chooser.addOption( "Source 2 Centerline", m_swerve.getAutonomousCommand("Source 2 Note Centerline")); - m_chooser.addOption("Source 2 Close", m_swerve.getAutonomousCommand("Source 2 Note Close")); - m_chooser.addOption("Source 3", m_swerve.getAutonomousCommand("Source 3 Note")); + m_chooser.addOption("Source 3 Close", m_swerve.getAutonomousCommand("Source 3 Note")); + m_chooser.addOption( + "Source 3 Centerlines", m_swerve.getAutonomousCommand("2 Centerline Notes")); m_chooser.addOption("Middle 1", m_swerve.getAutonomousCommand("Middle 1 Note")); - m_chooser.addOption("Middle 3", m_swerve.getAutonomousCommand("Middle 3 Note")); m_chooser.addOption("Amp 1", m_swerve.getAutonomousCommand("Amp 1 Note")); - m_chooser.addOption("Amp 2", m_swerve.getAutonomousCommand("Amp 2 Note")); - m_chooser.addOption("Amp 3", m_swerve.getAutonomousCommand("Amp 3 Note")); + m_chooser.addOption("Amp 4", m_swerve.getAutonomousCommand("Amp 3 Note")); m_chooser.addOption( "Just Shoot", new AlignLaunchAuto(m_swerve, m_launch, m_index, LaunchPreset.SUBWOOFER, 1)); m_chooser.addOption("2 Meter tuning", m_swerve.getAutonomousCommand("Choreo 2m")); @@ -135,7 +134,7 @@ public RobotContainer() { private void configureBindings() { JoystickButton leftBumper = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); - leftBumper.toggleOnTrue(new IntakeAndIndex(m_intake, m_index, m_transfer)); + leftBumper.whileTrue(new IntakeAndIndex(m_intake, m_index, m_transfer)); JoystickButton rb = new JoystickButton(driver, XboxController.Button.kRightBumper.value); rb.whileTrue(new LaunchWithVelo(m_launch, m_index, -1400, false)); diff --git a/src/main/java/frc/robot/commands/AlignLaunchAuto.java b/src/main/java/frc/robot/commands/AlignLaunchAuto.java index a820566..6a777ed 100644 --- a/src/main/java/frc/robot/commands/AlignLaunchAuto.java +++ b/src/main/java/frc/robot/commands/AlignLaunchAuto.java @@ -59,7 +59,6 @@ public void execute() { // Called once the command ends or is interrupted. @Override public void end(boolean interrupted) { - launcher.setLaunchVelocity(0); index.stop(); } diff --git a/src/main/java/frc/robot/subsystems/Intake.java b/src/main/java/frc/robot/subsystems/Intake.java index deeeae8..a83d9bf 100644 --- a/src/main/java/frc/robot/subsystems/Intake.java +++ b/src/main/java/frc/robot/subsystems/Intake.java @@ -13,10 +13,10 @@ public class Intake extends SubsystemBase { /** Creates a new Intake. */ - private TalonFX motorLeft = new TalonFX(Constants.Intake.leftCANID); - + // private TalonFX motorLeft = new TalonFX(Constants.Intake.leftCANID); private TalonFX motorCenter = new TalonFX(Constants.Intake.centerCANID); - private TalonFX motorRight = new TalonFX(Constants.Intake.rightCANID); + + // private TalonFX motorRight = new TalonFX(Constants.Intake.rightCANID); private SlewRateLimiter limiter = new SlewRateLimiter(1); @@ -24,9 +24,9 @@ public class Intake extends SubsystemBase { double targetSpeed = 0; public Intake() { - motorLeft.setInverted(Constants.Intake.leftInverted); + // motorLeft.setInverted(Constants.Intake.leftInverted); motorCenter.setInverted(Constants.Intake.centerInverted); - motorRight.setInverted(Constants.Intake.rightInverted); + // motorRight.setInverted(Constants.Intake.rightInverted); stop(); } @@ -60,8 +60,8 @@ public boolean isRunning() { public void inAndOut() { motorCenter.set(limiter.calculate(-targetSpeed)); - motorLeft.set(limiter.calculate(targetSpeed)); - motorRight.set(limiter.calculate(targetSpeed)); + // motorLeft.set(limiter.calculate(targetSpeed)); + // motorRight.set(limiter.calculate(targetSpeed)); } @Override @@ -69,9 +69,9 @@ public void periodic() { // This method will be called once per scheduler run SmartDashboard.putBoolean("Intake Running", isRunning()); - motorLeft.set(limiter.calculate(targetSpeed)); + // motorLeft.set(limiter.calculate(targetSpeed)); motorCenter.set(limiter.calculate(targetSpeed)); - motorRight.set(limiter.calculate(targetSpeed)); + // motorRight.set(limiter.calculate(targetSpeed)); } public Command reverseIntakeCommand() { diff --git a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java index 17c765f..8854bde 100644 --- a/src/main/java/frc/robot/subsystems/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/SwerveSubsystem.java @@ -94,7 +94,7 @@ public SwerveSubsystem(File directory) { throw new RuntimeException(e); } - swerveDrive.setHeadingCorrection(true); // Heading correction should only be used while controlling the robot via angle. + swerveDrive.setHeadingCorrection(false); // Heading correction should only be used while controlling the robot via angle. swerveDrive.setCosineCompensator(!SwerveDriveTelemetry.isSimulation); // Disables cosine compensation for simulations since it causes discrepancies not seen in real life. setupPathPlanner(); @@ -221,7 +221,7 @@ public Command driveToPose(Pose2d pose) public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier headingX, DoubleSupplier headingY) { - swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. + swerveDrive.setHeadingCorrection(false); // Normally you would want heading correction for this kind of control. return run(() -> { double xInput = Math.pow(translationX.getAsDouble(), 3); // Smooth controll out double yInput = Math.pow(translationY.getAsDouble(), 3); // Smooth controll out @@ -244,7 +244,7 @@ public Command driveCommand(DoubleSupplier translationX, DoubleSupplier translat */ public Command simDriveCommand(DoubleSupplier translationX, DoubleSupplier translationY, DoubleSupplier rotation) { - swerveDrive.setHeadingCorrection(true); // Normally you would want heading correction for this kind of control. + swerveDrive.setHeadingCorrection(false); // Normally you would want heading correction for this kind of control. return run(() -> { // Make the robot move driveFieldOriented(swerveDrive.swerveController.getTargetSpeeds(translationX.getAsDouble(), diff --git a/src/main/java/frc/robot/subsystems/Transfer.java b/src/main/java/frc/robot/subsystems/Transfer.java index b7eea4c..9563848 100644 --- a/src/main/java/frc/robot/subsystems/Transfer.java +++ b/src/main/java/frc/robot/subsystems/Transfer.java @@ -18,7 +18,7 @@ public class Transfer extends SubsystemBase { public Transfer() { motor.setInverted(Constants.Transfer.motorInverted); currentSpeed = 0; - motor.setSmartCurrentLimit(20); + motor.setSmartCurrentLimit(40); } public void start() {