From 4f65f56d1a2c83daedf239f283030c60ee37c1d6 Mon Sep 17 00:00:00 2001 From: CrolineCrois Date: Fri, 5 Apr 2024 15:26:29 -0700 Subject: [PATCH] added E side center auton --- src/main/ChoreoDoc.chor | 3176 ++++++++++------- src/main/deploy/choreo/6Y.traj | 386 +- src/main/deploy/choreo/7Y.traj | 488 ++- src/main/deploy/choreo/8Z.traj | 362 +- src/main/deploy/choreo/A1.traj | 279 +- src/main/deploy/choreo/E8.traj | 247 ++ src/main/deploy/choreo/OS8.traj | 400 --- src/main/deploy/choreo/S3.traj | 268 +- src/main/deploy/choreo/X6.traj | 395 +- src/main/deploy/choreo/Y6.traj | 247 ++ src/main/deploy/choreo/Z7.traj | 364 +- src/main/deploy/choreo/Z8.traj | 333 +- .../robot/commands/auton/AutonBuilder.java | 22 + 13 files changed, 3891 insertions(+), 3076 deletions(-) create mode 100644 src/main/deploy/choreo/E8.traj delete mode 100644 src/main/deploy/choreo/OS8.traj create mode 100644 src/main/deploy/choreo/Y6.traj diff --git a/src/main/ChoreoDoc.chor b/src/main/ChoreoDoc.chor index a1bd7dfa..22fea62d 100644 --- a/src/main/ChoreoDoc.chor +++ b/src/main/ChoreoDoc.chor @@ -3175,219 +3175,6 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "8Z": { - "waypoints": [ - { - "x": 8.288, - "y": 0.753, - "heading": -0.359, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 - }, - { - "x": 5.84647274017334, - "y": 1.3783090114593506, - "heading": -0.65, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 8.288, - "y": 0.753, - "heading": -0.359, - "angularVelocity": 0, - "velocityX": 7.848060634472957e-33, - "velocityY": -3.2516900550847044e-31, - "timestamp": 0 - }, - { - "x": 8.257857688531061, - "y": 0.7607198575910491, - "heading": -0.36259382076295077, - "angularVelocity": -0.04144012470689956, - "velocityX": -0.34756912729323597, - "velocityY": 0.08901719990897015, - "timestamp": 0.08672321303010541 - }, - { - "x": 8.19757306595335, - "y": 0.7761595734074377, - "heading": -0.36978140180451197, - "angularVelocity": -0.08287955197261455, - "velocityX": -0.6951382504334015, - "velocityY": 0.1780344071319038, - "timestamp": 0.17344642606021082 - }, - { - "x": 8.107146132657443, - "y": 0.7993191483957665, - "heading": -0.3805626441664111, - "angularVelocity": -0.12431783815661841, - "velocityX": -1.0427073690698763, - "velocityY": 0.2670516252700299, - "timestamp": 0.2601696390903162 - }, - { - "x": 7.986576889086077, - "y": 0.8301985838111292, - "heading": -0.3949373988890941, - "angularVelocity": -0.16575440669722594, - "velocityX": -1.3902764826011484, - "velocityY": 0.35606885788056725, - "timestamp": 0.34689285212042165 - }, - { - "x": 7.835865335793746, - "y": 0.8687978812136647, - "heading": -0.4129054484754803, - "angularVelocity": -0.20718846729246293, - "velocityX": -1.7378455897386196, - "velocityY": 0.4450861084809666, - "timestamp": 0.4336160651505271 - }, - { - "x": 7.655011473615787, - "y": 0.9151170424750372, - "heading": -0.4344664737940713, - "angularVelocity": -0.24861884800215203, - "velocityX": -2.085414687243842, - "velocityY": 0.5341033806634112, - "timestamp": 0.5203392781806325 - }, - { - "x": 7.444015304197123, - "y": 0.9691560698341772, - "heading": -0.45961997962793794, - "angularVelocity": -0.29004351839609516, - "velocityX": -2.432983765781574, - "velocityY": 0.6231206786628246, - "timestamp": 0.607062491210738 - }, - { - "x": 7.20287683222139, - "y": 1.030914966202055, - "heading": -0.48836503954685173, - "angularVelocity": -0.33145750618043834, - "velocityX": -2.78055279031259, - "velocityY": 0.7121380102284524, - "timestamp": 0.6937857042408434 - }, - { - "x": 6.931596093838579, - "y": 1.1003937371130306, - "heading": -0.5206978932748437, - "angularVelocity": -0.3728281344555003, - "velocityX": -3.1281213980002853, - "velocityY": 0.8011554056104477, - "timestamp": 0.7805089172709488 - }, - { - "x": 6.6904576020716755, - "y": 1.162152672705266, - "heading": -0.549434263924052, - "angularVelocity": -0.3313573107494592, - "velocityX": -2.780553018523351, - "velocityY": 0.7121384625220937, - "timestamp": 0.8672321303010543 - }, - { - "x": 6.479461402681092, - "y": 1.2161917502632715, - "heading": -0.5745770990267204, - "angularVelocity": -0.28992047485532396, - "velocityX": -2.4329841113859345, - "velocityY": 0.6231212575028354, - "timestamp": 0.9539553433311597 - }, - { - "x": 6.298607507686841, - "y": 1.262510964025961, - "heading": -0.5961272493805595, - "angularVelocity": -0.24849344945692042, - "velocityX": -2.085415065646478, - "velocityY": 0.5341039860528463, - "timestamp": 1.040678556361265 - }, - { - "x": 6.1478959228927685, - "y": 1.301110311282906, - "heading": -0.6140851971616693, - "angularVelocity": -0.20707198400127302, - "velocityX": -1.737845952983236, - "velocityY": 0.4450866833490784, - "timestamp": 1.1274017693913705 - }, - { - "x": 6.027326651802776, - "y": 1.3319897904748126, - "heading": -0.6284512647131738, - "angularVelocity": -0.16565423546423083, - "velocityX": -1.3902767999167456, - "velocityY": 0.3560693626651814, - "timestamp": 1.214124982421476 - }, - { - "x": 5.9368996967234855, - "y": 1.3551494006238816, - "heading": -0.6392256722190083, - "angularVelocity": -0.12423902585451516, - "velocityX": -1.0427076202527152, - "velocityY": 0.2670520307063474, - "timestamp": 1.3008481954515814 - }, - { - "x": 5.8766150591914466, - "y": 1.3705891410920916, - "heading": -0.6464085601564894, - "angularVelocity": -0.08282543608004028, - "velocityX": -0.6951384228708393, - "velocityY": 0.17803469139053074, - "timestamp": 1.3875714084816868 - }, - { - "x": 5.84647274017334, - "y": 1.3783090114593506, - "heading": -0.65, - "angularVelocity": -0.041412670472249856, - "velocityX": -0.34756921434221283, - "velocityY": 0.08901734723065605, - "timestamp": 1.4742946215117922 - }, - { - "x": 5.84647274017334, - "y": 1.3783090114593506, - "heading": -0.65, - "angularVelocity": 0, - "velocityX": -1.0272646100997073e-34, - "velocityY": 0, - "timestamp": 1.5610178345418977 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, "OA1": { "waypoints": [ { @@ -6503,13 +6290,13 @@ "Z8": { "waypoints": [ { - "x": 5.776048183441162, - "y": 1.512994647026062, - "heading": -0.65, + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 22 }, { "x": 7.842076778411865, @@ -6523,166 +6310,211 @@ ], "trajectory": [ { - "x": 5.776048183441162, - "y": 1.512994647026062, - "heading": -0.65, + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, "angularVelocity": 0, - "velocityX": -1.5789883675704282e-32, - "velocityY": 0, + "velocityX": 5.717023418431248e-32, + "velocityY": -1.425773996162293e-28, "timestamp": 0 }, { - "x": 5.804743050185828, - "y": 1.5043318888705361, - "heading": -0.6437581390191097, - "angularVelocity": 0.07330082168918192, - "velocityX": 0.3369759943524967, - "velocityY": -0.10173044430799516, - "timestamp": 0.08515403834567799 - }, - { - "x": 5.86213277976621, - "y": 1.4870063760319097, - "heading": -0.6312738261796365, - "angularVelocity": 0.14660858230579626, - "velocityX": 0.6739519428005418, - "velocityY": -0.20346084783782606, - "timestamp": 0.17030807669135598 - }, - { - "x": 5.9482173644624545, - "y": 1.4610181153106103, - "heading": -0.6125459823544488, - "angularVelocity": 0.21992901557014852, - "velocityX": 1.010927800591089, - "velocityY": -0.3051911715073518, - "timestamp": 0.255462115037034 - }, - { - "x": 6.062996793277665, - "y": 1.4263671179092674, - "heading": -0.5875730752792624, - "angularVelocity": 0.2932674428640758, - "velocityX": 1.347903529240383, - "velocityY": -0.40692136361964437, - "timestamp": 0.34061615338271195 - }, - { - "x": 6.206471052722686, - "y": 1.3830534007491153, - "heading": -0.5563531860432791, - "angularVelocity": 0.3666284047416284, - "velocityX": 1.684879099480822, - "velocityY": -0.5086513570187057, - "timestamp": 0.4257701917283899 - }, - { - "x": 6.378640127771531, - "y": 1.3310769879206097, - "heading": -0.5188841218821412, - "angularVelocity": 0.44001511718134073, - "velocityX": 2.0218544932647196, - "velocityY": -0.6103810675133283, - "timestamp": 0.510924230074068 - }, - { - "x": 6.579504002372721, - "y": 1.2704379122308411, - "heading": -0.4751635904846247, - "angularVelocity": 0.5134287492042999, - "velocityX": 2.3588296985492834, - "velocityY": -0.7121103927403597, - "timestamp": 0.596078268419746 - }, - { - "x": 6.809062654743387, - "y": 1.2011362179518417, - "heading": -0.42518944746652015, - "angularVelocity": 0.5868675636408143, - "velocityX": 2.695804648028377, - "velocityY": -0.8138391980621403, - "timestamp": 0.681232306765424 - }, - { - "x": 7.038621262896878, - "y": 1.1318364551262148, - "heading": -0.3751455547563511, - "angularVelocity": 0.5876866638669395, - "velocityX": 2.69580412876734, - "velocityY": -0.8138165161857421, - "timestamp": 0.766386345111102 - }, - { - "x": 7.2394850898490155, - "y": 1.071199229853428, - "heading": -0.33135757746423733, - "angularVelocity": 0.5142207949593519, - "velocityX": 2.3588291389862373, - "velocityY": -0.7120886625086819, - "timestamp": 0.8515403834567801 - }, - { - "x": 7.411654113486545, - "y": 1.0192245139134757, - "heading": -0.2938257380134279, - "angularVelocity": 0.4407523140411856, - "velocityX": 2.021853889519832, - "velocityY": -0.6103611402311183, - "timestamp": 0.9366944218024581 - }, - { - "x": 7.555128319715313, - "y": 0.9759122824392717, - "heading": -0.2625498891671241, - "angularVelocity": 0.3672855621872125, - "velocityX": 1.6848784745397705, - "velocityY": -0.5086339099782955, - "timestamp": 1.0218484601481361 - }, - { - "x": 7.669907697003263, - "y": 0.9412625159146415, - "heading": -0.23752970245661262, - "angularVelocity": 0.29382266768069665, - "velocityX": 1.3479029241339204, - "velocityY": -0.4069069089121928, - "timestamp": 1.107002498493814 - }, - { - "x": 7.75599223622601, - "y": 0.9152752001717872, - "heading": -0.21876481896635738, - "angularVelocity": 0.22036398807159627, - "velocityX": 1.0109272665764968, - "velocityY": -0.30518007422455146, - "timestamp": 1.192156536839492 - }, - { - "x": 7.813381931076612, - "y": 0.8979503260405115, - "heading": -0.20625496018104028, - "angularVelocity": 0.14690857918604, - "velocityX": 0.6739515349540018, - "velocityY": -0.20345334722642713, - "timestamp": 1.2773105751851699 + "x": 4.304732169950001, + "y": 1.5078399138540146, + "heading": -0.7950484987601832, + "angularVelocity": 0.058208034646547904, + "velocityX": 0.34653163303819684, + "velocityY": -0.06059575340920951, + "timestamp": 0.08506559738515405 + }, + { + "x": 4.363688008669768, + "y": 1.4975306838042393, + "heading": -0.7851452998454412, + "angularVelocity": 0.11641837851226666, + "velocityX": 0.693063242156879, + "velocityY": -0.12119153179043739, + "timestamp": 0.1701311947703081 + }, + { + "x": 4.452121762210008, + "y": 1.4820668346392138, + "heading": -0.7702900404294987, + "angularVelocity": 0.1746329876307565, + "velocityX": 1.0395948098716838, + "velocityY": -0.1817873457704544, + "timestamp": 0.25519679215546215 + }, + { + "x": 4.570033425335727, + "y": 1.4614483627248729, + "heading": -0.7504821592921513, + "angularVelocity": 0.2328541942483684, + "velocityX": 1.3861263160457933, + "velocityY": -0.24238320247121709, + "timestamp": 0.3402623895406162 + }, + { + "x": 4.717422990801947, + "y": 1.4356752642589519, + "heading": -0.7257208555275354, + "angularVelocity": 0.2910848160219101, + "velocityX": 1.7326577370506226, + "velocityY": -0.3029791038700053, + "timestamp": 0.4253279869257703 + }, + { + "x": 4.89429044896851, + "y": 1.4047475358875174, + "heading": -0.6960050379672905, + "angularVelocity": 0.3493282651709566, + "velocityX": 2.0791890447292576, + "velocityY": -0.3635750446964123, + "timestamp": 0.5103935843109243 + }, + { + "x": 5.100635787262615, + "y": 1.3686651755613142, + "heading": -0.6613332622001218, + "angularVelocity": 0.407588694290044, + "velocityX": 2.425720204606665, + "velocityY": -0.4241710096131092, + "timestamp": 0.5954591816960784 + }, + { + "x": 5.336458989275771, + "y": 1.3274281837545279, + "heading": -0.6217036408461484, + "angularVelocity": 0.46587131075560523, + "velocityX": 2.772251171591864, + "velocityY": -0.48476696895545185, + "timestamp": 0.6805247790812324 + }, + { + "x": 5.601760032758817, + "y": 1.281036565393316, + "heading": -0.5771136718696128, + "angularVelocity": 0.5241833402369976, + "velocityX": 3.1187818770240705, + "velocityY": -0.5453628703876953, + "timestamp": 0.7655903764663865 + }, + { + "x": 5.896538883462558, + "y": 1.229490333851042, + "heading": -0.5275597562412677, + "angularVelocity": 0.582537678588075, + "velocityX": 3.4653121798353, + "velocityY": -0.6059586146075705, + "timestamp": 0.8506559738515406 + }, + { + "x": 6.2207954703436386, + "y": 1.172789520169878, + "heading": -0.4730361468387663, + "angularVelocity": 0.6409595780083519, + "velocityX": 3.811841647486834, + "velocityY": -0.6665539939070283, + "timestamp": 0.9357215712366946 + }, + { + "x": 6.515573956709268, + "y": 1.1212436190020052, + "heading": -0.42340864440375947, + "angularVelocity": 0.5834027381250584, + "velocityX": 3.4653078968099584, + "velocityY": -0.605954730847158, + "timestamp": 1.0207871686218486 + }, + { + "x": 6.780874551524839, + "y": 1.0748523775635215, + "heading": -0.3787346711264011, + "angularVelocity": 0.5251708640202797, + "velocityX": 3.1187766026536243, + "velocityY": -0.5453584394221985, + "timestamp": 1.1058527660070026 + }, + { + "x": 7.016697283488896, + "y": 1.0336157628045721, + "heading": -0.33902011581118113, + "angularVelocity": 0.46686976328700697, + "velocityX": 2.772245645866879, + "velocityY": -0.48476253651921614, + "timestamp": 1.1909183633921565 + }, + { + "x": 7.223042164396211, + "y": 0.9975337560488634, + "heading": -0.30426743462836536, + "angularVelocity": 0.40853978871571445, + "velocityX": 2.4257148277351357, + "velocityY": -0.4241668531679094, + "timestamp": 1.2759839607773105 + }, + { + "x": 7.399909200334508, + "y": 0.9666063434187415, + "heading": -0.2744779957092185, + "angularVelocity": 0.35019373089436867, + "velocityX": 2.0791840811684517, + "velocityY": -0.3635713329572148, + "timestamp": 1.3610495581624644 + }, + { + "x": 7.547298394573816, + "y": 0.9408335139795543, + "heading": -0.2496526340730793, + "angularVelocity": 0.2918378568915062, + "velocityX": 1.7326533730430376, + "velocityY": -0.30297594129028455, + "timestamp": 1.4461151555476184 + }, + { + "x": 7.665209748822617, + "y": 0.9202152591870838, + "heading": -0.22979187484564018, + "angularVelocity": 0.23347580970332782, + "velocityX": 1.3861226850019162, + "velocityY": -0.24238065006605194, + "timestamp": 1.5311807529327723 + }, + { + "x": 7.753643263936589, + "y": 0.9047515726613659, + "heading": -0.21489605202877265, + "angularVelocity": 0.17510983611146802, + "velocityX": 1.039592007019826, + "velocityY": -0.18178543384233942, + "timestamp": 1.6162463503179263 + }, + { + "x": 7.812598940362171, + "y": 0.8944424500616808, + "heading": -0.20496538044326496, + "angularVelocity": 0.11674133716509057, + "velocityX": 0.6930613342859036, + "velocityY": -0.12119026864653992, + "timestamp": 1.7013119477030803 }, { "x": 7.842076778411865, "y": 0.8892878890037537, "heading": -0.2, - "angularVelocity": 0.07345465115404878, - "velocityX": 0.3369757664195322, - "velocityY": -0.10172667327406405, - "timestamp": 1.3624646135308478 + "angularVelocity": 0.058371193477605506, + "velocityX": 0.3465306652256466, + "velocityY": -0.0605951314794, + "timestamp": 1.7863775450882342 }, { "x": 7.842076778411865, "y": 0.8892878890037537, "heading": -0.2, "angularVelocity": 0, - "velocityX": -1.7603165656807992e-35, + "velocityX": 1.028052364086851e-30, "velocityY": 0, - "timestamp": 1.4476186518765257 + "timestamp": 1.8714431424733882 } ], "constraints": [ @@ -6704,211 +6536,25 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "Z7": { + "7Y": { "waypoints": [ { - "x": 5.776, - "y": 1.513, - "heading": -0.65, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 7.159897327423096, - "y": 2.4290637969970703, - "heading": 0, + "x": 7.920039653778076, + "y": 2.2926278114318848, + "heading": 0.39233985858715276, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 5.776, - "y": 1.513, - "heading": -0.65, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 5.79762140212684, - "y": 1.5273147458744445, - "heading": -0.6397819014636212, - "angularVelocity": 0.12881016597140443, - "velocityX": 0.27256112148236133, - "velocityY": 0.1804528293949252, - "timestamp": 0.07932680203709337 - }, - { - "x": 5.8408642615291155, - "y": 1.5559443036967622, - "heading": -0.6193516690577348, - "angularVelocity": 0.2575451408760095, - "velocityX": 0.5451229381723454, - "velocityY": 0.36090649171676586, - "timestamp": 0.15865360407418674 - }, - { - "x": 5.905728713931284, - "y": 1.598888770329341, - "heading": -0.5887202186008055, - "angularVelocity": 0.386142510101516, - "velocityX": 0.8176864658156355, - "velocityY": 0.5413613750936046, - "timestamp": 0.23798040611128013 - }, - { - "x": 5.992214995382535, - "y": 1.6561482404855234, - "heading": -0.5479033051820914, - "angularVelocity": 0.5145412694139371, - "velocityX": 1.0902529691139036, - "velocityY": 0.7218174524343958, - "timestamp": 0.3173072081483735 - }, - { - "x": 6.100323461823128, - "y": 1.7277227582713581, - "heading": -0.49692088625588365, - "angularVelocity": 0.6426884434641421, - "velocityX": 1.362823959423462, - "velocityY": 0.902274085779563, - "timestamp": 0.39663401018546685 - }, - { - "x": 6.230054604703474, - "y": 1.8136122498754594, - "heading": -0.43579566578740936, - "angularVelocity": 0.7705494095159927, - "velocityX": 1.6354011449961674, - "velocityY": 1.0827297886525065, - "timestamp": 0.4759608122225602 - }, - { - "x": 6.3814090857949495, - "y": 1.9138164127150836, - "heading": -0.36455118251674296, - "angularVelocity": 0.8981136443311097, - "velocityX": 1.907986672911659, - "velocityY": 1.2631816771432258, - "timestamp": 0.5552876142596536 - }, - { - "x": 6.554405526600661, - "y": 2.0283184347938064, - "heading": -0.2835830817160487, - "angularVelocity": 1.0206903432566645, - "velocityX": 2.180806944982083, - "velocityY": 1.4434216322647335, - "timestamp": 0.634614416296747 - }, - { - "x": 6.7057780488968515, - "y": 2.128506370748931, - "heading": -0.21272129699084932, - "angularVelocity": 0.8932893159119689, - "velocityX": 1.9082141017787242, - "velocityY": 1.262977119741651, - "timestamp": 0.7139412183338404 - }, - { - "x": 6.835526224253662, - "y": 2.2143808110875423, - "heading": -0.15196551010833717, - "angularVelocity": 0.7658923002354566, - "velocityX": 1.6356158577543578, - "velocityY": 1.0825400511980374, - "timestamp": 0.7932680203709338 - }, - { - "x": 6.9436498396674775, - "y": 2.285942234510729, - "heading": -0.10132145144694792, - "angularVelocity": 0.6384230469508654, - "velocityX": 1.3630149285894169, - "velocityY": 0.9021090171985522, - "timestamp": 0.8725948224080272 - }, - { - "x": 7.030148791972443, - "y": 2.343191019381516, - "heading": -0.06079681140478092, - "angularVelocity": 0.5108568478938251, - "velocityX": 1.0904126989074627, - "velocityY": 0.7216827528735796, - "timestamp": 0.9519216244451206 - }, - { - "x": 7.095023036988465, - "y": 2.3861274409630164, - "heading": -0.030398784130004894, - "angularVelocity": 0.3831999588305835, - "velocityX": 0.8178099122877435, - "velocityY": 0.5412599585373844, - "timestamp": 1.031248426482214 - }, - { - "x": 7.1382725548225325, - "y": 2.414751673887316, - "heading": -0.010132481200335029, - "angularVelocity": 0.2554786328105512, - "velocityX": 0.5452068748951213, - "velocityY": 0.36083936562720403, - "timestamp": 1.1105752285193073 - }, - { - "x": 7.159897327423096, - "y": 2.4290637969970703, - "heading": 3.713306132884575e-32, - "angularVelocity": 0.12773086699747532, - "velocityX": 0.2726036099432161, - "velocityY": 0.18041976661383763, - "timestamp": 1.1899020305564008 - }, - { - "x": 7.159897327423096, - "y": 2.4290637969970703, - "heading": -8.409337330142514e-33, - "angularVelocity": -5.74111111803187e-31, - "velocityX": 0, - "velocityY": 3.756127142115375e-32, - "timestamp": 1.2692288325934942 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" + "controlIntervalCount": 19 }, { - "scope": [ - "last" - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "7Y": { - "waypoints": [ - { - "x": 7.70564079284668, - "y": 2.4290637969970703, + "x": 5.8345208168029785, + "y": 4.0273118019104, "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 + "headingConstrained": false, + "controlIntervalCount": 15 }, { "x": 4.377503395080566, @@ -6922,211 +6568,319 @@ ], "trajectory": [ { - "x": 7.70564079284668, - "y": 2.4290637969970703, - "heading": 0, + "x": 7.920039653778076, + "y": 2.2926278114318848, + "heading": 0.3923398585871528, "angularVelocity": 0, - "velocityX": 1.1723852013574271e-35, - "velocityY": 0, + "velocityX": 5.239292900745689e-35, + "velocityY": 4.900926094151388e-36, "timestamp": 0 }, { - "x": 7.6781366296409645, - "y": 2.449095429911782, - "heading": -0.00313265857340496, - "angularVelocity": -0.03454542227160848, - "velocityX": -0.30330242185823125, - "velocityY": 0.22089902286300528, - "timestamp": 0.09068230658102473 - }, - { - "x": 7.623127975291382, - "y": 2.4891588943453957, - "heading": -0.009323742995781282, - "angularVelocity": -0.06827224246710774, - "velocityX": -0.6066084600575472, - "velocityY": 0.4418002358356082, - "timestamp": 0.18136461316204947 - }, - { - "x": 7.5406144347316175, - "y": 2.5492544282514427, - "heading": -0.018481510948534847, - "angularVelocity": -0.10098737337003104, - "velocityX": -0.9099188548544356, - "velocityY": 0.6627040728430443, - "timestamp": 0.2720469197430742 - }, - { - "x": 7.430595523488553, - "y": 2.62938232138062, - "heading": -0.030489675695970903, - "angularVelocity": -0.13242015118689937, - "velocityX": -1.2132345921832182, - "velocityY": 0.8836111050790585, - "timestamp": 0.36272922632409893 - }, - { - "x": 7.293070634953775, - "y": 2.7295429329651553, - "heading": -0.04519602680885156, - "angularVelocity": -0.16217442704482293, - "velocityX": -1.5165570188921023, - "velocityY": 1.1045220987518831, - "timestamp": 0.45341153290512365 - }, - { - "x": 7.128038991250458, - "y": 2.849736716787733, - "heading": -0.06239274145585804, - "angularVelocity": -0.18963693464988343, - "velocityX": -1.8198880236450683, - "velocityY": 1.3254380965175891, - "timestamp": 0.5440938394861484 - }, - { - "x": 6.935499569650833, - "y": 2.9899642543659004, - "heading": -0.0817791104402204, - "angularVelocity": -0.21378336872188647, - "velocityX": -2.1232303065382423, - "velocityY": 1.5463605069735835, - "timestamp": 0.6347761460671731 - }, - { - "x": 6.715451005465253, - "y": 3.1502262816178486, - "heading": -0.10288140612229774, - "angularVelocity": -0.2327057667332536, - "velocityX": -2.426587638559539, - "velocityY": 1.7672910327744458, - "timestamp": 0.7254584526481979 - }, - { - "x": 6.467891597202115, - "y": 3.330523569856046, - "heading": -0.12484344695747294, - "angularVelocity": -0.24218661460217836, - "velocityX": -2.7299637337956644, - "velocityY": 1.9882300642309005, - "timestamp": 0.8161407592292226 - }, - { - "x": 6.192821228785326, - "y": 3.5308551006779862, - "heading": -0.14565067265533538, - "angularVelocity": -0.22945187967038694, - "velocityX": -3.033341109062037, - "velocityY": 2.2091578652440216, - "timestamp": 0.9068230658102473 - }, - { - "x": 5.890325515595207, - "y": 3.7511506939174324, - "heading": -0.15487871649545845, - "angularVelocity": -0.10176234138770857, - "velocityX": -3.335774359906014, - "velocityY": 2.4293117538051567, - "timestamp": 0.997505372391272 - }, - { - "x": 5.615254839431095, - "y": 3.9514825873361543, - "heading": -0.17571165783481632, - "angularVelocity": -0.22973545915204097, - "velocityX": -3.0333445027485793, - "velocityY": 2.209161863783487, - "timestamp": 1.0881876789722968 - }, - { - "x": 5.36769520554361, - "y": 4.131780146286741, - "heading": -0.19763475436907516, - "angularVelocity": -0.24175715595269445, - "velocityX": -2.729966221870307, - "velocityY": 1.9882330495143554, - "timestamp": 1.1788699855533216 - }, - { - "x": 5.1476463659386065, - "y": 4.292042455188194, - "heading": -0.21866611691405802, - "angularVelocity": -0.23192355088797076, - "velocityX": -2.4265906757498517, - "velocityY": 1.767294138667049, - "timestamp": 1.2695522921343463 - }, - { - "x": 4.9551066280664715, - "y": 4.432270284422795, - "heading": -0.23796795023386977, - "angularVelocity": -0.21285115087545256, - "velocityX": -2.123233794236368, - "velocityY": 1.5463637232176781, - "timestamp": 1.360234598715371 - }, - { - "x": 4.790074653740174, - "y": 4.5524643546084755, - "heading": -0.25507770430341364, - "angularVelocity": -0.18867797605320388, - "velocityX": -1.819891669593139, - "velocityY": 1.325441254389438, - "timestamp": 1.4509169052963957 - }, - { - "x": 4.652549445753091, - "y": 4.652625231735707, - "heading": -0.26970206105635147, - "angularVelocity": -0.1612702334591693, - "velocityX": -1.5165605416554253, - "velocityY": 1.1045250270264895, - "timestamp": 1.5415992118774204 - }, - { - "x": 4.542530247982926, - "y": 4.732753356223354, - "heading": -0.2816384072506419, - "angularVelocity": -0.13162817140767386, - "velocityX": -1.2132377518635613, - "velocityY": 0.8836136563867962, - "timestamp": 1.6322815184584452 - }, - { - "x": 4.4600164719635496, - "y": 4.792849076177355, - "heading": -0.2907383923314908, - "angularVelocity": -0.10035017219944754, - "velocityX": -0.9099214513875524, - "velocityY": 0.6627061244886346, - "timestamp": 1.7229638250394699 - }, - { - "x": 4.405007648402666, - "y": 4.832912672083218, - "heading": -0.2968887094471758, - "angularVelocity": -0.0678226806040681, - "velocityX": -0.6066103260367861, - "velocityY": 0.44180168564709604, - "timestamp": 1.8136461316204946 + "x": 7.909158851999345, + "y": 2.302519913622325, + "heading": 0.3890667752206854, + "angularVelocity": -0.05487179586853456, + "velocityX": -0.18241183228183028, + "velocityY": 0.16583672070973293, + "timestamp": 0.059649649053026756 + }, + { + "x": 7.887381785333585, + "y": 2.3222881834031703, + "heading": 0.38257998798822546, + "angularVelocity": -0.10874812065857113, + "velocityX": -0.36508289673927247, + "velocityY": 0.33140630489329076, + "timestamp": 0.11929929810605351 + }, + { + "x": 7.854690906335249, + "y": 2.351914457579234, + "heading": 0.37294712546328734, + "angularVelocity": -0.1614906823068608, + "velocityX": -0.5480481363649796, + "velocityY": 0.49667139113805775, + "timestamp": 0.17894894715908027 + }, + { + "x": 7.811066132526158, + "y": 2.3913778416294793, + "heading": 0.36024582664287896, + "angularVelocity": -0.21293166048835158, + "velocityX": -0.7313500498604669, + "velocityY": 0.6615861899734534, + "timestamp": 0.23859859621210702 + }, + { + "x": 7.756484255635697, + "y": 2.440654043893404, + "heading": 0.34456606928633876, + "angularVelocity": -0.26286420130655497, + "velocityX": -0.9150410397543076, + "velocityY": 0.8260937498579348, + "timestamp": 0.29824824526513377 + }, + { + "x": 7.69091815186407, + "y": 2.499714474772593, + "heading": 0.32601331375601694, + "angularVelocity": -0.3110287457656111, + "velocityX": -1.0991867481624389, + "velocityY": 0.9901220177621776, + "timestamp": 0.35789789431816055 + }, + { + "x": 7.614335701831427, + "y": 2.5685249973363313, + "heading": 0.30471285340408133, + "angularVelocity": -0.3570928025578184, + "velocityX": -1.2838709237763215, + "velocityY": 1.1535779951122258, + "timestamp": 0.4175475433711873 + }, + { + "x": 7.52669827360479, + "y": 2.6470441428134692, + "heading": 0.28081600930802, + "angularVelocity": -0.4006200283729716, + "velocityX": -1.4692027466704807, + "velocityY": 1.3163387668439968, + "timestamp": 0.4771971924242141 + }, + { + "x": 7.4279585237024035, + "y": 2.735220470554149, + "heading": 0.2545092548827881, + "angularVelocity": -0.4410211098114924, + "velocityX": -1.655328262109456, + "velocityY": 1.4782371588187146, + "timestamp": 0.5368468414772408 + }, + { + "x": 7.318057085990513, + "y": 2.8329884911056196, + "heading": 0.2260282157059115, + "angularVelocity": -0.4774720325941529, + "velocityX": -1.842449024539805, + "velocityY": 1.6390376490657574, + "timestamp": 0.5964964905302675 + }, + { + "x": 7.1969173483134, + "y": 2.940262024261505, + "heading": 0.19568025987657156, + "angularVelocity": -0.5087700650570722, + "velocityX": -2.0308541558965927, + "velocityY": 1.7983933662463305, + "timestamp": 0.6561461395832943 + }, + { + "x": 7.064436716981741, + "y": 3.056922608996874, + "heading": 0.1638833679507438, + "angularVelocity": -0.5330608382550797, + "velocityX": -2.2209792251063867, + "velocityY": 1.9557631367061583, + "timestamp": 0.715795788636321 + }, + { + "x": 6.920470867128582, + "y": 3.182797341309092, + "heading": 0.13123894322791613, + "angularVelocity": -0.5472693509698228, + "velocityX": -2.4135238369160836, + "velocityY": 2.110234247988935, + "timestamp": 0.7754454376893477 + }, + { + "x": 6.7648023713494, + "y": 3.317610695629293, + "heading": 0.09868526463286269, + "angularVelocity": -0.5457480322493466, + "velocityX": -2.6097135230552215, + "velocityY": 2.2600862948976657, + "timestamp": 0.8350950867423744 + }, + { + "x": 6.5970691959884045, + "y": 3.4608573798428486, + "heading": 0.0678828870163061, + "angularVelocity": -0.5163882454559657, + "velocityX": -2.8119725434073444, + "velocityY": 2.401467342854161, + "timestamp": 0.8947447357954011 + }, + { + "x": 6.416573137261274, + "y": 3.6113350194856255, + "heading": 0.042516406516172674, + "angularVelocity": -0.4252578330776664, + "velocityX": -3.025936641583165, + "velocityY": 2.522691114393757, + "timestamp": 0.9543943848484279 + }, + { + "x": 6.222302964562549, + "y": 3.7637188891607205, + "heading": 0.038389382258997386, + "angularVelocity": -0.0691877374417826, + "velocityX": -3.2568535738747495, + "velocityY": 2.5546482182926207, + "timestamp": 1.0140440339014547 + }, + { + "x": 6.025130717726082, + "y": 3.9020216671538117, + "heading": 0.02063963006041546, + "angularVelocity": -0.29756674985301135, + "velocityX": -3.305505564017429, + "velocityY": 2.3185849403765046, + "timestamp": 1.0736936829544814 + }, + { + "x": 5.8345208168029785, + "y": 4.0273118019104, + "heading": -0.005999936110084742, + "angularVelocity": -0.4466005516112663, + "velocityX": -3.195490735471996, + "velocityY": 2.1004337283729697, + "timestamp": 1.1333433320075081 + }, + { + "x": 5.64520772360382, + "y": 4.144845020536227, + "heading": -0.03716938606419973, + "angularVelocity": -0.5015668245717481, + "velocityX": -3.046353629773372, + "velocityY": 1.8912994401452536, + "timestamp": 1.1954874939160003 + }, + { + "x": 5.467324746551753, + "y": 4.251208116911629, + "heading": -0.06924096986746248, + "angularVelocity": -0.5160836161969399, + "velocityX": -2.86242458807314, + "velocityY": 1.7115541204340636, + "timestamp": 1.2576316558244924 + }, + { + "x": 5.301847945063855, + "y": 4.347494392232743, + "heading": -0.10089524223846308, + "angularVelocity": -0.5093684008099074, + "velocityX": -2.6627891728842434, + "velocityY": 1.5494017839181233, + "timestamp": 1.3197758177329846 + }, + { + "x": 5.149312148886964, + "y": 4.434404475602975, + "heading": -0.1313397954808994, + "angularVelocity": -0.4899020649319596, + "velocityX": -2.4545474826983984, + "velocityY": 1.3985237020044896, + "timestamp": 1.3819199796414767 + }, + { + "x": 5.010049657244503, + "y": 4.512419916624008, + "heading": -0.1600502646992021, + "angularVelocity": -0.46199785042686925, + "velocityX": -2.240958560959058, + "velocityY": 1.2553945314430697, + "timestamp": 1.4440641415499689 + }, + { + "x": 4.884285206645829, + "y": 4.581890331315179, + "heading": -0.18665500914006652, + "angularVelocity": -0.42811333557028486, + "velocityX": -2.0237532655740638, + "velocityY": 1.1178912476680836, + "timestamp": 1.506208303458461 + }, + { + "x": 4.7721802272313445, + "y": 4.6430804555182, + "heading": -0.21087715173166974, + "angularVelocity": -0.38977342115049435, + "velocityX": -1.8039502983330915, + "velocityY": 0.9846479914416396, + "timestamp": 1.5683524653669532 + }, + { + "x": 4.673855978265995, + "y": 4.696197438200952, + "heading": -0.23250253397752355, + "angularVelocity": -0.34798735040786954, + "velocityX": -1.582196073544819, + "velocityY": 0.8547380968942208, + "timestamp": 1.6304966272754453 + }, + { + "x": 4.589406729682163, + "y": 4.74140765854193, + "heading": -0.2513606323100662, + "angularVelocity": -0.30345727986985116, + "velocityX": -1.358924893189216, + "velocityY": 0.7275055122241522, + "timestamp": 1.6926407891839375 + }, + { + "x": 4.518907788851921, + "y": 4.77884761639657, + "heading": -0.2673125046894516, + "angularVelocity": -0.2566914073581794, + "velocityX": -1.1344418955082554, + "velocityY": 0.6024694308335906, + "timestamp": 1.7547849510924296 + }, + { + "x": 4.462420653358707, + "y": 4.8086312807227785, + "heading": -0.28024280765740234, + "angularVelocity": -0.20806947218937044, + "velocityX": -0.908969302319856, + "velocityY": 0.4792672941677987, + "timestamp": 1.8169291130009217 + }, + { + "x": 4.419996463593151, + "y": 4.830855221116989, + "heading": -0.29005429976132174, + "angularVelocity": -0.15788276489056036, + "velocityX": -0.6826737775951831, + "velocityY": 0.35761911838050064, + "timestamp": 1.879073274909414 + }, + { + "x": 4.391678399570345, + "y": 4.845602296651357, + "heading": -0.29666393327897195, + "angularVelocity": -0.10635968552255907, + "velocityX": -0.4556834166418462, + "velocityY": 0.23730427897770429, + "timestamp": 1.941217436817906 }, { "x": 4.377503395080566, "y": 4.852944374084473, "heading": -0.3, - "angularVelocity": -0.03430978622102242, - "velocityX": -0.30330341561751617, - "velocityY": 0.22089978471552077, - "timestamp": 1.9043284382015193 + "angularVelocity": -0.05368270515805527, + "velocityX": -0.22809873131204866, + "velocityY": 0.11814589186876204, + "timestamp": 2.003361598726398 }, { "x": 4.377503395080566, "y": 4.852944374084473, "heading": -0.3, - "angularVelocity": 0, - "velocityX": -3.367069338391324e-35, + "angularVelocity": -7.790359878532085e-33, + "velocityX": 1.9699324146528777e-35, "velocityY": 0, - "timestamp": 1.995010744782544 + "timestamp": 2.06550576063489 } ], "constraints": [ @@ -7460,16 +7214,16 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 15 + "controlIntervalCount": 14 }, { - "x": 5.6591033935546875, - "y": 4.339165210723877, - "heading": -0.2, + "x": 5.873502254486084, + "y": 4.144256591796875, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 + "headingConstrained": false, + "controlIntervalCount": 14 }, { "x": 4.255763053894043, @@ -7486,262 +7240,262 @@ "x": 7.666658878326416, "y": 4.085784435272217, "heading": 0, - "angularVelocity": 1.209482524629029e-34, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": 1.8680748948039485e-35, + "velocityX": 1.59089430977121e-34, + "velocityY": -8.8306909217665e-34, "timestamp": 0 }, { - "x": 7.649455689544763, - "y": 4.086577766970284, - "heading": -0.0022997827801380754, - "angularVelocity": -0.03564306292608196, - "velocityX": -0.2666227200104806, - "velocityY": 0.012295409757681801, - "timestamp": 0.06452259125170103 - }, - { - "x": 7.615053343965649, - "y": 4.0882531279996925, - "heading": -0.0068609400958710215, - "angularVelocity": -0.07069085768642348, - "velocityX": -0.5331829505252058, - "velocityY": 0.02596549513754648, - "timestamp": 0.12904518250340205 - }, - { - "x": 7.563457156334565, - "y": 4.090914473875886, - "heading": -0.013638379729614996, - "angularVelocity": -0.10503979307504581, - "velocityX": -0.7996608107353951, - "velocityY": 0.041246729627017566, - "timestamp": 0.1935677737551031 - }, - { - "x": 7.494674268159643, - "y": 4.0946853102159935, - "heading": -0.022578417799948743, - "angularVelocity": -0.1385567116401054, - "velocityX": -1.0660279886559765, - "velocityY": 0.058442109452766046, - "timestamp": 0.2580903650068041 - }, - { - "x": 7.408714500593785, - "y": 4.09971475380561, - "heading": -0.03361607330634747, - "angularVelocity": -0.171066525573063, - "velocityX": -1.332242953952841, - "velocityY": 0.07794856796740106, - "timestamp": 0.3226129562585051 - }, - { - "x": 7.305591747951634, - "y": 4.10618637275211, - "heading": -0.0466711017764886, - "angularVelocity": -0.20233267475595532, - "velocityX": -1.598242578942184, - "velocityY": 0.10030004717657835, - "timestamp": 0.3871355475102061 - }, - { - "x": 7.18532637150992, - "y": 4.11433159194397, - "heading": -0.061641941793094415, - "angularVelocity": -0.23202477963424772, - "velocityX": -1.8639266357509128, - "velocityY": 0.1262382528948094, - "timestamp": 0.45165813876190714 - }, - { - "x": 7.047949574625153, - "y": 4.124451015500193, - "heading": -0.07839600669319866, - "angularVelocity": -0.25966199706313403, - "velocityX": -2.129127088973572, - "velocityY": 0.15683535580191885, - "timestamp": 0.5161807300136082 - }, - { - "x": 6.89351205381652, - "y": 4.136950431097767, - "heading": -0.09675311116467075, - "angularVelocity": -0.28450662187236314, - "velocityX": -2.3935418248497737, - "velocityY": 0.1937215377605359, - "timestamp": 0.5807033212653092 - }, - { - "x": 6.722102962824983, - "y": 4.152406451801245, - "heading": -0.11645479321673856, - "angularVelocity": -0.3053454870591373, - "velocityX": -2.6565748161426845, - "velocityY": 0.2395443270897933, - "timestamp": 0.6452259125170102 - }, - { - "x": 6.533897762196436, - "y": 4.171698896366532, - "heading": -0.13710103042159202, - "angularVelocity": -0.31998462560676116, - "velocityX": -2.9168884413578904, - "velocityY": 0.29900294131132776, - "timestamp": 0.7097485037687112 - }, - { - "x": 6.329305940744403, - "y": 4.196316085087732, - "heading": -0.1579986184766986, - "angularVelocity": -0.3238801735904445, - "velocityX": -3.1708556256509532, - "velocityY": 0.3815282096338637, - "timestamp": 0.7742710950204122 - }, - { - "x": 6.109588048034184, - "y": 4.229179536431487, - "heading": -0.1777172131023292, - "angularVelocity": -0.30560760569437684, - "velocityX": -3.405286248549814, - "velocityY": 0.5093324788453677, - "timestamp": 0.8387936862721133 - }, - { - "x": 5.880551755398176, - "y": 4.2764929326979475, - "heading": -0.19275157350982466, - "angularVelocity": -0.23300924708438825, - "velocityX": -3.5497069815832503, - "velocityY": 0.7332841931579067, - "timestamp": 0.9033162775238143 - }, - { - "x": 5.6591033935546875, - "y": 4.339165210723877, - "heading": -0.2, - "angularVelocity": -0.11233935819312842, - "velocityX": -3.432105833747789, - "velocityY": 0.9713230174133182, - "timestamp": 0.9678388687755153 - }, - { - "x": 5.438463656346767, - "y": 4.41684559934421, - "heading": -0.20078792979416127, - "angularVelocity": -0.011394992631344103, - "velocityX": -3.190878423810921, - "velocityY": 1.1234090428973296, - "timestamp": 1.0369858897216806 - }, - { - "x": 5.237468412635918, - "y": 4.49671689701659, - "heading": -0.1976367711927584, - "angularVelocity": 0.045571863520430765, - "velocityX": -2.9067809568735434, - "velocityY": 1.1550938359956655, - "timestamp": 1.106132910667846 - }, - { - "x": 5.056130424824854, - "y": 4.574366670818957, - "heading": -0.19242246838266966, - "angularVelocity": 0.0754089292458229, - "velocityX": -2.6224989208464313, - "velocityY": 1.1229662932669573, - "timestamp": 1.1752799316140112 - }, - { - "x": 4.894040423510362, - "y": 4.647429210924394, - "heading": -0.18621230925465354, - "angularVelocity": 0.08981094258349935, - "velocityX": -2.344135713969339, - "velocityY": 1.0566259992938798, - "timestamp": 1.2444269525601765 - }, - { - "x": 4.750806708701635, - "y": 4.714490254294218, - "heading": -0.17966576596113534, - "angularVelocity": 0.09467570987063913, - "velocityX": -2.0714372484715344, - "velocityY": 0.9698327195040657, - "timestamp": 1.3135739735063419 - }, - { - "x": 4.626109277283802, - "y": 4.774623849709137, - "heading": -0.17322414817467333, - "angularVelocity": 0.09315828358646269, - "velocityX": -1.803366648505599, - "velocityY": 0.8696483896498642, - "timestamp": 1.3827209944525072 - }, - { - "x": 4.519692351368254, - "y": 4.827181352429608, - "heading": -0.1672014372604398, - "angularVelocity": 0.08710007794728412, - "velocityX": -1.5389950927661693, - "velocityY": 0.7600833991299484, - "timestamp": 1.4518680153986725 - }, - { - "x": 4.431350072395874, - "y": 4.871684848267837, - "heading": -0.16183165746625428, - "angularVelocity": 0.0776574279080811, - "velocityX": -1.277600650954415, - "velocityY": 0.6436068427716991, - "timestamp": 1.5210150363448378 - }, - { - "x": 4.360914453717203, - "y": 4.907768355386048, - "heading": -0.1572956227267302, - "angularVelocity": 0.06559985777341837, - "velocityX": -1.0186356218225097, - "velocityY": 0.5218374793948632, - "timestamp": 1.5901620572910031 - }, - { - "x": 4.308246377396777, - "y": 4.935142992782337, - "heading": -0.15373706400788933, - "angularVelocity": 0.051463659173564456, - "velocityX": -0.7616825077891644, - "velocityY": 0.39589033658590256, - "timestamp": 1.6593090782371684 - }, - { - "x": 4.273229022688589, - "y": 4.953575130372871, - "heading": -0.15127288861048924, - "angularVelocity": 0.035636754319735044, - "velocityX": -0.5064188482603104, - "velocityY": 0.26656444975242444, - "timestamp": 1.7284560991833338 + "x": 7.649242795981677, + "y": 4.083908992536083, + "heading": -0.000715303364321087, + "angularVelocity": -0.010994761067841093, + "velocityX": -0.2676985369696649, + "velocityY": -0.0288269925862669, + "timestamp": 0.06505856379301718 + }, + { + "x": 7.614387448826771, + "y": 4.080387640647874, + "heading": -0.0021487131038914233, + "angularVelocity": -0.022032606562462285, + "velocityX": -0.535753406204884, + "velocityY": -0.054125878022934074, + "timestamp": 0.13011712758603436 + }, + { + "x": 7.562069419290744, + "y": 4.075490995112481, + "heading": -0.004303369490494938, + "angularVelocity": -0.03311872044176399, + "velocityX": -0.8041682214577686, + "velocityY": -0.07526519569309446, + "timestamp": 0.19517569137905155 + }, + { + "x": 7.492266246249156, + "y": 4.069542605603016, + "heading": -0.007182794508045951, + "angularVelocity": -0.04425896991381445, + "velocityX": -1.072928281412186, + "velocityY": -0.09143130685131902, + "timestamp": 0.2602342551720687 + }, + { + "x": 7.404958718161784, + "y": 4.062935713928273, + "heading": -0.010790929131606555, + "angularVelocity": -0.05545979519375734, + "velocityX": -1.3419836374676162, + "velocityY": -0.10155298994552601, + "timestamp": 0.3252928189650859 + }, + { + "x": 7.300135300436189, + "y": 4.05615764314467, + "heading": -0.015132141763964354, + "angularVelocity": -0.0667277661734039, + "velocityX": -1.6112162890513386, + "velocityY": -0.1041841440762307, + "timestamp": 0.39035138275810305 + }, + { + "x": 7.177800870279403, + "y": 4.049826474059511, + "heading": -0.02021115360823209, + "angularVelocity": -0.0780683056642115, + "velocityX": -1.8803739742240722, + "velocityY": -0.0973149223721115, + "timestamp": 0.4554099465511202 + }, + { + "x": 7.037994605220974, + "y": 4.044748235322315, + "heading": -0.02603274286903274, + "angularVelocity": -0.08948228982309019, + "velocityX": -2.148929470733793, + "velocityY": -0.07805642241584139, + "timestamp": 0.5204685103441374 + }, + { + "x": 6.880828580156438, + "y": 4.04200948621943, + "heading": -0.03260086081295486, + "angularVelocity": -0.10095700797851251, + "velocityX": -2.415762290181463, + "velocityY": -0.04209667326193334, + "timestamp": 0.5855270741371547 + }, + { + "x": 6.706576693390096, + "y": 4.043131786957125, + "heading": -0.0399161267255408, + "angularVelocity": -0.1124412450274706, + "velocityX": -2.678385082719035, + "velocityY": 0.017250622704578153, + "timestamp": 0.6505856379301719 + }, + { + "x": 6.515894643954466, + "y": 4.050327381007243, + "heading": -0.0479686286684313, + "angularVelocity": -0.12377312798526215, + "velocityX": -2.9309292784618175, + "velocityY": 0.11060179675977738, + "timestamp": 0.7156442017231891 + }, + { + "x": 6.310386764147224, + "y": 4.066852130952412, + "heading": -0.05671696687045534, + "angularVelocity": -0.13446866472270375, + "velocityX": -3.1588136568932224, + "velocityY": 0.2539980746845712, + "timestamp": 0.7807027655162063 + }, + { + "x": 6.0938831151450525, + "y": 4.097012876965575, + "heading": -0.06603485049011525, + "angularVelocity": -0.14322301441058957, + "velocityX": -3.3278270588784395, + "velocityY": 0.4635937877312989, + "timestamp": 0.8457613293092235 + }, + { + "x": 5.873502254486084, + "y": 4.144256591796875, + "heading": -0.07565133376897765, + "angularVelocity": -0.14781272008181154, + "velocityX": -3.3874227743499863, + "velocityY": 0.7261721144291506, + "timestamp": 0.9108198931022408 + }, + { + "x": 5.639993509614856, + "y": 4.215187853453379, + "heading": -0.08600693496822187, + "angularVelocity": -0.14746487395399271, + "velocityX": -3.3251896212558933, + "velocityY": 1.0100687886993767, + "timestamp": 0.9810440822367641 + }, + { + "x": 5.4200052502970575, + "y": 4.301407838205306, + "heading": -0.09591748086235549, + "angularVelocity": -0.1411272385808327, + "velocityX": -3.132656453980309, + "velocityY": 1.2277818485986531, + "timestamp": 1.0512682713712873 + }, + { + "x": 5.2193445983203315, + "y": 4.39418304628854, + "heading": -0.10506991591098606, + "angularVelocity": -0.13033165866960483, + "velocityX": -2.857429248379575, + "velocityY": 1.3211289333011804, + "timestamp": 1.1214924605058105 + }, + { + "x": 5.039091097114449, + "y": 4.486700158570476, + "heading": -0.1133645829032854, + "angularVelocity": -0.11811694936640497, + "velocityX": -2.56682922832449, + "velocityY": 1.3174536213541534, + "timestamp": 1.1917166496403337 + }, + { + "x": 4.878804683581523, + "y": 4.574994396360027, + "heading": -0.12078834137955481, + "angularVelocity": -0.10571511850494018, + "velocityX": -2.2824957540752475, + "velocityY": 1.2573194347664447, + "timestamp": 1.261940838774857 + }, + { + "x": 4.737844755029207, + "y": 4.656730803985265, + "heading": -0.12734948086758455, + "angularVelocity": -0.09343133141005204, + "velocityX": -2.007284531008099, + "velocityY": 1.163935228481773, + "timestamp": 1.3321650279093802 + }, + { + "x": 4.615644585302767, + "y": 4.730430369566539, + "heading": -0.13306001022788766, + "angularVelocity": -0.08131855177941563, + "velocityX": -1.740143549288271, + "velocityY": 1.0494897340871145, + "timestamp": 1.4023892170439034 + }, + { + "x": 4.511741946172554, + "y": 4.795089974494431, + "heading": -0.13793134059619286, + "angularVelocity": -0.0693682679478674, + "velocityX": -1.4795847472325239, + "velocityY": 0.9207597228930422, + "timestamp": 1.4726134061784266 + }, + { + "x": 4.425763852456849, + "y": 4.849990883042018, + "heading": -0.1419734007558902, + "angularVelocity": -0.057559371058800816, + "velocityX": -1.224337294247763, + "velocityY": 0.7817948377078033, + "timestamp": 1.5428375953129498 + }, + { + "x": 4.357406985219521, + "y": 4.894595448858866, + "heading": -0.14519463695889362, + "angularVelocity": -0.04587074970467533, + "velocityX": -0.9734091355100126, + "velocityY": 0.6351738107136011, + "timestamp": 1.613061784447473 + }, + { + "x": 4.3064215304671825, + "y": 4.928487544994276, + "heading": -0.1476022173640631, + "angularVelocity": -0.03428420370305067, + "velocityX": -0.7260383548846598, + "velocityY": 0.4826270912218785, + "timestamp": 1.6832859735819963 + }, + { + "x": 4.272598973020715, + "y": 4.951336158400459, + "heading": -0.1492022547191451, + "angularVelocity": -0.022784703886249428, + "velocityX": -0.4816368528182851, + "velocityY": 0.3253667103569352, + "timestamp": 1.7535101627165195 }, { "x": 4.255763053894043, "y": 4.96287202835083, "heading": -0.15, - "angularVelocity": 0.018408437457923484, - "velocityX": -0.2525917755465569, - "velocityY": 0.13445117158695233, - "timestamp": 1.797603120129499 + "angularVelocity": -0.011359978530000897, + "velocityX": -0.23974529765549837, + "velocityY": 0.1642720278089955, + "timestamp": 1.8237343518510427 }, { "x": 4.255763053894043, "y": 4.96287202835083, "heading": -0.15, - "angularVelocity": 9.849002219211576e-31, - "velocityX": 0, - "velocityY": -4.0048098642858166e-33, - "timestamp": 1.8667501410756644 + "angularVelocity": 0, + "velocityX": -7.182631856201558e-34, + "velocityY": 2.8363828288647375e-33, + "timestamp": 1.893958540985566 } ], "constraints": [ @@ -7763,25 +7517,16 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "OS8": { + "E8": { "waypoints": [ { - "x": 1.0787577629089355, - "y": 3.5595321655273438, - "heading": -1, + "x": 1.4101015329360962, + "y": 1.5324852466583252, + "heading": 0.010868942095556194, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 - }, - { - "x": 3.87052583694458, - "y": 1.7178549766540527, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 20 + "controlIntervalCount": 26 }, { "x": 7.257351398468018, @@ -7795,400 +7540,247 @@ ], "trajectory": [ { - "x": 1.0787577629089355, - "y": 3.559532165527344, - "heading": -1, + "x": 1.4101015329360962, + "y": 1.5324852466583252, + "heading": 0.010868942095556194, "angularVelocity": 0, - "velocityX": -1.6257982956725998e-33, - "velocityY": -6.646520386671484e-34, + "velocityX": -2.7109632930976606e-30, + "velocityY": 0, "timestamp": 0 }, { - "x": 1.0892450498408872, - "y": 3.5521951152447366, - "heading": -0.996813008661085, - "angularVelocity": 0.05725916143580944, - "velocityX": 0.18842010899995426, - "velocityY": -0.13182130163475733, - "timestamp": 0.05565906413924324 - }, - { - "x": 1.1102203082656286, - "y": 3.53752066901722, - "heading": -0.9904934103709541, - "angularVelocity": 0.11354122437849765, - "velocityX": 0.3768525171797188, - "velocityY": -0.2636488136201029, - "timestamp": 0.11131812827848649 - }, - { - "x": 1.1416842952224584, - "y": 3.515508446696833, - "heading": -0.9811023613749315, - "angularVelocity": 0.16872452207476613, - "velocityX": 0.5652985267254234, - "velocityY": -0.39548315554352664, - "timestamp": 0.16697719241772974 - }, - { - "x": 1.1836378528015412, - "y": 3.4861580280924316, - "heading": -0.9687092339674004, - "angularVelocity": 0.22266144066898186, - "velocityX": 0.7537596657056034, - "velocityY": -0.5273250468418692, - "timestamp": 0.22263625655697297 - }, - { - "x": 1.2360819233571791, - "y": 3.4494689463713466, - "heading": -0.9533934750113374, - "angularVelocity": 0.2751709751667287, - "velocityX": 0.9422377355184741, - "velocityY": -0.6591753255013372, - "timestamp": 0.2782953206962162 - }, - { - "x": 1.299017567979401, - "y": 3.4054406802542445, - "heading": -0.9352470387517811, - "angularVelocity": 0.326028411368169, - "velocityX": 1.1307348694324895, - "velocityY": -0.791034969739261, - "timestamp": 0.3339543848354595 - }, - { - "x": 1.372445988957865, - "y": 3.3540726449249463, - "heading": -0.9143776340365992, - "angularVelocity": 0.3749506937984565, - "velocityX": 1.3192536043144198, - "velocityY": -0.9229051210920455, - "timestamp": 0.38961344897470274 - }, - { - "x": 1.4563685570284315, - "y": 3.2953641818290134, - "heading": -0.8909131581838348, - "angularVelocity": 0.42157510579163254, - "velocityX": 1.5077969665572526, - "velocityY": -1.0547871043799972, - "timestamp": 0.445272513113946 - }, - { - "x": 1.5507868440157, - "y": 3.2293145481745777, - "heading": -0.8650079167097484, - "angularVelocity": 0.4654271837787104, - "velocityX": 1.6963685690269719, - "velocityY": -1.1866824330570491, - "timestamp": 0.5009315772531893 - }, - { - "x": 1.6557026605764604, - "y": 3.155922908528008, - "heading": -0.836851636980428, - "angularVelocity": 0.5058705201884368, - "velocityX": 1.8849727027082337, - "velocityY": -1.3185927715738206, - "timestamp": 0.5565906413924325 - }, - { - "x": 1.7711180956540333, - "y": 3.0751883348664535, - "heading": -0.8066830627738127, - "angularVelocity": 0.5420244604031119, - "velocityX": 2.0736143674431173, - "velocityY": -1.4505197834368828, - "timestamp": 0.6122497055316758 - }, - { - "x": 1.8970355444974412, - "y": 2.987109831930365, - "heading": -0.7748115054083663, - "angularVelocity": 0.5726211509002932, - "velocityX": 2.2622990664808507, - "velocityY": -1.5824646766561024, - "timestamp": 0.6679087696709191 - }, - { - "x": 2.033457680736698, - "y": 2.891686434652775, - "heading": -0.7416532565559409, - "angularVelocity": 0.5957385264235328, - "velocityX": 2.4510318013606396, - "velocityY": -1.7144269087756705, - "timestamp": 0.7235678338101623 - }, - { - "x": 2.1803872186925912, - "y": 2.7889175202257976, - "heading": -0.7077985324505156, - "angularVelocity": 0.608251766877191, - "velocityX": 2.639813303154317, - "velocityY": -1.8464003305890786, - "timestamp": 0.7792268979494056 - }, - { - "x": 2.3378258718031506, - "y": 2.6788038388008832, - "heading": -0.674149966624674, - "angularVelocity": 0.6045478188721016, - "velocityX": 2.8286255894761796, - "velocityY": -1.9783602747872513, - "timestamp": 0.8348859620886488 - }, - { - "x": 2.505769659808062, - "y": 2.5613515236455924, - "heading": -0.6422654949724597, - "angularVelocity": 0.5728531757639463, - "velocityX": 3.017366364349273, - "velocityY": -2.110210025476854, - "timestamp": 0.8905450262278921 - }, - { - "x": 2.684179931153943, - "y": 2.436594665396841, - "heading": -0.6155263817868243, - "angularVelocity": 0.4804089612204379, - "velocityX": 3.2054127050995533, - "velocityY": -2.2414472858660566, - "timestamp": 0.9462040903671354 - }, - { - "x": 2.8724826792658567, - "y": 2.3049578433226325, - "heading": -0.6081632212450389, - "angularVelocity": 0.13229041227435911, - "velocityX": 3.383146142034183, - "velocityY": -2.3650563319730016, - "timestamp": 1.0018631545063785 - }, - { - "x": 3.0626614751760415, - "y": 2.171875394056846, - "heading": -0.6081632133489759, - "angularVelocity": 1.4186481590135754e-7, - "velocityX": 3.4168522028040385, - "velocityY": -2.391029229899588, - "timestamp": 1.0575222186456217 - }, - { - "x": 3.2549073856236133, - "y": 2.0417968692290303, - "heading": -0.6081632054441575, - "angularVelocity": 1.4202212353431132e-7, - "velocityX": 3.4539910690310336, - "velocityY": -2.337059144623708, - "timestamp": 1.1131812827848648 - }, - { - "x": 3.4540409810049435, - "y": 1.9225294227125445, - "heading": -0.6081631974633424, - "angularVelocity": 1.43387518536565e-7, - "velocityX": 3.577738836627839, - "velocityY": -2.142821629521334, - "timestamp": 1.168840346924108 - }, - { - "x": 3.6594559882791042, - "y": 1.814438779406305, - "heading": -0.608163189305756, - "angularVelocity": 1.4656348644367822e-7, - "velocityX": 3.6905939841221627, - "velocityY": -1.9420133086648224, - "timestamp": 1.2244994110633511 - }, - { - "x": 3.87052583694458, - "y": 1.7178549766540527, - "heading": -0.6081631808165556, - "angularVelocity": 1.5252143525805278e-7, - "velocityX": 3.792191836669028, - "velocityY": -1.735275363427366, - "timestamp": 1.2801584752025943 - }, - { - "x": 4.138207291973022, - "y": 1.6167788253364266, - "heading": -0.6081631735186005, - "angularVelocity": 1.0636848870334246e-7, - "velocityX": 3.901486265783237, - "velocityY": -1.473195877996273, - "timestamp": 1.3487685988306226 - }, - { - "x": 4.412146386502647, - "y": 1.534152421215255, - "heading": -0.6081631664890607, - "angularVelocity": 1.0245630444469167e-7, - "velocityX": 3.992692040824656, - "velocityY": -1.2042888097554372, - "timestamp": 1.417378722458651 - }, - { - "x": 4.691072911982162, - "y": 1.4703578778498345, - "heading": -0.6081631595875161, - "angularVelocity": 1.0059076244967574e-7, - "velocityX": 4.06538438834075, - "velocityY": -0.9298123949066827, - "timestamp": 1.4859888460866792 - }, - { - "x": 4.9718690126735465, - "y": 1.4153733322733588, - "heading": -0.6081631527089094, - "angularVelocity": 1.0025643942534838e-7, - "velocityX": 4.092633650009569, - "velocityY": -0.8014057207443024, - "timestamp": 1.5545989697147076 - }, - { - "x": 5.252665227857277, - "y": 1.3603893713935364, - "heading": -0.6081631458298762, - "angularVelocity": 1.0026265459162975e-7, - "velocityX": 4.092635318747932, - "velocityY": -0.801397198727107, - "timestamp": 1.6232090933427359 - }, - { - "x": 5.519907312611867, - "y": 1.3079148292099916, - "heading": -0.5750862028334358, - "angularVelocity": 0.48210003491274794, - "velocityX": 3.8950823963450225, - "velocityY": -0.7648221488134482, - "timestamp": 1.6918192169707642 - }, - { - "x": 5.768179080456147, - "y": 1.2592050774901709, - "heading": -0.5321026446123915, - "angularVelocity": 0.6264900272455569, - "velocityX": 3.6185879680131667, - "velocityY": -0.7099499191096337, - "timestamp": 1.7604293405987925 - }, - { - "x": 5.997339300083197, - "y": 1.214259149807144, - "heading": -0.48752011650190086, - "angularVelocity": 0.6497951869639015, - "velocityX": 3.340035078051294, - "velocityY": -0.6550917751832426, - "timestamp": 1.8290394642268208 - }, - { - "x": 6.207384201480997, - "y": 1.1730703596699767, - "heading": -0.4438883449780598, - "angularVelocity": 0.6359378065020302, - "velocityX": 3.061427239754943, - "velocityY": -0.600331087588086, - "timestamp": 1.8976495878548492 - }, - { - "x": 6.398318666454552, - "y": 1.1356341428583274, - "heading": -0.40244988370615753, - "angularVelocity": 0.6039700714804472, - "velocityX": 2.7828905542965074, - "velocityY": -0.5456369239998863, - "timestamp": 1.9662597114828775 - }, - { - "x": 6.570147668959068, - "y": 1.1019474014538393, - "heading": -0.36394173985933226, - "angularVelocity": 0.5612603769029573, - "velocityX": 2.504426364775143, - "velocityY": -0.4909879129080382, - "timestamp": 2.034869835110906 - }, - { - "x": 6.722875355146629, - "y": 1.0720079227066874, - "heading": -0.32885233994862423, - "angularVelocity": 0.5114318129048492, - "velocityX": 2.2260226058705035, - "velocityY": -0.4363711528850982, - "timestamp": 2.103479958738934 - }, - { - "x": 6.856505086896474, - "y": 1.0458140505512963, - "heading": -0.2975293413887698, - "angularVelocity": 0.45653610434625136, - "velocityX": 1.9476678467207353, - "velocityY": -0.381778530197699, - "timestamp": 2.1720900823669624 - }, - { - "x": 6.971039601667182, - "y": 1.0233644985449248, - "heading": -0.2702328493192886, - "angularVelocity": 0.39784933514287446, - "velocityX": 1.6693529863269239, - "velocityY": -0.3272046575529071, - "timestamp": 2.2407002059949908 - }, - { - "x": 7.0664811548927515, - "y": 1.0046582381111746, - "heading": -0.24716475692257864, - "angularVelocity": 0.3362199508890913, - "velocityX": 1.3910709991284718, - "velocityY": -0.27264577652077626, - "timestamp": 2.309310329623019 - }, - { - "x": 7.142831629478478, - "y": 0.9896944283927934, - "heading": -0.22848628077286834, - "angularVelocity": 0.27224081756471324, - "velocityX": 1.1128164554791231, - "velocityY": -0.21809915107437988, - "timestamp": 2.3779204532510474 - }, - { - "x": 7.200092617495255, - "y": 0.9784723702199377, - "heading": -0.2143291042772057, - "angularVelocity": 0.20634238428743115, - "velocityX": 0.8345851164358524, - "velocityY": -0.16356271610435189, - "timestamp": 2.4465305768790757 - }, - { - "x": 7.238265481117147, - "y": 0.9709914747194018, - "heading": -0.20480280588812247, - "angularVelocity": 0.13884683316897098, - "velocityX": 0.5563736312274852, - "velocityY": -0.10903486402522468, - "timestamp": 2.515140700507104 + "x": 1.445714339271355, + "y": 1.5290426767816314, + "heading": 0.008951940936462094, + "angularVelocity": -0.02061695772149426, + "velocityX": 0.38300849170323675, + "velocityY": -0.037024139115655316, + "timestamp": 0.09298176700179253 + }, + { + "x": 1.5169399514953399, + "y": 1.5221575370547553, + "heading": 0.005117939665931709, + "angularVelocity": -0.0412339041755727, + "velocityX": 0.7660169786041023, + "velocityY": -0.07404827794618403, + "timestamp": 0.18596353400358506 + }, + { + "x": 1.6237783690472765, + "y": 1.5118298275067226, + "heading": -0.0006330612434550333, + "angularVelocity": -0.061850845547063264, + "velocityX": 1.1490254594739377, + "velocityY": -0.11107241646454527, + "timestamp": 0.2789453010053776 + }, + { + "x": 1.7662295912051191, + "y": 1.4980595481733934, + "heading": -0.00830106144931283, + "angularVelocity": -0.08246778323615674, + "velocityX": 1.5320339325783188, + "velocityY": -0.14809655459723625, + "timestamp": 0.3719270680071701 + }, + { + "x": 1.9442936170070497, + "y": 1.4808466991047853, + "heading": -0.01788606005843331, + "angularVelocity": -0.10308471131882355, + "velocityX": 1.9150423953385212, + "velocityY": -0.1851206921920015, + "timestamp": 0.46490883500896263 + }, + { + "x": 2.157970445109853, + "y": 1.4601912803784864, + "heading": -0.029388054546433492, + "angularVelocity": -0.12370161225204816, + "velocityX": 2.2980508436527027, + "velocityY": -0.2221448289523177, + "timestamp": 0.5578906020107551 + }, + { + "x": 2.407260073505042, + "y": 1.436093292126851, + "heading": -0.04280703888278391, + "angularVelocity": -0.14431844832782198, + "velocityX": 2.6810592703659726, + "velocityY": -0.2591689642891874, + "timestamp": 0.6508723690125476 + }, + { + "x": 2.6921624988551995, + "y": 1.4085527346009814, + "heading": -0.058142999714275743, + "angularVelocity": -0.1649351407919763, + "velocityX": 3.064067661185958, + "velocityY": -0.29619309692549817, + "timestamp": 0.7438541360143401 + }, + { + "x": 3.012677714499943, + "y": 1.3775696083634381, + "heading": -0.07539590621623399, + "angularVelocity": -0.18555150174179996, + "velocityX": 3.4470759803747364, + "velocityY": -0.3332172235115734, + "timestamp": 0.8368359030161326 + }, + { + "x": 3.3688057004723464, + "y": 1.3431439152534057, + "heading": -0.09456566508261226, + "angularVelocity": -0.20616685919988256, + "velocityX": 3.8300840848227304, + "velocityY": -0.3702413303175326, + "timestamp": 0.9298176700179251 + }, + { + "x": 3.7547740072816946, + "y": 1.3058336555455088, + "heading": -0.09456566629426387, + "angularVelocity": -1.3030834763042814e-8, + "velocityX": 4.151010668596085, + "velocityY": -0.4012642576170675, + "timestamp": 1.0227994370197178 + }, + { + "x": 4.140742314090614, + "y": 1.2685233958331112, + "heading": -0.09456566750542322, + "angularVelocity": -1.3025737916689111e-8, + "velocityX": 4.151010668590924, + "velocityY": -0.4012642576654214, + "timestamp": 1.1157812040215103 + }, + { + "x": 4.526710620899537, + "y": 1.231213136120722, + "heading": -0.09456566871658223, + "angularVelocity": -1.3025734308185157e-8, + "velocityX": 4.151010668590951, + "velocityY": -0.401264257665331, + "timestamp": 1.2087629710233028 + }, + { + "x": 4.912678927708486, + "y": 1.1939028764086315, + "heading": -0.09456566992774154, + "angularVelocity": -1.3025737526913983e-8, + "velocityX": 4.151010668591244, + "velocityY": -0.4012642576621162, + "timestamp": 1.3017447380250953 + }, + { + "x": 5.2986472345138385, + "y": 1.1565926166593978, + "heading": -0.09456567113938757, + "angularVelocity": -1.3030774721558205e-8, + "velocityX": 4.151010668553108, + "velocityY": -0.40126425806165034, + "timestamp": 1.3947265050268878 + }, + { + "x": 5.654775219974997, + "y": 1.1221669168423432, + "heading": -0.11373537224602893, + "angularVelocity": -0.20616623800565617, + "velocityX": 3.8300840793243975, + "velocityY": -0.3702414024501916, + "timestamp": 1.4877082720286803 + }, + { + "x": 5.975290435116198, + "y": 1.0911837840655934, + "heading": -0.13098822471299915, + "angularVelocity": -0.18555092060649123, + "velocityX": 3.4470759749592483, + "velocityY": -0.333217293839406, + "timestamp": 1.5806900390304728 + }, + { + "x": 6.260192859980051, + "y": 1.0636432203316484, + "heading": -0.1463241379521425, + "angularVelocity": -0.16493462894594582, + "velocityX": 3.0640676559558413, + "velocityY": -0.296193163692087, + "timestamp": 1.6736718060322653 + }, + { + "x": 6.509482487916198, + "y": 1.0395452263459009, + "heading": -0.15974308275948657, + "angularVelocity": -0.1443180232013969, + "velocityX": 2.6810592654290843, + "velocityY": -0.2591690259583974, + "timestamp": 1.7666535730340578 + }, + { + "x": 6.72315931559752, + "y": 1.0188898024827011, + "heading": -0.1712450464462443, + "angularVelocity": -0.12370128099094915, + "velocityX": 2.298050839119749, + "velocityY": -0.22214488419863948, + "timestamp": 1.8596353400358503 + }, + { + "x": 6.9012233410258, + "y": 1.0016769489785289, + "heading": -0.1808300228356812, + "angularVelocity": -0.1030844723506456, + "velocityX": 1.915042391319989, + "velocityY": -0.18512073989559208, + "timestamp": 1.9526171070376428 + }, + { + "x": 7.043674562867799, + "y": 0.9879066659964851, + "heading": -0.18849800859117913, + "angularVelocity": -0.08246762782547498, + "velocityX": 1.5320339291814782, + "velocityY": -0.14809659383842053, + "timestamp": 2.0455988740394355 + }, + { + "x": 7.150512980171106, + "y": 0.9775789536540375, + "heading": -0.19424900148191884, + "angularVelocity": -0.06185075930814551, + "velocityX": 1.1490254567999838, + "velocityY": -0.1110724465179094, + "timestamp": 2.1385806410412282 + }, + { + "x": 7.221738592222265, + "y": 0.9706938120369626, + "heading": -0.1980829994380352, + "angularVelocity": -0.04123386852972422, + "velocityX": 0.766016976745399, + "velocityY": -0.07404829827488661, + "timestamp": 2.231562408043021 }, { "x": 7.257351398468018, "y": 0.9672512412071228, "heading": -0.2, - "angularVelocity": 0.07000141719844519, - "velocityX": 0.27817931730228895, - "velocityY": -0.05451430947066675, - "timestamp": 2.5837508241351324 + "angularVelocity": -0.020616951299490285, + "velocityX": 0.38300849074060805, + "velocityY": -0.0370241493665455, + "timestamp": 2.3245441750448137 }, { "x": 7.257351398468018, "y": 0.9672512412071228, "heading": -0.2, "angularVelocity": 0, - "velocityX": 3.164263694858479e-35, + "velocityX": -1.1302654275028148e-31, "velocityY": 0, - "timestamp": 2.6523609477631607 + "timestamp": 2.4175259420466064 } ], "constraints": [ @@ -9064,6 +8656,816 @@ "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [] + }, + "8Z": { + "waypoints": [ + { + "x": 7.842076778411865, + "y": 0.8892878890037537, + "heading": -0.2, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 7.842076778411865, + "y": 0.8892878890037537, + "heading": -0.2, + "angularVelocity": 0, + "velocityX": 3.543965029852756e-29, + "velocityY": 6.066534800985091e-29, + "timestamp": 0 + }, + { + "x": 7.812598945860675, + "y": 0.8944424472583185, + "heading": -0.20496622782556165, + "angularVelocity": -0.05838115294284956, + "velocityX": -0.34653058841526674, + "velocityY": 0.06059509639568072, + "timestamp": 0.08506560037310541 + }, + { + "x": 7.753643280710999, + "y": 0.9047515675559099, + "heading": -0.21489854838800776, + "angularVelocity": -0.11676071783272662, + "velocityX": -0.6930611773865226, + "velocityY": 0.1211902373271282, + "timestamp": 0.17013120074621083 + }, + { + "x": 7.665209782688016, + "y": 0.9202152555182115, + "heading": -0.2297967433211858, + "angularVelocity": -0.17513771569016606, + "velocityX": -1.039591769588472, + "velocityY": 0.1817854443450271, + "timestamp": 0.25519680111931625 + }, + { + "x": 7.547298450975015, + "y": 0.9408335185902585, + "heading": -0.24966046486395704, + "angularVelocity": -0.23351062539515693, + "velocityX": -1.386122371391394, + "velocityY": 0.24238073888402914, + "timestamp": 0.34026240149242165 + }, + { + "x": 7.399909283767824, + "y": 0.9666063659909732, + "heading": -0.2744891709604369, + "angularVelocity": -0.29187716288991106, + "velocityX": -1.732652994403474, + "velocityY": 0.30297614179729954, + "timestamp": 0.42532800186552705 + }, + { + "x": 7.223042277660287, + "y": 0.9975338086066862, + "heading": -0.3042820351108166, + "angularVelocity": -0.3502339843559568, + "velocityX": -2.079183657457103, + "velocityY": 0.3635716726862842, + "timestamp": 0.5103936022386325 + }, + { + "x": 7.016697426765236, + "y": 1.0336158588375843, + "heading": -0.33903781326789106, + "angularVelocity": -0.40857618125920553, + "velocityX": -2.4257143897180984, + "velocityY": 0.42416734934731337, + "timestamp": 0.5954592026117379 + }, + { + "x": 6.780874721269294, + "y": 1.0748525305163557, + "heading": -0.37875461009111894, + "angularVelocity": -0.4668960972340024, + "velocityX": -2.7722452373415654, + "velocityY": 0.48476318862035184, + "timestamp": 0.6805248029848433 + }, + { + "x": 6.515574144192628, + "y": 1.1212438395106257, + "heading": -0.42342930599477324, + "angularVelocity": -0.5251793405028335, + "velocityX": -3.1187762845737237, + "velocityY": 0.5453592144273801, + "timestamp": 0.7655904033579487 + }, + { + "x": 6.220795658543064, + "y": 1.1727898092300164, + "heading": -0.47305507638202704, + "angularVelocity": -0.5833823563138879, + "velocityX": -3.4653077666723004, + "velocityY": 0.6059555154293409, + "timestamp": 0.8506560037310541 + }, + { + "x": 5.896539001466683, + "y": 1.2294906138948394, + "heading": -0.527568943776392, + "angularVelocity": -0.6408450320209716, + "velocityX": -3.8118423387850986, + "velocityY": 0.6665538645013592, + "timestamp": 0.9357216041041595 + }, + { + "x": 5.601760097539714, + "y": 1.281036808508122, + "heading": -0.5771161617191909, + "angularVelocity": -0.582458922590698, + "velocityX": -3.4653126837880555, + "velocityY": 0.6059581591994512, + "timestamp": 1.020787204477265 + }, + { + "x": 5.336459014638101, + "y": 1.3274283800189033, + "heading": -0.6217016980784473, + "angularVelocity": -0.5241312136014974, + "velocityX": -3.118782230866253, + "velocityY": 0.5453623004752052, + "timestamp": 1.1058528048503704 + }, + { + "x": 5.100635786430906, + "y": 1.3686653237117123, + "heading": -0.6613288423924588, + "angularVelocity": -0.46584217521829746, + "velocityX": -2.7722513821433346, + "velocityY": 0.48476638631762087, + "timestamp": 1.1909184052234758 + }, + { + "x": 4.894290433316691, + "y": 1.404747640025304, + "heading": -0.6959996876310143, + "angularVelocity": -0.40757774102030764, + "velocityX": -2.425720293622384, + "velocityY": 0.42417047731787116, + "timestamp": 1.2759840055965812 + }, + { + "x": 4.717422969310233, + "y": 1.4356753315766229, + "heading": -0.7257156923972772, + "angularVelocity": -0.34933045362611315, + "velocityX": -2.079189040348869, + "velocityY": 0.3635745990819682, + "timestamp": 1.3610496059696866 + }, + { + "x": 4.57003340445074, + "y": 1.4614484017859215, + "heading": -0.7504778958597699, + "angularVelocity": -0.29109538231590903, + "velocityX": -1.7326576690580915, + "velocityY": 0.3029787610533009, + "timestamp": 1.446115206342792 + }, + { + "x": 4.452121745929318, + "y": 1.4820668540702735, + "heading": -0.7702870288254154, + "angularVelocity": -0.23286890210383354, + "velocityX": -1.3861262132313132, + "velocityY": 0.24238296319449323, + "timestamp": 1.5311808067158974 + }, + { + "x": 4.363687998733753, + "y": 1.4975306913245487, + "heading": -0.785143583437153, + "angularVelocity": -0.17464820734362577, + "velocityX": -1.03959469876997, + "velocityY": 0.18178719936671658, + "timestamp": 1.6162464070890028 + }, + { + "x": 4.304732166058997, + "y": 1.5078399155840347, + "heading": -0.795047860373283, + "angularVelocity": -0.11643104724722543, + "velocityX": -0.6930631467499093, + "velocityY": 0.12119145946503349, + "timestamp": 1.7013120074621082 + }, + { + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, + "angularVelocity": -0.05821553724430813, + "velocityX": -0.3465315751249751, + "velocityY": 0.06059573094328386, + "timestamp": 1.7863776078352136 + }, + { + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, + "angularVelocity": 0, + "velocityX": 1.4177041903420033e-28, + "velocityY": 0, + "timestamp": 1.871443208208319 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "Z7": { + "waypoints": [ + { + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 7.335314750671387, + "y": 2.097719430923462, + "heading": 0.4335816497672733, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, + "angularVelocity": 0, + "velocityX": -1.4126138451676342e-35, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 4.296505463860315, + "y": 1.517055896140876, + "heading": -0.7914887789961823, + "angularVelocity": 0.11746007928103745, + "velocityX": 0.2932798142493999, + "velocityY": 0.05604937824134911, + "timestamp": 0.07246054196382429 + }, + { + "x": 4.339007949385954, + "y": 1.5251785389422798, + "heading": -0.774469146362606, + "angularVelocity": 0.23488138747393392, + "velocityX": 0.586560414450918, + "velocityY": 0.11209746133915541, + "timestamp": 0.14492108392764858 + }, + { + "x": 4.402761808267112, + "y": 1.5373623057737362, + "heading": -0.7489458109954873, + "angularVelocity": 0.35223771000582876, + "velocityX": 0.8798424239358591, + "velocityY": 0.16814346817249837, + "timestamp": 0.21738162589147286 + }, + { + "x": 4.487767176416459, + "y": 1.5536069760091191, + "heading": -0.714924557460497, + "angularVelocity": 0.4695142019773372, + "velocityX": 1.1731263090991852, + "velocityY": 0.22418643022975243, + "timestamp": 0.28984216785529715 + }, + { + "x": 4.594024205006697, + "y": 1.5739122453438348, + "heading": -0.6724110080488273, + "angularVelocity": 0.5867131028759709, + "velocityX": 1.4664122805386484, + "velocityY": 0.28022519269664276, + "timestamp": 0.36230270981912144 + }, + { + "x": 4.7215330332195835, + "y": 1.5982777172250429, + "heading": -0.6214089357318703, + "angularVelocity": 0.7038599344512155, + "velocityX": 1.7597001727718804, + "velocityY": 0.33625848249068047, + "timestamp": 0.4347632517829457 + }, + { + "x": 4.8702937518908325, + "y": 1.6267029058241753, + "heading": -0.561918181875302, + "angularVelocity": 0.8210089552776038, + "velocityX": 2.052989318593802, + "velocityY": 0.39228506755198983, + "timestamp": 0.5072237937467701 + }, + { + "x": 5.040306360048619, + "y": 1.6591872583478575, + "heading": -0.493932387640852, + "angularVelocity": 0.9382457319790903, + "velocityX": 2.34627845100393, + "velocityY": 0.44830402372507494, + "timestamp": 0.5796843357105943 + }, + { + "x": 5.231570719979306, + "y": 1.6957302040108748, + "heading": -0.4174369611049703, + "angularVelocity": 1.0556838861910776, + "velocityX": 2.639565682881242, + "velocityY": 0.5043151027115036, + "timestamp": 0.6521448776744185 + }, + { + "x": 5.444086520570827, + "y": 1.736331233452616, + "heading": -0.3324079212031443, + "angularVelocity": 1.173452993827683, + "velocityX": 2.9328486212208924, + "velocityY": 0.5603191522085388, + "timestamp": 0.7246054196382428 + }, + { + "x": 5.677853259899273, + "y": 1.7809900044282545, + "heading": -0.23881232004877342, + "angularVelocity": 1.2916768025430743, + "velocityX": 3.2261246327022186, + "velocityY": 0.6163184785166836, + "timestamp": 0.797065961602067 + }, + { + "x": 5.932870232495776, + "y": 1.8297064569167303, + "heading": -0.13660994045650976, + "angularVelocity": 1.4104556331263418, + "velocityX": 3.519390908279707, + "velocityY": 0.6723169764973237, + "timestamp": 0.8695265035658912 + }, + { + "x": 6.1666110103130665, + "y": 1.8743699539047558, + "heading": -0.04151626999903835, + "angularVelocity": 1.3123510793632576, + "velocityX": 3.2257663478971033, + "velocityY": 0.6163837003913594, + "timestamp": 0.9419870455297155 + }, + { + "x": 6.379102210383419, + "y": 1.9149751005455964, + "heading": 0.04493399031560497, + "angularVelocity": 1.1930667087447864, + "velocityX": 2.9325091189138175, + "velocityY": 0.5603759720857812, + "timestamp": 1.0144475874935397 + }, + { + "x": 6.570344223254283, + "y": 1.9515213437751668, + "heading": 0.12272453353903856, + "angularVelocity": 1.0735572922194023, + "velocityX": 2.639257279725309, + "velocityY": 0.5043606111560126, + "timestamp": 1.086908129457364 + }, + { + "x": 6.740337234658211, + "y": 1.9840081308159054, + "heading": 0.1918507517039546, + "angularVelocity": 0.9539842829139629, + "velocityX": 2.3460080037601223, + "velocityY": 0.44833762155626, + "timestamp": 1.1593686714211882 + }, + { + "x": 6.8890812999679625, + "y": 2.0124349614152695, + "heading": 0.252315028167953, + "angularVelocity": 0.8344441654077742, + "velocityX": 2.0527594919730525, + "velocityY": 0.3923077281640593, + "timestamp": 1.2318292133850124 + }, + { + "x": 7.016576391150559, + "y": 2.0368014172627156, + "heading": 0.30412372738559124, + "angularVelocity": 0.714991881284902, + "velocityX": 1.7595105933136266, + "velocityY": 0.33627206182933134, + "timestamp": 1.3042897553488366 + }, + { + "x": 7.12282243639323, + "y": 2.057107171032732, + "heading": 0.34728480849502247, + "angularVelocity": 0.5956494381587609, + "velocityX": 1.4662607035938906, + "velocityY": 0.2802318782014398, + "timestamp": 1.3767502973126609 + }, + { + "x": 7.207819351985288, + "y": 2.0733519822331425, + "heading": 0.3818060258723622, + "angularVelocity": 0.47641401018742496, + "velocityX": 1.1730096586151824, + "velocityY": 0.2241883756337472, + "timestamp": 1.449210839276485 + }, + { + "x": 7.271567065989971, + "y": 2.085535686633377, + "heading": 0.40769367366399617, + "angularVelocity": 0.3572654453034354, + "velocityX": 0.8797576208650236, + "velocityY": 0.16814260658327587, + "timestamp": 1.5216713812403093 + }, + { + "x": 7.314065534356957, + "y": 2.093658184470014, + "heading": 0.42495178086767527, + "angularVelocity": 0.23817248306388825, + "velocityX": 0.5865049751932958, + "velocityY": 0.11209546073630015, + "timestamp": 1.5941319232041335 + }, + { + "x": 7.335314750671387, + "y": 2.097719430923462, + "heading": 0.4335816497672733, + "angularVelocity": 0.11909749314194312, + "velocityX": 0.2932522409925998, + "velocityY": 0.056047696351426264, + "timestamp": 1.6665924651679578 + }, + { + "x": 7.335314750671387, + "y": 2.097719430923462, + "heading": 0.4335816497672733, + "angularVelocity": 0, + "velocityX": -1.6349703542384758e-35, + "velocityY": 0, + "timestamp": 1.739053007131782 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "Y6": { + "waypoints": [ + { + "x": 4.377503395080566, + "y": 4.852944374084473, + "heading": -0.3, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 5.67859411239624, + "y": 4.300183296203613, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 12 + }, + { + "x": 7.062443256378174, + "y": 4.124765872955322, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 4.377503395080566, + "y": 4.852944374084473, + "heading": -0.3, + "angularVelocity": -1.3779455514923654e-31, + "velocityX": 1.0651190916723646e-33, + "velocityY": 8.422201210225719e-34, + "timestamp": 0 + }, + { + "x": 4.3896055374193645, + "y": 4.846825912939869, + "heading": -0.2985310488008376, + "angularVelocity": 0.025658081384651203, + "velocityX": 0.21138738525455975, + "velocityY": -0.10687078923150421, + "timestamp": 0.05725101488068306 + }, + { + "x": 4.41384571535501, + "y": 4.83466051950474, + "heading": -0.2955924685875449, + "angularVelocity": 0.05132800212218199, + "velocityX": 0.4234017158676471, + "velocityY": -0.21249218831288152, + "timestamp": 0.11450202976136611 + }, + { + "x": 4.450265411872471, + "y": 4.816532199021785, + "heading": -0.29118353531337676, + "angularVelocity": 0.07701056974023836, + "velocityX": 0.636140627259864, + "velocityY": -0.3166462729217243, + "timestamp": 0.17175304464204916 + }, + { + "x": 4.498913092380378, + "y": 4.792540991453841, + "heading": -0.28530348440215925, + "angularVelocity": 0.10270649216389094, + "velocityX": 0.8497260810012408, + "velocityY": -0.41905296557529276, + "timestamp": 0.22900405952273223 + }, + { + "x": 4.55984608940975, + "y": 4.762808016255157, + "heading": -0.2779515279580758, + "angularVelocity": 0.1284161767159208, + "velocityX": 1.0643129585800981, + "velocityY": -0.5193440720771622, + "timestamp": 0.2862550744034153 + }, + { + "x": 4.6331332077909675, + "y": 4.727482864001583, + "heading": -0.26912689582630617, + "angularVelocity": 0.15413931351542365, + "velocityX": 1.280101645952553, + "velocityY": -0.6170222890754846, + "timestamp": 0.34350608928409837 + }, + { + "x": 4.718858408122676, + "y": 4.686754865040645, + "heading": -0.25882892740601127, + "angularVelocity": 0.17987398898966347, + "velocityX": 1.4973568680025728, + "velocityY": -0.7113934843918396, + "timestamp": 0.40075710416478144 + }, + { + "x": 4.817126132111491, + "y": 4.640871125529565, + "heading": -0.24705727650199802, + "angularVelocity": 0.20561471143431556, + "velocityX": 1.7164363669991671, + "velocityY": -0.8014484914670451, + "timestamp": 0.4580081190454645 + }, + { + "x": 4.928069127821043, + "y": 4.590167209877807, + "heading": -0.2338123853794331, + "angularVelocity": 0.23134770886016032, + "velocityX": 1.937834568361266, + "velocityY": -0.885642215381343, + "timestamp": 0.5152591339261475 + }, + { + "x": 5.051859843763298, + "y": 4.535123586269269, + "heading": -0.21909666295203592, + "angularVelocity": 0.2570386299363643, + "velocityX": 2.162244917408828, + "velocityY": -0.9614436307068738, + "timestamp": 0.5725101488068306 + }, + { + "x": 5.188725109230697, + "y": 4.476480746595766, + "heading": -0.20291776182097876, + "angularVelocity": 0.28259588349264136, + "velocityX": 2.390617279931895, + "velocityY": -1.0243109191290498, + "timestamp": 0.6297611636875137 + }, + { + "x": 5.338949906037725, + "y": 4.41550875888357, + "heading": -0.18529937753844272, + "angularVelocity": 0.3077392482780291, + "velocityX": 2.6239674025012714, + "velocityY": -1.0649940064689996, + "timestamp": 0.6870121785681967 + }, + { + "x": 5.50273444641256, + "y": 4.35475741462076, + "heading": -0.16632687899377058, + "angularVelocity": 0.33139147985781603, + "velocityX": 2.860814619901091, + "velocityY": -1.0611400407384983, + "timestamp": 0.7442631934488798 + }, + { + "x": 5.67859411239624, + "y": 4.300183296203613, + "heading": -0.1463958515995223, + "angularVelocity": 0.3481340450607809, + "velocityX": 3.0717301055742974, + "velocityY": -0.9532428120424219, + "timestamp": 0.8015142083295629 + }, + { + "x": 5.902938999773969, + "y": 4.25270379053833, + "heading": -0.12188146282745, + "angularVelocity": 0.3420695091436435, + "velocityX": 3.130469465003068, + "velocityY": -0.6625207484597521, + "timestamp": 0.8731791422590521 + }, + { + "x": 6.111777839654233, + "y": 4.219753932279735, + "heading": -0.09952223180713043, + "angularVelocity": 0.31199681342507896, + "velocityX": 2.9141007802468644, + "velocityY": -0.45977658042655406, + "timestamp": 0.9448440761885414 + }, + { + "x": 6.301050755084051, + "y": 4.1950916994922665, + "heading": -0.07947119870245584, + "angularVelocity": 0.2797886219277436, + "velocityX": 2.641081279947074, + "velocityY": -0.3441324987717529, + "timestamp": 1.0165090101180305 + }, + { + "x": 6.469818526680435, + "y": 4.176002062195389, + "heading": -0.06171169483230788, + "angularVelocity": 0.24781302230211338, + "velocityX": 2.354956076041784, + "velocityY": -0.2663734723548429, + "timestamp": 1.0881739440475198 + }, + { + "x": 6.617746045928132, + "y": 4.161060334088034, + "heading": -0.046218926010036, + "angularVelocity": 0.2161833964364665, + "velocityX": 2.064154826309365, + "velocityY": -0.20849427032272078, + "timestamp": 1.159838877977009 + }, + { + "x": 6.74467949537251, + "y": 4.1494012139137455, + "heading": -0.032972770112789145, + "angularVelocity": 0.18483455116668027, + "velocityX": 1.771207234618634, + "velocityY": -0.16268933123917845, + "timestamp": 1.2315038119064983 + }, + { + "x": 6.850536459076317, + "y": 4.140446204164987, + "heading": -0.021957698041216698, + "angularVelocity": 0.15370239624319212, + "velocityX": 1.4771096253009843, + "velocityY": -0.12495664556908366, + "timestamp": 1.3031687458359875 + }, + { + "x": 6.935267964024793, + "y": 4.1337821626583535, + "heading": -0.013161616848593847, + "angularVelocity": 0.12273898419103196, + "velocityX": 1.1823286550691923, + "velocityY": -0.0929888739336499, + "timestamp": 1.3748336797654768 + }, + { + "x": 6.998842680180015, + "y": 4.129099600800989, + "heading": -0.006574926597859502, + "angularVelocity": 0.09190952798777466, + "velocityX": 0.8871105109476977, + "velocityY": -0.06533965219268564, + "timestamp": 1.446498613694966 + }, + { + "x": 7.041239426446096, + "y": 4.126158166211539, + "heading": -0.002189855651721091, + "angularVelocity": 0.06118851585704257, + "velocityX": 0.5915968095051178, + "velocityY": -0.041044265698249975, + "timestamp": 1.5181635476244553 + }, + { + "x": 7.062443256378174, + "y": 4.124765872955322, + "heading": -5.255644557217204e-33, + "angularVelocity": 0.030556864168404942, + "velocityX": 0.2958745479754329, + "velocityY": -0.01942781748165762, + "timestamp": 1.5898284815539445 + }, + { + "x": 7.062443256378174, + "y": 4.124765872955322, + "heading": -2.1819063016426035e-33, + "angularVelocity": 4.289059739434279e-32, + "velocityX": 0, + "velocityY": 6.467184527739888e-33, + "timestamp": 1.6614934154834338 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] } }, "splitTrajectoriesAtStopPoints": false, diff --git a/src/main/deploy/choreo/6Y.traj b/src/main/deploy/choreo/6Y.traj index 29f053b2..3f2bafd2 100644 --- a/src/main/deploy/choreo/6Y.traj +++ b/src/main/deploy/choreo/6Y.traj @@ -4,262 +4,262 @@ "x": 7.666658878326416, "y": 4.085784435272217, "heading": 0, - "angularVelocity": 1.209482524629029e-34, - "velocityX": 0, - "velocityY": 0, + "angularVelocity": 1.8680748948039485e-35, + "velocityX": 1.59089430977121e-34, + "velocityY": -8.8306909217665e-34, "timestamp": 0 }, { - "x": 7.649455689544763, - "y": 4.086577766970284, - "heading": -0.0022997827801380754, - "angularVelocity": -0.03564306292608196, - "velocityX": -0.2666227200104806, - "velocityY": 0.012295409757681801, - "timestamp": 0.06452259125170103 + "x": 7.649242795981677, + "y": 4.083908992536083, + "heading": -0.000715303364321087, + "angularVelocity": -0.010994761067841093, + "velocityX": -0.2676985369696649, + "velocityY": -0.0288269925862669, + "timestamp": 0.06505856379301718 }, { - "x": 7.615053343965649, - "y": 4.0882531279996925, - "heading": -0.0068609400958710215, - "angularVelocity": -0.07069085768642348, - "velocityX": -0.5331829505252058, - "velocityY": 0.02596549513754648, - "timestamp": 0.12904518250340205 + "x": 7.614387448826771, + "y": 4.080387640647874, + "heading": -0.0021487131038914233, + "angularVelocity": -0.022032606562462285, + "velocityX": -0.535753406204884, + "velocityY": -0.054125878022934074, + "timestamp": 0.13011712758603436 }, { - "x": 7.563457156334565, - "y": 4.090914473875886, - "heading": -0.013638379729614996, - "angularVelocity": -0.10503979307504581, - "velocityX": -0.7996608107353951, - "velocityY": 0.041246729627017566, - "timestamp": 0.1935677737551031 + "x": 7.562069419290744, + "y": 4.075490995112481, + "heading": -0.004303369490494938, + "angularVelocity": -0.03311872044176399, + "velocityX": -0.8041682214577686, + "velocityY": -0.07526519569309446, + "timestamp": 0.19517569137905155 }, { - "x": 7.494674268159643, - "y": 4.0946853102159935, - "heading": -0.022578417799948743, - "angularVelocity": -0.1385567116401054, - "velocityX": -1.0660279886559765, - "velocityY": 0.058442109452766046, - "timestamp": 0.2580903650068041 + "x": 7.492266246249156, + "y": 4.069542605603016, + "heading": -0.007182794508045951, + "angularVelocity": -0.04425896991381445, + "velocityX": -1.072928281412186, + "velocityY": -0.09143130685131902, + "timestamp": 0.2602342551720687 }, { - "x": 7.408714500593785, - "y": 4.09971475380561, - "heading": -0.03361607330634747, - "angularVelocity": -0.171066525573063, - "velocityX": -1.332242953952841, - "velocityY": 0.07794856796740106, - "timestamp": 0.3226129562585051 + "x": 7.404958718161784, + "y": 4.062935713928273, + "heading": -0.010790929131606555, + "angularVelocity": -0.05545979519375734, + "velocityX": -1.3419836374676162, + "velocityY": -0.10155298994552601, + "timestamp": 0.3252928189650859 }, { - "x": 7.305591747951634, - "y": 4.10618637275211, - "heading": -0.0466711017764886, - "angularVelocity": -0.20233267475595532, - "velocityX": -1.598242578942184, - "velocityY": 0.10030004717657835, - "timestamp": 0.3871355475102061 + "x": 7.300135300436189, + "y": 4.05615764314467, + "heading": -0.015132141763964354, + "angularVelocity": -0.0667277661734039, + "velocityX": -1.6112162890513386, + "velocityY": -0.1041841440762307, + "timestamp": 0.39035138275810305 }, { - "x": 7.18532637150992, - "y": 4.11433159194397, - "heading": -0.061641941793094415, - "angularVelocity": -0.23202477963424772, - "velocityX": -1.8639266357509128, - "velocityY": 0.1262382528948094, - "timestamp": 0.45165813876190714 + "x": 7.177800870279403, + "y": 4.049826474059511, + "heading": -0.02021115360823209, + "angularVelocity": -0.0780683056642115, + "velocityX": -1.8803739742240722, + "velocityY": -0.0973149223721115, + "timestamp": 0.4554099465511202 }, { - "x": 7.047949574625153, - "y": 4.124451015500193, - "heading": -0.07839600669319866, - "angularVelocity": -0.25966199706313403, - "velocityX": -2.129127088973572, - "velocityY": 0.15683535580191885, - "timestamp": 0.5161807300136082 + "x": 7.037994605220974, + "y": 4.044748235322315, + "heading": -0.02603274286903274, + "angularVelocity": -0.08948228982309019, + "velocityX": -2.148929470733793, + "velocityY": -0.07805642241584139, + "timestamp": 0.5204685103441374 }, { - "x": 6.89351205381652, - "y": 4.136950431097767, - "heading": -0.09675311116467075, - "angularVelocity": -0.28450662187236314, - "velocityX": -2.3935418248497737, - "velocityY": 0.1937215377605359, - "timestamp": 0.5807033212653092 + "x": 6.880828580156438, + "y": 4.04200948621943, + "heading": -0.03260086081295486, + "angularVelocity": -0.10095700797851251, + "velocityX": -2.415762290181463, + "velocityY": -0.04209667326193334, + "timestamp": 0.5855270741371547 }, { - "x": 6.722102962824983, - "y": 4.152406451801245, - "heading": -0.11645479321673856, - "angularVelocity": -0.3053454870591373, - "velocityX": -2.6565748161426845, - "velocityY": 0.2395443270897933, - "timestamp": 0.6452259125170102 + "x": 6.706576693390096, + "y": 4.043131786957125, + "heading": -0.0399161267255408, + "angularVelocity": -0.1124412450274706, + "velocityX": -2.678385082719035, + "velocityY": 0.017250622704578153, + "timestamp": 0.6505856379301719 }, { - "x": 6.533897762196436, - "y": 4.171698896366532, - "heading": -0.13710103042159202, - "angularVelocity": -0.31998462560676116, - "velocityX": -2.9168884413578904, - "velocityY": 0.29900294131132776, - "timestamp": 0.7097485037687112 + "x": 6.515894643954466, + "y": 4.050327381007243, + "heading": -0.0479686286684313, + "angularVelocity": -0.12377312798526215, + "velocityX": -2.9309292784618175, + "velocityY": 0.11060179675977738, + "timestamp": 0.7156442017231891 }, { - "x": 6.329305940744403, - "y": 4.196316085087732, - "heading": -0.1579986184766986, - "angularVelocity": -0.3238801735904445, - "velocityX": -3.1708556256509532, - "velocityY": 0.3815282096338637, - "timestamp": 0.7742710950204122 + "x": 6.310386764147224, + "y": 4.066852130952412, + "heading": -0.05671696687045534, + "angularVelocity": -0.13446866472270375, + "velocityX": -3.1588136568932224, + "velocityY": 0.2539980746845712, + "timestamp": 0.7807027655162063 }, { - "x": 6.109588048034184, - "y": 4.229179536431487, - "heading": -0.1777172131023292, - "angularVelocity": -0.30560760569437684, - "velocityX": -3.405286248549814, - "velocityY": 0.5093324788453677, - "timestamp": 0.8387936862721133 + "x": 6.0938831151450525, + "y": 4.097012876965575, + "heading": -0.06603485049011525, + "angularVelocity": -0.14322301441058957, + "velocityX": -3.3278270588784395, + "velocityY": 0.4635937877312989, + "timestamp": 0.8457613293092235 }, { - "x": 5.880551755398176, - "y": 4.2764929326979475, - "heading": -0.19275157350982466, - "angularVelocity": -0.23300924708438825, - "velocityX": -3.5497069815832503, - "velocityY": 0.7332841931579067, - "timestamp": 0.9033162775238143 + "x": 5.873502254486084, + "y": 4.144256591796875, + "heading": -0.07565133376897765, + "angularVelocity": -0.14781272008181154, + "velocityX": -3.3874227743499863, + "velocityY": 0.7261721144291506, + "timestamp": 0.9108198931022408 }, { - "x": 5.6591033935546875, - "y": 4.339165210723877, - "heading": -0.2, - "angularVelocity": -0.11233935819312842, - "velocityX": -3.432105833747789, - "velocityY": 0.9713230174133182, - "timestamp": 0.9678388687755153 + "x": 5.639993509614856, + "y": 4.215187853453379, + "heading": -0.08600693496822187, + "angularVelocity": -0.14746487395399271, + "velocityX": -3.3251896212558933, + "velocityY": 1.0100687886993767, + "timestamp": 0.9810440822367641 }, { - "x": 5.438463656346767, - "y": 4.41684559934421, - "heading": -0.20078792979416127, - "angularVelocity": -0.011394992631344103, - "velocityX": -3.190878423810921, - "velocityY": 1.1234090428973296, - "timestamp": 1.0369858897216806 + "x": 5.4200052502970575, + "y": 4.301407838205306, + "heading": -0.09591748086235549, + "angularVelocity": -0.1411272385808327, + "velocityX": -3.132656453980309, + "velocityY": 1.2277818485986531, + "timestamp": 1.0512682713712873 }, { - "x": 5.237468412635918, - "y": 4.49671689701659, - "heading": -0.1976367711927584, - "angularVelocity": 0.045571863520430765, - "velocityX": -2.9067809568735434, - "velocityY": 1.1550938359956655, - "timestamp": 1.106132910667846 + "x": 5.2193445983203315, + "y": 4.39418304628854, + "heading": -0.10506991591098606, + "angularVelocity": -0.13033165866960483, + "velocityX": -2.857429248379575, + "velocityY": 1.3211289333011804, + "timestamp": 1.1214924605058105 }, { - "x": 5.056130424824854, - "y": 4.574366670818957, - "heading": -0.19242246838266966, - "angularVelocity": 0.0754089292458229, - "velocityX": -2.6224989208464313, - "velocityY": 1.1229662932669573, - "timestamp": 1.1752799316140112 + "x": 5.039091097114449, + "y": 4.486700158570476, + "heading": -0.1133645829032854, + "angularVelocity": -0.11811694936640497, + "velocityX": -2.56682922832449, + "velocityY": 1.3174536213541534, + "timestamp": 1.1917166496403337 }, { - "x": 4.894040423510362, - "y": 4.647429210924394, - "heading": -0.18621230925465354, - "angularVelocity": 0.08981094258349935, - "velocityX": -2.344135713969339, - "velocityY": 1.0566259992938798, - "timestamp": 1.2444269525601765 + "x": 4.878804683581523, + "y": 4.574994396360027, + "heading": -0.12078834137955481, + "angularVelocity": -0.10571511850494018, + "velocityX": -2.2824957540752475, + "velocityY": 1.2573194347664447, + "timestamp": 1.261940838774857 }, { - "x": 4.750806708701635, - "y": 4.714490254294218, - "heading": -0.17966576596113534, - "angularVelocity": 0.09467570987063913, - "velocityX": -2.0714372484715344, - "velocityY": 0.9698327195040657, - "timestamp": 1.3135739735063419 + "x": 4.737844755029207, + "y": 4.656730803985265, + "heading": -0.12734948086758455, + "angularVelocity": -0.09343133141005204, + "velocityX": -2.007284531008099, + "velocityY": 1.163935228481773, + "timestamp": 1.3321650279093802 }, { - "x": 4.626109277283802, - "y": 4.774623849709137, - "heading": -0.17322414817467333, - "angularVelocity": 0.09315828358646269, - "velocityX": -1.803366648505599, - "velocityY": 0.8696483896498642, - "timestamp": 1.3827209944525072 + "x": 4.615644585302767, + "y": 4.730430369566539, + "heading": -0.13306001022788766, + "angularVelocity": -0.08131855177941563, + "velocityX": -1.740143549288271, + "velocityY": 1.0494897340871145, + "timestamp": 1.4023892170439034 }, { - "x": 4.519692351368254, - "y": 4.827181352429608, - "heading": -0.1672014372604398, - "angularVelocity": 0.08710007794728412, - "velocityX": -1.5389950927661693, - "velocityY": 0.7600833991299484, - "timestamp": 1.4518680153986725 + "x": 4.511741946172554, + "y": 4.795089974494431, + "heading": -0.13793134059619286, + "angularVelocity": -0.0693682679478674, + "velocityX": -1.4795847472325239, + "velocityY": 0.9207597228930422, + "timestamp": 1.4726134061784266 }, { - "x": 4.431350072395874, - "y": 4.871684848267837, - "heading": -0.16183165746625428, - "angularVelocity": 0.0776574279080811, - "velocityX": -1.277600650954415, - "velocityY": 0.6436068427716991, - "timestamp": 1.5210150363448378 + "x": 4.425763852456849, + "y": 4.849990883042018, + "heading": -0.1419734007558902, + "angularVelocity": -0.057559371058800816, + "velocityX": -1.224337294247763, + "velocityY": 0.7817948377078033, + "timestamp": 1.5428375953129498 }, { - "x": 4.360914453717203, - "y": 4.907768355386048, - "heading": -0.1572956227267302, - "angularVelocity": 0.06559985777341837, - "velocityX": -1.0186356218225097, - "velocityY": 0.5218374793948632, - "timestamp": 1.5901620572910031 + "x": 4.357406985219521, + "y": 4.894595448858866, + "heading": -0.14519463695889362, + "angularVelocity": -0.04587074970467533, + "velocityX": -0.9734091355100126, + "velocityY": 0.6351738107136011, + "timestamp": 1.613061784447473 }, { - "x": 4.308246377396777, - "y": 4.935142992782337, - "heading": -0.15373706400788933, - "angularVelocity": 0.051463659173564456, - "velocityX": -0.7616825077891644, - "velocityY": 0.39589033658590256, - "timestamp": 1.6593090782371684 + "x": 4.3064215304671825, + "y": 4.928487544994276, + "heading": -0.1476022173640631, + "angularVelocity": -0.03428420370305067, + "velocityX": -0.7260383548846598, + "velocityY": 0.4826270912218785, + "timestamp": 1.6832859735819963 }, { - "x": 4.273229022688589, - "y": 4.953575130372871, - "heading": -0.15127288861048924, - "angularVelocity": 0.035636754319735044, - "velocityX": -0.5064188482603104, - "velocityY": 0.26656444975242444, - "timestamp": 1.7284560991833338 + "x": 4.272598973020715, + "y": 4.951336158400459, + "heading": -0.1492022547191451, + "angularVelocity": -0.022784703886249428, + "velocityX": -0.4816368528182851, + "velocityY": 0.3253667103569352, + "timestamp": 1.7535101627165195 }, { "x": 4.255763053894043, "y": 4.96287202835083, "heading": -0.15, - "angularVelocity": 0.018408437457923484, - "velocityX": -0.2525917755465569, - "velocityY": 0.13445117158695233, - "timestamp": 1.797603120129499 + "angularVelocity": -0.011359978530000897, + "velocityX": -0.23974529765549837, + "velocityY": 0.1642720278089955, + "timestamp": 1.8237343518510427 }, { "x": 4.255763053894043, "y": 4.96287202835083, "heading": -0.15, - "angularVelocity": 9.849002219211576e-31, - "velocityX": 0, - "velocityY": -4.0048098642858166e-33, - "timestamp": 1.8667501410756644 + "angularVelocity": 0, + "velocityX": -7.182631856201558e-34, + "velocityY": 2.8363828288647375e-33, + "timestamp": 1.893958540985566 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/7Y.traj b/src/main/deploy/choreo/7Y.traj index 5e1f7a6d..a2876e8b 100644 --- a/src/main/deploy/choreo/7Y.traj +++ b/src/main/deploy/choreo/7Y.traj @@ -1,211 +1,319 @@ { "samples": [ { - "x": 7.70564079284668, - "y": 2.4290637969970703, - "heading": 0, + "x": 7.920039653778076, + "y": 2.2926278114318848, + "heading": 0.3923398585871528, "angularVelocity": 0, - "velocityX": 1.1723852013574271e-35, - "velocityY": 0, + "velocityX": 5.239292900745689e-35, + "velocityY": 4.900926094151388e-36, "timestamp": 0 }, { - "x": 7.6781366296409645, - "y": 2.449095429911782, - "heading": -0.00313265857340496, - "angularVelocity": -0.03454542227160848, - "velocityX": -0.30330242185823125, - "velocityY": 0.22089902286300528, - "timestamp": 0.09068230658102473 - }, - { - "x": 7.623127975291382, - "y": 2.4891588943453957, - "heading": -0.009323742995781282, - "angularVelocity": -0.06827224246710774, - "velocityX": -0.6066084600575472, - "velocityY": 0.4418002358356082, - "timestamp": 0.18136461316204947 - }, - { - "x": 7.5406144347316175, - "y": 2.5492544282514427, - "heading": -0.018481510948534847, - "angularVelocity": -0.10098737337003104, - "velocityX": -0.9099188548544356, - "velocityY": 0.6627040728430443, - "timestamp": 0.2720469197430742 - }, - { - "x": 7.430595523488553, - "y": 2.62938232138062, - "heading": -0.030489675695970903, - "angularVelocity": -0.13242015118689937, - "velocityX": -1.2132345921832182, - "velocityY": 0.8836111050790585, - "timestamp": 0.36272922632409893 - }, - { - "x": 7.293070634953775, - "y": 2.7295429329651553, - "heading": -0.04519602680885156, - "angularVelocity": -0.16217442704482293, - "velocityX": -1.5165570188921023, - "velocityY": 1.1045220987518831, - "timestamp": 0.45341153290512365 - }, - { - "x": 7.128038991250458, - "y": 2.849736716787733, - "heading": -0.06239274145585804, - "angularVelocity": -0.18963693464988343, - "velocityX": -1.8198880236450683, - "velocityY": 1.3254380965175891, - "timestamp": 0.5440938394861484 - }, - { - "x": 6.935499569650833, - "y": 2.9899642543659004, - "heading": -0.0817791104402204, - "angularVelocity": -0.21378336872188647, - "velocityX": -2.1232303065382423, - "velocityY": 1.5463605069735835, - "timestamp": 0.6347761460671731 - }, - { - "x": 6.715451005465253, - "y": 3.1502262816178486, - "heading": -0.10288140612229774, - "angularVelocity": -0.2327057667332536, - "velocityX": -2.426587638559539, - "velocityY": 1.7672910327744458, - "timestamp": 0.7254584526481979 - }, - { - "x": 6.467891597202115, - "y": 3.330523569856046, - "heading": -0.12484344695747294, - "angularVelocity": -0.24218661460217836, - "velocityX": -2.7299637337956644, - "velocityY": 1.9882300642309005, - "timestamp": 0.8161407592292226 - }, - { - "x": 6.192821228785326, - "y": 3.5308551006779862, - "heading": -0.14565067265533538, - "angularVelocity": -0.22945187967038694, - "velocityX": -3.033341109062037, - "velocityY": 2.2091578652440216, - "timestamp": 0.9068230658102473 - }, - { - "x": 5.890325515595207, - "y": 3.7511506939174324, - "heading": -0.15487871649545845, - "angularVelocity": -0.10176234138770857, - "velocityX": -3.335774359906014, - "velocityY": 2.4293117538051567, - "timestamp": 0.997505372391272 - }, - { - "x": 5.615254839431095, - "y": 3.9514825873361543, - "heading": -0.17571165783481632, - "angularVelocity": -0.22973545915204097, - "velocityX": -3.0333445027485793, - "velocityY": 2.209161863783487, - "timestamp": 1.0881876789722968 - }, - { - "x": 5.36769520554361, - "y": 4.131780146286741, - "heading": -0.19763475436907516, - "angularVelocity": -0.24175715595269445, - "velocityX": -2.729966221870307, - "velocityY": 1.9882330495143554, - "timestamp": 1.1788699855533216 - }, - { - "x": 5.1476463659386065, - "y": 4.292042455188194, - "heading": -0.21866611691405802, - "angularVelocity": -0.23192355088797076, - "velocityX": -2.4265906757498517, - "velocityY": 1.767294138667049, - "timestamp": 1.2695522921343463 - }, - { - "x": 4.9551066280664715, - "y": 4.432270284422795, - "heading": -0.23796795023386977, - "angularVelocity": -0.21285115087545256, - "velocityX": -2.123233794236368, - "velocityY": 1.5463637232176781, - "timestamp": 1.360234598715371 - }, - { - "x": 4.790074653740174, - "y": 4.5524643546084755, - "heading": -0.25507770430341364, - "angularVelocity": -0.18867797605320388, - "velocityX": -1.819891669593139, - "velocityY": 1.325441254389438, - "timestamp": 1.4509169052963957 - }, - { - "x": 4.652549445753091, - "y": 4.652625231735707, - "heading": -0.26970206105635147, - "angularVelocity": -0.1612702334591693, - "velocityX": -1.5165605416554253, - "velocityY": 1.1045250270264895, - "timestamp": 1.5415992118774204 - }, - { - "x": 4.542530247982926, - "y": 4.732753356223354, - "heading": -0.2816384072506419, - "angularVelocity": -0.13162817140767386, - "velocityX": -1.2132377518635613, - "velocityY": 0.8836136563867962, - "timestamp": 1.6322815184584452 - }, - { - "x": 4.4600164719635496, - "y": 4.792849076177355, - "heading": -0.2907383923314908, - "angularVelocity": -0.10035017219944754, - "velocityX": -0.9099214513875524, - "velocityY": 0.6627061244886346, - "timestamp": 1.7229638250394699 - }, - { - "x": 4.405007648402666, - "y": 4.832912672083218, - "heading": -0.2968887094471758, - "angularVelocity": -0.0678226806040681, - "velocityX": -0.6066103260367861, - "velocityY": 0.44180168564709604, - "timestamp": 1.8136461316204946 + "x": 7.909158851999345, + "y": 2.302519913622325, + "heading": 0.3890667752206854, + "angularVelocity": -0.05487179586853456, + "velocityX": -0.18241183228183028, + "velocityY": 0.16583672070973293, + "timestamp": 0.059649649053026756 + }, + { + "x": 7.887381785333585, + "y": 2.3222881834031703, + "heading": 0.38257998798822546, + "angularVelocity": -0.10874812065857113, + "velocityX": -0.36508289673927247, + "velocityY": 0.33140630489329076, + "timestamp": 0.11929929810605351 + }, + { + "x": 7.854690906335249, + "y": 2.351914457579234, + "heading": 0.37294712546328734, + "angularVelocity": -0.1614906823068608, + "velocityX": -0.5480481363649796, + "velocityY": 0.49667139113805775, + "timestamp": 0.17894894715908027 + }, + { + "x": 7.811066132526158, + "y": 2.3913778416294793, + "heading": 0.36024582664287896, + "angularVelocity": -0.21293166048835158, + "velocityX": -0.7313500498604669, + "velocityY": 0.6615861899734534, + "timestamp": 0.23859859621210702 + }, + { + "x": 7.756484255635697, + "y": 2.440654043893404, + "heading": 0.34456606928633876, + "angularVelocity": -0.26286420130655497, + "velocityX": -0.9150410397543076, + "velocityY": 0.8260937498579348, + "timestamp": 0.29824824526513377 + }, + { + "x": 7.69091815186407, + "y": 2.499714474772593, + "heading": 0.32601331375601694, + "angularVelocity": -0.3110287457656111, + "velocityX": -1.0991867481624389, + "velocityY": 0.9901220177621776, + "timestamp": 0.35789789431816055 + }, + { + "x": 7.614335701831427, + "y": 2.5685249973363313, + "heading": 0.30471285340408133, + "angularVelocity": -0.3570928025578184, + "velocityX": -1.2838709237763215, + "velocityY": 1.1535779951122258, + "timestamp": 0.4175475433711873 + }, + { + "x": 7.52669827360479, + "y": 2.6470441428134692, + "heading": 0.28081600930802, + "angularVelocity": -0.4006200283729716, + "velocityX": -1.4692027466704807, + "velocityY": 1.3163387668439968, + "timestamp": 0.4771971924242141 + }, + { + "x": 7.4279585237024035, + "y": 2.735220470554149, + "heading": 0.2545092548827881, + "angularVelocity": -0.4410211098114924, + "velocityX": -1.655328262109456, + "velocityY": 1.4782371588187146, + "timestamp": 0.5368468414772408 + }, + { + "x": 7.318057085990513, + "y": 2.8329884911056196, + "heading": 0.2260282157059115, + "angularVelocity": -0.4774720325941529, + "velocityX": -1.842449024539805, + "velocityY": 1.6390376490657574, + "timestamp": 0.5964964905302675 + }, + { + "x": 7.1969173483134, + "y": 2.940262024261505, + "heading": 0.19568025987657156, + "angularVelocity": -0.5087700650570722, + "velocityX": -2.0308541558965927, + "velocityY": 1.7983933662463305, + "timestamp": 0.6561461395832943 + }, + { + "x": 7.064436716981741, + "y": 3.056922608996874, + "heading": 0.1638833679507438, + "angularVelocity": -0.5330608382550797, + "velocityX": -2.2209792251063867, + "velocityY": 1.9557631367061583, + "timestamp": 0.715795788636321 + }, + { + "x": 6.920470867128582, + "y": 3.182797341309092, + "heading": 0.13123894322791613, + "angularVelocity": -0.5472693509698228, + "velocityX": -2.4135238369160836, + "velocityY": 2.110234247988935, + "timestamp": 0.7754454376893477 + }, + { + "x": 6.7648023713494, + "y": 3.317610695629293, + "heading": 0.09868526463286269, + "angularVelocity": -0.5457480322493466, + "velocityX": -2.6097135230552215, + "velocityY": 2.2600862948976657, + "timestamp": 0.8350950867423744 + }, + { + "x": 6.5970691959884045, + "y": 3.4608573798428486, + "heading": 0.0678828870163061, + "angularVelocity": -0.5163882454559657, + "velocityX": -2.8119725434073444, + "velocityY": 2.401467342854161, + "timestamp": 0.8947447357954011 + }, + { + "x": 6.416573137261274, + "y": 3.6113350194856255, + "heading": 0.042516406516172674, + "angularVelocity": -0.4252578330776664, + "velocityX": -3.025936641583165, + "velocityY": 2.522691114393757, + "timestamp": 0.9543943848484279 + }, + { + "x": 6.222302964562549, + "y": 3.7637188891607205, + "heading": 0.038389382258997386, + "angularVelocity": -0.0691877374417826, + "velocityX": -3.2568535738747495, + "velocityY": 2.5546482182926207, + "timestamp": 1.0140440339014547 + }, + { + "x": 6.025130717726082, + "y": 3.9020216671538117, + "heading": 0.02063963006041546, + "angularVelocity": -0.29756674985301135, + "velocityX": -3.305505564017429, + "velocityY": 2.3185849403765046, + "timestamp": 1.0736936829544814 + }, + { + "x": 5.8345208168029785, + "y": 4.0273118019104, + "heading": -0.005999936110084742, + "angularVelocity": -0.4466005516112663, + "velocityX": -3.195490735471996, + "velocityY": 2.1004337283729697, + "timestamp": 1.1333433320075081 + }, + { + "x": 5.64520772360382, + "y": 4.144845020536227, + "heading": -0.03716938606419973, + "angularVelocity": -0.5015668245717481, + "velocityX": -3.046353629773372, + "velocityY": 1.8912994401452536, + "timestamp": 1.1954874939160003 + }, + { + "x": 5.467324746551753, + "y": 4.251208116911629, + "heading": -0.06924096986746248, + "angularVelocity": -0.5160836161969399, + "velocityX": -2.86242458807314, + "velocityY": 1.7115541204340636, + "timestamp": 1.2576316558244924 + }, + { + "x": 5.301847945063855, + "y": 4.347494392232743, + "heading": -0.10089524223846308, + "angularVelocity": -0.5093684008099074, + "velocityX": -2.6627891728842434, + "velocityY": 1.5494017839181233, + "timestamp": 1.3197758177329846 + }, + { + "x": 5.149312148886964, + "y": 4.434404475602975, + "heading": -0.1313397954808994, + "angularVelocity": -0.4899020649319596, + "velocityX": -2.4545474826983984, + "velocityY": 1.3985237020044896, + "timestamp": 1.3819199796414767 + }, + { + "x": 5.010049657244503, + "y": 4.512419916624008, + "heading": -0.1600502646992021, + "angularVelocity": -0.46199785042686925, + "velocityX": -2.240958560959058, + "velocityY": 1.2553945314430697, + "timestamp": 1.4440641415499689 + }, + { + "x": 4.884285206645829, + "y": 4.581890331315179, + "heading": -0.18665500914006652, + "angularVelocity": -0.42811333557028486, + "velocityX": -2.0237532655740638, + "velocityY": 1.1178912476680836, + "timestamp": 1.506208303458461 + }, + { + "x": 4.7721802272313445, + "y": 4.6430804555182, + "heading": -0.21087715173166974, + "angularVelocity": -0.38977342115049435, + "velocityX": -1.8039502983330915, + "velocityY": 0.9846479914416396, + "timestamp": 1.5683524653669532 + }, + { + "x": 4.673855978265995, + "y": 4.696197438200952, + "heading": -0.23250253397752355, + "angularVelocity": -0.34798735040786954, + "velocityX": -1.582196073544819, + "velocityY": 0.8547380968942208, + "timestamp": 1.6304966272754453 + }, + { + "x": 4.589406729682163, + "y": 4.74140765854193, + "heading": -0.2513606323100662, + "angularVelocity": -0.30345727986985116, + "velocityX": -1.358924893189216, + "velocityY": 0.7275055122241522, + "timestamp": 1.6926407891839375 + }, + { + "x": 4.518907788851921, + "y": 4.77884761639657, + "heading": -0.2673125046894516, + "angularVelocity": -0.2566914073581794, + "velocityX": -1.1344418955082554, + "velocityY": 0.6024694308335906, + "timestamp": 1.7547849510924296 + }, + { + "x": 4.462420653358707, + "y": 4.8086312807227785, + "heading": -0.28024280765740234, + "angularVelocity": -0.20806947218937044, + "velocityX": -0.908969302319856, + "velocityY": 0.4792672941677987, + "timestamp": 1.8169291130009217 + }, + { + "x": 4.419996463593151, + "y": 4.830855221116989, + "heading": -0.29005429976132174, + "angularVelocity": -0.15788276489056036, + "velocityX": -0.6826737775951831, + "velocityY": 0.35761911838050064, + "timestamp": 1.879073274909414 + }, + { + "x": 4.391678399570345, + "y": 4.845602296651357, + "heading": -0.29666393327897195, + "angularVelocity": -0.10635968552255907, + "velocityX": -0.4556834166418462, + "velocityY": 0.23730427897770429, + "timestamp": 1.941217436817906 }, { "x": 4.377503395080566, "y": 4.852944374084473, "heading": -0.3, - "angularVelocity": -0.03430978622102242, - "velocityX": -0.30330341561751617, - "velocityY": 0.22089978471552077, - "timestamp": 1.9043284382015193 + "angularVelocity": -0.05368270515805527, + "velocityX": -0.22809873131204866, + "velocityY": 0.11814589186876204, + "timestamp": 2.003361598726398 }, { "x": 4.377503395080566, "y": 4.852944374084473, "heading": -0.3, - "angularVelocity": 0, - "velocityX": -3.367069338391324e-35, + "angularVelocity": -7.790359878532085e-33, + "velocityX": 1.9699324146528777e-35, "velocityY": 0, - "timestamp": 1.995010744782544 + "timestamp": 2.06550576063489 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/8Z.traj b/src/main/deploy/choreo/8Z.traj index c24ced3f..5cc6aa31 100644 --- a/src/main/deploy/choreo/8Z.traj +++ b/src/main/deploy/choreo/8Z.traj @@ -1,175 +1,211 @@ { "samples": [ { - "x": 8.288, - "y": 0.753, - "heading": -0.359, + "x": 7.842076778411865, + "y": 0.8892878890037537, + "heading": -0.2, "angularVelocity": 0, - "velocityX": 7.848060634472957e-33, - "velocityY": -3.2516900550847044e-31, + "velocityX": 3.543965029852756e-29, + "velocityY": 6.066534800985091e-29, "timestamp": 0 }, { - "x": 8.257857688531061, - "y": 0.7607198575910491, - "heading": -0.36259382076295077, - "angularVelocity": -0.04144012470689956, - "velocityX": -0.34756912729323597, - "velocityY": 0.08901719990897015, - "timestamp": 0.08672321303010541 - }, - { - "x": 8.19757306595335, - "y": 0.7761595734074377, - "heading": -0.36978140180451197, - "angularVelocity": -0.08287955197261455, - "velocityX": -0.6951382504334015, - "velocityY": 0.1780344071319038, - "timestamp": 0.17344642606021082 - }, - { - "x": 8.107146132657443, - "y": 0.7993191483957665, - "heading": -0.3805626441664111, - "angularVelocity": -0.12431783815661841, - "velocityX": -1.0427073690698763, - "velocityY": 0.2670516252700299, - "timestamp": 0.2601696390903162 - }, - { - "x": 7.986576889086077, - "y": 0.8301985838111292, - "heading": -0.3949373988890941, - "angularVelocity": -0.16575440669722594, - "velocityX": -1.3902764826011484, - "velocityY": 0.35606885788056725, - "timestamp": 0.34689285212042165 - }, - { - "x": 7.835865335793746, - "y": 0.8687978812136647, - "heading": -0.4129054484754803, - "angularVelocity": -0.20718846729246293, - "velocityX": -1.7378455897386196, - "velocityY": 0.4450861084809666, - "timestamp": 0.4336160651505271 - }, - { - "x": 7.655011473615787, - "y": 0.9151170424750372, - "heading": -0.4344664737940713, - "angularVelocity": -0.24861884800215203, - "velocityX": -2.085414687243842, - "velocityY": 0.5341033806634112, - "timestamp": 0.5203392781806325 - }, - { - "x": 7.444015304197123, - "y": 0.9691560698341772, - "heading": -0.45961997962793794, - "angularVelocity": -0.29004351839609516, - "velocityX": -2.432983765781574, - "velocityY": 0.6231206786628246, - "timestamp": 0.607062491210738 - }, - { - "x": 7.20287683222139, - "y": 1.030914966202055, - "heading": -0.48836503954685173, - "angularVelocity": -0.33145750618043834, - "velocityX": -2.78055279031259, - "velocityY": 0.7121380102284524, - "timestamp": 0.6937857042408434 - }, - { - "x": 6.931596093838579, - "y": 1.1003937371130306, - "heading": -0.5206978932748437, - "angularVelocity": -0.3728281344555003, - "velocityX": -3.1281213980002853, - "velocityY": 0.8011554056104477, - "timestamp": 0.7805089172709488 - }, - { - "x": 6.6904576020716755, - "y": 1.162152672705266, - "heading": -0.549434263924052, - "angularVelocity": -0.3313573107494592, - "velocityX": -2.780553018523351, - "velocityY": 0.7121384625220937, - "timestamp": 0.8672321303010543 - }, - { - "x": 6.479461402681092, - "y": 1.2161917502632715, - "heading": -0.5745770990267204, - "angularVelocity": -0.28992047485532396, - "velocityX": -2.4329841113859345, - "velocityY": 0.6231212575028354, - "timestamp": 0.9539553433311597 - }, - { - "x": 6.298607507686841, - "y": 1.262510964025961, - "heading": -0.5961272493805595, - "angularVelocity": -0.24849344945692042, - "velocityX": -2.085415065646478, - "velocityY": 0.5341039860528463, - "timestamp": 1.040678556361265 - }, - { - "x": 6.1478959228927685, - "y": 1.301110311282906, - "heading": -0.6140851971616693, - "angularVelocity": -0.20707198400127302, - "velocityX": -1.737845952983236, - "velocityY": 0.4450866833490784, - "timestamp": 1.1274017693913705 - }, - { - "x": 6.027326651802776, - "y": 1.3319897904748126, - "heading": -0.6284512647131738, - "angularVelocity": -0.16565423546423083, - "velocityX": -1.3902767999167456, - "velocityY": 0.3560693626651814, - "timestamp": 1.214124982421476 - }, - { - "x": 5.9368996967234855, - "y": 1.3551494006238816, - "heading": -0.6392256722190083, - "angularVelocity": -0.12423902585451516, - "velocityX": -1.0427076202527152, - "velocityY": 0.2670520307063474, - "timestamp": 1.3008481954515814 - }, - { - "x": 5.8766150591914466, - "y": 1.3705891410920916, - "heading": -0.6464085601564894, - "angularVelocity": -0.08282543608004028, - "velocityX": -0.6951384228708393, - "velocityY": 0.17803469139053074, - "timestamp": 1.3875714084816868 - }, - { - "x": 5.84647274017334, - "y": 1.3783090114593506, - "heading": -0.65, - "angularVelocity": -0.041412670472249856, - "velocityX": -0.34756921434221283, - "velocityY": 0.08901734723065605, - "timestamp": 1.4742946215117922 - }, - { - "x": 5.84647274017334, - "y": 1.3783090114593506, - "heading": -0.65, + "x": 7.812598945860675, + "y": 0.8944424472583185, + "heading": -0.20496622782556165, + "angularVelocity": -0.05838115294284956, + "velocityX": -0.34653058841526674, + "velocityY": 0.06059509639568072, + "timestamp": 0.08506560037310541 + }, + { + "x": 7.753643280710999, + "y": 0.9047515675559099, + "heading": -0.21489854838800776, + "angularVelocity": -0.11676071783272662, + "velocityX": -0.6930611773865226, + "velocityY": 0.1211902373271282, + "timestamp": 0.17013120074621083 + }, + { + "x": 7.665209782688016, + "y": 0.9202152555182115, + "heading": -0.2297967433211858, + "angularVelocity": -0.17513771569016606, + "velocityX": -1.039591769588472, + "velocityY": 0.1817854443450271, + "timestamp": 0.25519680111931625 + }, + { + "x": 7.547298450975015, + "y": 0.9408335185902585, + "heading": -0.24966046486395704, + "angularVelocity": -0.23351062539515693, + "velocityX": -1.386122371391394, + "velocityY": 0.24238073888402914, + "timestamp": 0.34026240149242165 + }, + { + "x": 7.399909283767824, + "y": 0.9666063659909732, + "heading": -0.2744891709604369, + "angularVelocity": -0.29187716288991106, + "velocityX": -1.732652994403474, + "velocityY": 0.30297614179729954, + "timestamp": 0.42532800186552705 + }, + { + "x": 7.223042277660287, + "y": 0.9975338086066862, + "heading": -0.3042820351108166, + "angularVelocity": -0.3502339843559568, + "velocityX": -2.079183657457103, + "velocityY": 0.3635716726862842, + "timestamp": 0.5103936022386325 + }, + { + "x": 7.016697426765236, + "y": 1.0336158588375843, + "heading": -0.33903781326789106, + "angularVelocity": -0.40857618125920553, + "velocityX": -2.4257143897180984, + "velocityY": 0.42416734934731337, + "timestamp": 0.5954592026117379 + }, + { + "x": 6.780874721269294, + "y": 1.0748525305163557, + "heading": -0.37875461009111894, + "angularVelocity": -0.4668960972340024, + "velocityX": -2.7722452373415654, + "velocityY": 0.48476318862035184, + "timestamp": 0.6805248029848433 + }, + { + "x": 6.515574144192628, + "y": 1.1212438395106257, + "heading": -0.42342930599477324, + "angularVelocity": -0.5251793405028335, + "velocityX": -3.1187762845737237, + "velocityY": 0.5453592144273801, + "timestamp": 0.7655904033579487 + }, + { + "x": 6.220795658543064, + "y": 1.1727898092300164, + "heading": -0.47305507638202704, + "angularVelocity": -0.5833823563138879, + "velocityX": -3.4653077666723004, + "velocityY": 0.6059555154293409, + "timestamp": 0.8506560037310541 + }, + { + "x": 5.896539001466683, + "y": 1.2294906138948394, + "heading": -0.527568943776392, + "angularVelocity": -0.6408450320209716, + "velocityX": -3.8118423387850986, + "velocityY": 0.6665538645013592, + "timestamp": 0.9357216041041595 + }, + { + "x": 5.601760097539714, + "y": 1.281036808508122, + "heading": -0.5771161617191909, + "angularVelocity": -0.582458922590698, + "velocityX": -3.4653126837880555, + "velocityY": 0.6059581591994512, + "timestamp": 1.020787204477265 + }, + { + "x": 5.336459014638101, + "y": 1.3274283800189033, + "heading": -0.6217016980784473, + "angularVelocity": -0.5241312136014974, + "velocityX": -3.118782230866253, + "velocityY": 0.5453623004752052, + "timestamp": 1.1058528048503704 + }, + { + "x": 5.100635786430906, + "y": 1.3686653237117123, + "heading": -0.6613288423924588, + "angularVelocity": -0.46584217521829746, + "velocityX": -2.7722513821433346, + "velocityY": 0.48476638631762087, + "timestamp": 1.1909184052234758 + }, + { + "x": 4.894290433316691, + "y": 1.404747640025304, + "heading": -0.6959996876310143, + "angularVelocity": -0.40757774102030764, + "velocityX": -2.425720293622384, + "velocityY": 0.42417047731787116, + "timestamp": 1.2759840055965812 + }, + { + "x": 4.717422969310233, + "y": 1.4356753315766229, + "heading": -0.7257156923972772, + "angularVelocity": -0.34933045362611315, + "velocityX": -2.079189040348869, + "velocityY": 0.3635745990819682, + "timestamp": 1.3610496059696866 + }, + { + "x": 4.57003340445074, + "y": 1.4614484017859215, + "heading": -0.7504778958597699, + "angularVelocity": -0.29109538231590903, + "velocityX": -1.7326576690580915, + "velocityY": 0.3029787610533009, + "timestamp": 1.446115206342792 + }, + { + "x": 4.452121745929318, + "y": 1.4820668540702735, + "heading": -0.7702870288254154, + "angularVelocity": -0.23286890210383354, + "velocityX": -1.3861262132313132, + "velocityY": 0.24238296319449323, + "timestamp": 1.5311808067158974 + }, + { + "x": 4.363687998733753, + "y": 1.4975306913245487, + "heading": -0.785143583437153, + "angularVelocity": -0.17464820734362577, + "velocityX": -1.03959469876997, + "velocityY": 0.18178719936671658, + "timestamp": 1.6162464070890028 + }, + { + "x": 4.304732166058997, + "y": 1.5078399155840347, + "heading": -0.795047860373283, + "angularVelocity": -0.11643104724722543, + "velocityX": -0.6930631467499093, + "velocityY": 0.12119145946503349, + "timestamp": 1.7013120074621082 + }, + { + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, + "angularVelocity": -0.05821553724430813, + "velocityX": -0.3465315751249751, + "velocityY": 0.06059573094328386, + "timestamp": 1.7863776078352136 + }, + { + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, "angularVelocity": 0, - "velocityX": -1.0272646100997073e-34, + "velocityX": 1.4177041903420033e-28, "velocityY": 0, - "timestamp": 1.5610178345418977 + "timestamp": 1.871443208208319 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/A1.traj b/src/main/deploy/choreo/A1.traj index 526967c0..ae81221c 100644 --- a/src/main/deploy/choreo/A1.traj +++ b/src/main/deploy/choreo/A1.traj @@ -4,163 +4,136 @@ "x": 0.735704243183136, "y": 6.708577632904053, "heading": 1.05, - "angularVelocity": 5.129859183698386e-34, - "velocityX": -1.897188232792028e-32, + "angularVelocity": -3.2052264357833964e-33, + "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 0.7518942164195387, - "y": 6.712774885224698, - "heading": 1.0360266067067383, - "angularVelocity": -0.2176708788653528, - "velocityX": 0.2521997076310485, - "velocityY": 0.06538280160588324, - "timestamp": 0.06419505156638627 - }, - { - "x": 0.7842732990077126, - "y": 6.7211732237608, - "heading": 1.0080810493162287, - "angularVelocity": -0.4353226098994694, - "velocityX": 0.5043859580779306, - "velocityY": 0.13082532580283415, - "timestamp": 0.12839010313277255 - }, - { - "x": 0.8328395869801072, - "y": 6.733778092662747, - "heading": 0.9661471413335745, - "angularVelocity": -0.6532264864572923, - "velocityX": 0.7565425494233072, - "velocityY": 0.19635265638678046, - "timestamp": 0.19258515469915882 - }, - { - "x": 0.8975895806447234, - "y": 6.750595971117333, - "heading": 0.9101723598037398, - "angularVelocity": -0.8719485406433664, - "velocityX": 1.0086446242302025, - "velocityY": 0.2619809166629292, - "timestamp": 0.2567802062655451 - }, - { - "x": 0.9785176746344029, - "y": 6.771633282588873, - "heading": 0.8400465790839613, - "angularVelocity": -1.0923860797473, - "velocityX": 1.2606593812918574, - "velocityY": 0.3277092386129448, - "timestamp": 0.32097525783193137 - }, - { - "x": 1.0756159936179712, - "y": 6.796894668273899, - "heading": 0.75558565761099, - "angularVelocity": -1.3156920885970516, - "velocityX": 1.5125514601877916, - "velocityY": 0.3935098589164984, - "timestamp": 0.38517030939831765 - }, - { - "x": 1.1888749801301643, - "y": 6.826380733124152, - "heading": 0.6565316471315795, - "angularVelocity": -1.5430162927274755, - "velocityX": 1.764294657432715, - "velocityY": 0.4593199028707055, - "timestamp": 0.4493653609647039 - }, - { - "x": 1.3182851194837182, - "y": 6.860085809037716, - "heading": 0.5425841203452523, - "angularVelocity": -1.7750204105451466, - "velocityX": 2.0158896394019683, - "velocityY": 0.5250416518274347, - "timestamp": 0.5135604125310902 - }, - { - "x": 1.4475274189498422, - "y": 6.8934428420016385, - "heading": 0.4215466833591603, - "angularVelocity": -1.8854636616486855, - "velocityX": 2.0132751094135295, - "velocityY": 0.5196200041903175, - "timestamp": 0.5777554640974765 - }, - { - "x": 1.5606259047746505, - "y": 6.922606418122232, - "heading": 0.31578482859238616, - "angularVelocity": -1.6475079026519317, - "velocityX": 1.761794454014094, - "velocityY": 0.45429632672598025, - "timestamp": 0.6419505156638627 - }, - { - "x": 1.6575780935492714, - "y": 6.94758845811189, - "heading": 0.22531533794231037, - "angularVelocity": -1.4092907232345635, - "velocityX": 1.5102751132516714, - "velocityY": 0.3891583444531321, - "timestamp": 0.706145567230249 - }, - { - "x": 1.738379613314278, - "y": 6.968397960555784, - "heading": 0.15008332290666232, - "angularVelocity": -1.1719285708158644, - "velocityX": 1.2586876681833796, - "velocityY": 0.3241605378628613, - "timestamp": 0.7703406187966353 - }, - { - "x": 1.8030257964282208, - "y": 6.98504096016579, - "heading": 0.09000459900753742, - "angularVelocity": -0.93587780417925, - "velocityX": 1.0070275128152204, - "velocityY": 0.25925673714578995, - "timestamp": 0.8345356703630216 - }, - { - "x": 1.851512723927302, - "y": 6.9975211192975015, - "heading": 0.044997689562143374, - "angularVelocity": -0.7010962425795362, - "velocityX": 0.7553063096917884, - "velocityY": 0.19440998686332248, - "timestamp": 0.8987307219294078 - }, - { - "x": 1.883837797047269, - "y": 7.0058404673101915, - "heading": 0.015004206753892985, - "angularVelocity": -0.46722421863361946, - "velocityX": 0.5035446242540792, - "velocityY": 0.12959484897502807, - "timestamp": 0.9629257734957941 - }, - { - "x": 1.9, - "y": 7.01, - "heading": 1.0124926487877066e-33, - "angularVelocity": -0.23372840098705003, - "velocityX": 0.2517671153518252, - "velocityY": 0.06479522312567318, - "timestamp": 1.0271208250621804 - }, - { - "x": 1.9, - "y": 7.01, - "heading": 0, - "angularVelocity": -2.3076499101828583e-32, - "velocityX": -9.966354466455041e-35, - "velocityY": 0, - "timestamp": 1.0913158766285667 + "x": 0.761064337643975, + "y": 6.703974702961187, + "heading": 1.0394085866051899, + "angularVelocity": -0.13389480430550454, + "velocityX": 0.3205978993008612, + "velocityY": -0.058189439025577605, + "timestamp": 0.07910249729066414 + }, + { + "x": 0.8117846303959374, + "y": 6.694768796125527, + "heading": 1.0182331720265752, + "angularVelocity": -0.2676959047298492, + "velocityX": 0.6411971112061043, + "velocityY": -0.11637947158397395, + "timestamp": 0.15820499458132828 + }, + { + "x": 0.8878653305388823, + "y": 6.680959872996509, + "heading": 0.9864869362485963, + "angularVelocity": -0.40133038608536603, + "velocityX": 0.9617989665153607, + "velocityY": -0.17457000223742059, + "timestamp": 0.2373074918719924 + }, + { + "x": 0.9893067581195191, + "y": 6.662547949391534, + "heading": 0.9441886485207825, + "angularVelocity": -0.5347275898558242, + "velocityX": 1.2824048677994033, + "velocityY": -0.23276033293007722, + "timestamp": 0.31640998916265656 + }, + { + "x": 1.1161093426409772, + "y": 6.639533162216395, + "heading": 0.8913618933378138, + "angularVelocity": -0.6678266425503051, + "velocityX": 1.6030161987872353, + "velocityY": -0.2909489328835039, + "timestamp": 0.3955124864533207 + }, + { + "x": 1.2682736197755877, + "y": 6.61191586671661, + "heading": 0.82803387429705, + "angularVelocity": -0.8005817921027615, + "velocityX": 1.9236343016514252, + "velocityY": -0.34913304188493594, + "timestamp": 0.47461498374398486 + }, + { + "x": 1.4458123214779228, + "y": 6.5797129796333005, + "heading": 0.7547732843458872, + "angularVelocity": -0.9261476244165204, + "velocityX": 2.2444133596688465, + "velocityY": -0.4071032923901237, + "timestamp": 0.553717481034649 + }, + { + "x": 1.5979887104865267, + "y": 6.552111718249667, + "heading": 0.6919847501093475, + "angularVelocity": -0.7937617191252718, + "velocityX": 1.9237874178539225, + "velocityY": -0.3489303413798874, + "timestamp": 0.6328199783253131 + }, + { + "x": 1.7248025102359055, + "y": 6.529111475679183, + "heading": 0.6396601519231939, + "angularVelocity": -0.6614784612157752, + "velocityX": 1.60315797974618, + "velocityY": -0.2907650625234928, + "timestamp": 0.7119224756159772 + }, + { + "x": 1.8262535980988925, + "y": 6.510711767453513, + "heading": 0.5977979127482742, + "angularVelocity": -0.5292151399606996, + "velocityX": 1.2825269914071422, + "velocityY": -0.23260590823144406, + "timestamp": 0.7910249729066413 + }, + { + "x": 1.9023419276714661, + "y": 6.496912228683403, + "heading": 0.5663993287229007, + "angularVelocity": -0.39693543315072166, + "velocityX": 0.9618954164365399, + "velocityY": -0.1744513668058135, + "timestamp": 0.8701274701973054 + }, + { + "x": 1.9530674867918099, + "y": 6.487712617138059, + "heading": 0.545466318050499, + "angularVelocity": -0.2646314767469699, + "velocityX": 0.6412636877183707, + "velocityY": -0.11629988761973552, + "timestamp": 0.9492299674879695 + }, + { + "x": 1.9784302711486816, + "y": 6.483112812042236, + "heading": 0.535, + "angularVelocity": -0.13231337074024702, + "velocityX": 0.32063190449823076, + "velocityY": -0.05814993525325002, + "timestamp": 1.0283324647786336 + }, + { + "x": 1.9784302711486816, + "y": 6.483112812042236, + "heading": 0.535, + "angularVelocity": 2.6777041505112823e-32, + "velocityX": 0, + "velocityY": -2.6634846912610644e-34, + "timestamp": 1.1074349620692978 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/E8.traj b/src/main/deploy/choreo/E8.traj new file mode 100644 index 00000000..c57046c4 --- /dev/null +++ b/src/main/deploy/choreo/E8.traj @@ -0,0 +1,247 @@ +{ + "samples": [ + { + "x": 1.4101015329360962, + "y": 1.5324852466583252, + "heading": 0.010868942095556194, + "angularVelocity": 0, + "velocityX": -2.7109632930976606e-30, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.445714339271355, + "y": 1.5290426767816314, + "heading": 0.008951940936462094, + "angularVelocity": -0.02061695772149426, + "velocityX": 0.38300849170323675, + "velocityY": -0.037024139115655316, + "timestamp": 0.09298176700179253 + }, + { + "x": 1.5169399514953399, + "y": 1.5221575370547553, + "heading": 0.005117939665931709, + "angularVelocity": -0.0412339041755727, + "velocityX": 0.7660169786041023, + "velocityY": -0.07404827794618403, + "timestamp": 0.18596353400358506 + }, + { + "x": 1.6237783690472765, + "y": 1.5118298275067226, + "heading": -0.0006330612434550333, + "angularVelocity": -0.061850845547063264, + "velocityX": 1.1490254594739377, + "velocityY": -0.11107241646454527, + "timestamp": 0.2789453010053776 + }, + { + "x": 1.7662295912051191, + "y": 1.4980595481733934, + "heading": -0.00830106144931283, + "angularVelocity": -0.08246778323615674, + "velocityX": 1.5320339325783188, + "velocityY": -0.14809655459723625, + "timestamp": 0.3719270680071701 + }, + { + "x": 1.9442936170070497, + "y": 1.4808466991047853, + "heading": -0.01788606005843331, + "angularVelocity": -0.10308471131882355, + "velocityX": 1.9150423953385212, + "velocityY": -0.1851206921920015, + "timestamp": 0.46490883500896263 + }, + { + "x": 2.157970445109853, + "y": 1.4601912803784864, + "heading": -0.029388054546433492, + "angularVelocity": -0.12370161225204816, + "velocityX": 2.2980508436527027, + "velocityY": -0.2221448289523177, + "timestamp": 0.5578906020107551 + }, + { + "x": 2.407260073505042, + "y": 1.436093292126851, + "heading": -0.04280703888278391, + "angularVelocity": -0.14431844832782198, + "velocityX": 2.6810592703659726, + "velocityY": -0.2591689642891874, + "timestamp": 0.6508723690125476 + }, + { + "x": 2.6921624988551995, + "y": 1.4085527346009814, + "heading": -0.058142999714275743, + "angularVelocity": -0.1649351407919763, + "velocityX": 3.064067661185958, + "velocityY": -0.29619309692549817, + "timestamp": 0.7438541360143401 + }, + { + "x": 3.012677714499943, + "y": 1.3775696083634381, + "heading": -0.07539590621623399, + "angularVelocity": -0.18555150174179996, + "velocityX": 3.4470759803747364, + "velocityY": -0.3332172235115734, + "timestamp": 0.8368359030161326 + }, + { + "x": 3.3688057004723464, + "y": 1.3431439152534057, + "heading": -0.09456566508261226, + "angularVelocity": -0.20616685919988256, + "velocityX": 3.8300840848227304, + "velocityY": -0.3702413303175326, + "timestamp": 0.9298176700179251 + }, + { + "x": 3.7547740072816946, + "y": 1.3058336555455088, + "heading": -0.09456566629426387, + "angularVelocity": -1.3030834763042814e-8, + "velocityX": 4.151010668596085, + "velocityY": -0.4012642576170675, + "timestamp": 1.0227994370197178 + }, + { + "x": 4.140742314090614, + "y": 1.2685233958331112, + "heading": -0.09456566750542322, + "angularVelocity": -1.3025737916689111e-8, + "velocityX": 4.151010668590924, + "velocityY": -0.4012642576654214, + "timestamp": 1.1157812040215103 + }, + { + "x": 4.526710620899537, + "y": 1.231213136120722, + "heading": -0.09456566871658223, + "angularVelocity": -1.3025734308185157e-8, + "velocityX": 4.151010668590951, + "velocityY": -0.401264257665331, + "timestamp": 1.2087629710233028 + }, + { + "x": 4.912678927708486, + "y": 1.1939028764086315, + "heading": -0.09456566992774154, + "angularVelocity": -1.3025737526913983e-8, + "velocityX": 4.151010668591244, + "velocityY": -0.4012642576621162, + "timestamp": 1.3017447380250953 + }, + { + "x": 5.2986472345138385, + "y": 1.1565926166593978, + "heading": -0.09456567113938757, + "angularVelocity": -1.3030774721558205e-8, + "velocityX": 4.151010668553108, + "velocityY": -0.40126425806165034, + "timestamp": 1.3947265050268878 + }, + { + "x": 5.654775219974997, + "y": 1.1221669168423432, + "heading": -0.11373537224602893, + "angularVelocity": -0.20616623800565617, + "velocityX": 3.8300840793243975, + "velocityY": -0.3702414024501916, + "timestamp": 1.4877082720286803 + }, + { + "x": 5.975290435116198, + "y": 1.0911837840655934, + "heading": -0.13098822471299915, + "angularVelocity": -0.18555092060649123, + "velocityX": 3.4470759749592483, + "velocityY": -0.333217293839406, + "timestamp": 1.5806900390304728 + }, + { + "x": 6.260192859980051, + "y": 1.0636432203316484, + "heading": -0.1463241379521425, + "angularVelocity": -0.16493462894594582, + "velocityX": 3.0640676559558413, + "velocityY": -0.296193163692087, + "timestamp": 1.6736718060322653 + }, + { + "x": 6.509482487916198, + "y": 1.0395452263459009, + "heading": -0.15974308275948657, + "angularVelocity": -0.1443180232013969, + "velocityX": 2.6810592654290843, + "velocityY": -0.2591690259583974, + "timestamp": 1.7666535730340578 + }, + { + "x": 6.72315931559752, + "y": 1.0188898024827011, + "heading": -0.1712450464462443, + "angularVelocity": -0.12370128099094915, + "velocityX": 2.298050839119749, + "velocityY": -0.22214488419863948, + "timestamp": 1.8596353400358503 + }, + { + "x": 6.9012233410258, + "y": 1.0016769489785289, + "heading": -0.1808300228356812, + "angularVelocity": -0.1030844723506456, + "velocityX": 1.915042391319989, + "velocityY": -0.18512073989559208, + "timestamp": 1.9526171070376428 + }, + { + "x": 7.043674562867799, + "y": 0.9879066659964851, + "heading": -0.18849800859117913, + "angularVelocity": -0.08246762782547498, + "velocityX": 1.5320339291814782, + "velocityY": -0.14809659383842053, + "timestamp": 2.0455988740394355 + }, + { + "x": 7.150512980171106, + "y": 0.9775789536540375, + "heading": -0.19424900148191884, + "angularVelocity": -0.06185075930814551, + "velocityX": 1.1490254567999838, + "velocityY": -0.1110724465179094, + "timestamp": 2.1385806410412282 + }, + { + "x": 7.221738592222265, + "y": 0.9706938120369626, + "heading": -0.1980829994380352, + "angularVelocity": -0.04123386852972422, + "velocityX": 0.766016976745399, + "velocityY": -0.07404829827488661, + "timestamp": 2.231562408043021 + }, + { + "x": 7.257351398468018, + "y": 0.9672512412071228, + "heading": -0.2, + "angularVelocity": -0.020616951299490285, + "velocityX": 0.38300849074060805, + "velocityY": -0.0370241493665455, + "timestamp": 2.3245441750448137 + }, + { + "x": 7.257351398468018, + "y": 0.9672512412071228, + "heading": -0.2, + "angularVelocity": 0, + "velocityX": -1.1302654275028148e-31, + "velocityY": 0, + "timestamp": 2.4175259420466064 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/OS8.traj b/src/main/deploy/choreo/OS8.traj deleted file mode 100644 index b8046459..00000000 --- a/src/main/deploy/choreo/OS8.traj +++ /dev/null @@ -1,400 +0,0 @@ -{ - "samples": [ - { - "x": 1.0787577629089355, - "y": 3.559532165527344, - "heading": -1, - "angularVelocity": 0, - "velocityX": -1.6257982956725998e-33, - "velocityY": -6.646520386671484e-34, - "timestamp": 0 - }, - { - "x": 1.0892450498408872, - "y": 3.5521951152447366, - "heading": -0.996813008661085, - "angularVelocity": 0.05725916143580944, - "velocityX": 0.18842010899995426, - "velocityY": -0.13182130163475733, - "timestamp": 0.05565906413924324 - }, - { - "x": 1.1102203082656286, - "y": 3.53752066901722, - "heading": -0.9904934103709541, - "angularVelocity": 0.11354122437849765, - "velocityX": 0.3768525171797188, - "velocityY": -0.2636488136201029, - "timestamp": 0.11131812827848649 - }, - { - "x": 1.1416842952224584, - "y": 3.515508446696833, - "heading": -0.9811023613749315, - "angularVelocity": 0.16872452207476613, - "velocityX": 0.5652985267254234, - "velocityY": -0.39548315554352664, - "timestamp": 0.16697719241772974 - }, - { - "x": 1.1836378528015412, - "y": 3.4861580280924316, - "heading": -0.9687092339674004, - "angularVelocity": 0.22266144066898186, - "velocityX": 0.7537596657056034, - "velocityY": -0.5273250468418692, - "timestamp": 0.22263625655697297 - }, - { - "x": 1.2360819233571791, - "y": 3.4494689463713466, - "heading": -0.9533934750113374, - "angularVelocity": 0.2751709751667287, - "velocityX": 0.9422377355184741, - "velocityY": -0.6591753255013372, - "timestamp": 0.2782953206962162 - }, - { - "x": 1.299017567979401, - "y": 3.4054406802542445, - "heading": -0.9352470387517811, - "angularVelocity": 0.326028411368169, - "velocityX": 1.1307348694324895, - "velocityY": -0.791034969739261, - "timestamp": 0.3339543848354595 - }, - { - "x": 1.372445988957865, - "y": 3.3540726449249463, - "heading": -0.9143776340365992, - "angularVelocity": 0.3749506937984565, - "velocityX": 1.3192536043144198, - "velocityY": -0.9229051210920455, - "timestamp": 0.38961344897470274 - }, - { - "x": 1.4563685570284315, - "y": 3.2953641818290134, - "heading": -0.8909131581838348, - "angularVelocity": 0.42157510579163254, - "velocityX": 1.5077969665572526, - "velocityY": -1.0547871043799972, - "timestamp": 0.445272513113946 - }, - { - "x": 1.5507868440157, - "y": 3.2293145481745777, - "heading": -0.8650079167097484, - "angularVelocity": 0.4654271837787104, - "velocityX": 1.6963685690269719, - "velocityY": -1.1866824330570491, - "timestamp": 0.5009315772531893 - }, - { - "x": 1.6557026605764604, - "y": 3.155922908528008, - "heading": -0.836851636980428, - "angularVelocity": 0.5058705201884368, - "velocityX": 1.8849727027082337, - "velocityY": -1.3185927715738206, - "timestamp": 0.5565906413924325 - }, - { - "x": 1.7711180956540333, - "y": 3.0751883348664535, - "heading": -0.8066830627738127, - "angularVelocity": 0.5420244604031119, - "velocityX": 2.0736143674431173, - "velocityY": -1.4505197834368828, - "timestamp": 0.6122497055316758 - }, - { - "x": 1.8970355444974412, - "y": 2.987109831930365, - "heading": -0.7748115054083663, - "angularVelocity": 0.5726211509002932, - "velocityX": 2.2622990664808507, - "velocityY": -1.5824646766561024, - "timestamp": 0.6679087696709191 - }, - { - "x": 2.033457680736698, - "y": 2.891686434652775, - "heading": -0.7416532565559409, - "angularVelocity": 0.5957385264235328, - "velocityX": 2.4510318013606396, - "velocityY": -1.7144269087756705, - "timestamp": 0.7235678338101623 - }, - { - "x": 2.1803872186925912, - "y": 2.7889175202257976, - "heading": -0.7077985324505156, - "angularVelocity": 0.608251766877191, - "velocityX": 2.639813303154317, - "velocityY": -1.8464003305890786, - "timestamp": 0.7792268979494056 - }, - { - "x": 2.3378258718031506, - "y": 2.6788038388008832, - "heading": -0.674149966624674, - "angularVelocity": 0.6045478188721016, - "velocityX": 2.8286255894761796, - "velocityY": -1.9783602747872513, - "timestamp": 0.8348859620886488 - }, - { - "x": 2.505769659808062, - "y": 2.5613515236455924, - "heading": -0.6422654949724597, - "angularVelocity": 0.5728531757639463, - "velocityX": 3.017366364349273, - "velocityY": -2.110210025476854, - "timestamp": 0.8905450262278921 - }, - { - "x": 2.684179931153943, - "y": 2.436594665396841, - "heading": -0.6155263817868243, - "angularVelocity": 0.4804089612204379, - "velocityX": 3.2054127050995533, - "velocityY": -2.2414472858660566, - "timestamp": 0.9462040903671354 - }, - { - "x": 2.8724826792658567, - "y": 2.3049578433226325, - "heading": -0.6081632212450389, - "angularVelocity": 0.13229041227435911, - "velocityX": 3.383146142034183, - "velocityY": -2.3650563319730016, - "timestamp": 1.0018631545063785 - }, - { - "x": 3.0626614751760415, - "y": 2.171875394056846, - "heading": -0.6081632133489759, - "angularVelocity": 1.4186481590135754e-7, - "velocityX": 3.4168522028040385, - "velocityY": -2.391029229899588, - "timestamp": 1.0575222186456217 - }, - { - "x": 3.2549073856236133, - "y": 2.0417968692290303, - "heading": -0.6081632054441575, - "angularVelocity": 1.4202212353431132e-7, - "velocityX": 3.4539910690310336, - "velocityY": -2.337059144623708, - "timestamp": 1.1131812827848648 - }, - { - "x": 3.4540409810049435, - "y": 1.9225294227125445, - "heading": -0.6081631974633424, - "angularVelocity": 1.43387518536565e-7, - "velocityX": 3.577738836627839, - "velocityY": -2.142821629521334, - "timestamp": 1.168840346924108 - }, - { - "x": 3.6594559882791042, - "y": 1.814438779406305, - "heading": -0.608163189305756, - "angularVelocity": 1.4656348644367822e-7, - "velocityX": 3.6905939841221627, - "velocityY": -1.9420133086648224, - "timestamp": 1.2244994110633511 - }, - { - "x": 3.87052583694458, - "y": 1.7178549766540527, - "heading": -0.6081631808165556, - "angularVelocity": 1.5252143525805278e-7, - "velocityX": 3.792191836669028, - "velocityY": -1.735275363427366, - "timestamp": 1.2801584752025943 - }, - { - "x": 4.138207291973022, - "y": 1.6167788253364266, - "heading": -0.6081631735186005, - "angularVelocity": 1.0636848870334246e-7, - "velocityX": 3.901486265783237, - "velocityY": -1.473195877996273, - "timestamp": 1.3487685988306226 - }, - { - "x": 4.412146386502647, - "y": 1.534152421215255, - "heading": -0.6081631664890607, - "angularVelocity": 1.0245630444469167e-7, - "velocityX": 3.992692040824656, - "velocityY": -1.2042888097554372, - "timestamp": 1.417378722458651 - }, - { - "x": 4.691072911982162, - "y": 1.4703578778498345, - "heading": -0.6081631595875161, - "angularVelocity": 1.0059076244967574e-7, - "velocityX": 4.06538438834075, - "velocityY": -0.9298123949066827, - "timestamp": 1.4859888460866792 - }, - { - "x": 4.9718690126735465, - "y": 1.4153733322733588, - "heading": -0.6081631527089094, - "angularVelocity": 1.0025643942534838e-7, - "velocityX": 4.092633650009569, - "velocityY": -0.8014057207443024, - "timestamp": 1.5545989697147076 - }, - { - "x": 5.252665227857277, - "y": 1.3603893713935364, - "heading": -0.6081631458298762, - "angularVelocity": 1.0026265459162975e-7, - "velocityX": 4.092635318747932, - "velocityY": -0.801397198727107, - "timestamp": 1.6232090933427359 - }, - { - "x": 5.519907312611867, - "y": 1.3079148292099916, - "heading": -0.5750862028334358, - "angularVelocity": 0.48210003491274794, - "velocityX": 3.8950823963450225, - "velocityY": -0.7648221488134482, - "timestamp": 1.6918192169707642 - }, - { - "x": 5.768179080456147, - "y": 1.2592050774901709, - "heading": -0.5321026446123915, - "angularVelocity": 0.6264900272455569, - "velocityX": 3.6185879680131667, - "velocityY": -0.7099499191096337, - "timestamp": 1.7604293405987925 - }, - { - "x": 5.997339300083197, - "y": 1.214259149807144, - "heading": -0.48752011650190086, - "angularVelocity": 0.6497951869639015, - "velocityX": 3.340035078051294, - "velocityY": -0.6550917751832426, - "timestamp": 1.8290394642268208 - }, - { - "x": 6.207384201480997, - "y": 1.1730703596699767, - "heading": -0.4438883449780598, - "angularVelocity": 0.6359378065020302, - "velocityX": 3.061427239754943, - "velocityY": -0.600331087588086, - "timestamp": 1.8976495878548492 - }, - { - "x": 6.398318666454552, - "y": 1.1356341428583274, - "heading": -0.40244988370615753, - "angularVelocity": 0.6039700714804472, - "velocityX": 2.7828905542965074, - "velocityY": -0.5456369239998863, - "timestamp": 1.9662597114828775 - }, - { - "x": 6.570147668959068, - "y": 1.1019474014538393, - "heading": -0.36394173985933226, - "angularVelocity": 0.5612603769029573, - "velocityX": 2.504426364775143, - "velocityY": -0.4909879129080382, - "timestamp": 2.034869835110906 - }, - { - "x": 6.722875355146629, - "y": 1.0720079227066874, - "heading": -0.32885233994862423, - "angularVelocity": 0.5114318129048492, - "velocityX": 2.2260226058705035, - "velocityY": -0.4363711528850982, - "timestamp": 2.103479958738934 - }, - { - "x": 6.856505086896474, - "y": 1.0458140505512963, - "heading": -0.2975293413887698, - "angularVelocity": 0.45653610434625136, - "velocityX": 1.9476678467207353, - "velocityY": -0.381778530197699, - "timestamp": 2.1720900823669624 - }, - { - "x": 6.971039601667182, - "y": 1.0233644985449248, - "heading": -0.2702328493192886, - "angularVelocity": 0.39784933514287446, - "velocityX": 1.6693529863269239, - "velocityY": -0.3272046575529071, - "timestamp": 2.2407002059949908 - }, - { - "x": 7.0664811548927515, - "y": 1.0046582381111746, - "heading": -0.24716475692257864, - "angularVelocity": 0.3362199508890913, - "velocityX": 1.3910709991284718, - "velocityY": -0.27264577652077626, - "timestamp": 2.309310329623019 - }, - { - "x": 7.142831629478478, - "y": 0.9896944283927934, - "heading": -0.22848628077286834, - "angularVelocity": 0.27224081756471324, - "velocityX": 1.1128164554791231, - "velocityY": -0.21809915107437988, - "timestamp": 2.3779204532510474 - }, - { - "x": 7.200092617495255, - "y": 0.9784723702199377, - "heading": -0.2143291042772057, - "angularVelocity": 0.20634238428743115, - "velocityX": 0.8345851164358524, - "velocityY": -0.16356271610435189, - "timestamp": 2.4465305768790757 - }, - { - "x": 7.238265481117147, - "y": 0.9709914747194018, - "heading": -0.20480280588812247, - "angularVelocity": 0.13884683316897098, - "velocityX": 0.5563736312274852, - "velocityY": -0.10903486402522468, - "timestamp": 2.515140700507104 - }, - { - "x": 7.257351398468018, - "y": 0.9672512412071228, - "heading": -0.2, - "angularVelocity": 0.07000141719844519, - "velocityX": 0.27817931730228895, - "velocityY": -0.05451430947066675, - "timestamp": 2.5837508241351324 - }, - { - "x": 7.257351398468018, - "y": 0.9672512412071228, - "heading": -0.2, - "angularVelocity": 0, - "velocityX": 3.164263694858479e-35, - "velocityY": 0, - "timestamp": 2.6523609477631607 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/S3.traj b/src/main/deploy/choreo/S3.traj index 95aa43e3..4d57e11b 100644 --- a/src/main/deploy/choreo/S3.traj +++ b/src/main/deploy/choreo/S3.traj @@ -4,154 +4,136 @@ "x": 0.7390319108963013, "y": 4.394268035888672, "heading": -1.05, - "angularVelocity": -1.1172014722037894e-31, + "angularVelocity": -1.42039595844255e-32, "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 0.7571933032499027, - "y": 4.389818892605314, - "heading": -1.0342336922831046, - "angularVelocity": 0.23223812416934533, - "velocityX": 0.2675177833795592, - "velocityY": -0.06553599668617616, - "timestamp": 0.06788854230237745 - }, - { - "x": 0.7935151251431718, - "y": 4.38091553593728, - "heading": -1.002705893464414, - "angularVelocity": 0.4644052994725502, - "velocityX": 0.5350213844847446, - "velocityY": -0.13114667609709774, - "timestamp": 0.1357770846047549 - }, - { - "x": 0.8479950673400368, - "y": 4.367550754321936, - "heading": -0.9553984368970493, - "angularVelocity": 0.6968400699584296, - "velocityX": 0.8024909704823213, - "velocityY": -0.19686358201382703, - "timestamp": 0.20366562690713236 - }, - { - "x": 0.9206286419560185, - "y": 4.3497160444231815, - "heading": -0.8922424202408584, - "angularVelocity": 0.9302897737130964, - "velocityX": 1.0698944498243854, - "velocityY": -0.2627057422932853, - "timestamp": 0.2715541692095098 - }, - { - "x": 1.0114084585277832, - "y": 4.3274032827050695, - "heading": -0.8130879810632846, - "angularVelocity": 1.1659469550107187, - "velocityX": 1.3371890674486555, - "velocityY": -0.32866756246923856, - "timestamp": 0.33944271151188726 - }, - { - "x": 1.1203241193682845, - "y": 4.30060737967286, - "heading": -0.7176841911738273, - "angularVelocity": 1.4053003151036254, - "velocityX": 1.604330526871355, - "velocityY": -0.3947043510355547, - "timestamp": 0.4073312538142647 - }, - { - "x": 1.2473633964229762, - "y": 4.269329623801374, - "heading": -0.6056889773008814, - "angularVelocity": 1.6496924234153572, - "velocityX": 1.8712918667314336, - "velocityY": -0.46072216033411856, - "timestamp": 0.47521979611664217 - }, - { - "x": 1.392515159392732, - "y": 4.233580656924137, - "heading": -0.47672975284718977, - "angularVelocity": 1.8995727420291677, - "velocityX": 2.138089256994883, - "velocityY": -0.5265832151471296, - "timestamp": 0.5431083384190196 - }, - { - "x": 1.5193632658919598, - "y": 4.202650378846915, - "heading": -0.35714109334344585, - "angularVelocity": 1.7615440757454204, - "velocityX": 1.8684759194599092, - "velocityY": -0.4556038033554743, - "timestamp": 0.610996880721397 - }, - { - "x": 1.6281019189892685, - "y": 4.176160742808517, - "heading": -0.25481154838503933, - "angularVelocity": 1.507316868030967, - "velocityX": 1.6017231981942384, - "velocityY": -0.3901930302231607, - "timestamp": 0.6788854230237744 - }, - { - "x": 1.7187271243324231, - "y": 4.154098965427716, - "heading": -0.16971637395021472, - "angularVelocity": 1.2534541404027932, - "velocityX": 1.3349116400158851, - "velocityY": -0.32497055662996915, - "timestamp": 0.7467739653261518 - }, - { - "x": 1.7912335760671219, - "y": 4.136456276445617, - "heading": -0.10176996511823568, - "angularVelocity": 1.0008523754913499, - "velocityX": 1.0680219264651876, - "velocityY": -0.25987726917920023, - "timestamp": 0.8146625076285292 - }, - { - "x": 1.845616448865919, - "y": 4.123227280338756, - "heading": -0.050876879855985754, - "angularVelocity": 0.7496564742187419, - "velocityX": 0.80106113571528, - "velocityY": -0.19486345792990323, - "timestamp": 0.8825510499309066 - }, - { - "x": 1.8818723916386764, - "y": 4.114408992463889, - "heading": -0.01696431328583086, - "angularVelocity": 0.4995329906938869, - "velocityX": 0.5340509833201647, - "velocityY": -0.12989361055344087, - "timestamp": 0.950439592233284 - }, - { - "x": 1.9, - "y": 4.11, - "heading": -4.1561834640364843e-29, - "angularVelocity": 0.24988477746762355, - "velocityX": 0.26702014429154675, - "velocityY": -0.0649445740674598, - "timestamp": 1.0183281345356614 - }, - { - "x": 1.9, - "y": 4.11, - "heading": -1.087656704067454e-28, - "angularVelocity": 2.4807548250000706e-30, - "velocityX": 0, - "velocityY": 1.3216215980164627e-31, - "timestamp": 1.0862166768380388 + "x": 0.7651437671205386, + "y": 4.398651735511244, + "heading": -1.0392091078900427, + "angularVelocity": 0.13459888616794363, + "velocityX": 0.32570307697884177, + "velocityY": 0.054679546462018135, + "timestamp": 0.0801707385341453 + }, + { + "x": 0.817367588836673, + "y": 4.407419167420995, + "heading": -1.0176349612340314, + "angularVelocity": 0.26910250610729297, + "velocityX": 0.6514075168946079, + "velocityY": 0.10935950036205129, + "timestamp": 0.1603414770682906 + }, + { + "x": 0.8957035925551177, + "y": 4.420570347353103, + "heading": -0.9852910452071313, + "angularVelocity": 0.4034379203470197, + "velocityX": 0.9771146574267956, + "velocityY": 0.16403965053292643, + "timestamp": 0.2405122156024359 + }, + { + "x": 1.0001521051022733, + "y": 4.438105224493772, + "heading": -0.942196381665077, + "angularVelocity": 0.5375360677724069, + "velocityX": 1.3028258745885222, + "velocityY": 0.21871916688407136, + "timestamp": 0.3206829541365812 + }, + { + "x": 1.1307135580503673, + "y": 4.460023613513803, + "heading": -0.888374546540725, + "angularVelocity": 0.6713401436538962, + "velocityX": 1.6285424749141706, + "velocityY": 0.27339637155389257, + "timestamp": 0.4008536926707265 + }, + { + "x": 1.287388478944155, + "y": 4.486325092461348, + "heading": -0.8238521841946843, + "angularVelocity": 0.8048118743294378, + "velocityX": 1.9542656555054532, + "velocityY": 0.3280683130584222, + "timestamp": 0.4810244312048718 + }, + { + "x": 1.470188592457945, + "y": 4.516991294048374, + "heading": -0.7491443367227937, + "angularVelocity": 0.9318592897840392, + "velocityX": 2.280135082402091, + "velocityY": 0.3825111524196008, + "timestamp": 0.5611951697390172 + }, + { + "x": 1.626874682798592, + "y": 4.5432753433356226, + "heading": -0.6851111221374321, + "angularVelocity": 0.7987105489628158, + "velocityX": 1.9544049762484528, + "velocityY": 0.32785090630110136, + "timestamp": 0.6413659082731624 + }, + { + "x": 1.7574465106168242, + "y": 4.56517788745821, + "heading": -0.631746395437431, + "angularVelocity": 0.665638457069628, + "velocityX": 1.6286718846006478, + "velocityY": 0.2731987321441458, + "timestamp": 0.7215366468073077 + }, + { + "x": 1.8619039874752719, + "y": 4.582699429495963, + "heading": -0.5890503399232441, + "angularVelocity": 0.5325640788004256, + "velocityX": 1.3029376898400211, + "velocityY": 0.21855283309246581, + "timestamp": 0.8017073853414529 + }, + { + "x": 1.9402470937608582, + "y": 4.5958403420243075, + "heading": -0.5570256157366094, + "angularVelocity": 0.39945651957534534, + "velocityX": 0.9772032504380553, + "velocityY": 0.16391158131526215, + "timestamp": 0.8818781238755982 + }, + { + "x": 1.9924758344835722, + "y": 4.604600869409716, + "heading": -0.5356750146862916, + "angularVelocity": 0.26631413706167006, + "velocityX": 0.6514688735275852, + "velocityY": 0.10927337761367518, + "timestamp": 0.9620488624097434 + }, + { + "x": 2.018590211868286, + "y": 4.608981132507324, + "heading": -0.525, + "angularVelocity": 0.13315350315433505, + "velocityX": 0.32573452436878464, + "velocityY": 0.0546366813839713, + "timestamp": 1.0422196009438887 + }, + { + "x": 2.018590211868286, + "y": 4.608981132507324, + "heading": -0.525, + "angularVelocity": 2.602590767682023e-32, + "velocityX": -2.497311852298303e-34, + "velocityY": -2.1742604688427545e-32, + "timestamp": 1.122390339478034 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/X6.traj b/src/main/deploy/choreo/X6.traj index 3f5a30fa..2b53ecfb 100644 --- a/src/main/deploy/choreo/X6.traj +++ b/src/main/deploy/choreo/X6.traj @@ -1,175 +1,256 @@ { "samples": [ { - "x": 4.503084659576416, - "y": 4.545476913452148, - "heading": -0.3, - "angularVelocity": 0, - "velocityX": 1.3775324423698682e-39, - "velocityY": 2.667412759246627e-32, + "x": 5.817, + "y": 6.847, + "heading": 0.234, + "angularVelocity": 4.2225178427239466e-38, + "velocityX": 5.2861559836627645e-34, + "velocityY": 4.002568813289242e-37, "timestamp": 0 }, { - "x": 4.535884823966022, - "y": 4.5402829452206666, - "heading": -0.2962963545738625, - "angularVelocity": 0.041338894747682375, - "velocityX": 0.3661048473591824, - "velocityY": -0.05797339683997343, - "timestamp": 0.08959227015485637 - }, - { - "x": 4.60148515224976, - "y": 4.529895009628685, - "heading": -0.28888903544724676, - "angularVelocity": 0.0826781050844292, - "velocityX": 0.7322096891880244, - "velocityY": -0.11594678395833084, - "timestamp": 0.17918454030971273 - }, - { - "x": 4.699885643722846, - "y": 4.514313107942318, - "heading": -0.27777799238048634, - "angularVelocity": 0.12401787618011612, - "velocityX": 1.098314523150324, - "velocityY": -0.17392015694473859, - "timestamp": 0.2687768104645691 - }, - { - "x": 4.831086297424245, - "y": 4.49353724189291, - "heading": -0.26296315953262395, - "angularVelocity": 0.16535838217131477, - "velocityX": 1.4644193463858546, - "velocityY": -0.2318935106064126, - "timestamp": 0.35836908061942546 - }, - { - "x": 4.995087112032728, - "y": 4.46756741379766, - "heading": -0.24444446467786612, - "angularVelocity": 0.20669969432350724, - "velocityX": 1.8305241548742301, - "velocityY": -0.28986683840427246, - "timestamp": 0.44796135077428184 - }, - { - "x": 5.191888085608439, - "y": 4.436403626799179, - "heading": -0.22222184220566124, - "angularVelocity": 0.24804173879949776, - "velocityX": 2.1966289417105944, - "velocityY": -0.34784013112532575, - "timestamp": 0.5375536209291382 - }, - { - "x": 5.421489214846517, - "y": 4.4000458854654045, - "heading": -0.19629525254976224, - "angularVelocity": 0.289384224901166, - "velocityX": 2.5627336916591403, - "velocityY": -0.4058133728605406, - "timestamp": 0.6271458910839945 - }, - { - "x": 5.683890492166353, - "y": 4.358494197979682, - "heading": -0.1666647206388611, - "angularVelocity": 0.3307264327568205, - "velocityX": 2.928838356995378, - "velocityY": -0.463786523255888, - "timestamp": 0.7167381612388509 - }, - { - "x": 5.979091878252505, - "y": 4.311748596170855, - "heading": -0.13333055788996895, - "angularVelocity": 0.3720651646763175, - "velocityX": 3.2949425835053443, - "velocityY": -0.5217593183879428, - "timestamp": 0.8063304313937073 - }, - { - "x": 6.241493168416245, - "y": 4.27019697201608, - "heading": -0.10370083853079468, - "angularVelocity": 0.3307173633167286, - "velocityX": 2.9288385003548862, - "velocityY": -0.4637858163762743, - "timestamp": 0.8959227015485637 - }, - { - "x": 6.471094313755057, - "y": 4.233839308833591, - "heading": -0.0777751923679469, - "angularVelocity": 0.28937369393627815, - "velocityX": 2.562733871370336, - "velocityY": -0.4058125005611213, - "timestamp": 0.9855149717034201 - }, - { - "x": 6.667895303733172, - "y": 4.202675602068972, - "heading": -0.05555346222386864, - "angularVelocity": 0.2480317789209827, - "velocityX": 2.1966291247889234, - "velocityY": -0.3478392355808552, - "timestamp": 1.0751072418582763 - }, - { - "x": 6.83189613345896, - "y": 4.17670584917834, - "heading": -0.03703552336208346, - "angularVelocity": 0.20669125617397324, - "velocityX": 1.8305243236087168, - "velocityY": -0.2898659989946003, - "timestamp": 1.1646995120131327 - }, - { - "x": 6.963096800012264, - "y": 4.155930048478115, - "heading": -0.022221272692350115, - "angularVelocity": 0.16535188408695997, - "velocityX": 1.4644194898346672, - "velocityY": -0.23189278119992932, - "timestamp": 1.254291782167989 - }, - { - "x": 7.061497301461547, - "y": 4.140348198810691, - "heading": -0.011110631132494058, - "angularVelocity": 0.12401339468965188, - "velocityX": 1.0983146345014219, - "velocityY": -0.17391957632608143, - "timestamp": 1.3438840523228455 - }, - { - "x": 7.12709763649646, - "y": 4.129960299402646, - "heading": -0.0037035472512008906, - "angularVelocity": 0.08267547935207345, - "velocityX": 0.7322097645424621, - "velocityY": -0.11594638008490167, - "timestamp": 1.4334763224777018 + "x": 5.827092818691048, + "y": 6.836640444623551, + "heading": 0.23290827050378918, + "angularVelocity": -0.01846630380756111, + "velocityX": 0.1707172489800741, + "velocityY": -0.1752290265644902, + "timestamp": 0.059120087462432476 + }, + { + "x": 5.847162697988382, + "y": 6.81580981961747, + "heading": 0.23072211564801187, + "angularVelocity": -0.03697820740144453, + "velocityX": 0.3394764818317907, + "velocityY": -0.35234428601476925, + "timestamp": 0.11824017492486495 + }, + { + "x": 5.877072599439809, + "y": 6.784379259471569, + "heading": 0.22743855300065416, + "angularVelocity": -0.05554055801159481, + "velocityX": 0.5059177469998274, + "velocityY": -0.5316392700851961, + "timestamp": 0.17736026238729743 + }, + { + "x": 5.916657876422342, + "y": 6.7421982876337765, + "heading": 0.22305428725294427, + "angularVelocity": -0.07415864786221664, + "velocityX": 0.6695740598774843, + "velocityY": -0.7134795235984001, + "timestamp": 0.2364803498497299 + }, + { + "x": 5.965717226201233, + "y": 6.68908910795113, + "heading": 0.21756569779070734, + "angularVelocity": -0.09283797940462643, + "velocityX": 0.8298253924280056, + "velocityY": -0.8983271500806653, + "timestamp": 0.2956004373121624 + }, + { + "x": 6.023999328755403, + "y": 6.624838828190035, + "heading": 0.21096886342235058, + "angularVelocity": -0.11158363682308173, + "velocityX": 0.9858257160259868, + "velocityY": -1.0867757900717288, + "timestamp": 0.35472052477459487 + }, + { + "x": 6.091182353843822, + "y": 6.549188709495219, + "heading": 0.20325967797819372, + "angularVelocity": -0.13039874897099427, + "velocityX": 1.1363823697167148, + "velocityY": -1.2796009265528723, + "timestamp": 0.41384061223702734 + }, + { + "x": 6.166841087704512, + "y": 6.461819179818017, + "heading": 0.19443418504219476, + "angularVelocity": -0.1492807828068109, + "velocityX": 1.279746649711348, + "velocityY": -1.4778315362391972, + "timestamp": 0.4729606996994598 + }, + { + "x": 6.250391326549386, + "y": 6.362329172081929, + "heading": 0.1844894546954548, + "angularVelocity": -0.1682123754140137, + "velocityX": 1.4132292834980353, + "velocityY": -1.6828460850858449, + "timestamp": 0.5320807871618923 + }, + { + "x": 6.340989832538309, + "y": 6.250209683455231, + "heading": 0.17342587375941393, + "angularVelocity": -0.18713742504307268, + "velocityX": 1.5324487814144865, + "velocityY": -1.896470276671073, + "timestamp": 0.5912008746243248 + }, + { + "x": 6.437341958267089, + "y": 6.1248207212545625, + "heading": 0.16125340180931888, + "angularVelocity": -0.20589401119932219, + "velocityX": 1.6297696749857906, + "velocityY": -2.120919768265675, + "timestamp": 0.6503209620867573 + }, + { + "x": 6.537312703515491, + "y": 5.985428428886056, + "heading": 0.14800981318431106, + "angularVelocity": -0.22401165481061264, + "velocityX": 1.6909776277297848, + "velocityY": -2.3577822420692787, + "timestamp": 0.7094410495491897 + }, + { + "x": 6.637191132327962, + "y": 5.83157263911623, + "heading": 0.13381496006922441, + "angularVelocity": -0.24010203171817635, + "velocityX": 1.689416120636461, + "velocityY": -2.602428317914673, + "timestamp": 0.7685611370116222 + }, + { + "x": 6.7310991287231445, + "y": 5.664541721343994, + "heading": 0.11899191581046477, + "angularVelocity": -0.25072771193341653, + "velocityX": 1.588427900328373, + "velocityY": -2.825281980145458, + "timestamp": 0.8276812244740547 + }, + { + "x": 6.827071304911456, + "y": 5.451531523955929, + "heading": 0.1010502513823198, + "angularVelocity": -0.2525856403338842, + "velocityX": 1.3511117473992316, + "velocityY": -2.998791852360776, + "timestamp": 0.8987132293197303 + }, + { + "x": 6.902209054870137, + "y": 5.239913279632194, + "heading": 0.08404272380261314, + "angularVelocity": -0.23943471139040853, + "velocityX": 1.0578013407044498, + "velocityY": -2.979195713023971, + "timestamp": 0.9697452341654059 + }, + { + "x": 6.96109855546152, + "y": 5.041407006580284, + "heading": 0.06855695771575047, + "angularVelocity": -0.21801110809848362, + "velocityX": 0.8290558702281757, + "velocityY": -2.7946032705001835, + "timestamp": 1.0407772390110817 + }, + { + "x": 7.008382229837669, + "y": 4.86025636676961, + "heading": 0.05468561717579363, + "angularVelocity": -0.19528296533504552, + "velocityX": 0.6656671802925685, + "velocityY": -2.5502678715635776, + "timestamp": 1.1118092438567573 + }, + { + "x": 7.046818118928818, + "y": 4.698016877379498, + "heading": 0.04242022938635542, + "angularVelocity": -0.1726741039632221, + "velocityX": 0.5411066346030107, + "velocityY": -2.284033651346236, + "timestamp": 1.182841248702433 + }, + { + "x": 7.078089936261228, + "y": 4.555388481878325, + "heading": 0.03173911036922952, + "angularVelocity": -0.15037051312759409, + "velocityX": 0.4402496790052802, + "velocityY": -2.0079455142938496, + "timestamp": 1.2538732535481085 + }, + { + "x": 7.103305929625202, + "y": 4.4327413175795956, + "heading": 0.022621978547101074, + "angularVelocity": -0.1283524496026329, + "velocityX": 0.35499481422155044, + "velocityY": -1.7266465245517455, + "timestamp": 1.3249052583937841 + }, + { + "x": 7.123243650862787, + "y": 4.330294783654633, + "heading": 0.015051928802047752, + "angularVelocity": -0.10657237904941788, + "velocityX": 0.28068644945191357, + "velocityY": -1.4422587979536587, + "timestamp": 1.3959372632394598 + }, + { + "x": 7.138476280585093, + "y": 4.248190082970356, + "heading": 0.009015136402082834, + "angularVelocity": -0.08498693529882159, + "velocityX": 0.21444741360461653, + "velocityY": -1.1558831946621517, + "timestamp": 1.4669692680851354 + }, + { + "x": 7.149442842095887, + "y": 4.186523843939905, + "heading": 0.004500248099379863, + "angularVelocity": -0.06356132439893981, + "velocityX": 0.1543890185082034, + "velocityY": -0.8681472410137935, + "timestamp": 1.538001272930811 + }, + { + "x": 7.156489910575149, + "y": 4.145365390362028, + "heading": 0.001497840898069894, + "angularVelocity": -0.042268371951954174, + "velocityX": 0.09920976459235319, + "velocityY": -0.5794353357658713, + "timestamp": 1.6090332777764866 }, { "x": 7.159897804260254, "y": 4.1247663497924805, - "heading": 0, - "angularVelocity": 0.04133779895072974, - "velocityX": 0.36610488502078126, - "velocityY": -0.057973188994861924, - "timestamp": 1.5230685926325582 + "heading": -8.486883305020825e-34, + "angularVelocity": -0.021086845307606617, + "velocityX": 0.04797687595202675, + "velocityY": -0.2899966094762708, + "timestamp": 1.6800652826221623 }, { "x": 7.159897804260254, "y": 4.1247663497924805, - "heading": 0, - "angularVelocity": 0, - "velocityX": -1.5986633328718683e-34, + "heading": -4.844202103484773e-34, + "angularVelocity": 5.1282267159186814e-33, + "velocityX": 0, "velocityY": 0, - "timestamp": 1.6126608627874146 + "timestamp": 1.7510972874678379 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Y6.traj b/src/main/deploy/choreo/Y6.traj new file mode 100644 index 00000000..402ac3ab --- /dev/null +++ b/src/main/deploy/choreo/Y6.traj @@ -0,0 +1,247 @@ +{ + "samples": [ + { + "x": 4.377503395080566, + "y": 4.852944374084473, + "heading": -0.3, + "angularVelocity": -1.3779455514923654e-31, + "velocityX": 1.0651190916723646e-33, + "velocityY": 8.422201210225719e-34, + "timestamp": 0 + }, + { + "x": 4.3896055374193645, + "y": 4.846825912939869, + "heading": -0.2985310488008376, + "angularVelocity": 0.025658081384651203, + "velocityX": 0.21138738525455975, + "velocityY": -0.10687078923150421, + "timestamp": 0.05725101488068306 + }, + { + "x": 4.41384571535501, + "y": 4.83466051950474, + "heading": -0.2955924685875449, + "angularVelocity": 0.05132800212218199, + "velocityX": 0.4234017158676471, + "velocityY": -0.21249218831288152, + "timestamp": 0.11450202976136611 + }, + { + "x": 4.450265411872471, + "y": 4.816532199021785, + "heading": -0.29118353531337676, + "angularVelocity": 0.07701056974023836, + "velocityX": 0.636140627259864, + "velocityY": -0.3166462729217243, + "timestamp": 0.17175304464204916 + }, + { + "x": 4.498913092380378, + "y": 4.792540991453841, + "heading": -0.28530348440215925, + "angularVelocity": 0.10270649216389094, + "velocityX": 0.8497260810012408, + "velocityY": -0.41905296557529276, + "timestamp": 0.22900405952273223 + }, + { + "x": 4.55984608940975, + "y": 4.762808016255157, + "heading": -0.2779515279580758, + "angularVelocity": 0.1284161767159208, + "velocityX": 1.0643129585800981, + "velocityY": -0.5193440720771622, + "timestamp": 0.2862550744034153 + }, + { + "x": 4.6331332077909675, + "y": 4.727482864001583, + "heading": -0.26912689582630617, + "angularVelocity": 0.15413931351542365, + "velocityX": 1.280101645952553, + "velocityY": -0.6170222890754846, + "timestamp": 0.34350608928409837 + }, + { + "x": 4.718858408122676, + "y": 4.686754865040645, + "heading": -0.25882892740601127, + "angularVelocity": 0.17987398898966347, + "velocityX": 1.4973568680025728, + "velocityY": -0.7113934843918396, + "timestamp": 0.40075710416478144 + }, + { + "x": 4.817126132111491, + "y": 4.640871125529565, + "heading": -0.24705727650199802, + "angularVelocity": 0.20561471143431556, + "velocityX": 1.7164363669991671, + "velocityY": -0.8014484914670451, + "timestamp": 0.4580081190454645 + }, + { + "x": 4.928069127821043, + "y": 4.590167209877807, + "heading": -0.2338123853794331, + "angularVelocity": 0.23134770886016032, + "velocityX": 1.937834568361266, + "velocityY": -0.885642215381343, + "timestamp": 0.5152591339261475 + }, + { + "x": 5.051859843763298, + "y": 4.535123586269269, + "heading": -0.21909666295203592, + "angularVelocity": 0.2570386299363643, + "velocityX": 2.162244917408828, + "velocityY": -0.9614436307068738, + "timestamp": 0.5725101488068306 + }, + { + "x": 5.188725109230697, + "y": 4.476480746595766, + "heading": -0.20291776182097876, + "angularVelocity": 0.28259588349264136, + "velocityX": 2.390617279931895, + "velocityY": -1.0243109191290498, + "timestamp": 0.6297611636875137 + }, + { + "x": 5.338949906037725, + "y": 4.41550875888357, + "heading": -0.18529937753844272, + "angularVelocity": 0.3077392482780291, + "velocityX": 2.6239674025012714, + "velocityY": -1.0649940064689996, + "timestamp": 0.6870121785681967 + }, + { + "x": 5.50273444641256, + "y": 4.35475741462076, + "heading": -0.16632687899377058, + "angularVelocity": 0.33139147985781603, + "velocityX": 2.860814619901091, + "velocityY": -1.0611400407384983, + "timestamp": 0.7442631934488798 + }, + { + "x": 5.67859411239624, + "y": 4.300183296203613, + "heading": -0.1463958515995223, + "angularVelocity": 0.3481340450607809, + "velocityX": 3.0717301055742974, + "velocityY": -0.9532428120424219, + "timestamp": 0.8015142083295629 + }, + { + "x": 5.902938999773969, + "y": 4.25270379053833, + "heading": -0.12188146282745, + "angularVelocity": 0.3420695091436435, + "velocityX": 3.130469465003068, + "velocityY": -0.6625207484597521, + "timestamp": 0.8731791422590521 + }, + { + "x": 6.111777839654233, + "y": 4.219753932279735, + "heading": -0.09952223180713043, + "angularVelocity": 0.31199681342507896, + "velocityX": 2.9141007802468644, + "velocityY": -0.45977658042655406, + "timestamp": 0.9448440761885414 + }, + { + "x": 6.301050755084051, + "y": 4.1950916994922665, + "heading": -0.07947119870245584, + "angularVelocity": 0.2797886219277436, + "velocityX": 2.641081279947074, + "velocityY": -0.3441324987717529, + "timestamp": 1.0165090101180305 + }, + { + "x": 6.469818526680435, + "y": 4.176002062195389, + "heading": -0.06171169483230788, + "angularVelocity": 0.24781302230211338, + "velocityX": 2.354956076041784, + "velocityY": -0.2663734723548429, + "timestamp": 1.0881739440475198 + }, + { + "x": 6.617746045928132, + "y": 4.161060334088034, + "heading": -0.046218926010036, + "angularVelocity": 0.2161833964364665, + "velocityX": 2.064154826309365, + "velocityY": -0.20849427032272078, + "timestamp": 1.159838877977009 + }, + { + "x": 6.74467949537251, + "y": 4.1494012139137455, + "heading": -0.032972770112789145, + "angularVelocity": 0.18483455116668027, + "velocityX": 1.771207234618634, + "velocityY": -0.16268933123917845, + "timestamp": 1.2315038119064983 + }, + { + "x": 6.850536459076317, + "y": 4.140446204164987, + "heading": -0.021957698041216698, + "angularVelocity": 0.15370239624319212, + "velocityX": 1.4771096253009843, + "velocityY": -0.12495664556908366, + "timestamp": 1.3031687458359875 + }, + { + "x": 6.935267964024793, + "y": 4.1337821626583535, + "heading": -0.013161616848593847, + "angularVelocity": 0.12273898419103196, + "velocityX": 1.1823286550691923, + "velocityY": -0.0929888739336499, + "timestamp": 1.3748336797654768 + }, + { + "x": 6.998842680180015, + "y": 4.129099600800989, + "heading": -0.006574926597859502, + "angularVelocity": 0.09190952798777466, + "velocityX": 0.8871105109476977, + "velocityY": -0.06533965219268564, + "timestamp": 1.446498613694966 + }, + { + "x": 7.041239426446096, + "y": 4.126158166211539, + "heading": -0.002189855651721091, + "angularVelocity": 0.06118851585704257, + "velocityX": 0.5915968095051178, + "velocityY": -0.041044265698249975, + "timestamp": 1.5181635476244553 + }, + { + "x": 7.062443256378174, + "y": 4.124765872955322, + "heading": -5.255644557217204e-33, + "angularVelocity": 0.030556864168404942, + "velocityX": 0.2958745479754329, + "velocityY": -0.01942781748165762, + "timestamp": 1.5898284815539445 + }, + { + "x": 7.062443256378174, + "y": 4.124765872955322, + "heading": -2.1819063016426035e-33, + "angularVelocity": 4.289059739434279e-32, + "velocityX": 0, + "velocityY": 6.467184527739888e-33, + "timestamp": 1.6614934154834338 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Z7.traj b/src/main/deploy/choreo/Z7.traj index 55e87fa4..7fb31a7a 100644 --- a/src/main/deploy/choreo/Z7.traj +++ b/src/main/deploy/choreo/Z7.traj @@ -1,157 +1,229 @@ { "samples": [ { - "x": 5.776, - "y": 1.513, - "heading": -0.65, + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, "angularVelocity": 0, - "velocityX": 0, + "velocityX": -1.4126138451676342e-35, "velocityY": 0, "timestamp": 0 }, { - "x": 5.79762140212684, - "y": 1.5273147458744445, - "heading": -0.6397819014636212, - "angularVelocity": 0.12881016597140443, - "velocityX": 0.27256112148236133, - "velocityY": 0.1804528293949252, - "timestamp": 0.07932680203709337 - }, - { - "x": 5.8408642615291155, - "y": 1.5559443036967622, - "heading": -0.6193516690577348, - "angularVelocity": 0.2575451408760095, - "velocityX": 0.5451229381723454, - "velocityY": 0.36090649171676586, - "timestamp": 0.15865360407418674 - }, - { - "x": 5.905728713931284, - "y": 1.598888770329341, - "heading": -0.5887202186008055, - "angularVelocity": 0.386142510101516, - "velocityX": 0.8176864658156355, - "velocityY": 0.5413613750936046, - "timestamp": 0.23798040611128013 - }, - { - "x": 5.992214995382535, - "y": 1.6561482404855234, - "heading": -0.5479033051820914, - "angularVelocity": 0.5145412694139371, - "velocityX": 1.0902529691139036, - "velocityY": 0.7218174524343958, - "timestamp": 0.3173072081483735 - }, - { - "x": 6.100323461823128, - "y": 1.7277227582713581, - "heading": -0.49692088625588365, - "angularVelocity": 0.6426884434641421, - "velocityX": 1.362823959423462, - "velocityY": 0.902274085779563, - "timestamp": 0.39663401018546685 - }, - { - "x": 6.230054604703474, - "y": 1.8136122498754594, - "heading": -0.43579566578740936, - "angularVelocity": 0.7705494095159927, - "velocityX": 1.6354011449961674, - "velocityY": 1.0827297886525065, - "timestamp": 0.4759608122225602 - }, - { - "x": 6.3814090857949495, - "y": 1.9138164127150836, - "heading": -0.36455118251674296, - "angularVelocity": 0.8981136443311097, - "velocityX": 1.907986672911659, - "velocityY": 1.2631816771432258, - "timestamp": 0.5552876142596536 - }, - { - "x": 6.554405526600661, - "y": 2.0283184347938064, - "heading": -0.2835830817160487, - "angularVelocity": 1.0206903432566645, - "velocityX": 2.180806944982083, - "velocityY": 1.4434216322647335, - "timestamp": 0.634614416296747 - }, - { - "x": 6.7057780488968515, - "y": 2.128506370748931, - "heading": -0.21272129699084932, - "angularVelocity": 0.8932893159119689, - "velocityX": 1.9082141017787242, - "velocityY": 1.262977119741651, - "timestamp": 0.7139412183338404 - }, - { - "x": 6.835526224253662, - "y": 2.2143808110875423, - "heading": -0.15196551010833717, - "angularVelocity": 0.7658923002354566, - "velocityX": 1.6356158577543578, - "velocityY": 1.0825400511980374, - "timestamp": 0.7932680203709338 - }, - { - "x": 6.9436498396674775, - "y": 2.285942234510729, - "heading": -0.10132145144694792, - "angularVelocity": 0.6384230469508654, - "velocityX": 1.3630149285894169, - "velocityY": 0.9021090171985522, - "timestamp": 0.8725948224080272 - }, - { - "x": 7.030148791972443, - "y": 2.343191019381516, - "heading": -0.06079681140478092, - "angularVelocity": 0.5108568478938251, - "velocityX": 1.0904126989074627, - "velocityY": 0.7216827528735796, - "timestamp": 0.9519216244451206 - }, - { - "x": 7.095023036988465, - "y": 2.3861274409630164, - "heading": -0.030398784130004894, - "angularVelocity": 0.3831999588305835, - "velocityX": 0.8178099122877435, - "velocityY": 0.5412599585373844, - "timestamp": 1.031248426482214 - }, - { - "x": 7.1382725548225325, - "y": 2.414751673887316, - "heading": -0.010132481200335029, - "angularVelocity": 0.2554786328105512, - "velocityX": 0.5452068748951213, - "velocityY": 0.36083936562720403, - "timestamp": 1.1105752285193073 - }, - { - "x": 7.159897327423096, - "y": 2.4290637969970703, - "heading": 3.713306132884575e-32, - "angularVelocity": 0.12773086699747532, - "velocityX": 0.2726036099432161, - "velocityY": 0.18041976661383763, - "timestamp": 1.1899020305564008 - }, - { - "x": 7.159897327423096, - "y": 2.4290637969970703, - "heading": -8.409337330142514e-33, - "angularVelocity": -5.74111111803187e-31, - "velocityX": 0, - "velocityY": 3.756127142115375e-32, - "timestamp": 1.2692288325934942 + "x": 4.296505463860315, + "y": 1.517055896140876, + "heading": -0.7914887789961823, + "angularVelocity": 0.11746007928103745, + "velocityX": 0.2932798142493999, + "velocityY": 0.05604937824134911, + "timestamp": 0.07246054196382429 + }, + { + "x": 4.339007949385954, + "y": 1.5251785389422798, + "heading": -0.774469146362606, + "angularVelocity": 0.23488138747393392, + "velocityX": 0.586560414450918, + "velocityY": 0.11209746133915541, + "timestamp": 0.14492108392764858 + }, + { + "x": 4.402761808267112, + "y": 1.5373623057737362, + "heading": -0.7489458109954873, + "angularVelocity": 0.35223771000582876, + "velocityX": 0.8798424239358591, + "velocityY": 0.16814346817249837, + "timestamp": 0.21738162589147286 + }, + { + "x": 4.487767176416459, + "y": 1.5536069760091191, + "heading": -0.714924557460497, + "angularVelocity": 0.4695142019773372, + "velocityX": 1.1731263090991852, + "velocityY": 0.22418643022975243, + "timestamp": 0.28984216785529715 + }, + { + "x": 4.594024205006697, + "y": 1.5739122453438348, + "heading": -0.6724110080488273, + "angularVelocity": 0.5867131028759709, + "velocityX": 1.4664122805386484, + "velocityY": 0.28022519269664276, + "timestamp": 0.36230270981912144 + }, + { + "x": 4.7215330332195835, + "y": 1.5982777172250429, + "heading": -0.6214089357318703, + "angularVelocity": 0.7038599344512155, + "velocityX": 1.7597001727718804, + "velocityY": 0.33625848249068047, + "timestamp": 0.4347632517829457 + }, + { + "x": 4.8702937518908325, + "y": 1.6267029058241753, + "heading": -0.561918181875302, + "angularVelocity": 0.8210089552776038, + "velocityX": 2.052989318593802, + "velocityY": 0.39228506755198983, + "timestamp": 0.5072237937467701 + }, + { + "x": 5.040306360048619, + "y": 1.6591872583478575, + "heading": -0.493932387640852, + "angularVelocity": 0.9382457319790903, + "velocityX": 2.34627845100393, + "velocityY": 0.44830402372507494, + "timestamp": 0.5796843357105943 + }, + { + "x": 5.231570719979306, + "y": 1.6957302040108748, + "heading": -0.4174369611049703, + "angularVelocity": 1.0556838861910776, + "velocityX": 2.639565682881242, + "velocityY": 0.5043151027115036, + "timestamp": 0.6521448776744185 + }, + { + "x": 5.444086520570827, + "y": 1.736331233452616, + "heading": -0.3324079212031443, + "angularVelocity": 1.173452993827683, + "velocityX": 2.9328486212208924, + "velocityY": 0.5603191522085388, + "timestamp": 0.7246054196382428 + }, + { + "x": 5.677853259899273, + "y": 1.7809900044282545, + "heading": -0.23881232004877342, + "angularVelocity": 1.2916768025430743, + "velocityX": 3.2261246327022186, + "velocityY": 0.6163184785166836, + "timestamp": 0.797065961602067 + }, + { + "x": 5.932870232495776, + "y": 1.8297064569167303, + "heading": -0.13660994045650976, + "angularVelocity": 1.4104556331263418, + "velocityX": 3.519390908279707, + "velocityY": 0.6723169764973237, + "timestamp": 0.8695265035658912 + }, + { + "x": 6.1666110103130665, + "y": 1.8743699539047558, + "heading": -0.04151626999903835, + "angularVelocity": 1.3123510793632576, + "velocityX": 3.2257663478971033, + "velocityY": 0.6163837003913594, + "timestamp": 0.9419870455297155 + }, + { + "x": 6.379102210383419, + "y": 1.9149751005455964, + "heading": 0.04493399031560497, + "angularVelocity": 1.1930667087447864, + "velocityX": 2.9325091189138175, + "velocityY": 0.5603759720857812, + "timestamp": 1.0144475874935397 + }, + { + "x": 6.570344223254283, + "y": 1.9515213437751668, + "heading": 0.12272453353903856, + "angularVelocity": 1.0735572922194023, + "velocityX": 2.639257279725309, + "velocityY": 0.5043606111560126, + "timestamp": 1.086908129457364 + }, + { + "x": 6.740337234658211, + "y": 1.9840081308159054, + "heading": 0.1918507517039546, + "angularVelocity": 0.9539842829139629, + "velocityX": 2.3460080037601223, + "velocityY": 0.44833762155626, + "timestamp": 1.1593686714211882 + }, + { + "x": 6.8890812999679625, + "y": 2.0124349614152695, + "heading": 0.252315028167953, + "angularVelocity": 0.8344441654077742, + "velocityX": 2.0527594919730525, + "velocityY": 0.3923077281640593, + "timestamp": 1.2318292133850124 + }, + { + "x": 7.016576391150559, + "y": 2.0368014172627156, + "heading": 0.30412372738559124, + "angularVelocity": 0.714991881284902, + "velocityX": 1.7595105933136266, + "velocityY": 0.33627206182933134, + "timestamp": 1.3042897553488366 + }, + { + "x": 7.12282243639323, + "y": 2.057107171032732, + "heading": 0.34728480849502247, + "angularVelocity": 0.5956494381587609, + "velocityX": 1.4662607035938906, + "velocityY": 0.2802318782014398, + "timestamp": 1.3767502973126609 + }, + { + "x": 7.207819351985288, + "y": 2.0733519822331425, + "heading": 0.3818060258723622, + "angularVelocity": 0.47641401018742496, + "velocityX": 1.1730096586151824, + "velocityY": 0.2241883756337472, + "timestamp": 1.449210839276485 + }, + { + "x": 7.271567065989971, + "y": 2.085535686633377, + "heading": 0.40769367366399617, + "angularVelocity": 0.3572654453034354, + "velocityX": 0.8797576208650236, + "velocityY": 0.16814260658327587, + "timestamp": 1.5216713812403093 + }, + { + "x": 7.314065534356957, + "y": 2.093658184470014, + "heading": 0.42495178086767527, + "angularVelocity": 0.23817248306388825, + "velocityX": 0.5865049751932958, + "velocityY": 0.11209546073630015, + "timestamp": 1.5941319232041335 + }, + { + "x": 7.335314750671387, + "y": 2.097719430923462, + "heading": 0.4335816497672733, + "angularVelocity": 0.11909749314194312, + "velocityX": 0.2932522409925998, + "velocityY": 0.056047696351426264, + "timestamp": 1.6665924651679578 + }, + { + "x": 7.335314750671387, + "y": 2.097719430923462, + "heading": 0.4335816497672733, + "angularVelocity": 0, + "velocityX": -1.6349703542384758e-35, + "velocityY": 0, + "timestamp": 1.739053007131782 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Z8.traj b/src/main/deploy/choreo/Z8.traj index 38cdf9ff..e5e4a99d 100644 --- a/src/main/deploy/choreo/Z8.traj +++ b/src/main/deploy/choreo/Z8.traj @@ -1,166 +1,211 @@ { "samples": [ { - "x": 5.776048183441162, - "y": 1.512994647026062, - "heading": -0.65, + "x": 4.275254249572754, + "y": 1.5129945278167725, + "heading": -0.8, "angularVelocity": 0, - "velocityX": -1.5789883675704282e-32, - "velocityY": 0, + "velocityX": 5.717023418431248e-32, + "velocityY": -1.425773996162293e-28, "timestamp": 0 }, { - "x": 5.804743050185828, - "y": 1.5043318888705361, - "heading": -0.6437581390191097, - "angularVelocity": 0.07330082168918192, - "velocityX": 0.3369759943524967, - "velocityY": -0.10173044430799516, - "timestamp": 0.08515403834567799 - }, - { - "x": 5.86213277976621, - "y": 1.4870063760319097, - "heading": -0.6312738261796365, - "angularVelocity": 0.14660858230579626, - "velocityX": 0.6739519428005418, - "velocityY": -0.20346084783782606, - "timestamp": 0.17030807669135598 - }, - { - "x": 5.9482173644624545, - "y": 1.4610181153106103, - "heading": -0.6125459823544488, - "angularVelocity": 0.21992901557014852, - "velocityX": 1.010927800591089, - "velocityY": -0.3051911715073518, - "timestamp": 0.255462115037034 - }, - { - "x": 6.062996793277665, - "y": 1.4263671179092674, - "heading": -0.5875730752792624, - "angularVelocity": 0.2932674428640758, - "velocityX": 1.347903529240383, - "velocityY": -0.40692136361964437, - "timestamp": 0.34061615338271195 - }, - { - "x": 6.206471052722686, - "y": 1.3830534007491153, - "heading": -0.5563531860432791, - "angularVelocity": 0.3666284047416284, - "velocityX": 1.684879099480822, - "velocityY": -0.5086513570187057, - "timestamp": 0.4257701917283899 - }, - { - "x": 6.378640127771531, - "y": 1.3310769879206097, - "heading": -0.5188841218821412, - "angularVelocity": 0.44001511718134073, - "velocityX": 2.0218544932647196, - "velocityY": -0.6103810675133283, - "timestamp": 0.510924230074068 - }, - { - "x": 6.579504002372721, - "y": 1.2704379122308411, - "heading": -0.4751635904846247, - "angularVelocity": 0.5134287492042999, - "velocityX": 2.3588296985492834, - "velocityY": -0.7121103927403597, - "timestamp": 0.596078268419746 - }, - { - "x": 6.809062654743387, - "y": 1.2011362179518417, - "heading": -0.42518944746652015, - "angularVelocity": 0.5868675636408143, - "velocityX": 2.695804648028377, - "velocityY": -0.8138391980621403, - "timestamp": 0.681232306765424 - }, - { - "x": 7.038621262896878, - "y": 1.1318364551262148, - "heading": -0.3751455547563511, - "angularVelocity": 0.5876866638669395, - "velocityX": 2.69580412876734, - "velocityY": -0.8138165161857421, - "timestamp": 0.766386345111102 - }, - { - "x": 7.2394850898490155, - "y": 1.071199229853428, - "heading": -0.33135757746423733, - "angularVelocity": 0.5142207949593519, - "velocityX": 2.3588291389862373, - "velocityY": -0.7120886625086819, - "timestamp": 0.8515403834567801 - }, - { - "x": 7.411654113486545, - "y": 1.0192245139134757, - "heading": -0.2938257380134279, - "angularVelocity": 0.4407523140411856, - "velocityX": 2.021853889519832, - "velocityY": -0.6103611402311183, - "timestamp": 0.9366944218024581 - }, - { - "x": 7.555128319715313, - "y": 0.9759122824392717, - "heading": -0.2625498891671241, - "angularVelocity": 0.3672855621872125, - "velocityX": 1.6848784745397705, - "velocityY": -0.5086339099782955, - "timestamp": 1.0218484601481361 - }, - { - "x": 7.669907697003263, - "y": 0.9412625159146415, - "heading": -0.23752970245661262, - "angularVelocity": 0.29382266768069665, - "velocityX": 1.3479029241339204, - "velocityY": -0.4069069089121928, - "timestamp": 1.107002498493814 - }, - { - "x": 7.75599223622601, - "y": 0.9152752001717872, - "heading": -0.21876481896635738, - "angularVelocity": 0.22036398807159627, - "velocityX": 1.0109272665764968, - "velocityY": -0.30518007422455146, - "timestamp": 1.192156536839492 - }, - { - "x": 7.813381931076612, - "y": 0.8979503260405115, - "heading": -0.20625496018104028, - "angularVelocity": 0.14690857918604, - "velocityX": 0.6739515349540018, - "velocityY": -0.20345334722642713, - "timestamp": 1.2773105751851699 + "x": 4.304732169950001, + "y": 1.5078399138540146, + "heading": -0.7950484987601832, + "angularVelocity": 0.058208034646547904, + "velocityX": 0.34653163303819684, + "velocityY": -0.06059575340920951, + "timestamp": 0.08506559738515405 + }, + { + "x": 4.363688008669768, + "y": 1.4975306838042393, + "heading": -0.7851452998454412, + "angularVelocity": 0.11641837851226666, + "velocityX": 0.693063242156879, + "velocityY": -0.12119153179043739, + "timestamp": 0.1701311947703081 + }, + { + "x": 4.452121762210008, + "y": 1.4820668346392138, + "heading": -0.7702900404294987, + "angularVelocity": 0.1746329876307565, + "velocityX": 1.0395948098716838, + "velocityY": -0.1817873457704544, + "timestamp": 0.25519679215546215 + }, + { + "x": 4.570033425335727, + "y": 1.4614483627248729, + "heading": -0.7504821592921513, + "angularVelocity": 0.2328541942483684, + "velocityX": 1.3861263160457933, + "velocityY": -0.24238320247121709, + "timestamp": 0.3402623895406162 + }, + { + "x": 4.717422990801947, + "y": 1.4356752642589519, + "heading": -0.7257208555275354, + "angularVelocity": 0.2910848160219101, + "velocityX": 1.7326577370506226, + "velocityY": -0.3029791038700053, + "timestamp": 0.4253279869257703 + }, + { + "x": 4.89429044896851, + "y": 1.4047475358875174, + "heading": -0.6960050379672905, + "angularVelocity": 0.3493282651709566, + "velocityX": 2.0791890447292576, + "velocityY": -0.3635750446964123, + "timestamp": 0.5103935843109243 + }, + { + "x": 5.100635787262615, + "y": 1.3686651755613142, + "heading": -0.6613332622001218, + "angularVelocity": 0.407588694290044, + "velocityX": 2.425720204606665, + "velocityY": -0.4241710096131092, + "timestamp": 0.5954591816960784 + }, + { + "x": 5.336458989275771, + "y": 1.3274281837545279, + "heading": -0.6217036408461484, + "angularVelocity": 0.46587131075560523, + "velocityX": 2.772251171591864, + "velocityY": -0.48476696895545185, + "timestamp": 0.6805247790812324 + }, + { + "x": 5.601760032758817, + "y": 1.281036565393316, + "heading": -0.5771136718696128, + "angularVelocity": 0.5241833402369976, + "velocityX": 3.1187818770240705, + "velocityY": -0.5453628703876953, + "timestamp": 0.7655903764663865 + }, + { + "x": 5.896538883462558, + "y": 1.229490333851042, + "heading": -0.5275597562412677, + "angularVelocity": 0.582537678588075, + "velocityX": 3.4653121798353, + "velocityY": -0.6059586146075705, + "timestamp": 0.8506559738515406 + }, + { + "x": 6.2207954703436386, + "y": 1.172789520169878, + "heading": -0.4730361468387663, + "angularVelocity": 0.6409595780083519, + "velocityX": 3.811841647486834, + "velocityY": -0.6665539939070283, + "timestamp": 0.9357215712366946 + }, + { + "x": 6.515573956709268, + "y": 1.1212436190020052, + "heading": -0.42340864440375947, + "angularVelocity": 0.5834027381250584, + "velocityX": 3.4653078968099584, + "velocityY": -0.605954730847158, + "timestamp": 1.0207871686218486 + }, + { + "x": 6.780874551524839, + "y": 1.0748523775635215, + "heading": -0.3787346711264011, + "angularVelocity": 0.5251708640202797, + "velocityX": 3.1187766026536243, + "velocityY": -0.5453584394221985, + "timestamp": 1.1058527660070026 + }, + { + "x": 7.016697283488896, + "y": 1.0336157628045721, + "heading": -0.33902011581118113, + "angularVelocity": 0.46686976328700697, + "velocityX": 2.772245645866879, + "velocityY": -0.48476253651921614, + "timestamp": 1.1909183633921565 + }, + { + "x": 7.223042164396211, + "y": 0.9975337560488634, + "heading": -0.30426743462836536, + "angularVelocity": 0.40853978871571445, + "velocityX": 2.4257148277351357, + "velocityY": -0.4241668531679094, + "timestamp": 1.2759839607773105 + }, + { + "x": 7.399909200334508, + "y": 0.9666063434187415, + "heading": -0.2744779957092185, + "angularVelocity": 0.35019373089436867, + "velocityX": 2.0791840811684517, + "velocityY": -0.3635713329572148, + "timestamp": 1.3610495581624644 + }, + { + "x": 7.547298394573816, + "y": 0.9408335139795543, + "heading": -0.2496526340730793, + "angularVelocity": 0.2918378568915062, + "velocityX": 1.7326533730430376, + "velocityY": -0.30297594129028455, + "timestamp": 1.4461151555476184 + }, + { + "x": 7.665209748822617, + "y": 0.9202152591870838, + "heading": -0.22979187484564018, + "angularVelocity": 0.23347580970332782, + "velocityX": 1.3861226850019162, + "velocityY": -0.24238065006605194, + "timestamp": 1.5311807529327723 + }, + { + "x": 7.753643263936589, + "y": 0.9047515726613659, + "heading": -0.21489605202877265, + "angularVelocity": 0.17510983611146802, + "velocityX": 1.039592007019826, + "velocityY": -0.18178543384233942, + "timestamp": 1.6162463503179263 + }, + { + "x": 7.812598940362171, + "y": 0.8944424500616808, + "heading": -0.20496538044326496, + "angularVelocity": 0.11674133716509057, + "velocityX": 0.6930613342859036, + "velocityY": -0.12119026864653992, + "timestamp": 1.7013119477030803 }, { "x": 7.842076778411865, "y": 0.8892878890037537, "heading": -0.2, - "angularVelocity": 0.07345465115404878, - "velocityX": 0.3369757664195322, - "velocityY": -0.10172667327406405, - "timestamp": 1.3624646135308478 + "angularVelocity": 0.058371193477605506, + "velocityX": 0.3465306652256466, + "velocityY": -0.0605951314794, + "timestamp": 1.7863775450882342 }, { "x": 7.842076778411865, "y": 0.8892878890037537, "heading": -0.2, "angularVelocity": 0, - "velocityX": -1.7603165656807992e-35, + "velocityX": 1.028052364086851e-30, "velocityY": 0, - "timestamp": 1.4476186518765257 + "timestamp": 1.8714431424733882 } ] } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auton/AutonBuilder.java b/src/main/java/frc/robot/commands/auton/AutonBuilder.java index 01acaccc..5f3b0e83 100644 --- a/src/main/java/frc/robot/commands/auton/AutonBuilder.java +++ b/src/main/java/frc/robot/commands/auton/AutonBuilder.java @@ -415,6 +415,28 @@ public SequentialCommandGroup getBottomTwoPiece() { ); } + /** starts in E position, intakes and shoots speaker side 3 center notes. */ + public SequentialCommandGroup getBottomCenter3Piece() { + + ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("E8"); + ChoreoTrajectory piece1ToWing = Choreo.getTrajectory("8Z"); + ChoreoTrajectory wingToPiece2 = Choreo.getTrajectory("Z7"); + ChoreoTrajectory piece2ToWing = Choreo.getTrajectory("7Y"); + ChoreoTrajectory wingToPiece3 = Choreo.getTrajectory("Y6"); + ChoreoTrajectory piece3ToWing = Choreo.getTrajectory("6Y"); + + return buildAuton( + new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), + shoot(), + goIntake(startToPiece1), + goShoot(piece1ToWing), + goIntake(wingToPiece2), + goShoot(piece2ToWing), + goIntake(wingToPiece3), + goShoot(piece3ToWing) + ); + } + /** * Starts: right in front of subwoofer. Shoots preloaded note, intakes middle note, shoots, * intakes top note, shoots.