From f3fd98a3fce28770c24401492240785c2db9c401 Mon Sep 17 00:00:00 2001 From: fcuellar13 <105395555+fcuellar13@users.noreply.github.com> Date: Sat, 15 Jun 2024 12:54:50 -0700 Subject: [PATCH] changes to six piece with dynamic paths --- Choreo_Trajectories.chor | 4134 +++++++++-------- src/main/deploy/choreo/Red 4 to 5.1 2.traj | 140 + src/main/deploy/choreo/Red 4 to 5.1.traj | 275 +- src/main/deploy/choreo/Red 4 to 5.traj | 275 +- src/main/deploy/choreo/Red 4 to 6.traj | 140 + src/main/deploy/choreo/Red 4 to CWS 2.traj | 122 + src/main/deploy/choreo/Red 4 to CWS.1 2.traj | 122 + src/main/deploy/choreo/Red 4 to CWS.1.traj | 175 +- src/main/deploy/choreo/Red 4 to CWS.traj | 175 +- src/main/deploy/choreo/Red 5 to 6.1 2.traj | 140 + src/main/deploy/choreo/Red 5 to 6.1.traj | 280 +- src/main/deploy/choreo/Red 5 to 6.traj | 280 +- src/main/deploy/choreo/Red 5 to 7.traj | 140 + src/main/deploy/choreo/Red 5 to CWS 2.traj | 122 + src/main/deploy/choreo/Red 5 to CWS.1 2.traj | 122 + src/main/deploy/choreo/Red 5 to CWS.1.traj | 195 +- src/main/deploy/choreo/Red 5 to CWS.traj | 195 +- src/main/deploy/choreo/Red 6 to CWS 2.traj | 203 + src/main/deploy/choreo/Red 6 to CWS.1 2.traj | 203 + src/main/deploy/choreo/Red 6 to CWS.1.traj | 398 +- src/main/deploy/choreo/Red 6 to CWS.traj | 398 +- src/main/deploy/choreo/Red CWS to 5.1 2.traj | 122 + src/main/deploy/choreo/Red CWS to 5.1.traj | 340 +- src/main/deploy/choreo/Red CWS to 5.traj | 340 +- src/main/deploy/choreo/Red CWS to 6.1 2.traj | 257 + src/main/deploy/choreo/Red CWS to 6.1.traj | 392 +- src/main/deploy/choreo/Red CWS to 6.traj | 392 +- src/main/deploy/choreo/Red CWS to 7.traj | 122 + src/main/deploy/choreo/Red CWS to 8.traj | 257 + .../choreo/Red Six Piece 2 to 4 (1) 2.traj | 897 ++++ .../choreo/Red Six Piece 2 to 4 (1).1 2.traj | 68 + .../choreo/Red Six Piece 2 to 4 (1).1.traj | 84 +- .../choreo/Red Six Piece 2 to 4 (1).2 2.traj | 96 + .../choreo/Red Six Piece 2 to 4 (1).2.traj | 102 +- .../choreo/Red Six Piece 2 to 4 (1).3 2.traj | 113 + .../choreo/Red Six Piece 2 to 4 (1).3.traj | 150 +- .../choreo/Red Six Piece 2 to 4 (1).4 2.traj | 132 + .../choreo/Red Six Piece 2 to 4 (1).4.traj | 168 +- .../choreo/Red Six Piece 2 to 4 (1).5 2.traj | 131 + .../choreo/Red Six Piece 2 to 4 (1).5.traj | 199 +- .../choreo/Red Six Piece 2 to 4 (1).6 2.traj | 105 + .../choreo/Red Six Piece 2 to 4 (1).6.traj | 136 +- .../choreo/Red Six Piece 2 to 4 (1).7 2.traj | 221 + .../choreo/Red Six Piece 2 to 4 (1).7.traj | 416 +- .../choreo/Red Six Piece 2 to 4 (1).8 2.traj | 131 + .../choreo/Red Six Piece 2 to 4 (1).8.traj | 155 +- .../choreo/Red Six Piece 2 to 4 (1).traj | 1535 +++--- .../autos/Red Six Piece 2 to 4.1.auto | 14 +- 48 files changed, 10158 insertions(+), 5151 deletions(-) create mode 100644 src/main/deploy/choreo/Red 4 to 5.1 2.traj create mode 100644 src/main/deploy/choreo/Red 4 to 6.traj create mode 100644 src/main/deploy/choreo/Red 4 to CWS 2.traj create mode 100644 src/main/deploy/choreo/Red 4 to CWS.1 2.traj create mode 100644 src/main/deploy/choreo/Red 5 to 6.1 2.traj create mode 100644 src/main/deploy/choreo/Red 5 to 7.traj create mode 100644 src/main/deploy/choreo/Red 5 to CWS 2.traj create mode 100644 src/main/deploy/choreo/Red 5 to CWS.1 2.traj create mode 100644 src/main/deploy/choreo/Red 6 to CWS 2.traj create mode 100644 src/main/deploy/choreo/Red 6 to CWS.1 2.traj create mode 100644 src/main/deploy/choreo/Red CWS to 5.1 2.traj create mode 100644 src/main/deploy/choreo/Red CWS to 6.1 2.traj create mode 100644 src/main/deploy/choreo/Red CWS to 7.traj create mode 100644 src/main/deploy/choreo/Red CWS to 8.traj create mode 100644 src/main/deploy/choreo/Red Six Piece 2 to 4 (1) 2.traj create mode 100644 src/main/deploy/choreo/Red Six Piece 2 to 4 (1).1 2.traj create mode 100644 src/main/deploy/choreo/Red Six Piece 2 to 4 (1).2 2.traj create mode 100644 src/main/deploy/choreo/Red Six Piece 2 to 4 (1).3 2.traj create mode 100644 src/main/deploy/choreo/Red Six Piece 2 to 4 (1).4 2.traj create mode 100644 src/main/deploy/choreo/Red Six Piece 2 to 4 (1).5 2.traj create mode 100644 src/main/deploy/choreo/Red Six Piece 2 to 4 (1).6 2.traj create mode 100644 src/main/deploy/choreo/Red Six Piece 2 to 4 (1).7 2.traj create mode 100644 src/main/deploy/choreo/Red Six Piece 2 to 4 (1).8 2.traj diff --git a/Choreo_Trajectories.chor b/Choreo_Trajectories.chor index f2c775b2..8cd23df4 100644 --- a/Choreo_Trajectories.chor +++ b/Choreo_Trajectories.chor @@ -3,7 +3,7 @@ "robotConfiguration": { "mass": 74.63128883444794, "rotationalInertia": 6.000000000000001, - "motorMaxTorque": 1.5519668737060042, + "motorMaxTorque": 1.163975155279503, "motorMaxVelocity": 4640, "gearing": 6.122448979591837, "wheelbase": 0.7769855804277865, @@ -76,20 +76,20 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 14 }, { - "x": 14.352291107177734, - "y": 6.525911808013916, - "heading": -0.48570563852984416, + "x": 14.446789741516113, + "y": 6.612919330596924, + "heading": -0.5944409566225068, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 4 }, { - "x": 14.129294395446777, - "y": 6.708604335784912, + "x": 14.166648864746094, + "y": 6.843034744262695, "heading": -0.6055455776100214, "isInitialGuess": false, "translationConstrained": false, @@ -97,8 +97,8 @@ "controlIntervalCount": 5 }, { - "x": 13.670247077941895, - "y": 7.0260772705078125, + "x": 13.75644302368164, + "y": 7.143185615539551, "heading": -0.5880017968591525, "isInitialGuess": false, "translationConstrained": true, @@ -106,8 +106,8 @@ "controlIntervalCount": 12 }, { - "x": 11.93593692779541, - "y": 7.216775417327881, + "x": 11.942968368530273, + "y": 7.280330657958984, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -121,12 +121,12 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 6 }, { - "x": 11.927, - "y": 6.728, - "heading": -0.271, + "x": 10.155044555664062, + "y": 7.437562942504883, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -137,866 +137,812 @@ { "x": 15.198025703430176, "y": 5.552863121032715, - "heading": 1.1788293001141444e-28, - "angularVelocity": 2.606876389920206e-29, - "velocityX": 1.6147849318653567e-22, - "velocityY": -1.7720524977117054e-24, + "heading": -9.080591826526709e-28, + "angularVelocity": -1.1603484864380112e-28, + "velocityX": -3.747628767048725e-22, + "velocityY": 4.167074864344212e-24, "timestamp": 0 }, { - "x": 15.10708649187578, - "y": 5.553873539056015, - "heading": 1.956931650032567e-18, - "angularVelocity": 2.0546044837018224e-17, - "velocityX": -0.9547809798230291, - "velocityY": 0.010608492132583473, - "timestamp": 0.09524614909196333 + "x": 15.107086496476764, + "y": 5.553873539004894, + "heading": -2.0171687380914284e-18, + "angularVelocity": -2.117848210218587e-17, + "velocityX": -0.95478099189959, + "velocityY": 0.010608492266763213, + "timestamp": 0.09524614306835108 }, { - "x": 14.925208076423274, - "y": 5.555894375017549, - "heading": 4.8821720585032204e-18, - "angularVelocity": 3.0712427077008165e-17, - "velocityX": -1.909561879261876, - "velocityY": 0.02121698337202498, - "timestamp": 0.19049229818392666 + "x": 14.925208085625242, + "y": 5.555894374915306, + "heading": -4.5506349702476365e-18, + "angularVelocity": -2.659914774113284e-17, + "velocityX": -1.9095619517212512, + "velocityY": 0.021216984177111247, + "timestamp": 0.19049228613670216 }, { - "x": 14.65239049535407, - "y": 5.558925628491973, - "heading": 4.9529034125739184e-18, - "angularVelocity": 7.426164168014883e-19, - "velocityX": -2.864342376779887, - "velocityY": 0.03182547014575747, - "timestamp": 0.28573844727589 + "x": 14.652390486152102, + "y": 5.558925628594215, + "heading": -6.069838807935888e-18, + "angularVelocity": -1.5950292451107106e-17, + "velocityX": -2.8643427511532806, + "velocityY": 0.03182547430538356, + "timestamp": 0.2857384292050532 }, { - "x": 14.470512079901564, - "y": 5.560946464453506, - "heading": -5.155775683106669e-18, - "angularVelocity": -1.0613215539074661e-16, - "velocityX": -1.909561879261876, - "velocityY": 0.021216983372024975, - "timestamp": 0.3809845963678533 + "x": 14.47051207530058, + "y": 5.560946464504627, + "heading": -5.0495636395818696e-18, + "angularVelocity": 1.0711984090840416e-17, + "velocityX": -1.9095619517212512, + "velocityY": 0.021216984177111247, + "timestamp": 0.3809845722734043 }, { "x": 14.379572868347168, "y": 5.561956882476807, - "heading": -2.3969600276869747e-28, - "angularVelocity": 5.413106704997615e-17, - "velocityX": -0.9547809798230291, - "velocityY": 0.010608492132583473, - "timestamp": 0.47623074545981664 + "heading": 1.854505670800523e-27, + "angularVelocity": 5.3015938280259253e-17, + "velocityX": -0.95478099189959, + "velocityY": 0.01060849226676321, + "timestamp": 0.47623071534175543 }, { "x": 14.379572868347168, "y": 5.561956882476807, - "heading": -1.1788127589783305e-28, - "angularVelocity": 4.1297903821940984e-29, - "velocityX": -1.6147929435574846e-22, - "velocityY": 1.8251378962940275e-24, - "timestamp": 0.57147689455178 + "heading": 9.080546216345213e-28, + "angularVelocity": -4.029142807111623e-28, + "velocityX": 3.747658167813594e-22, + "velocityY": -4.159522027499615e-24, + "timestamp": 0.5714768584101065 }, { - "x": 14.303949787857668, + "x": 14.303949799853617, "y": 5.561956882476807, - "heading": 4.2371272764586235e-25, - "angularVelocity": 4.879852542216884e-24, - "velocityX": -0.8707004020209776, - "velocityY": 4.615774993405339e-16, - "timestamp": 0.6583300535878512 + "heading": -6.436370661864032e-21, + "angularVelocity": -7.410637050778398e-20, + "velocityX": -0.8707003739387539, + "velocityY": 2.6828250389840444e-16, + "timestamp": 0.6583300064700506 }, { - "x": 14.152703635762082, + "x": 14.15270366641151, "y": 5.561956882476807, - "heading": 9.62666688048605e-25, - "angularVelocity": 6.205346660780305e-24, - "velocityX": -1.7414007017611408, - "velocityY": 9.23154942545405e-16, - "timestamp": 0.7451832126239225 + "heading": -2.0042157210092705e-20, + "angularVelocity": -1.5665277370046843e-19, + "velocityX": -1.741400707061555, + "velocityY": 5.368432878740444e-16, + "timestamp": 0.7451831545299946 }, { - "x": 13.925834474244276, + "x": 13.925834492835799, "y": 5.561956882476807, - "heading": -6.35334199578853e-25, - "angularVelocity": -1.8398880424876378e-23, - "velocityX": -2.6121002855357762, - "velocityY": 1.3847320055814999e-15, - "timestamp": 0.8320363716599938 + "heading": -1.7073389524704635e-20, + "angularVelocity": 3.4181463213745686e-20, + "velocityX": -2.612100754472715, + "velocityY": 8.033174756016209e-16, + "timestamp": 0.8320363025899387 }, { - "x": 13.774588363604606, + "x": 13.774588375936995, "y": 5.561956882476807, - "heading": -4.288440082411824e-25, - "angularVelocity": 2.3774632221708446e-24, - "velocityX": -1.7414002244507438, - "velocityY": 9.231546882646077e-16, - "timestamp": 0.918889530696065 + "heading": -7.883060482060434e-21, + "angularVelocity": 1.0581457607411195e-19, + "velocityX": -1.7414005165871203, + "velocityY": 5.360485261048027e-16, + "timestamp": 0.9188894506498827 }, { - "x": 13.717871079476808, + "x": 13.717871085037839, "y": 5.561956882476807, - "heading": -2.590171361320003e-25, - "angularVelocity": 3.910667216659344e-24, - "velocityX": -1.3060502198614803, - "velocityY": 6.923660434900599e-16, - "timestamp": 0.9623161028640279 + "heading": -3.134968058954825e-21, + "angularVelocity": 1.0933611158328031e-19, + "velocityX": -1.3060504080011666, + "velocityY": 4.021698310203676e-16, + "timestamp": 0.9623160217467509 }, { - "x": 13.680059555654978, + "x": 13.680059557344757, "y": 5.561956882476807, - "heading": -1.0277255516428025e-25, - "angularVelocity": 3.597902693388258e-24, - "velocityX": -0.8707001712128097, - "velocityY": 4.615773753780885e-16, - "timestamp": 1.0057426750319909 + "heading": 7.130869940497685e-22, + "angularVelocity": 8.861061225492244e-20, + "velocityX": -0.8707002818329597, + "velocityY": 2.681499176641714e-16, + "timestamp": 1.0057425928436192 }, { "x": 13.66115379333496, "y": 5.561956882476807, - "heading": 4.1528332423630384e-27, - "angularVelocity": 2.4622111087440337e-24, - "velocityX": -0.43535009502700467, - "velocityY": 2.3078869268024055e-16, - "timestamp": 1.0491692471999539 + "heading": 2.9039653211073e-21, + "angularVelocity": 5.0450179964504564e-20, + "velocityX": -0.4353501446758437, + "velocityY": 1.3407690569526743e-16, + "timestamp": 1.0491691639404874 }, { "x": 13.66115379333496, "y": 5.561956882476807, - "heading": 4.171558674345914e-27, - "angularVelocity": 4.311975603546073e-28, - "velocityX": -1.6145553776744748e-27, - "velocityY": 2.588288608432755e-27, - "timestamp": 1.0925958193679168 - }, - { - "x": 13.691513815926383, - "y": 5.525701060761086, - "heading": 0.023562463335368315, - "angularVelocity": 0.34182634637050774, - "velocityX": 0.4404401802325416, - "velocityY": -0.5259719620716655, - "timestamp": 1.1615269113845306 - }, - { - "x": 13.752233823940973, - "y": 5.453149387533433, - "heading": 0.06932129795634334, - "angularVelocity": 0.663834465438997, - "velocityX": 0.8808798212562146, - "velocityY": -1.0525246460648914, - "timestamp": 1.2304580034011443 - }, - { - "x": 13.843313276711916, - "y": 5.344233687044581, - "heading": 0.13498389635529368, - "angularVelocity": 0.9525831736877814, - "velocityX": 1.3213116186958351, - "velocityY": -1.580066372118415, - "timestamp": 1.299389095417758 - }, - { - "x": 13.964751433548663, - "y": 5.198837902534373, - "heading": 0.21647867376005756, - "angularVelocity": 1.1822644182848892, - "velocityX": 1.7617326707587837, - "velocityY": -2.109291761621367, - "timestamp": 1.3683201874343718 - }, - { - "x": 14.116535425780143, - "y": 5.016750311314966, - "heading": 0.30307237204373855, - "angularVelocity": 1.2562356949576599, - "velocityX": 2.2019670339024615, - "velocityY": -2.6415886632917585, - "timestamp": 1.4372512794509855 - }, - { - "x": 14.26793452721354, - "y": 4.8345584717676635, - "heading": 0.39204433398389704, - "angularVelocity": 1.290737740216195, - "velocityX": 2.196383330136527, - "velocityY": -2.6431010189624082, - "timestamp": 1.5061823714675993 - }, - { - "x": 14.389009217305178, - "y": 4.688953541309653, - "heading": 0.4689267567346741, - "angularVelocity": 1.1153518753517935, - "velocityX": 1.7564597709036147, - "velocityY": -2.1123258923987027, - "timestamp": 1.575113463484213 - }, - { - "x": 14.479788015237448, - "y": 4.5797997732912155, - "heading": 0.5284493552871049, - "angularVelocity": 0.8635087129924586, - "velocityX": 1.3169499463375665, - "velocityY": -1.583520075267771, - "timestamp": 1.6440445555008267 - }, - { - "x": 14.540294978830758, - "y": 4.507052135096341, - "heading": 0.568910729781111, - "angularVelocity": 0.5869829319438942, - "velocityX": 0.8777891343826024, - "velocityY": -1.0553675571734895, - "timestamp": 1.7129756475174405 + "heading": 2.9289616731652808e-21, + "angularVelocity": 5.756004088616709e-22, + "velocityX": -2.138357283045396e-22, + "velocityY": 2.150198010410122e-22, + "timestamp": 1.0925957350373556 + }, + { + "x": 13.69151381424092, + "y": 5.525701061208603, + "heading": 0.023562396214272266, + "angularVelocity": 0.3418254011415353, + "velocityX": 0.44044019251996336, + "velocityY": -0.5259719994527794, + "timestamp": 1.1615268213041603 + }, + { + "x": 13.75223381956499, + "y": 5.45314938890248, + "heading": 0.06932112146119182, + "angularVelocity": 0.6638329340960312, + "velocityX": 0.880879855701796, + "velocityY": -1.05252472049121, + "timestamp": 1.230457907570965 + }, + { + "x": 13.843313269738008, + "y": 5.344233689629052, + "heading": 0.13498360807083573, + "angularVelocity": 0.9525816313918327, + "velocityX": 1.3213116912228196, + "velocityY": -1.580066486285422, + "timestamp": 1.2993889938377696 + }, + { + "x": 13.964751426359838, + "y": 5.198837905171001, + "heading": 0.21647833458525872, + "angularVelocity": 1.182263778623618, + "velocityX": 1.7617328145938624, + "velocityY": -2.1092919368089142, + "timestamp": 1.3683200801045743 + }, + { + "x": 14.116535429050689, + "y": 5.0167503033742005, + "heading": 0.3030721451467355, + "angularVelocity": 1.2562374285863827, + "velocityX": 2.2019673693137145, + "velocityY": -2.64158903708572, + "timestamp": 1.437251166371379 + }, + { + "x": 14.267934537167369, + "y": 4.834558459237534, + "heading": 0.3920443693842881, + "angularVelocity": 1.290741653093595, + "velocityX": 2.1963836103013628, + "velocityY": -2.6431013060127135, + "timestamp": 1.5061822526381836 + }, + { + "x": 14.389009225126475, + "y": 4.688953532451243, + "heading": 0.4689268613880663, + "angularVelocity": 1.115352973057713, + "velocityX": 1.756459886479585, + "velocityY": -2.1123260153294563, + "timestamp": 1.5751133389049883 + }, + { + "x": 14.47978801964584, + "y": 4.5797997685436185, + "heading": 0.5284494394989407, + "angularVelocity": 0.8635084884704485, + "velocityX": 1.3169500066776294, + "velocityY": -1.5835201477187593, + "timestamp": 1.644044425171793 + }, + { + "x": 14.540294980406806, + "y": 4.507052133449136, + "heading": 0.5689107661779228, + "angularVelocity": 0.5869822872422525, + "velocityX": 0.8777891665128892, + "velocityY": -1.0553676002276473, + "timestamp": 1.7129755114385976 }, { "x": 14.570545196533203, "y": 4.470687389373779, "heading": 0.5894308145365931, - "angularVelocity": 0.29768982552222245, - "velocityX": 0.43884721418825445, - "velocityY": -0.5275521489460464, - "timestamp": 1.7819067395340542 + "angularVelocity": 0.2976893223363026, + "velocityX": 0.43884722793008496, + "velocityY": -0.5275521690548106, + "timestamp": 1.7819065977054023 }, { "x": 14.570545196533203, "y": 4.470687389373779, "heading": 0.5894308145365931, - "angularVelocity": 3.863669117287725e-28, - "velocityX": -1.9871518401102085e-27, - "velocityY": 2.8088430719951805e-27, - "timestamp": 1.850837831550668 - }, - { - "x": 14.45709086591278, - "y": 4.4278439544867165, - "heading": 0.488089493217441, - "angularVelocity": -0.9146778399911556, - "velocityX": -1.0240064044825004, - "velocityY": -0.38669261432744145, - "timestamp": 1.961632383346229 - }, - { - "x": 14.230046021105487, - "y": 4.34248670484011, - "heading": 0.2854587836923866, - "angularVelocity": -1.8288869465255828, - "velocityX": -2.0492419629643726, - "velocityY": -0.7704101714686167, - "timestamp": 2.0724269351417903 - }, - { - "x": 14.002349455992734, - "y": 4.258200754530428, - "heading": 0.09515293619351321, - "angularVelocity": -1.7176462598091191, - "velocityX": -2.055124204418461, - "velocityY": -0.760740929438543, - "timestamp": 2.1832214869373514 - }, - { - "x": 13.888501167358198, - "y": 4.216057777427628, - "heading": 8.28079525180121e-11, - "angularVelocity": -0.8588232414738415, - "velocityX": -1.027562157068967, - "velocityY": -0.3803704823009896, - "timestamp": 2.2940160387329125 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 8.268158682804007e-19, - "angularVelocity": -7.474009357788098e-10, - "velocityX": -5.490850602427426e-10, - "velocityY": -2.0617274991432905e-10, - "timestamp": 2.4048105905284736 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 5.905827430465516e-19, - "angularVelocity": -6.406293808708934e-10, - "velocityX": -4.706443421676888e-10, - "velocityY": -1.7671950173476352e-10, - "timestamp": 2.4048105908972253 + "angularVelocity": 4.237174098560341e-22, + "velocityX": -2.826594351845562e-22, + "velocityY": 3.573387212832748e-22, + "timestamp": 1.850837683972207 + }, + { + "x": 14.53016602593679, + "y": 4.455446506235451, + "heading": 0.5540064540349209, + "angularVelocity": -0.5360505707818769, + "velocityX": -0.6110280366214283, + "velocityY": -0.2306289793187704, + "timestamp": 1.9169216738651802 + }, + { + "x": 14.44938649587058, + "y": 4.425019141773473, + "heading": 0.4829804666462581, + "angularVelocity": -1.0747835822820835, + "velocityX": -1.2223767087464938, + "velocityY": -0.4604347363295905, + "timestamp": 1.9830056637581535 + }, + { + "x": 14.328164393235157, + "y": 4.379500232629268, + "heading": 0.37640102935392167, + "angularVelocity": -1.6127875672299412, + "velocityX": -1.8343641603927023, + "velocityY": -0.6888038875667982, + "timestamp": 2.049089653651127 + }, + { + "x": 14.16632561176535, + "y": 4.319179292971803, + "heading": 0.23626694546085536, + "angularVelocity": -2.120545144444536, + "velocityX": -2.448986233003096, + "velocityY": -0.9127920356376347, + "timestamp": 2.1151736435441 + }, + { + "x": 14.0448416017969, + "y": 4.274051603899093, + "heading": 0.1325451584186999, + "angularVelocity": -1.569544865710116, + "velocityX": -1.838327409788673, + "velocityY": -0.6828838444197568, + "timestamp": 2.1812576334370735 + }, + { + "x": 14.000178586698231, + "y": 4.2574722138771985, + "heading": 0.0945578464623197, + "angularVelocity": -1.340385412793797, + "velocityX": -1.5759381447780825, + "velocityY": -0.585005134448155, + "timestamp": 2.2095982223406803 + }, + { + "x": 13.96295541639372, + "y": 4.243662024781544, + "heading": 0.06298966033692675, + "angularVelocity": -1.1138860322473925, + "velocityX": -1.31342261203944, + "velocityY": -0.4872936530227246, + "timestamp": 2.237938811244287 + }, + { + "x": 13.933174653940528, + "y": 4.2326176532188775, + "heading": 0.03778230634920248, + "angularVelocity": -0.8894435494428381, + "velocityX": -1.0508166416190299, + "velocityY": -0.3897015549053042, + "timestamp": 2.266279400147894 + }, + { + "x": 13.910838071206133, + "y": 4.2243366192852125, + "heading": 0.018894460026481222, + "angularVelocity": -0.6664592040399531, + "velocityX": -0.788148150709412, + "velocityY": -0.2921969604029915, + "timestamp": 2.2946199890515007 + }, + { + "x": 13.895946755796466, + "y": 4.218817098366333, + "heading": 0.006302298120624352, + "angularVelocity": -0.4443154638982944, + "velocityX": -0.5254412835356537, + "velocityY": -0.19475674756274655, + "timestamp": 2.3229605779551075 }, { "x": 13.888501167297363, "y": 4.216057777404785, - "heading": 3.9372180312859395e-19, - "angularVelocity": -5.33857823518088e-10, - "velocityX": -3.922036224873799e-10, - "velocityY": -1.472662529525917e-10, - "timestamp": 2.404810591265977 + "heading": -7.359942498471454e-23, + "angularVelocity": -0.22237710522036122, + "velocityX": -0.26271819983798683, + "velocityY": -0.0973628660622863, + "timestamp": 2.3513011668587143 }, { "x": 13.888501167297363, "y": 4.216057777404785, - "heading": 2.3623304942893484e-19, - "angularVelocity": -4.270862637180886e-10, - "velocityX": -3.137629012016172e-10, - "velocityY": -1.1781300356773835e-10, - "timestamp": 2.4048105916347287 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 1.1811648285083364e-19, - "angularVelocity": -3.203147014685827e-10, - "velocityX": -2.353221783101984e-10, - "velocityY": -8.835975358012759e-11, - "timestamp": 2.4048105920034804 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 3.937210429839709e-20, - "angularVelocity": -2.1354313676727285e-10, - "velocityX": -1.568814538129234e-10, - "velocityY": -5.890650298968465e-11, - "timestamp": 2.404810592372232 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": -8.532341534406571e-26, - "angularVelocity": -1.0677156961184637e-10, - "velocityX": -7.84407277095909e-11, - "velocityY": -2.945325179633351e-11, - "timestamp": 2.4048105927409837 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": -8.510879482431963e-26, - "angularVelocity": -1.6866441387712243e-26, - "velocityX": -2.0583941335553829e-25, - "velocityY": 9.841185153624076e-26, - "timestamp": 2.4048105931097354 - }, - { - "x": 13.899986336038381, - "y": 4.272873358599484, - "heading": -0.023038991913601637, - "angularVelocity": -0.3021063684546837, - "velocityX": 0.15060305730606005, - "velocityY": 0.7450130183968425, - "timestamp": 2.481071785587899 - }, - { - "x": 13.922959662828976, - "y": 4.386578192081037, - "heading": -0.0663634759005527, - "angularVelocity": -0.5681065634969774, - "velocityX": 0.3012453129050349, - "velocityY": 1.4909920732502444, - "timestamp": 2.557332978066063 - }, - { - "x": 13.957441252375148, - "y": 4.557273568298352, - "heading": -0.12543465168712403, - "angularVelocity": -0.7745902452743002, - "velocityX": 0.45215119808209714, - "velocityY": 2.238299332470985, - "timestamp": 2.6335941705442267 - }, - { - "x": 14.00350258986825, - "y": 4.785079242541037, - "heading": -0.19129776367156157, - "angularVelocity": -0.8636517453263874, - "velocityX": 0.6039944563716563, - "velocityY": 2.987176922363417, - "timestamp": 2.7098553630223905 - }, - { - "x": 14.06127377629267, - "y": 5.069635038422791, - "heading": -0.23370464392529416, - "angularVelocity": -0.556074182368382, - "velocityX": 0.757543706662608, - "velocityY": 3.731331580780533, - "timestamp": 2.7861165555005543 - }, - { - "x": 14.121438982529087, - "y": 5.371150739294933, - "heading": -0.23370465578542146, - "angularVelocity": -1.5551982461065506e-7, - "velocityX": 0.7889360798238878, - "velocityY": 3.953723920045926, - "timestamp": 2.862377747978718 - }, - { - "x": 14.181604184628418, - "y": 5.672666440992599, - "heading": -0.23370466764554898, - "angularVelocity": -1.5551982762801367e-7, - "velocityX": 0.7889360255749569, - "velocityY": 3.9537239308708902, - "timestamp": 2.938638940456882 - }, - { - "x": 14.238820898893755, - "y": 5.957011236905596, - "heading": -0.27574816661054075, - "angularVelocity": -0.5513092255544053, - "velocityX": 0.7502730078830246, - "velocityY": 3.7285647742056445, - "timestamp": 3.0149001329350456 - }, - { - "x": 14.284420707366433, - "y": 6.1847837505193715, - "heading": -0.34721144483836985, - "angularVelocity": -0.9370857693877743, - "velocityX": 0.5979425051048811, - "velocityY": 2.98674209269669, - "timestamp": 3.0911613254132093 - }, - { - "x": 14.318439308265718, - "y": 6.35544203945297, - "heading": -0.41225183766095985, - "angularVelocity": -0.8528635693864027, - "velocityX": 0.44608010698268263, - "velocityY": 2.2378130132500083, - "timestamp": 3.167422517891373 - }, - { - "x": 14.341028135354547, - "y": 6.469114800114249, - "heading": -0.4602003337578672, - "angularVelocity": -0.6287404450256496, - "velocityX": 0.2962034339457177, - "velocityY": 1.490571507832474, - "timestamp": 3.243683710369537 - }, - { - "x": 14.352291107177734, - "y": 6.525911808013916, - "heading": -0.48570563852984416, - "angularVelocity": -0.33444670799345433, - "velocityX": 0.14768942704917307, - "velocityY": 0.7447694699493517, - "timestamp": 3.3199449028477006 - }, - { - "x": 14.352291107177734, - "y": 6.525911808013916, - "heading": -0.48570563852984416, - "angularVelocity": 1.0539627829247847e-26, - "velocityX": -5.1337066004257686e-26, - "velocityY": -2.752712125666017e-26, - "timestamp": 3.3962060953258644 - }, - { - "x": 14.281244102466694, - "y": 6.578011637665101, - "heading": -0.49633092681747565, - "angularVelocity": -0.11332344447203622, - "velocityX": -0.7577480323661817, - "velocityY": 0.555667949202982, - "timestamp": 3.489966820100281 - }, - { - "x": 14.139150572999801, - "y": 6.682211842382296, - "heading": -0.5176048283391019, - "angularVelocity": -0.22689565991314806, - "velocityX": -1.515490945795936, - "velocityY": 1.1113417154986245, - "timestamp": 3.5837275448746975 - }, - { - "x": 13.926011946794734, - "y": 6.838513890829568, - "heading": -0.5495863998363797, - "angularVelocity": -0.3410977418767151, - "velocityX": -2.273218628779447, - "velocityY": 1.6670311457524134, - "timestamp": 3.677488269649114 - }, - { - "x": 13.783920159167808, - "y": 6.942715781556149, - "heading": -0.5709265094735697, - "angularVelocity": -0.22760179903187838, - "velocityX": -1.5154723682948443, - "velocityY": 1.111359697541642, - "timestamp": 3.7712489944235306 - }, - { - "x": 13.738450910492517, - "y": 6.97606035494089, - "heading": -0.577756226947398, - "angularVelocity": -0.1821053451622054, - "velocityX": -1.2123771233598004, - "velocityY": 0.8890887608138573, - "timestamp": 3.808753206437433 - }, - { - "x": 13.704348988985252, - "y": 7.001068805104032, - "heading": -0.582878916551458, - "angularVelocity": -0.13658971430091627, - "velocityX": -0.9092824425859869, - "velocityY": 0.6668171072057679, - "timestamp": 3.846257418451336 - }, - { - "x": 13.681614380767433, - "y": 7.017741113917072, - "heading": -0.5862941684873881, - "angularVelocity": -0.09106315671061506, - "velocityX": -0.6061881318661277, - "velocityY": 0.44454497022520045, - "timestamp": 3.8837616304652385 - }, - { - "x": 13.670247077941896, - "y": 7.0260772705078125, + "heading": 2.117727432484989e-22, + "angularVelocity": 5.073032730422753e-22, + "velocityX": 1.319201559585301e-22, + "velocityY": -1.2833790442104632e-22, + "timestamp": 2.379641755762321 + }, + { + "x": 13.90055456208268, + "y": 4.2672774930951745, + "heading": -0.024639723698078066, + "angularVelocity": -0.3387505660309093, + "velocityX": 0.16571185440844172, + "velocityY": 0.7041762275692574, + "timestamp": 2.4523788259516883 + }, + { + "x": 13.924664422654004, + "y": 4.369780859826108, + "heading": -0.07184553791598466, + "angularVelocity": -0.648992516402005, + "velocityX": 0.33146592938861846, + "velocityY": 1.4092314477895682, + "timestamp": 2.5251158961410556 + }, + { + "x": 13.960850027131373, + "y": 4.523662093222596, + "heading": -0.1381918964160448, + "angularVelocity": -0.9121395504016173, + "velocityX": 0.4974850428146382, + "velocityY": 2.1155819583586095, + "timestamp": 2.597852966330423 + }, + { + "x": 14.00918910605255, + "y": 4.729054591196495, + "heading": -0.21706345100390917, + "angularVelocity": -1.0843377989040044, + "velocityX": 0.6645728071714858, + "velocityY": 2.8237664431516296, + "timestamp": 2.67059003651979 + }, + { + "x": 14.069933310565814, + "y": 4.985967503680229, + "heading": -0.2879560349605607, + "angularVelocity": -0.9746417304420771, + "velocityX": 0.8351203087382914, + "velocityY": 3.532076722569039, + "timestamp": 2.7433271067091574 + }, + { + "x": 14.136089967453417, + "y": 5.271659493582205, + "heading": -0.2879560398332574, + "angularVelocity": -6.699055461306423e-8, + "velocityX": 0.909531504573521, + "velocityY": 3.9277357358248457, + "timestamp": 2.8160641768985246 + }, + { + "x": 14.202246205709509, + "y": 5.557351580424848, + "heading": -0.2879560447059434, + "angularVelocity": -6.699040765465796e-8, + "velocityX": 0.9095257491655405, + "velocityY": 3.9277370685794617, + "timestamp": 2.888801247087892 + }, + { + "x": 14.268402835983203, + "y": 5.843043576489709, + "heading": -0.2879560495786414, + "angularVelocity": -6.699057305241701e-8, + "velocityX": 0.9095311386815427, + "velocityY": 3.9277358205530803, + "timestamp": 2.961538317277259 + }, + { + "x": 14.328385114782021, + "y": 6.0998732839416245, + "heading": -0.35678639279969876, + "angularVelocity": -0.9462897397690232, + "velocityX": 0.8246452413144653, + "velocityY": 3.5309328074841777, + "timestamp": 3.0342753874666264 + }, + { + "x": 14.376005525808099, + "y": 6.305305162261247, + "heading": -0.44145888026224256, + "angularVelocity": -1.1640898821206782, + "velocityX": 0.654692454646584, + "velocityY": 2.8243078499696304, + "timestamp": 3.1070124576559937 + }, + { + "x": 14.41149328213472, + "y": 6.459199589883294, + "heading": -0.5144824922677588, + "angularVelocity": -1.0039394192727746, + "velocityX": 0.48789092321468885, + "velocityY": 2.1157633545232315, + "timestamp": 3.179749527845361 + }, + { + "x": 14.435047453373745, + "y": 6.561702808468181, + "heading": -0.5669658281865239, + "angularVelocity": -0.7215486653796684, + "velocityX": 0.3238262302523786, + "velocityY": 1.4092294110558068, + "timestamp": 3.252486598034728 + }, + { + "x": 14.446789741516112, + "y": 6.612919330596924, + "heading": -0.5944409566225068, + "angularVelocity": -0.37773212977169646, + "velocityX": 0.16143471426325046, + "velocityY": 0.7041323220113681, + "timestamp": 3.3252236682240954 + }, + { + "x": 14.446789741516112, + "y": 6.612919330596924, + "heading": -0.5944409566225068, + "angularVelocity": -2.2907035566312707e-22, + "velocityX": -2.4801938086703075e-22, + "velocityY": 8.637122930206518e-23, + "timestamp": 3.3979607384134627 + }, + { + "x": 14.374878621007513, + "y": 6.6681554053884105, + "heading": -0.5937701281033373, + "angularVelocity": 0.0070535118408139325, + "velocityX": -0.7561186286794916, + "velocityY": 0.5807867382623025, + "timestamp": 3.4930663445723633 + }, + { + "x": 14.231056382755842, + "y": 6.778627552624006, + "heading": -0.5924285241888535, + "angularVelocity": 0.014106465104088719, + "velocityX": -1.5122372282804897, + "velocityY": 1.1615734518427945, + "timestamp": 3.588171950731264 + }, + { + "x": 14.015323043710728, + "y": 6.944335758664245, + "heading": -0.5904163161957059, + "angularVelocity": 0.021157617036641953, + "velocityX": -2.268355649662441, + "velocityY": 1.7423600220093918, + "timestamp": 3.6832775568901646 + }, + { + "x": 13.871500813115434, + "y": 7.0548078992888055, + "heading": -0.5890749095880986, + "angularVelocity": 0.014104390495824683, + "velocityX": -1.512237147776525, + "velocityY": 1.161573382330228, + "timestamp": 3.778383163049065 + }, + { + "x": 13.82547769782775, + "y": 7.0901589854242735, + "heading": -0.5886456630608746, + "angularVelocity": 0.011283417909435272, + "velocityX": -1.209789737015094, + "velocityY": 0.9292587199213779, + "timestamp": 3.8164254061592073 + }, + { + "x": 13.790960360957698, + "y": 7.116672300328794, + "heading": -0.5883237297397991, + "angularVelocity": 0.008462522048014776, + "velocityX": -0.9073423133887173, + "velocityY": 0.696944047903743, + "timestamp": 3.8544676492693495 + }, + { + "x": 13.767948802831496, + "y": 7.134347843758721, + "heading": -0.588109107903455, + "angularVelocity": 0.005641671436740162, + "velocityX": -0.6048948811871482, + "velocityY": 0.46462936948148964, + "timestamp": 3.8925098923794916 + }, + { + "x": 13.75644302368164, + "y": 7.143185615539551, "heading": -0.5880017968591525, - "angularVelocity": -0.04553164244942512, - "velocityX": -0.3030940317136526, - "velocityY": 0.2222725433519379, - "timestamp": 3.921265842479141 + "angularVelocity": 0.0028208390339083305, + "velocityX": -0.3024474428740401, + "velocityY": 0.2323146864721631, + "timestamp": 3.930552135489634 }, { - "x": 13.670247077941896, - "y": 7.0260772705078125, + "x": 13.75644302368164, + "y": 7.143185615539551, "heading": -0.5880017968591525, - "angularVelocity": 4.3220894143993686e-27, - "velocityX": -2.1854050012083175e-26, - "velocityY": 3.319166050857422e-26, - "timestamp": 3.9587700544930438 - }, - { - "x": 13.645024237315694, - "y": 7.029213812474465, - "heading": -0.5586050184455363, - "angularVelocity": 0.5751667660211687, - "velocityX": -0.49350100438627437, - "velocityY": 0.061368449088751095, - "timestamp": 4.009880063190129 - }, - { - "x": 13.59445850581916, - "y": 7.035391208480669, - "heading": -0.5016300369058596, - "angularVelocity": 1.1147519437406412, - "velocityX": -0.9893508685592758, - "velocityY": 0.1208647027007058, - "timestamp": 4.060990071887214 - }, - { - "x": 13.518403853674121, - "y": 7.044499236825847, - "heading": -0.41964577392325547, - "angularVelocity": 1.6040745261559681, - "velocityX": -1.4880578987140245, - "velocityY": 0.17820439826490078, - "timestamp": 4.112100080584299 - }, - { - "x": 13.416671986943191, - "y": 7.056423820145038, - "heading": -0.3169282187933418, - "angularVelocity": 2.0097346439264405, - "velocityX": -1.9904490201491905, - "velocityY": 0.2333120972423329, - "timestamp": 4.163210089281384 - }, - { - "x": 13.289036124529042, - "y": 7.071060906808593, - "heading": -0.20104341684391944, - "angularVelocity": 2.2673602471140164, - "velocityX": -2.497277258757889, - "velocityY": 0.28638395955486035, - "timestamp": 4.21432009797847 - }, - { - "x": 13.135342270967683, - "y": 7.08828685694279, - "heading": -0.08640408959186506, - "angularVelocity": 2.2429917383010047, - "velocityX": -3.007118517084588, - "velocityY": 0.3370367286824492, - "timestamp": 4.265430106675555 - }, - { - "x": 12.956762188284944, - "y": 7.1075748581915015, - "heading": -0.010726520537828105, - "angularVelocity": 1.48068002693087, - "velocityX": -3.4940335021489295, - "velocityY": 0.3773820772175376, - "timestamp": 4.31654011537264 - }, - { - "x": 12.755389428383719, - "y": 7.12888688951605, - "heading": -1.8456979469985217e-7, - "angularVelocity": 0.20986762165519152, - "velocityX": -3.9399868056119236, - "velocityY": 0.41698351982012405, - "timestamp": 4.367650124069725 - }, - { - "x": 12.55069190372683, - "y": 7.152531606779918, - "heading": -1.385534968264371e-7, - "angularVelocity": 9.003382908060325e-7, - "velocityX": -4.00503795391767, - "velocityY": 0.4626240117469566, - "timestamp": 4.418760132766811 - }, - { - "x": 12.345994384979965, - "y": 7.176176375208352, - "heading": -9.253727372935047e-8, - "angularVelocity": 9.003368277593551e-7, - "velocityX": -4.005037838284301, - "velocityY": 0.46262501281442936, - "timestamp": 4.469870141463896 - }, - { - "x": 12.141296845030876, - "y": 7.1998209600841285, - "heading": -4.652103543469606e-8, - "angularVelocity": 9.003371251094857e-7, - "velocityX": -4.005038253119366, - "velocityY": 0.46262142148932034, - "timestamp": 4.520980150160981 - }, - { - "x": 11.93593692779541, - "y": 7.216775417327881, - "heading": -7.736119630596452e-26, - "angularVelocity": 9.102138039226991e-7, - "velocityX": -4.017998088252668, - "velocityY": 0.33172479668779814, - "timestamp": 4.5720901588580665 - }, - { - "x": 11.603934607227266, - "y": 7.244185435917073, - "heading": 2.6079666498891626e-17, - "angularVelocity": 3.1562448816546114e-16, - "velocityX": -4.017998550432804, - "velocityY": 0.33172483484526066, - "timestamp": 4.654718939431514 - }, - { - "x": 11.271932286659125, - "y": 7.271595454506265, - "heading": 1.288603670926865e-16, - "angularVelocity": 1.2438849984287554e-15, - "velocityX": -4.017998550432804, - "velocityY": 0.33172483484526066, - "timestamp": 4.737347720004961 - }, - { - "x": 10.93992996609098, - "y": 7.299005473095457, - "heading": 2.271885871201871e-16, - "angularVelocity": 1.1899996507881914e-15, - "velocityX": -4.017998550432804, - "velocityY": 0.3317248348452606, - "timestamp": 4.8199765005784085 - }, - { - "x": 10.607927645522839, - "y": 7.326415491684649, - "heading": 2.5614842689098653e-16, - "angularVelocity": 3.504812677828607e-16, - "velocityX": -4.017998550432804, - "velocityY": 0.3317248348452606, - "timestamp": 4.902605281151856 - }, - { - "x": 10.275925324954695, - "y": 7.3538255102738415, - "heading": 2.977295806911055e-16, - "angularVelocity": 5.032284575841044e-16, - "velocityX": -4.017998550432804, - "velocityY": 0.3317248348452606, - "timestamp": 4.985234061725303 - }, - { - "x": 9.943923004386694, - "y": 7.381235528863022, - "heading": 2.9677600290999526e-16, - "angularVelocity": -1.1540504102827907e-17, - "velocityX": -4.017998550431105, - "velocityY": 0.3317248348451205, - "timestamp": 5.067862842298751 - }, - { - "x": 9.671068786342074, - "y": 7.4037622966758025, - "heading": 1.766717126358e-16, - "angularVelocity": -1.453540636092317e-15, - "velocityX": -3.302169246006048, - "velocityY": 0.2726261679821815, - "timestamp": 5.150491622872198 - }, - { - "x": 9.466428105073092, - "y": 7.420657373999625, - "heading": 8.769803938528749e-17, - "angularVelocity": -1.0767879258667162e-15, - "velocityX": -2.476627149145434, - "velocityY": 0.20446964370732854, - "timestamp": 5.233120403445645 - }, - { - "x": 9.330000979086366, - "y": 7.4319207593065935, - "heading": 3.000420973683897e-17, - "angularVelocity": -6.982292277353821e-16, - "velocityX": -1.6510848283118424, - "velocityY": 0.13631310094132928, - "timestamp": 5.315749184019093 + "angularVelocity": -2.989139878822297e-21, + "velocityX": -3.903367328466188e-21, + "velocityY": -2.1704144058469085e-20, + "timestamp": 3.968594378599776 + }, + { + "x": 13.7295528779877, + "y": 7.145512508632667, + "heading": -0.5593356739909306, + "angularVelocity": 0.5453918054970608, + "velocityX": -0.5116026739128892, + "velocityY": 0.04427066858979402, + "timestamp": 4.021154982883326 + }, + { + "x": 13.675696703739712, + "y": 7.150081094861737, + "heading": -0.5031004725393259, + "angularVelocity": 1.0699116233183206, + "velocityX": -1.024649069052714, + "velocityY": 0.08692035206491855, + "timestamp": 4.073715587166876 + }, + { + "x": 13.594778136456949, + "y": 7.156781935585546, + "heading": -0.4208691999200477, + "angularVelocity": 1.5645039424520941, + "velocityX": -1.5395288616970724, + "velocityY": 0.12748789354969425, + "timestamp": 4.126276191450426 + }, + { + "x": 13.486657017941033, + "y": 7.165480150396445, + "heading": -0.31559001044646323, + "angularVelocity": 2.003005690452726, + "velocityX": -2.0570752560726255, + "velocityY": 0.16548924673647072, + "timestamp": 4.178836795733976 + }, + { + "x": 13.351125165106843, + "y": 7.1760168934848485, + "heading": -0.19335854854743198, + "angularVelocity": 2.3255338016973006, + "velocityX": -2.5785824703048017, + "velocityY": 0.20046845412127823, + "timestamp": 4.231397400017526 + }, + { + "x": 13.187951040267704, + "y": 7.1881615298982275, + "heading": -0.06830550460961216, + "angularVelocity": 2.3792162522179776, + "velocityX": -3.1044948410193456, + "velocityY": 0.23105968013346895, + "timestamp": 4.283958004301076 + }, + { + "x": 12.999485780940217, + "y": 7.200902665906217, + "heading": -7.219474003499003e-8, + "angularVelocity": 1.299555690919823, + "velocityX": -3.58567527707191, + "velocityY": 0.24240847649419411, + "timestamp": 4.336518608584626 + }, + { + "x": 12.78823863885508, + "y": 7.217612049538315, + "heading": -5.77661422338363e-8, + "angularVelocity": 2.745135448469375e-7, + "velocityX": -4.019115551745117, + "velocityY": 0.31790699250631954, + "timestamp": 4.389079212868176 + }, + { + "x": 12.576998356930917, + "y": 7.234407936932018, + "heading": -4.3339952330086093e-8, + "angularVelocity": 2.7446773301816857e-7, + "velocityX": -4.018985032679267, + "velocityY": 0.3195527833564148, + "timestamp": 4.441639817151726 + }, + { + "x": 12.365757195669019, + "y": 7.251192761311872, + "heading": -2.8913607283949816e-8, + "angularVelocity": 2.7447068470390713e-7, + "velocityX": -4.019001762656892, + "velocityY": 0.31934230225557386, + "timestamp": 4.494200421435276 + }, + { + "x": 12.154511938095094, + "y": 7.267925952676431, + "heading": -1.4486541183922857e-8, + "angularVelocity": 2.7448440322751447e-7, + "velocityX": -4.019079697682207, + "velocityY": 0.31835995024503166, + "timestamp": 4.546761025718826 + }, + { + "x": 11.942968368530272, + "y": 7.280330657958984, + "heading": 2.0785063958808085e-20, + "angularVelocity": 2.756159557415578e-7, + "velocityX": -4.024755279136523, + "velocityY": 0.23600766109220464, + "timestamp": 4.599321630002376 + }, + { + "x": 11.610157071294928, + "y": 7.29984638261751, + "heading": 5.720799166605624e-18, + "angularVelocity": 6.893144194831854e-17, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 4.682012691106358 + }, + { + "x": 11.277345774059585, + "y": 7.319362107276035, + "heading": 4.27914940906101e-18, + "angularVelocity": -1.7434167161617802e-17, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 4.76470375221034 + }, + { + "x": 10.944534476824241, + "y": 7.3388778319345604, + "heading": -2.8918690030091486e-17, + "angularVelocity": -4.01468296820159e-16, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 4.847394813314322 + }, + { + "x": 10.611723179588898, + "y": 7.358393556593086, + "heading": -7.60328295215413e-17, + "angularVelocity": -5.697609740032433e-16, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 4.930085874418304 + }, + { + "x": 10.278911882353555, + "y": 7.377909281251611, + "heading": -1.4437311459656085e-16, + "angularVelocity": -8.264531158162608e-16, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 5.012776935522286 + }, + { + "x": 9.946100585118234, + "y": 7.3974250059101365, + "heading": -1.6062472011936515e-16, + "angularVelocity": -1.9653400658253493e-16, + "velocityX": -4.024755430539445, + "velocityY": 0.236007669970322, + "timestamp": 5.095467996626268 + }, + { + "x": 9.672375324044506, + "y": 7.413475983961583, + "heading": 2.0694001348599773e-12, + "angularVelocity": 2.5027623686889767e-11, + "velocityX": -3.310215849443808, + "velocityY": 0.19410777703363524, + "timestamp": 5.17815905773025 + }, + { + "x": 9.467081371147623, + "y": 7.425514217916012, + "heading": 1.4773324091984537e-12, + "angularVelocity": -7.159996712547157e-12, + "velocityX": -2.482661972842874, + "velocityY": 0.14558083780410894, + "timestamp": 5.260850118834232 + }, + { + "x": 9.3302187338275, + "y": 7.433539707339499, + "heading": 5.797823580481929e-13, + "angularVelocity": -1.0854257269980322e-11, + "velocityX": -1.655108006753246, + "velocityY": 0.09705389332705425, + "timestamp": 5.343541179938214 }, { "x": 9.26178741455078, "y": 7.437552452087402, - "heading": -5.153249325171924e-25, - "angularVelocity": -3.631205742599535e-16, - "velocityX": -0.8255424328204009, - "velocityY": 0.06815655201159886, - "timestamp": 5.39837796459254 + "heading": -4.962355351536534e-26, + "angularVelocity": -7.011427236605464e-12, + "velocityX": -0.8275540108340242, + "velocityY": 0.04852694710082174, + "timestamp": 5.426232241042196 }, { "x": 9.26178741455078, "y": 7.437552452087402, - "heading": -5.204793542360701e-25, - "angularVelocity": -6.317967395111268e-26, - "velocityX": -1.2129880403241932e-23, - "velocityY": 1.4316814179344083e-22, - "timestamp": 5.481006745165987 - }, - { - "x": 9.329755853999478, - "y": 7.419457395378352, - "heading": -0.013376630543289742, - "angularVelocity": -0.15963467895053327, - "velocityX": 0.8111250419190595, - "velocityY": -0.21594366077412155, - "timestamp": 5.564802012154527 - }, - { - "x": 9.46569267412481, - "y": 7.383267297606982, - "heading": -0.04017390350981554, - "angularVelocity": -0.31979458899738056, - "velocityX": 1.6222493824612316, - "velocityY": -0.4318871348225413, - "timestamp": 5.648597279143067 - }, - { - "x": 9.669597656216824, - "y": 7.32898221699986, - "heading": -0.08046638000926655, - "angularVelocity": -0.4808443000123345, - "velocityX": 2.4333711129520146, - "velocityY": -0.6478299140039275, - "timestamp": 5.732392546131607 - }, - { - "x": 9.94147049419242, - "y": 7.256602235044674, - "heading": -0.13433954177465304, - "angularVelocity": -0.6429141370568642, - "velocityX": 3.2444891906935323, - "velocityY": -0.8637717207236001, - "timestamp": 5.816187813120147 - }, - { - "x": 10.267933929791393, - "y": 7.169688743599082, - "heading": -0.1343395455343343, - "angularVelocity": -4.4867465828483885e-8, - "velocityX": 3.8959650984061396, - "velocityY": -1.0372124174684092, - "timestamp": 5.899983080108687 - }, - { - "x": 10.594397365390467, - "y": 7.082775252153461, - "heading": -0.13433954929395236, - "angularVelocity": -4.486671145978699e-8, - "velocityX": 3.8959650984073204, - "velocityY": -1.0372124174687236, - "timestamp": 5.9837783470972274 - }, - { - "x": 10.92086080098954, - "y": 6.99586176070784, - "heading": -0.1343395530535705, - "angularVelocity": -4.486671228851664e-8, - "velocityX": 3.8959650984073204, - "velocityY": -1.0372124174687236, - "timestamp": 6.0675736140857675 - }, - { - "x": 11.247324236588515, - "y": 6.908948269262247, - "heading": -0.13433955681325235, - "angularVelocity": -4.486747226873845e-8, - "velocityX": 3.895965098406137, - "velocityY": -1.0372124174684085, - "timestamp": 6.1513688810743075 - }, - { - "x": 11.519194528616957, - "y": 6.836568965107796, - "heading": -0.18900176869601656, - "angularVelocity": -0.6523305414163806, - "velocityX": 3.244458807746568, - "velocityY": -0.8637636319525063, - "timestamp": 6.2351641480628475 - }, - { - "x": 11.723097253083957, - "y": 6.7822844855421724, - "heading": -0.2300018718976995, - "angularVelocity": -0.4892890096918049, - "velocityX": 2.4333441707977026, - "velocityY": -0.6478227412659024, - "timestamp": 6.3189594150513875 - }, - { - "x": 11.859032415291114, - "y": 6.746094829153945, - "heading": -0.2573341284430376, - "angularVelocity": -0.32617900184119325, - "velocityX": 1.6222295971173368, - "velocityY": -0.4318818674231029, - "timestamp": 6.4027546820399275 - }, - { - "x": 11.927, - "y": 6.728, - "heading": -0.271, - "angularVelocity": -0.16308643731431074, - "velocityX": 0.8111148415838417, - "velocityY": -0.21594094516602882, - "timestamp": 6.4865499490284675 - }, - { - "x": 11.927, - "y": 6.728, - "heading": -0.271, - "angularVelocity": -1.104828910146849e-25, - "velocityX": -2.8779219726051263e-23, - "velocityY": -1.0811880313092697e-22, - "timestamp": 6.5703452160170075 + "heading": -2.4523596562464635e-26, + "angularVelocity": 6.97031270768781e-27, + "velocityX": 1.5654039367177217e-19, + "velocityY": 1.335931106132843e-18, + "timestamp": 5.508923302146178 + }, + { + "x": 9.361038211199778, + "y": 7.437553617689382, + "heading": -1.6076494634632555e-20, + "angularVelocity": -1.6157169576201665e-19, + "velocityX": 0.9974900839285512, + "velocityY": 0.000011714529816125984, + "timestamp": 5.608423836786188 + }, + { + "x": 9.559539801305808, + "y": 7.437555948893304, + "heading": -2.775286477301083e-20, + "angularVelocity": -1.173498230999548e-19, + "velocityX": 1.9949801357771932, + "velocityY": 0.000023429059255504534, + "timestamp": 5.7079243714261985 + }, + { + "x": 9.857292168909034, + "y": 7.437559445698981, + "heading": -1.615205586446462e-20, + "angularVelocity": 1.1659041783583904e-19, + "velocityX": 2.9924700272263003, + "velocityY": 0.000035143586811149936, + "timestamp": 5.807424906066209 + }, + { + "x": 10.055793759015064, + "y": 7.437561776902903, + "heading": -5.186936203377308e-21, + "angularVelocity": 1.1020161550669728e-19, + "velocityX": 1.9949801357771932, + "velocityY": 0.000023429059255504534, + "timestamp": 5.906925440706219 + }, + { + "x": 10.155044555664062, + "y": 7.437562942504883, + "heading": 4.39993089223087e-32, + "angularVelocity": 5.2129731987967565e-20, + "velocityX": 0.9974900839285512, + "velocityY": 0.000011714529816125986, + "timestamp": 6.00642597534623 + }, + { + "x": 10.155044555664062, + "y": 7.437562942504883, + "heading": 2.1939125534073598e-32, + "angularVelocity": -1.2166553134589197e-33, + "velocityX": 1.4750876026512068e-22, + "velocityY": 2.5957646699888015e-19, + "timestamp": 6.10592650998624 } ], "trajectoryWaypoints": [ @@ -1012,7 +958,7 @@ "controlIntervalCount": 6 }, { - "timestamp": 0.57147689455178, + "timestamp": 0.5714768584101065, "isStopPoint": true, "x": 14.379572868347168, "y": 5.561956882476807, @@ -1023,7 +969,7 @@ "controlIntervalCount": 4 }, { - "timestamp": 0.918889530696065, + "timestamp": 0.9188894506498827, "isStopPoint": false, "x": 14.040164947509766, "y": 5.558465480804443, @@ -1034,7 +980,7 @@ "controlIntervalCount": 4 }, { - "timestamp": 1.0925958193679168, + "timestamp": 1.0925957350373556, "isStopPoint": true, "x": 13.661153793334961, "y": 5.561956882476807, @@ -1045,7 +991,7 @@ "controlIntervalCount": 11 }, { - "timestamp": 1.850837831550668, + "timestamp": 1.850837683972207, "isStopPoint": true, "x": 14.570545196533203, "y": 4.470687389373779, @@ -1056,7 +1002,7 @@ "controlIntervalCount": 5 }, { - "timestamp": 2.4048105905284736, + "timestamp": 2.1812576334370735, "isStopPoint": false, "x": 14.218940734863281, "y": 4.332696437835693, @@ -1067,7 +1013,7 @@ "controlIntervalCount": 7 }, { - "timestamp": 2.4048105931097354, + "timestamp": 2.379641755762321, "isStopPoint": true, "x": 13.888501167297363, "y": 4.216057777404785, @@ -1075,24 +1021,24 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 14 }, { - "timestamp": 3.3962060953258644, + "timestamp": 3.3979607384134627, "isStopPoint": true, - "x": 14.352291107177734, - "y": 6.525911808013916, - "heading": -0.48570563852984416, + "x": 14.446789741516113, + "y": 6.612919330596924, + "heading": -0.5944409566225068, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 4 }, { - "timestamp": 3.7712489944235306, + "timestamp": 3.778383163049065, "isStopPoint": false, - "x": 14.129294395446777, - "y": 6.708604335784912, + "x": 14.166648864746094, + "y": 6.843034744262695, "heading": -0.6055455776100214, "isInitialGuess": false, "translationConstrained": false, @@ -1100,10 +1046,10 @@ "controlIntervalCount": 5 }, { - "timestamp": 3.9587700544930438, + "timestamp": 3.968594378599776, "isStopPoint": true, - "x": 13.670247077941895, - "y": 7.0260772705078125, + "x": 13.75644302368164, + "y": 7.143185615539551, "heading": -0.5880017968591525, "isInitialGuess": false, "translationConstrained": true, @@ -1111,10 +1057,10 @@ "controlIntervalCount": 12 }, { - "timestamp": 4.5720901588580665, + "timestamp": 4.599321630002376, "isStopPoint": false, - "x": 11.93593692779541, - "y": 7.216775417327881, + "x": 11.942968368530273, + "y": 7.280330657958984, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -1122,7 +1068,7 @@ "controlIntervalCount": 11 }, { - "timestamp": 5.481006745165987, + "timestamp": 5.508923302146178, "isStopPoint": true, "x": 9.261787414550781, "y": 7.437552452087402, @@ -1130,14 +1076,14 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 6 }, { - "timestamp": 6.5703452160170075, + "timestamp": 6.10592650998624, "isStopPoint": true, - "x": 11.927, - "y": 6.728, - "heading": -0.271, + "x": 10.155044555664062, + "y": 7.437562942504883, + "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -1223,7 +1169,7 @@ "name": "intake", "target": 2, "trajTargetIndex": 2, - "targetTimestamp": 0.918889530696065, + "targetTimestamp": 0.9188894506498827, "offset": 0, "command": { "type": "named", @@ -1236,7 +1182,7 @@ "name": "intake", "target": 5, "trajTargetIndex": 5, - "targetTimestamp": 2.4048105905284736, + "targetTimestamp": 2.1812576334370735, "offset": 0, "command": { "type": "named", @@ -1249,7 +1195,20 @@ "name": "intake", "target": 8, "trajTargetIndex": 8, - "targetTimestamp": 3.7712489944235306, + "targetTimestamp": 3.778383163049065, + "offset": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + }, + { + "name": "intake", + "target": 10, + "trajTargetIndex": 10, + "targetTimestamp": 4.599321630002376, "offset": 0, "command": { "type": "named", @@ -2421,18 +2380,18 @@ "Red 4 to CWS": { "waypoints": [ { - "x": 9.1292085647583, - "y": 7.4632415771484375, + "x": 9.9902982711792, + "y": 7.458646297454834, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 11 }, { - "x": 11.589872360229492, - "y": 6.55954647064209, - "heading": -0.2163660265240023, + "x": 11.792760848999023, + "y": 6.560247898101807, + "heading": -0.22426516530504303, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -2441,141 +2400,132 @@ ], "trajectory": [ { - "x": 9.1292085647583, - "y": 7.463241577148437, - "heading": -1.667402920110202e-23, - "angularVelocity": -7.105893464585413e-24, - "velocityX": 1.9107951304231947e-24, - "velocityY": -3.9513703650294445e-24, + "x": 9.9902982711792, + "y": 7.458646297454834, + "heading": 5.231965390296604e-25, + "angularVelocity": 6.579755902937532e-26, + "velocityX": -8.869878344137844e-27, + "velocityY": 1.2616478819313838e-26, "timestamp": 0 }, { - "x": 9.202228802975283, - "y": 7.43643053208001, - "heading": -0.010601683346708838, - "angularVelocity": -0.12032963165578175, - "velocityX": 0.8287833243759717, - "velocityY": -0.30430669091732365, - "timestamp": 0.08810534197458779 - }, - { - "x": 9.348269051180246, - "y": 7.382807931423719, - "heading": -0.03184320758536198, - "angularVelocity": -0.24109235334197815, - "velocityX": 1.6575640583415123, - "velocityY": -0.6086191762555923, - "timestamp": 0.17621068394917558 - }, - { - "x": 9.567328874222518, - "y": 7.302373052328402, - "heading": -0.06379484985933483, - "angularVelocity": -0.362652724090086, - "velocityX": 2.4863398533252936, - "velocityY": -0.9129398659903866, - "timestamp": 0.26431602592376335 - }, - { - "x": 9.859407635240762, - "y": 7.195124920858945, - "heading": -0.10655016324134052, - "angularVelocity": -0.4852749268521946, - "velocityX": 3.315108419901344, - "velocityY": -1.2172716099370104, - "timestamp": 0.35242136789835116 - }, - { - "x": 10.192837587328619, - "y": 7.072651571321672, - "heading": -0.10655016599578679, - "angularVelocity": -3.126310170419999e-8, - "velocityX": 3.7844464888863056, - "velocityY": -1.3900785899292845, - "timestamp": 0.44052670987293896 - }, - { - "x": 10.52626753919339, - "y": 6.950178221176961, - "heading": -0.1065501687502156, - "angularVelocity": -3.126290359484911e-8, - "velocityX": 3.7844464863542937, - "velocityY": -1.3900785968237475, - "timestamp": 0.5286320518475267 - }, - { - "x": 10.859697491223413, - "y": 6.82770487148224, - "heading": -0.10655017150466216, - "angularVelocity": -3.126310516103417e-8, - "velocityX": 3.7844464882298987, - "velocityY": -1.390078591716332, - "timestamp": 0.6167373938221145 - }, - { - "x": 11.151767702106236, - "y": 6.720442093033459, - "heading": -0.1504495662331604, - "angularVelocity": -0.4982603068626659, - "velocityX": 3.315011375440373, - "velocityY": -1.2174378538785793, - "timestamp": 0.7048427357967022 - }, - { - "x": 11.37082008036848, - "y": 6.6399944366452095, - "heading": -0.1834043776397133, - "angularVelocity": -0.3740387434857022, - "velocityX": 2.486255354702837, - "velocityY": -0.9130848888987183, - "timestamp": 0.79294807777129 - }, - { - "x": 11.516854922707223, - "y": 6.586362464735785, - "heading": -0.20538061595377177, - "angularVelocity": -0.2494313945276675, - "velocityX": 1.657502701491852, - "velocityY": -0.6087255404433256, - "timestamp": 0.8810534197458777 - }, - { - "x": 11.589872360229492, - "y": 6.55954647064209, - "heading": -0.2163660265240023, - "angularVelocity": -0.1246849546693668, - "velocityX": 0.8287515363521442, - "velocityY": -0.3043628625995168, - "timestamp": 0.9691587617204654 - }, - { - "x": 11.589872360229492, - "y": 6.55954647064209, - "heading": -0.2163660265240023, - "angularVelocity": 1.5342116317474738e-23, - "velocityX": 3.420153298016769e-24, - "velocityY": -5.557275176460609e-24, - "timestamp": 1.0572641036950532 + "x": 10.050789739247952, + "y": 7.428504152821708, + "heading": -0.010882323813907078, + "angularVelocity": -0.13249687678312677, + "velocityX": 0.7365091067124155, + "velocityY": -0.36699330875732633, + "timestamp": 0.08213268175158164 + }, + { + "x": 10.171772327451892, + "y": 7.368219305255567, + "heading": -0.03269948942306371, + "angularVelocity": -0.2656331821131178, + "velocityX": 1.473013977187104, + "velocityY": -0.7339934150510075, + "timestamp": 0.1642653635031633 + }, + { + "x": 10.353245366410322, + "y": 7.27779102201178, + "heading": -0.06555089127342774, + "angularVelocity": -0.39997965669410784, + "velocityX": 2.2095106976698102, + "velocityY": -1.1010024428192526, + "timestamp": 0.24639804525474493 + }, + { + "x": 10.595207857729005, + "y": 7.157218328320408, + "heading": -0.10957454059477183, + "angularVelocity": -0.536006476136965, + "velocityX": 2.9459952622820897, + "velocityY": -1.4680233388221322, + "timestamp": 0.3285307270063266 + }, + { + "x": 10.89154952082099, + "y": 7.009468977921484, + "heading": -0.10957454363253621, + "angularVelocity": -3.698606117126844e-8, + "velocityX": 3.608084586697149, + "velocityY": -1.7989105828274894, + "timestamp": 0.4106634087579082 + }, + { + "x": 11.187891183520787, + "y": 6.861719626735956, + "heading": -0.10957454667030299, + "angularVelocity": -3.6986090083451744e-8, + "velocityX": 3.608084581922121, + "velocityY": -1.7989105924047368, + "timestamp": 0.4927960905094898 + }, + { + "x": 11.429839524035081, + "y": 6.74113159453185, + "heading": -0.1554014401167673, + "angularVelocity": -0.5579617330050446, + "velocityX": 2.9458229702774172, + "velocityY": -1.4682100916762528, + "timestamp": 0.5749287722610714 + }, + { + "x": 11.611300274126046, + "y": 6.650689937948947, + "heading": -0.18982654217252887, + "angularVelocity": -0.41914011964060394, + "velocityX": 2.209361075531547, + "velocityY": -1.1011652688567102, + "timestamp": 0.657061454012653 + }, + { + "x": 11.732273970188768, + "y": 6.5903952606856615, + "heading": -0.21278890540487266, + "angularVelocity": -0.2795764456077998, + "velocityX": 1.4729057116218145, + "velocityY": -0.7341130957546612, + "timestamp": 0.7391941357642346 + }, + { + "x": 11.792760848999023, + "y": 6.560247898101807, + "heading": -0.22426516530504303, + "angularVelocity": -0.13972829883823154, + "velocityX": 0.7364532305569309, + "velocityY": -0.3670568395056038, + "timestamp": 0.8213268175158163 + }, + { + "x": 11.792760848999023, + "y": 6.560247898101807, + "heading": -0.22426516530504303, + "angularVelocity": 8.181375385736729e-26, + "velocityX": 3.520921012184917e-26, + "velocityY": 4.276560943665093e-27, + "timestamp": 0.9034594992673979 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 9.1292085647583, - "y": 7.4632415771484375, + "x": 9.9902982711792, + "y": 7.458646297454834, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 11 }, { - "timestamp": 1.0572641036950532, + "timestamp": 0.9034594992673979, "isStopPoint": true, - "x": 11.589872360229492, - "y": 6.55954647064209, - "heading": -0.2163660265240023, + "x": 11.792760848999023, + "y": 6.560247898101807, + "heading": -0.22426516530504303, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -2612,11 +2562,29 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 11 }, { - "x": 9.11832046508789, - "y": 5.753842830657959, + "x": 10.161416053771973, + "y": 6.3065690994262695, + "heading": 0.2749093466778342, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 9.459155082702637, + "y": 5.7285990715026855, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 9.925745010375977, + "y": 5.720115661621094, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -2628,119 +2596,227 @@ { "x": 11.633423805236816, "y": 6.701088905334473, - "heading": -0.23748596442563125, - "angularVelocity": 4.0633007276282394e-23, - "velocityX": 6.7184741034051145e-22, - "velocityY": 5.702159340164939e-22, + "heading": -0.2489626838461611, + "angularVelocity": -3.9911915239081645e-28, + "velocityX": 3.750722112253487e-27, + "velocityY": 7.88866766822753e-27, "timestamp": 0 }, { - "x": 11.558314016783294, - "y": 6.672811015849246, - "heading": -0.22610284681530854, - "angularVelocity": 0.1271903732506324, - "velocityX": -0.8392465364248116, - "velocityY": -0.31596575222103795, - "timestamp": 0.08949669160803514 - }, - { - "x": 11.40809470327327, - "y": 6.616254752098494, - "heading": -0.2032649088498646, - "angularVelocity": 0.25518192410358925, - "velocityX": -1.678490130479727, - "velocityY": -0.6319369234749451, - "timestamp": 0.17899338321607028 - }, - { - "x": 11.182766460817703, - "y": 6.531419518701983, - "heading": -0.16883330424227505, - "angularVelocity": 0.384724887467217, - "velocityX": -2.5177270572792247, - "velocityY": -0.947914739617524, - "timestamp": 0.2684900748241054 - }, - { - "x": 10.882330233029782, - "y": 6.418304523161176, - "heading": -0.12260874966451532, - "angularVelocity": 0.5164945625052538, - "velocityX": -3.3569534704515043, - "velocityY": -1.2639014234259063, - "timestamp": 0.35798676643214056 - }, - { - "x": 10.544678568903556, - "y": 6.291091383419362, - "heading": -0.12260874853394456, - "angularVelocity": 1.2632542470994776e-8, - "velocityX": -3.7727837463491594, - "velocityY": -1.4214284021947707, - "timestamp": 0.4474834580401757 - }, - { - "x": 10.20702690477794, - "y": 6.163878243477181, - "heading": -0.12260874740337606, - "angularVelocity": 1.2632517262639448e-8, - "velocityX": -3.772783744689189, - "velocityY": -1.4214284066008342, - "timestamp": 0.5369801496482108 - }, - { - "x": 9.869375240962672, - "y": 6.036665104106448, - "heading": -0.12260874627280524, - "angularVelocity": 1.263254313511497e-8, - "velocityX": -3.7727837462233933, - "velocityY": -1.421428402528582, - "timestamp": 0.626476841256246 - }, - { - "x": 9.568952859507139, - "y": 5.923536823964721, - "heading": -0.07364693995564293, - "angularVelocity": 0.5470795114035975, - "velocityX": -3.3567987476160837, - "velocityY": -1.2640498599150238, - "timestamp": 0.7159735328642811 - }, - { - "x": 9.34363659158264, - "y": 5.838690025801379, - "heading": -0.0368350275632345, - "angularVelocity": 0.4113214883253116, - "velocityX": -2.5175932510012293, - "velocityY": -0.948043960921556, - "timestamp": 0.8054702244723162 - }, - { - "x": 9.193425871877203, - "y": 5.782125253556939, - "heading": -0.012272886719585112, - "angularVelocity": 0.2744474728878599, - "velocityX": -1.6783941053799585, - "velocityY": -0.632031990730871, - "timestamp": 0.8949669160803513 - }, - { - "x": 9.11832046508789, - "y": 5.753842830657959, - "heading": -3.541695678797396e-22, - "angularVelocity": 0.13713229504992377, - "velocityX": -0.839197582807272, - "velocityY": -0.31601640745180465, - "timestamp": 0.9844636076883864 - }, - { - "x": 9.11832046508789, - "y": 5.753842830657959, - "heading": -1.742218903707832e-22, - "angularVelocity": 5.002572264403276e-23, - "velocityX": 8.398171768741014e-22, - "velocityY": 7.127695018988845e-22, - "timestamp": 1.0739602992964215 + "x": 11.60880277589231, + "y": 6.695431678512334, + "heading": -0.22436926819218947, + "angularVelocity": 0.48466715646299596, + "velocityX": -0.485211344755504, + "velocityY": -0.11148805338511579, + "timestamp": 0.05074289711200875 + }, + { + "x": 11.559508022875812, + "y": 6.684033868840708, + "heading": -0.17658066697813565, + "angularVelocity": 0.9417791244470394, + "velocityX": -0.9714611467233332, + "velocityY": -0.22461881997920483, + "timestamp": 0.1014857942240175 + }, + { + "x": 11.485475054339716, + "y": 6.666795704493466, + "heading": -0.10751199441271837, + "angularVelocity": 1.361149569622653, + "velocityX": -1.4589819018941346, + "velocityY": -0.33971580907553484, + "timestamp": 0.15222869133602626 + }, + { + "x": 11.386615817361433, + "y": 6.643600374642708, + "heading": -0.020248136416301205, + "angularVelocity": 1.7197255766416497, + "velocityX": -1.9482379328886914, + "velocityY": -0.45711481154804423, + "timestamp": 0.202971588448035 + }, + { + "x": 11.26281279008371, + "y": 6.614317797925505, + "heading": 0.07980607344820692, + "angularVelocity": 1.9717874926149894, + "velocityX": -2.439809989651213, + "velocityY": -0.5770773523743771, + "timestamp": 0.2537144855600438 + }, + { + "x": 11.113964594314254, + "y": 6.578808862374169, + "heading": 0.18235735558472177, + "angularVelocity": 2.0209977745288255, + "velocityX": -2.9333799258819067, + "velocityY": -0.6997813994135037, + "timestamp": 0.3044573826720525 + }, + { + "x": 10.940530970314622, + "y": 6.536962266114887, + "heading": 0.2622073240603324, + "angularVelocity": 1.5736186347293408, + "velocityX": -3.4178896726530357, + "velocityY": -0.8246788938146384, + "timestamp": 0.35520027978406127 + }, + { + "x": 10.745529341112256, + "y": 6.489615298655598, + "heading": 0.27490923290973734, + "angularVelocity": 0.250318952450961, + "velocityX": -3.8429344854300362, + "velocityY": -0.9330757633876594, + "timestamp": 0.40594317689607 + }, + { + "x": 10.546411363954949, + "y": 6.442664173600506, + "heading": 0.2749092704823136, + "angularVelocity": 7.404499621771453e-7, + "velocityX": -3.924056143617072, + "velocityY": -0.9252748212513897, + "timestamp": 0.45668607400807876 + }, + { + "x": 10.349612345474712, + "y": 6.386784631613737, + "heading": 0.27490930845823947, + "angularVelocity": 7.483988509456561e-7, + "velocityX": -3.8783559804602334, + "velocityY": -1.1012288451607661, + "timestamp": 0.5074289711200876 + }, + { + "x": 10.161416053771973, + "y": 6.3065690994262695, + "heading": 0.2749093466778342, + "angularVelocity": 7.532008787450534e-7, + "velocityX": -3.70882039484896, + "velocityY": -1.5808228688717099, + "timestamp": 0.5581718682320963 + }, + { + "x": 9.999363853803423, + "y": 6.213952472147155, + "heading": 0.2662334616188675, + "angularVelocity": -0.18415006033277892, + "velocityX": -3.4396401287527363, + "velocityY": -1.9658348843200237, + "timestamp": 0.6052849945774712 + }, + { + "x": 9.858714919564335, + "y": 6.124248911568337, + "heading": 0.23140228534900956, + "angularVelocity": -0.7393093808829219, + "velocityX": -2.985344959025316, + "velocityY": -1.9040035662508, + "timestamp": 0.6523981209228461 + }, + { + "x": 9.739490758645776, + "y": 6.040365578797747, + "heading": 0.1893230151311145, + "angularVelocity": -0.8931538507850689, + "velocityX": -2.5305932797699513, + "velocityY": -1.7804662793052928, + "timestamp": 0.699511247268221 + }, + { + "x": 9.641470727591456, + "y": 5.9632238044536345, + "heading": 0.14586307693951026, + "angularVelocity": -0.9224592287298015, + "velocityX": -2.0805248697732264, + "velocityY": -1.637373282736633, + "timestamp": 0.7466243736135959 + }, + { + "x": 9.5645101355505, + "y": 5.893262282723749, + "heading": 0.103840530644284, + "angularVelocity": -0.8919498567590098, + "velocityX": -1.6335275964659406, + "velocityY": -1.4849687795501898, + "timestamp": 0.7937374999589708 + }, + { + "x": 9.50851272054425, + "y": 5.83073553526098, + "heading": 0.0648829602133342, + "angularVelocity": -0.8268941896439074, + "velocityX": -1.1885735324746936, + "velocityY": -1.3271619252010773, + "timestamp": 0.8408506263043457 + }, + { + "x": 9.473410925926867, + "y": 5.775809115188849, + "heading": 0.030031627756018166, + "angularVelocity": -0.7397372061838857, + "velocityX": -0.7450533925526411, + "velocityY": -1.165841121845301, + "timestamp": 0.8879637526497206 + }, + { + "x": 9.459155082702637, + "y": 5.7285990715026855, + "heading": -8.372965025783953e-28, + "angularVelocity": -0.637436529596096, + "velocityX": -0.3025875022541161, + "velocityY": -1.002057119709671, + "timestamp": 0.9350768789950955 + }, + { + "x": 9.516865915616641, + "y": 5.664716307428782, + "heading": -0.03919049606692322, + "angularVelocity": -0.40720727806946266, + "velocityX": 0.5996420955198722, + "velocityY": -0.6637712987778043, + "timestamp": 1.0313190096680105 + }, + { + "x": 9.660745822884815, + "y": 5.635065101552019, + "heading": -0.054962369362559035, + "angularVelocity": -0.16387701711672975, + "velocityX": 1.4949784077116863, + "velocityY": -0.3080896658193722, + "timestamp": 1.1275611403409254 + }, + { + "x": 9.837618900841257, + "y": 5.691130117547053, + "heading": -0.018915350923725704, + "angularVelocity": 0.3745450998102011, + "velocityX": 1.837792624911395, + "velocityY": 0.5825413008111203, + "timestamp": 1.2238032710138402 + }, + { + "x": 9.925745010375977, + "y": 5.720115661621094, + "heading": 6.588751442065098e-28, + "angularVelocity": 0.19653919537599127, + "velocityX": 0.9156708077694437, + "velocityY": 0.30117313354740216, + "timestamp": 1.320045401686755 + }, + { + "x": 9.925745010375977, + "y": 5.720115661621094, + "heading": 2.990444886128271e-28, + "angularVelocity": -2.5147012717587645e-28, + "velocityX": -1.474992207507212e-27, + "velocityY": -9.524307703496857e-27, + "timestamp": 1.41628753235967 } ], "trajectoryWaypoints": [ @@ -2749,17 +2825,39 @@ "isStopPoint": true, "x": 11.633423805236816, "y": 6.701088905334473, - "heading": -0.23748596442563125, + "heading": -0.2489626838461611, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 11 + }, + { + "timestamp": 0.5581718682320963, + "isStopPoint": false, + "x": 10.161416053771973, + "y": 6.3065690994262695, + "heading": 0.2749093466778342, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "timestamp": 0.9350768789950955, + "isStopPoint": false, + "x": 9.459155082702637, + "y": 5.7285990715026855, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 }, { - "timestamp": 1.0739602992964215, + "timestamp": 1.41628753235967, "isStopPoint": true, - "x": 9.11832046508789, - "y": 5.753842830657959, + "x": 9.925745010375977, + "y": 5.720115661621094, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -2785,19 +2883,42 @@ "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [], - "eventMarkers": [], - "isTrajectoryStale": true + "eventMarkers": [ + { + "name": "intake", + "target": 0, + "trajTargetIndex": 0, + "targetTimestamp": 0, + "offset": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ], + "isTrajectoryStale": false }, "Red 5 to CWS": { "waypoints": [ { - "x": 8.976778030395508, - "y": 5.797394275665283, + "x": 9.661816596984863, + "y": 5.803494453430176, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 6 + }, + { + "x": 10.208364486694336, + "y": 6.476681709289551, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { "x": 11.426553726196289, @@ -2811,137 +2932,175 @@ ], "trajectory": [ { - "x": 8.976778030395508, - "y": 5.797394275665283, - "heading": 5.68893776700111e-24, - "angularVelocity": -2.9835246511928784e-24, - "velocityX": 6.5296719081595606e-24, - "velocityY": 1.747545176771818e-24, + "x": 9.661816596984863, + "y": 5.803494453430176, + "heading": 1.3611922399510842e-21, + "angularVelocity": 3.0579268596230436e-22, + "velocityX": -1.736689415628626e-20, + "velocityY": 5.376921496421165e-21, "timestamp": 0 }, { - "x": 9.049243245948405, - "y": 5.822190512926668, - "heading": -0.011075196763805075, - "angularVelocity": -0.1266788603327554, - "velocityX": 0.8288621065414437, - "velocityY": 0.28362106279487775, - "timestamp": 0.08742734766253149 - }, - { - "x": 9.194173918876237, - "y": 5.871782400887453, - "heading": -0.033181848616755605, - "angularVelocity": -0.25285740039011523, - "velocityX": 1.657726979059941, - "velocityY": 0.5672354164535425, - "timestamp": 0.17485469532506298 - }, - { - "x": 9.411570535802515, - "y": 5.946169185203648, - "heading": -0.06622649901979259, - "angularVelocity": -0.3779669781426854, - "velocityX": 2.486597417611544, - "velocityY": 0.8508411418739135, - "timestamp": 0.2622820429875945 - }, - { - "x": 9.701433824736373, - "y": 6.045350005092207, - "heading": -0.11005848510673794, - "angularVelocity": -0.5013532636965756, - "velocityX": 3.315476183181578, - "velocityY": 1.1344370215987225, - "timestamp": 0.34970939065012596 - }, - { - "x": 10.034912241269211, - "y": 6.159512024287684, - "heading": -0.11005848780565615, - "angularVelocity": -3.0870411745699127e-8, - "velocityX": 3.814349004616508, - "velocityY": 1.3057930069678114, - "timestamp": 0.4371367383126574 - }, - { - "x": 10.368390657402738, - "y": 6.273674044649711, - "heading": -0.11005849050455542, - "angularVelocity": -3.087019508851263e-8, - "velocityX": 3.8143490000491562, - "velocityY": 1.305793020310885, - "timestamp": 0.5245640859751889 - }, - { - "x": 10.70186907395756, - "y": 6.387836063780976, - "heading": -0.11005849320347309, - "angularVelocity": -3.0870405575183246e-8, - "velocityX": 3.8143490048679434, - "velocityY": 1.3057930062333485, - "timestamp": 0.6119914336377204 - }, - { - "x": 10.99174259097438, - "y": 6.487006632069917, - "heading": -0.15169988410125665, - "angularVelocity": -0.47629708565011974, - "velocityX": 3.3155931726961243, - "velocityY": 1.1343197630990574, - "timestamp": 0.6994187813002518 - }, - { - "x": 11.209148113872045, - "y": 6.561384295183812, - "heading": -0.18284402599158536, - "angularVelocity": -0.3562288314011849, - "velocityX": 2.4866992847232186, - "velocityY": 0.8507368129362852, - "timestamp": 0.7868461289627833 - }, - { - "x": 11.354085215408697, - "y": 6.610969282515083, - "heading": -0.2035851968769597, - "angularVelocity": -0.2372389354122352, - "velocityX": 1.6578005099286304, - "velocityY": 0.5671564865797899, - "timestamp": 0.8742734766253147 + "x": 9.678564574450153, + "y": 5.844997973970278, + "heading": 0.0029848399099582116, + "angularVelocity": 0.044669071267147434, + "velocityX": 0.250638768424957, + "velocityY": 0.6211132833818533, + "timestamp": 0.0668211768296481 + }, + { + "x": 9.716025235630987, + "y": 5.926178744283693, + "heading": 0.0082949313366775, + "angularVelocity": 0.0794671940641913, + "velocityX": 0.5606106171485411, + "velocityY": 1.214895848377748, + "timestamp": 0.1336423536592962 + }, + { + "x": 9.780171845440192, + "y": 6.043296248773044, + "heading": 0.014648503288511429, + "angularVelocity": 0.09508320944468747, + "velocityX": 0.9599742604353574, + "velocityY": 1.752700416924237, + "timestamp": 0.20046353048894427 + }, + { + "x": 9.879764902858732, + "y": 6.187745327513362, + "heading": 0.019081464409680428, + "angularVelocity": 0.0663406622195574, + "velocityX": 1.4904415358074388, + "velocityY": 2.1617260514459766, + "timestamp": 0.2672847073185924 + }, + { + "x": 10.023311845369117, + "y": 6.3404966880582885, + "heading": 0.015505580615079386, + "angularVelocity": -0.053514229534108525, + "velocityX": 2.14822529809053, + "velocityY": 2.285972318857878, + "timestamp": 0.3341058841482405 + }, + { + "x": 10.208364486694336, + "y": 6.476681709289551, + "heading": -1.1002971311721428e-21, + "angularVelocity": -0.23204590746147513, + "velocityX": 2.7693711799631684, + "velocityY": 2.038051822663229, + "timestamp": 0.4009270609778886 + }, + { + "x": 10.40827692519543, + "y": 6.576214186706722, + "heading": -0.02372971218677946, + "angularVelocity": -0.3811944081806325, + "velocityX": 3.2113960372748394, + "velocityY": 1.5988910242982828, + "timestamp": 0.46317800610595805 + }, + { + "x": 10.628550652151867, + "y": 6.642722598659207, + "heading": -0.055585275097017335, + "angularVelocity": -0.5117281809087577, + "velocityX": 3.5384800424135903, + "velocityY": 1.0683920029330198, + "timestamp": 0.5254289512340276 + }, + { + "x": 10.849046714110486, + "y": 6.670407742335224, + "heading": -0.09244567690717581, + "angularVelocity": -0.5921259787192825, + "velocityX": 3.542051634778082, + "velocityY": 0.4447345116254647, + "timestamp": 0.587679896362097 + }, + { + "x": 11.040961688225199, + "y": 6.671780771980835, + "heading": -0.12955345019997333, + "angularVelocity": -0.5960997574648119, + "velocityX": 3.0829246643653434, + "velocityY": 0.02205636615921845, + "timestamp": 0.6499308414901666 + }, + { + "x": 11.195452845995485, + "y": 6.662777946369766, + "heading": -0.16199848111936757, + "angularVelocity": -0.5211974027485814, + "velocityX": 2.4817479871651686, + "velocityY": -0.14462150883566724, + "timestamp": 0.7121817866182361 + }, + { + "x": 11.311201580164841, + "y": 6.651301472143275, + "heading": -0.18750887826283402, + "angularVelocity": -0.4097993547083289, + "velocityX": 1.8593891856799902, + "velocityY": -0.18435823268511917, + "timestamp": 0.7744327317463056 + }, + { + "x": 11.388172590876854, + "y": 6.641477967871726, + "heading": -0.20502458756132477, + "angularVelocity": -0.28137258418254585, + "velocityX": 1.2364633268336311, + "velocityY": -0.15780490158609067, + "timestamp": 0.8366836768743751 }, { "x": 11.426553726196287, "y": 6.635761737823486, "heading": -0.21396366633395164, - "angularVelocity": -0.11870964560257202, - "velocityX": 0.8288997976619157, - "velocityY": 0.2835778045572377, - "timestamp": 0.9617008242878462 + "angularVelocity": -0.14359747878905968, + "velocityX": 0.6165550617831983, + "velocityY": -0.0918255945708951, + "timestamp": 0.8989346220024446 }, { "x": 11.426553726196287, "y": 6.635761737823486, "heading": -0.21396366633395164, - "angularVelocity": 1.2331869799730271e-23, - "velocityX": -5.618152052891591e-25, - "velocityY": -9.56708867463376e-25, - "timestamp": 1.0491281719503778 + "angularVelocity": 6.005893376627867e-22, + "velocityX": -1.2386565547332063e-20, + "velocityY": -6.661443938725831e-20, + "timestamp": 0.9611855671305141 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 8.976778030395508, - "y": 5.797394275665283, + "x": 9.661816596984863, + "y": 5.803494453430176, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 12 + "controlIntervalCount": 6 + }, + { + "timestamp": 0.4009270609778886, + "isStopPoint": false, + "x": 10.208364486694336, + "y": 6.476681709289551, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "timestamp": 1.0491281719503778, + "timestamp": 0.9611855671305141, "isStopPoint": true, "x": 11.426553726196289, "y": 6.635761737823486, @@ -2976,8 +3135,8 @@ "Red 5 to 6": { "waypoints": [ { - "x": 9.150984764099121, - "y": 5.797394275665283, + "x": 9.628934860229492, + "y": 5.792125701904297, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -2991,11 +3150,20 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 7 + "controlIntervalCount": 6 + }, + { + "x": 9.289485931396484, + "y": 4.1082587242126465, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 }, { - "x": 9.1292085647583, - "y": 4.109771251678467, + "x": 9.815460205078125, + "y": 4.1082587242126465, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -3005,147 +3173,183 @@ ], "trajectory": [ { - "x": 9.15098476409912, - "y": 5.797394275665283, - "heading": -4.065640793519508e-29, - "angularVelocity": -3.268176405529047e-29, - "velocityX": 1.157597527355568e-20, - "velocityY": 1.4018882473037506e-19, + "x": 9.628934860229492, + "y": 5.792125701904297, + "heading": 1.9970068949058212e-28, + "angularVelocity": 6.405261716169362e-29, + "velocityX": 4.955467456720226e-23, + "velocityY": 1.462552204852727e-23, "timestamp": 0 }, { - "x": 9.192684743588645, - "y": 5.767902935794363, - "heading": 8.806771020594398e-18, - "angularVelocity": 1.23382974768875e-16, - "velocityX": 0.5842172682176147, - "velocityY": -0.4131740645050891, - "timestamp": 0.07137751956005578 + "x": 9.64209754532163, + "y": 5.757664815288066, + "heading": 3.6472115225969175e-18, + "angularVelocity": 6.012474053277396e-17, + "velocityX": 0.21698851877782935, + "velocityY": -0.5680920488702936, + "timestamp": 0.0606607444775193 }, { - "x": 9.273759250876711, - "y": 5.70588047685832, - "heading": 7.597661113768503e-18, - "angularVelocity": -1.6939650399948386e-17, - "velocityX": 1.1358549272625345, - "velocityY": -0.8689354760200882, - "timestamp": 0.14275503912011156 + "x": 9.666165788175904, + "y": 5.6879636437386, + "heading": 9.687140289512023e-18, + "angularVelocity": 9.956898516114172e-17, + "velocityX": 0.396768009716635, + "velocityY": -1.1490325769954366, + "timestamp": 0.1213214889550386 }, { - "x": 9.38913435315891, - "y": 5.606014975885555, - "heading": -6.605907220253839e-18, - "angularVelocity": -1.9899218262115755e-16, - "velocityX": 1.6164067201176004, - "velocityY": -1.399116999136334, - "timestamp": 0.21413255868016734 + "x": 9.69754267815799, + "y": 5.5821045909015545, + "heading": 1.7213238235683665e-17, + "angularVelocity": 1.240686709476666e-16, + "velocityX": 0.5172519765845578, + "velocityY": -1.7450997963975958, + "timestamp": 0.1819822334325579 }, { - "x": 9.523623783255955, - "y": 5.458786324088157, - "heading": -1.545595323632644e-17, - "angularVelocity": -1.2398926619047138e-16, - "velocityX": 1.8841987074633166, - "velocityY": -2.062675373212134, - "timestamp": 0.2855100782402231 + "x": 9.729912323289412, + "y": 5.439369761908001, + "heading": 2.899948078854106e-17, + "angularVelocity": 1.9429769044629593e-16, + "velocityX": 0.5336176700471545, + "velocityY": -2.3530016029798486, + "timestamp": 0.2426429779100772 }, { - "x": 9.632223213447787, - "y": 5.26753117493535, - "heading": -9.150147904566991e-18, - "angularVelocity": 8.834441437765331e-17, - "velocityX": 1.521479463860705, - "velocityY": -2.679487187725441, - "timestamp": 0.3568875978002789 + "x": 9.751348345266472, + "y": 5.261403357718118, + "heading": 4.0782917255307413e-17, + "angularVelocity": 1.9425143176374398e-16, + "velocityX": 0.35337551758868835, + "velocityY": -2.9337985499969643, + "timestamp": 0.3033037223875965 }, { - "x": 9.691761227634146, - "y": 5.062077314595765, - "heading": -6.8541498963009875e-18, - "angularVelocity": 3.216696488819412e-17, - "velocityX": 0.8341283719759774, - "velocityY": -2.878411320621096, - "timestamp": 0.4282651173603347 + "x": 9.744181294864156, + "y": 5.060141633962775, + "heading": 4.198809491304761e-17, + "angularVelocity": 1.9867505220008126e-17, + "velocityX": -0.11814972704420049, + "velocityY": -3.3178248221125908, + "timestamp": 0.3639644668651158 }, { "x": 9.70022678375244, "y": 4.856142997741699, - "heading": -9.683146911978938e-18, - "angularVelocity": -3.963428214808329e-17, - "velocityX": 0.1186025540038291, - "velocityY": -2.8851425227921963, - "timestamp": 0.49964263692039046 - }, - { - "x": 9.65307144790573, - "y": 4.645942615589113, - "heading": -5.5462217098690175e-18, - "angularVelocity": 5.5158786569436616e-17, - "velocityX": -0.6287353341669664, - "velocityY": -2.8026607199750178, - "timestamp": 0.5746429324090815 - }, - { - "x": 9.551846573573934, - "y": 4.451755307762366, - "heading": -7.328868694792328e-18, - "angularVelocity": -2.3768530508516856e-17, - "velocityX": -1.3496596736354354, - "velocityY": -2.5891539034802524, - "timestamp": 0.6496432278977725 - }, - { - "x": 9.409128969103543, - "y": 4.29575550701389, - "heading": -1.2264974248358262e-17, - "angularVelocity": -6.58144800800035e-17, - "velocityX": -1.9028938958235035, - "velocityY": -2.0799891484682558, - "timestamp": 0.7246435233864635 - }, - { - "x": 9.274801512075, - "y": 4.195518932576719, - "heading": -1.0315851786469037e-17, - "angularVelocity": 2.598819788396303e-17, - "velocityX": -1.7910257040087993, - "velocityY": -1.3364823936217263, - "timestamp": 0.7996438188751546 - }, - { - "x": 9.178765638394157, - "y": 4.136679072340064, - "heading": -1.7595474741066158e-18, - "angularVelocity": 1.1408360752736715e-16, - "velocityX": -1.2804732708731046, - "velocityY": -0.7845283789014882, - "timestamp": 0.8746441143638456 - }, - { - "x": 9.1292085647583, - "y": 4.109771251678467, - "heading": 8.197183256276926e-29, - "angularVelocity": 2.3460541527021614e-17, - "velocityX": -0.6607583785231516, - "velocityY": -0.3587695286556818, - "timestamp": 0.9496444098525366 - }, - { - "x": 9.1292085647583, - "y": 4.109771251678467, - "heading": 4.0649014975521164e-29, - "angularVelocity": -8.983999410224412e-30, - "velocityX": 2.1617137820797256e-20, - "velocityY": 3.116448033899274e-19, - "timestamp": 1.0246447053412275 + "heading": 4.117789366500127e-17, + "angularVelocity": -1.3356269448868408e-17, + "velocityX": -0.7245956423763401, + "velocityY": -3.3629431682408346, + "timestamp": 0.4246252113426351 + }, + { + "x": 9.62367202292937, + "y": 4.666896180193955, + "heading": 3.883641520117362e-17, + "angularVelocity": -3.959883629943507e-17, + "velocityX": -1.2946860234610496, + "velocityY": -3.2005221756203603, + "timestamp": 0.4837551930029131 + }, + { + "x": 9.520833726243332, + "y": 4.500838515926915, + "heading": 2.7882820869042386e-17, + "angularVelocity": -1.8524602958829465e-16, + "velocityX": -1.7391904038949337, + "velocityY": -2.8083496663519774, + "timestamp": 0.5428851746631911 + }, + { + "x": 9.422837551158624, + "y": 4.369495613480981, + "heading": 1.2414905453524928e-17, + "angularVelocity": -2.6159175059594473e-16, + "velocityX": -1.6573009551691247, + "velocityY": -2.221257283666062, + "timestamp": 0.6020151563234691 + }, + { + "x": 9.348748263717006, + "y": 4.263785158181079, + "heading": 8.266599657957546e-18, + "angularVelocity": -7.01557091454596e-17, + "velocityX": -1.2529901982260223, + "velocityY": -1.7877640467951625, + "timestamp": 0.6611451379837472 + }, + { + "x": 9.30366679578196, + "y": 4.177749774701769, + "heading": 8.751765261420785e-18, + "angularVelocity": 8.205069415908739e-18, + "velocityX": -0.7624130207591719, + "velocityY": -1.4550213117537047, + "timestamp": 0.7202751196440252 + }, + { + "x": 9.289485931396484, + "y": 4.1082587242126465, + "heading": -9.673823625011814e-28, + "angularVelocity": -1.4800892906397257e-16, + "velocityX": -0.23982527961788674, + "velocityY": -1.1752253009035645, + "timestamp": 0.7794051013043033 + }, + { + "x": 9.359415889552718, + "y": 4.032157790510569, + "heading": -1.916769609398054e-18, + "angularVelocity": -1.8826829061024516e-17, + "velocityX": 0.6868636491748086, + "velocityY": -0.7474759946436492, + "timestamp": 0.8812156400338537 + }, + { + "x": 9.52218848262578, + "y": 4.002726888178056, + "heading": 1.6766816122200447e-18, + "angularVelocity": 3.5295473979561707e-17, + "velocityX": 1.5987794103069355, + "velocityY": -0.28907520478496473, + "timestamp": 0.983026178763404 + }, + { + "x": 9.718168604909845, + "y": 4.071759502986741, + "heading": 1.7799244325302356e-18, + "angularVelocity": 1.0140681418433122e-18, + "velocityX": 1.9249492707691656, + "velocityY": 0.6780497939615223, + "timestamp": 1.0848367174929543 + }, + { + "x": 9.815460205078123, + "y": 4.1082587242126465, + "heading": 1.5656793258881755e-27, + "angularVelocity": -1.7482713005339804e-17, + "velocityX": 0.9556142358378643, + "velocityY": 0.35850140546709003, + "timestamp": 1.1866472562225046 + }, + { + "x": 9.815460205078123, + "y": 4.1082587242126465, + "heading": 7.676097442920992e-28, + "angularVelocity": -2.991815747657937e-28, + "velocityX": 3.353769132505619e-22, + "velocityY": 1.0669370220059503e-21, + "timestamp": 1.2884577949520548 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 9.150984764099121, - "y": 5.797394275665283, + "x": 9.628934860229492, + "y": 5.792125701904297, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -3153,7 +3357,7 @@ "controlIntervalCount": 7 }, { - "timestamp": 0.49964263692039046, + "timestamp": 0.4246252113426351, "isStopPoint": false, "x": 9.700226783752441, "y": 4.856142997741699, @@ -3161,13 +3365,24 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "timestamp": 1.0246447053412275, + "timestamp": 0.7794051013043033, + "isStopPoint": false, + "x": 9.289485931396484, + "y": 4.1082587242126465, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "timestamp": 1.2884577949520548, "isStopPoint": true, - "x": 9.1292085647583, - "y": 4.109771251678467, + "x": 9.815460205078125, + "y": 4.1082587242126465, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -3193,19 +3408,33 @@ "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "intake", + "target": 1, + "trajTargetIndex": 1, + "targetTimestamp": 0.4246252113426351, + "offset": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ], "isTrajectoryStale": false }, "Red 4 to 5": { "waypoints": [ { - "x": 9.111647605895996, - "y": 7.470791816711426, + "x": 9.570359230041504, + "y": 7.533205986022949, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { "x": 9.564400672912598, @@ -3214,11 +3443,20 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "x": 9.100329399108887, - "y": 5.88615608215332, + "x": 9.213134765625, + "y": 5.889784812927246, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 9.641109466552734, + "y": 5.9059624671936035, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -3228,155 +3466,182 @@ ], "trajectory": [ { - "x": 9.111647605895996, - "y": 7.470791816711426, - "heading": -1.05718871216561e-30, - "angularVelocity": 3.118504998701382e-31, - "velocityX": 6.039100561599308e-21, - "velocityY": 1.8009343842569663e-19, + "x": 9.570359230041504, + "y": 7.533205986022949, + "heading": 2.837553697468469e-27, + "angularVelocity": 1.2858073985093775e-27, + "velocityX": 3.929456586583098e-19, + "velocityY": 1.3369892836254632e-18, "timestamp": 0 }, { - "x": 9.143340108113769, - "y": 7.446687839925814, - "heading": -1.1700310711180642e-18, - "angularVelocity": -1.8565347315473353e-17, - "velocityX": 0.5028775532448245, - "velocityY": -0.3824673983179698, - "timestamp": 0.06302230436271758 - }, - { - "x": 9.20468145119787, - "y": 7.396006382726051, - "heading": 1.8056475843181295e-18, - "angularVelocity": 4.721627840095709e-17, - "velocityX": 0.9733275180014863, - "velocityY": -0.8041828636983419, - "timestamp": 0.12604460872543516 + "x": 9.578782459095901, + "y": 7.491411310315301, + "heading": 4.923887190188839e-18, + "angularVelocity": 7.550337998258628e-17, + "velocityX": 0.12916263916652715, + "velocityY": -0.6408837492897412, + "timestamp": 0.06521412932371412 }, { - "x": 9.29159191314905, - "y": 7.314802210827562, - "heading": 6.5446419450669345e-18, - "angularVelocity": 7.519550919609085e-17, - "velocityX": 1.3790429091743241, - "velocityY": -1.2884989325681133, - "timestamp": 0.18906691308815274 + "x": 9.593047663479659, + "y": 7.407383744923941, + "heading": 8.7575474267071e-18, + "angularVelocity": 5.878573045312757e-17, + "velocityX": 0.21874407481462338, + "velocityY": -1.2884871156411322, + "timestamp": 0.13042825864742824 }, { - "x": 9.394024992378341, - "y": 7.196931108324088, - "heading": 1.0466872659140665e-17, - "angularVelocity": 6.223559712325196e-17, - "velocityX": 1.6253464589248696, - "velocityY": -1.8703077219217359, - "timestamp": 0.2520892174508703 + "x": 9.609050762578377, + "y": 7.280756587054724, + "heading": 1.5830775069210892e-17, + "angularVelocity": 1.0846158204666518e-16, + "velocityX": 0.24539312668395535, + "velocityY": -1.9417135394180687, + "timestamp": 0.19564238797114236 }, { - "x": 9.486406073668979, - "y": 7.040532454821936, - "heading": 9.387684387047981e-18, - "angularVelocity": -1.7123910513967864e-17, - "velocityX": 1.465847404737292, - "velocityY": -2.4816397160349983, - "timestamp": 0.3151115218135879 + "x": 9.619581483588787, + "y": 7.111847063024372, + "heading": 1.351977297182082e-17, + "angularVelocity": -3.543713803053726e-17, + "velocityX": 0.16147913220059756, + "velocityY": -2.5900755830367284, + "timestamp": 0.2608565172948565 }, { - "x": 9.544986688827217, - "y": 6.8630875726594285, - "heading": 8.793699397125095e-18, - "angularVelocity": -9.424997001991433e-18, - "velocityX": 0.929521948627871, - "velocityY": -2.815588607181346, - "timestamp": 0.37813382617630553 + "x": 9.610988346334986, + "y": 6.904832116628309, + "heading": 1.1213563371751803e-17, + "angularVelocity": -3.536364929500549e-17, + "velocityX": -0.131768028537904, + "velocityY": -3.174387951550641, + "timestamp": 0.3260706466185706 }, { "x": 9.564400672912598, "y": 6.678473949432373, - "heading": 6.026195272855988e-18, - "angularVelocity": -4.391308904044169e-17, - "velocityX": 0.3080494164990886, - "velocityY": -2.9293378763945945, - "timestamp": 0.44115613053902314 - }, - { - "x": 9.533920060898456, - "y": 6.465337858341574, - "heading": 3.2791995300552913e-18, - "angularVelocity": -3.783392353465437e-17, - "velocityX": -0.41980447713101565, - "velocityY": -2.9354884749825843, - "timestamp": 0.5137628169242787 - }, - { - "x": 9.451829657549295, - "y": 6.26357893370136, - "heading": 6.745548569249782e-18, - "angularVelocity": 4.7741455890632045e-17, - "velocityX": -1.130617680491636, - "velocityY": -2.778792624830332, - "timestamp": 0.5863695033095343 - }, - { - "x": 9.332723735038355, - "y": 6.099540834625063, - "heading": 4.720380341741389e-18, - "angularVelocity": -2.7892309176467646e-17, - "velocityX": -1.640426363475722, - "velocityY": -2.2592698722790585, - "timestamp": 0.6589761896947899 - }, - { - "x": 9.221886486725465, - "y": 5.987700806646064, - "heading": -2.9444142095504363e-18, - "angularVelocity": -1.0556595929632475e-16, - "velocityX": -1.5265432679955857, - "velocityY": -1.5403543880980908, - "timestamp": 0.7315828760800455 - }, - { - "x": 9.14193306171042, - "y": 5.918746675175855, - "heading": -1.587123146449334e-18, - "angularVelocity": 1.8693746948193755e-17, - "velocityX": -1.1011854278925208, - "velocityY": -0.9496939593725585, - "timestamp": 0.804189562465301 - }, - { - "x": 9.100329399108888, - "y": 5.88615608215332, - "heading": 2.2037577276388683e-30, - "angularVelocity": 2.185918820959246e-17, - "velocityX": -0.5730004311286885, - "velocityY": -0.44886489998548856, - "timestamp": 0.8767962488505566 - }, - { - "x": 9.100329399108888, - "y": 5.88615608215332, - "heading": 1.055345960195893e-30, - "angularVelocity": -1.2817805459557936e-30, - "velocityX": -1.5709732296828935e-20, - "velocityY": 3.1336350844643724e-19, - "timestamp": 0.9494029352358122 + "heading": -3.0718939053161175e-18, + "angularVelocity": -2.1905463476658995e-16, + "velocityX": -0.7143800569832521, + "velocityY": -3.4709988394129123, + "timestamp": 0.3912847759422847 + }, + { + "x": 9.486888499519376, + "y": 6.477915488845292, + "heading": -2.382566973004584e-17, + "angularVelocity": -3.487118187109928e-16, + "velocityX": -1.3023852218818446, + "velocityY": -3.3698497120818054, + "timestamp": 0.4508003264037865 + }, + { + "x": 9.388684393644013, + "y": 6.306214692673101, + "heading": -2.812446729719055e-17, + "angularVelocity": -7.22298214874531e-17, + "velocityX": -1.6500579279508787, + "velocityY": -2.8849736722717, + "timestamp": 0.5103158768652882 + }, + { + "x": 9.305060350101293, + "y": 6.166892021614753, + "heading": -2.368886627734559e-17, + "angularVelocity": 7.452843840961614e-17, + "velocityX": -1.4050788893704838, + "velocityY": -2.3409456852536437, + "timestamp": 0.5698314273267899 + }, + { + "x": 9.246133814938752, + "y": 6.053083162641967, + "heading": -1.5812896843887468e-17, + "angularVelocity": 1.3233464831351022e-16, + "velocityX": -0.990103169770027, + "velocityY": -1.9122541603039365, + "timestamp": 0.6293469777882916 + }, + { + "x": 9.21502434791928, + "y": 5.961345379431105, + "heading": -1.0280341318353077e-17, + "angularVelocity": 9.295983120434094e-17, + "velocityX": -0.5227115733323473, + "velocityY": -1.5414086318533518, + "timestamp": 0.6888625282497933 + }, + { + "x": 9.213134765625, + "y": 5.889784812927246, + "heading": -5.475042515337732e-27, + "angularVelocity": 1.727337015993494e-16, + "velocityX": -0.03174938784280189, + "velocityY": -1.2023843507949883, + "timestamp": 0.748378078711295 + }, + { + "x": 9.279972650601232, + "y": 5.82484633107135, + "heading": 1.650291714944183e-17, + "angularVelocity": 1.818917451335665e-16, + "velocityX": 0.7366733685548789, + "velocityY": -0.7157385395219245, + "timestamp": 0.8391074138946035 + }, + { + "x": 9.414002802389295, + "y": 5.807817742851688, + "heading": 4.3811469472123005e-17, + "angularVelocity": 3.009892258719419e-16, + "velocityX": 1.4772526605344272, + "velocityY": -0.1876855835574857, + "timestamp": 0.929836749077912 + }, + { + "x": 9.566226598810443, + "y": 5.87128228242156, + "heading": 1.3002245043408661e-17, + "angularVelocity": -3.395729106505042e-16, + "velocityX": 1.6777792553378463, + "velocityY": 0.6994930519621869, + "timestamp": 1.0205660842612205 + }, + { + "x": 9.641109466552734, + "y": 5.9059624671936035, + "heading": 5.3076276692349445e-27, + "angularVelocity": -1.4330806011987793e-16, + "velocityX": 0.8253435075987224, + "velocityY": 0.38223783632907515, + "timestamp": 1.111295419444529 + }, + { + "x": 9.641109466552734, + "y": 5.9059624671936035, + "heading": 2.6375377363273866e-27, + "angularVelocity": -3.5878359006669536e-28, + "velocityX": -7.830786140194078e-19, + "velocityY": -5.392178903208723e-18, + "timestamp": 1.2020247546278375 } ], "trajectoryWaypoints": [ { "timestamp": 0, "isStopPoint": true, - "x": 9.111647605895996, - "y": 7.470791816711426, + "x": 9.570359230041504, + "y": 7.533205986022949, "heading": 0, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "timestamp": 0.44115613053902314, + "timestamp": 0.3912847759422847, "isStopPoint": false, "x": 9.564400672912598, "y": 6.678473949432373, @@ -3384,13 +3649,24 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 7 + "controlIntervalCount": 6 }, { - "timestamp": 0.9494029352358122, + "timestamp": 0.748378078711295, + "isStopPoint": false, + "x": 9.213134765625, + "y": 5.889784812927246, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "timestamp": 1.2020247546278375, "isStopPoint": true, - "x": 9.100329399108887, - "y": 5.88615608215332, + "x": 9.641109466552734, + "y": 5.9059624671936035, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -3416,7 +3692,21 @@ "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "intake", + "target": 1, + "trajTargetIndex": 1, + "targetTimestamp": 0.3912847759422847, + "offset": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ], "isTrajectoryStale": false }, "Red CWS to 6": { @@ -3428,20 +3718,20 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 16 }, { "x": 9.536823272705078, "y": 6.212279796600342, - "heading": 0.6304013989449005, + "heading": 0.9431692146044316, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 11 }, { - "x": 9.113101959228516, - "y": 4.53757381439209, + "x": 9.47612190246582, + "y": 4.60029935836792, "heading": 1.3909420707626843, "isInitialGuess": false, "translationConstrained": true, @@ -3454,253 +3744,253 @@ "x": 11.584806442260742, "y": 6.726798057556152, "heading": -0.23521537604856152, - "angularVelocity": 1.679246454495359e-21, - "velocityX": 3.240973484928515e-21, - "velocityY": 9.774800818350314e-21, + "angularVelocity": -1.366573471191084e-21, + "velocityX": -2.3464364057737896e-21, + "velocityY": 1.0451672994386824e-20, "timestamp": 0 }, { - "x": 11.559846058604368, - "y": 6.7282627420674075, - "heading": -0.175905915948779, - "angularVelocity": 1.1294257669789647, - "velocityX": -0.47531878401457256, - "velocityY": 0.02789188140851985, - "timestamp": 0.052512933416089806 - }, - { - "x": 11.50927509086037, - "y": 6.730802663220887, - "heading": -0.06477184086460809, - "angularVelocity": 2.1163181687755372, - "velocityX": -0.9630192879018148, - "velocityY": 0.048367535161469424, - "timestamp": 0.10502586683217961 - }, - { - "x": 11.432308982244173, - "y": 6.73416856654449, - "heading": 0.08661428832321974, - "angularVelocity": 2.882835129173027, - "velocityX": -1.4656600499985364, - "velocityY": 0.06409665400140648, - "timestamp": 0.1575388002482694 - }, - { - "x": 11.328136267216523, - "y": 6.738372418011969, - "heading": 0.26104185351142656, - "angularVelocity": 3.3216115315082115, - "velocityX": -1.9837534917776496, - "velocityY": 0.08005364000769541, - "timestamp": 0.21005173366435922 - }, - { - "x": 11.196338566137927, - "y": 6.743423511543135, - "heading": 0.4326071308552776, - "angularVelocity": 3.267105190724006, - "velocityX": -2.5098141068274407, - "velocityY": 0.09618760946319428, - "timestamp": 0.262564667080449 - }, - { - "x": 11.039433341611105, - "y": 6.747254994852584, - "heading": 0.551089395188952, - "angularVelocity": 2.256249206169307, - "velocityX": -2.9879348632760006, - "velocityY": 0.07296266005527433, - "timestamp": 0.3150776004965388 - }, - { - "x": 10.857659948269687, - "y": 6.7497818385588255, - "heading": 0.6153346073386814, - "angularVelocity": 1.2234169369415742, - "velocityX": -3.461497606715688, - "velocityY": 0.04811850228015354, - "timestamp": 0.36759053391262864 - }, - { - "x": 10.650955539971601, - "y": 6.7509399404848525, - "heading": 0.6261370902534954, - "angularVelocity": 0.20571090228800812, - "velocityX": -3.936257124702039, - "velocityY": 0.02205365137457731, - "timestamp": 0.42010346732871845 - }, - { - "x": 10.440862907331802, - "y": 6.724782250121588, - "heading": 0.6261371223269522, - "angularVelocity": 6.107725221919589e-7, - "velocityX": -4.0007788362366865, - "velocityY": -0.49811900919924323, - "timestamp": 0.47261640074480826 - }, - { - "x": 10.235969605153617, - "y": 6.671473034783826, - "heading": 0.6261371641263257, - "angularVelocity": 7.959824540999628e-7, - "velocityX": -3.901768361609107, - "velocityY": -1.015163539150922, - "timestamp": 0.525129334160898 - }, - { - "x": 10.039769105618355, - "y": 6.591921175301338, - "heading": 0.62613723036689, - "angularVelocity": 0.0000012614142817927116, - "velocityX": -3.7362319484355355, - "velocityY": -1.5149003170727215, - "timestamp": 0.5776422675769878 - }, - { - "x": 9.855606702521321, - "y": 6.487483048858524, - "heading": 0.6261374583126166, - "angularVelocity": 0.0000043407540164823, - "velocityX": -3.506991347023285, - "velocityY": -1.9888077021943713, - "timestamp": 0.6301552009930775 - }, - { - "x": 9.687066510308895, - "y": 6.360242791536438, - "heading": 0.6279452088235031, - "angularVelocity": 0.0344248624726932, - "velocityX": -3.2094987129548294, - "velocityY": -2.423027034387871, - "timestamp": 0.6826681344091673 + "x": 11.564450104674838, + "y": 6.730098062196231, + "heading": -0.17841273737044008, + "angularVelocity": 1.174862132661435, + "velocityX": -0.42103484531543234, + "velocityY": 0.06825476033261743, + "timestamp": 0.048348344115445675 + }, + { + "x": 11.52300211540097, + "y": 6.736367992138528, + "heading": -0.07239159740120889, + "angularVelocity": 2.192859795075401, + "velocityX": -0.857278362520844, + "velocityY": 0.12968241326601065, + "timestamp": 0.09669668823089135 + }, + { + "x": 11.459608536886385, + "y": 6.745383256813499, + "heading": 0.07165765421788575, + "angularVelocity": 2.979404036571249, + "velocityX": -1.311184067921649, + "velocityY": 0.18646480742824664, + "timestamp": 0.14504503234633703 + }, + { + "x": 11.373429519712674, + "y": 6.757049979355221, + "heading": 0.238364044928656, + "angularVelocity": 3.4480268923525164, + "velocityX": -1.7824605733739418, + "velocityY": 0.24130552462770832, + "timestamp": 0.1933933764617827 + }, + { + "x": 11.263930858755637, + "y": 6.771011078683092, + "heading": 0.4063780100927094, + "angularVelocity": 3.4750717576360297, + "velocityX": -2.264786167145887, + "velocityY": 0.288760651133037, + "timestamp": 0.24174172057722837 + }, + { + "x": 11.132329406214573, + "y": 6.785201936985669, + "heading": 0.5391063310201619, + "angularVelocity": 2.745250604871289, + "velocityX": -2.721943325024747, + "velocityY": 0.29351280922227524, + "timestamp": 0.29009006469267407 + }, + { + "x": 10.979560638214958, + "y": 6.797464602241318, + "heading": 0.6268214972685803, + "angularVelocity": 1.8142331004961214, + "velocityX": -3.15975181352334, + "velocityY": 0.2536315458161054, + "timestamp": 0.33843840880811976 + }, + { + "x": 10.805774130613937, + "y": 6.805172026883863, + "heading": 0.671009580464918, + "angularVelocity": 0.9139523597918021, + "velocityX": -3.5944665899214026, + "velocityY": 0.15941444910711128, + "timestamp": 0.38678675292356546 + }, + { + "x": 10.613420175544595, + "y": 6.800914326974529, + "heading": 0.6764207825530686, + "angularVelocity": 0.1119211461561101, + "velocityX": -3.9785014065850066, + "velocityY": -0.0880629934086431, + "timestamp": 0.43513509703901115 + }, + { + "x": 10.420466599593615, + "y": 6.773266562186232, + "heading": 0.6764212702275126, + "angularVelocity": 0.000010086683483707061, + "velocityX": -3.9909035041660488, + "velocityY": -0.5718451230165833, + "timestamp": 0.48348344115445685 + }, + { + "x": 10.236338023860592, + "y": 6.7240814934873905, + "heading": 0.6866220877955258, + "angularVelocity": 0.21098587251831752, + "velocityX": -3.8083739805733132, + "velocityY": -1.017306168363344, + "timestamp": 0.5318317852699025 + }, + { + "x": 10.065259656682233, + "y": 6.655935600538377, + "heading": 0.7131660865513336, + "angularVelocity": 0.549015674506377, + "velocityX": -3.538453494287846, + "velocityY": -1.409477288122344, + "timestamp": 0.5801801293853481 + }, + { + "x": 9.908917358821068, + "y": 6.57009894257851, + "heading": 0.7558905021068901, + "angularVelocity": 0.8836789829562635, + "velocityX": -3.233663959366384, + "velocityY": -1.7753794784569745, + "timestamp": 0.6285284735007938 + }, + { + "x": 9.768262559074747, + "y": 6.467223145022078, + "heading": 0.8117281749189443, + "angularVelocity": 1.1549035201438473, + "velocityX": -2.909195802249071, + "velocityY": -2.1278039494108354, + "timestamp": 0.6768768176162394 + }, + { + "x": 9.644017833038875, + "y": 6.347779813464664, + "heading": 0.8760133154282924, + "angularVelocity": 1.3296244511673165, + "velocityX": -2.569782446719601, + "velocityY": -2.4704740926033413, + "timestamp": 0.725225161731685 }, { "x": 9.536823272705078, "y": 6.212279796600342, - "heading": 0.6304013989449005, - "angularVelocity": 0.0467730511631427, - "velocityX": -2.86107112724699, - "velocityY": -2.817648630740343, - "timestamp": 0.735181067825257 - }, - { - "x": 9.40747136536471, - "y": 6.046554907280894, - "heading": 0.6312946793268718, - "angularVelocity": 0.01710285270996669, - "velocityX": -2.476587042148973, - "velocityY": -3.172988492315768, - "timestamp": 0.7874109733924533 - }, - { - "x": 9.300530012868803, - "y": 5.865158084455799, - "heading": 0.6312947993803139, - "angularVelocity": 0.0000022985575187704037, - "velocityX": -2.0475118868118565, - "velocityY": -3.4730451999709184, - "timestamp": 0.8396408789596495 - }, - { - "x": 9.217999283134946, - "y": 5.6714315991563975, - "heading": 0.631294847742659, - "angularVelocity": 9.259512276328381e-7, - "velocityX": -1.5801431926310214, - "velocityY": -3.7091103879204836, - "timestamp": 0.8918707845268457 - }, - { - "x": 9.161271222388658, - "y": 5.468643047045803, - "heading": 0.6312949083428273, - "angularVelocity": 0.00000116025804741646, - "velocityX": -1.0861222154286778, - "velocityY": -3.882613799668738, - "timestamp": 0.9441006900940419 - }, - { - "x": 9.131676641005107, - "y": 5.267072350448047, - "heading": 0.6455753422536832, - "angularVelocity": 0.273414890488045, - "velocityX": -0.5666213840932584, - "velocityY": -3.8592965928001113, - "timestamp": 0.996330595661238 - }, - { - "x": 9.117872673375993, - "y": 5.085803506042164, - "heading": 0.7054966882434258, - "angularVelocity": 1.1472612354745892, - "velocityX": -0.2642924102422165, - "velocityY": -3.4705949098965836, - "timestamp": 1.0485605012284342 - }, - { - "x": 9.112327598986647, - "y": 4.928252929148819, - "heading": 0.8165929141384998, - "angularVelocity": 2.127061588348913, - "velocityX": -0.10616665546460329, - "velocityY": -3.0164821318819866, - "timestamp": 1.1007904067956304 - }, - { - "x": 9.111030628381306, - "y": 4.796361729914607, - "heading": 0.967191729179588, - "angularVelocity": 2.8833828705153617, - "velocityX": -0.024831953864649082, - "velocityY": -2.5252046275387223, - "timestamp": 1.1530203123628266 - }, - { - "x": 9.111245919986635, - "y": 4.691776297697822, - "heading": 1.1173854949370192, - "angularVelocity": 2.875627748631485, - "velocityX": 0.004121998749548773, - "velocityY": -2.002405156222834, - "timestamp": 1.2052502179300228 - }, - { - "x": 9.11196538752937, - "y": 4.614168205295782, - "heading": 1.2463793467539295, - "angularVelocity": 2.469731668401231, - "velocityX": 0.013775011362305339, - "velocityY": -1.4858937912920456, - "timestamp": 1.257480123497219 - }, - { - "x": 9.112681926635897, - "y": 4.562954142521823, - "heading": 1.3406788657665367, - "angularVelocity": 1.8054698355004044, - "velocityX": 0.013718943174322498, - "velocityY": -0.9805505527490918, - "timestamp": 1.3097100290644152 - }, - { - "x": 9.113101959228516, - "y": 4.53757381439209, + "heading": 0.9431692146044316, + "angularVelocity": 1.3890010176105514, + "velocityX": -2.2171299202690298, + "velocityY": -2.8025782339260843, + "timestamp": 0.7735735058471307 + }, + { + "x": 9.430745921751914, + "y": 6.018891391860547, + "heading": 1.0161761837183603, + "angularVelocity": 1.2029114308437592, + "velocityX": -1.7478010601434386, + "velocityY": -3.1863961136520125, + "timestamp": 0.8342653965732162 + }, + { + "x": 9.355607279894146, + "y": 5.806111268245478, + "heading": 1.0689458128041136, + "angularVelocity": 0.8694675426065267, + "velocityX": -1.2380342902298767, + "velocityY": -3.505906984763242, + "timestamp": 0.8949572872993017 + }, + { + "x": 9.314003494910018, + "y": 5.578711557562336, + "heading": 1.103303372284574, + "angularVelocity": 0.5660980251138138, + "velocityX": -0.6854916610176038, + "velocityY": -3.746789034954244, + "timestamp": 0.9556491780253872 + }, + { + "x": 9.308988486748726, + "y": 5.346516543470362, + "heading": 1.1325554053485514, + "angularVelocity": 0.481975972638548, + "velocityX": -0.08263061343597497, + "velocityY": -3.8257996466105637, + "timestamp": 1.0163410687514727 + }, + { + "x": 9.335752490116109, + "y": 5.1322569060829615, + "heading": 1.1866325336002834, + "angularVelocity": 0.8910107693924489, + "velocityX": 0.44098153883827, + "velocityY": -3.530284438729908, + "timestamp": 1.0770329594775583 + }, + { + "x": 9.37362677314899, + "y": 4.953188872464991, + "heading": 1.2470437020271732, + "angularVelocity": 0.9953746324947627, + "velocityX": 0.6240419037824511, + "velocityY": -2.9504441446082046, + "timestamp": 1.137724850203644 + }, + { + "x": 9.410828568507965, + "y": 4.811020644344707, + "heading": 1.3015321362189178, + "angularVelocity": 0.8977877199058055, + "velocityX": 0.6129615491280562, + "velocityY": -2.3424583814978592, + "timestamp": 1.1984167409297295 + }, + { + "x": 9.442024749275184, + "y": 4.705213488198149, + "heading": 1.345141357678761, + "angularVelocity": 0.7185345676024537, + "velocityX": 0.5140090446023834, + "velocityY": -1.7433491506154644, + "timestamp": 1.259108631655815 + }, + { + "x": 9.464368518697883, + "y": 4.635140949017796, + "heading": 1.3754091619410362, + "angularVelocity": 0.49871249519774746, + "velocityX": 0.36815082139289257, + "velocityY": -1.154561809527337, + "timestamp": 1.3198005223819007 + }, + { + "x": 9.47612190246582, + "y": 4.60029935836792, "heading": 1.3909420707626845, - "angularVelocity": 0.9623453163529443, - "velocityX": 0.008041994105460236, - "velocityY": -0.48593478877875557, - "timestamp": 1.3619399346316114 + "angularVelocity": 0.2559305474886485, + "velocityX": 0.19365657631241665, + "velocityY": -0.5740732449269984, + "timestamp": 1.3804924131079863 }, { - "x": 9.113101959228516, - "y": 4.53757381439209, + "x": 9.47612190246582, + "y": 4.60029935836792, "heading": 1.3909420707626845, - "angularVelocity": 1.3615947899255843e-21, - "velocityX": 7.756185722534006e-21, - "velocityY": 3.1945663211699707e-21, - "timestamp": 1.4141698401988076 + "angularVelocity": -2.1753265335064506e-21, + "velocityX": 3.063926461949092e-22, + "velocityY": 1.462560101380818e-21, + "timestamp": 1.441184303834072 } ], "trajectoryWaypoints": [ @@ -3713,24 +4003,24 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 14 + "controlIntervalCount": 16 }, { - "timestamp": 0.735181067825257, + "timestamp": 0.7735735058471307, "isStopPoint": false, "x": 9.536823272705078, "y": 6.212279796600342, - "heading": 0.6304013989449005, + "heading": 0.9431692146044316, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 13 + "controlIntervalCount": 11 }, { - "timestamp": 1.4141698401988076, + "timestamp": 1.441184303834072, "isStopPoint": true, - "x": 9.113101959228516, - "y": 4.53757381439209, + "x": 9.47612190246582, + "y": 4.60029935836792, "heading": 1.3909420707626843, "isInitialGuess": false, "translationConstrained": true, @@ -3756,7 +4046,21 @@ "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, "circleObstacles": [], - "eventMarkers": [], + "eventMarkers": [ + { + "name": "intake", + "target": 1, + "trajTargetIndex": 1, + "targetTimestamp": 0.7735735058471307, + "offset": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ], "isTrajectoryStale": false }, "Red 6 to CWS": { @@ -3764,11 +4068,11 @@ { "x": 9.165976524353027, "y": 4.114032745361328, - "heading": 0.019227672507796556, + "heading": 1.5776176711763843, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 17 }, { "x": 9.472442626953125, @@ -3793,200 +4097,272 @@ { "x": 9.165976524353027, "y": 4.114032745361328, - "heading": 0.019227672507796556, - "angularVelocity": -3.1003593284430024e-27, - "velocityX": 1.318659078564603e-26, - "velocityY": 2.316078077174801e-28, + "heading": 1.5776176711763843, + "angularVelocity": -5.58468760766859e-21, + "velocityX": -1.677172172736469e-20, + "velocityY": -6.242016627731418e-21, "timestamp": 0 }, { - "x": 9.16027596499261, - "y": 4.166403545411132, - "heading": 0.012282530601989756, - "angularVelocity": -0.09577565199197573, - "velocityX": -0.07861247428311936, - "velocityY": 0.7222095081912742, - "timestamp": 0.07251469200531965 - }, - { - "x": 9.149102494916367, - "y": 4.27117070053216, - "heading": -0.0014851703204856132, - "angularVelocity": -0.18986084808117745, - "velocityX": -0.15408560344467495, - "velocityY": 1.4447714280209942, - "timestamp": 0.1450293840106393 - }, - { - "x": 9.132833593950462, - "y": 4.428374277760363, - "heading": -0.021876491257742454, - "angularVelocity": -0.2812026138890714, - "velocityX": -0.22435316921309337, - "velocityY": 2.167885884651771, - "timestamp": 0.21754407601595896 - }, - { - "x": 9.112216918047768, - "y": 4.6380853538080355, - "heading": -0.0484894595052067, - "angularVelocity": -0.3670010519456103, - "velocityX": -0.28431032846671456, - "velocityY": 2.891980511098197, - "timestamp": 0.2900587680212786 - }, - { - "x": 9.089430616903869, - "y": 4.900448711996779, - "heading": -0.0801093822030687, - "angularVelocity": -0.4360485002893258, - "velocityX": -0.31423013066410976, - "velocityY": 3.618071744268, - "timestamp": 0.3625734600265983 - }, - { - "x": 9.109885650378216, - "y": 5.192087462357156, - "heading": -0.08010938704614348, - "angularVelocity": -6.678749717871197e-8, - "velocityX": 0.28208122945412023, - "velocityY": 4.02178844445733, - "timestamp": 0.435088152031918 - }, - { - "x": 9.182379800478888, - "y": 5.475312047806293, - "heading": -0.0801093912765566, - "angularVelocity": -5.8338703480250706e-8, - "velocityX": 0.9997167207902494, - "velocityY": 3.9057545114907084, - "timestamp": 0.5076028440372377 - }, - { - "x": 9.304556200250488, - "y": 5.740914171376622, - "heading": -0.08010939802300308, - "angularVelocity": -9.303558071395237e-8, - "velocityX": 1.6848502888578374, - "velocityY": 3.6627353192211642, - "timestamp": 0.5801175360425573 + "x": 9.164796155364039, + "y": 4.128053853665708, + "heading": 1.5381182980542132, + "angularVelocity": -0.9864912857636485, + "velocityX": -0.029479549410119352, + "velocityY": 0.3501752069896988, + "timestamp": 0.04004026562849411 + }, + { + "x": 9.162495402237571, + "y": 4.156489712170952, + "heading": 1.4635174045753923, + "angularVelocity": -1.863146817530908, + "velocityX": -0.05746098559369431, + "velocityY": 0.7101815649578848, + "timestamp": 0.08008053125698822 + }, + { + "x": 9.159060655214276, + "y": 4.199811476239254, + "heading": 1.359950023881047, + "angularVelocity": -2.5865807598599746, + "velocityX": -0.08578232360400655, + "velocityY": 1.08195496178715, + "timestamp": 0.12012079688548233 + }, + { + "x": 9.154362820944076, + "y": 4.258508481897705, + "heading": 1.235236776636678, + "angularVelocity": -3.114695801508823, + "velocityX": -0.1173277498610463, + "velocityY": 1.465949457059093, + "timestamp": 0.16016106251397644 + }, + { + "x": 9.148245807581972, + "y": 4.333003352649315, + "heading": 1.0988055478710193, + "angularVelocity": -3.40735074116415, + "velocityX": -0.152771547995445, + "velocityY": 1.8604989148409636, + "timestamp": 0.20020132814247055 + }, + { + "x": 9.140718433613461, + "y": 4.423506380828508, + "heading": 0.9635080390549446, + "angularVelocity": -3.379036244949183, + "velocityX": -0.1879951056831576, + "velocityY": 2.260300394080245, + "timestamp": 0.24024159377096466 + }, + { + "x": 9.132548850946765, + "y": 4.529323965266172, + "heading": 0.8487168791915694, + "angularVelocity": -2.866893065306883, + "velocityX": -0.2040341775602698, + "velocityY": 2.6427792817240476, + "timestamp": 0.28028185939945877 + }, + { + "x": 9.124926332439184, + "y": 4.64950362338172, + "heading": 0.7639545864905695, + "angularVelocity": -2.1169263332928523, + "velocityX": -0.19037132715629668, + "velocityY": 3.001470050939277, + "timestamp": 0.3203221250279529 + }, + { + "x": 9.118434012961915, + "y": 4.7840493325662905, + "heading": 0.7087764113022048, + "angularVelocity": -1.378067161200295, + "velocityX": -0.1621447654096238, + "velocityY": 3.3602601549350606, + "timestamp": 0.360362390656447 + }, + { + "x": 9.114821619295993, + "y": 4.932875254785403, + "heading": 0.6828478055885947, + "angularVelocity": -0.6475632792795059, + "velocityX": -0.09021902350671948, + "velocityY": 3.7169064661081084, + "timestamp": 0.4004026562849411 + }, + { + "x": 9.119733054745478, + "y": 5.094229573217587, + "heading": 0.6828477010361423, + "angularVelocity": -0.0000026111827875269163, + "velocityX": 0.12266240926382038, + "velocityY": 4.029801398657725, + "timestamp": 0.4404429219134352 + }, + { + "x": 9.140665009898335, + "y": 5.254295817851585, + "heading": 0.6828476668832145, + "angularVelocity": -8.529645663166833e-7, + "velocityX": 0.522772634603688, + "velocityY": 3.9976319367908455, + "timestamp": 0.4804831875419293 + }, + { + "x": 9.177410010476544, + "y": 5.411487271920564, + "heading": 0.6828476246296896, + "angularVelocity": -0.0000010552758389959, + "velocityX": 0.9177012190492992, + "velocityY": 3.925834446937349, + "timestamp": 0.5205234531704235 + }, + { + "x": 9.229603825061764, + "y": 5.564245747672749, + "heading": 0.6828475667567384, + "angularVelocity": -0.0000014453688142033488, + "velocityX": 1.3035331750660322, + "velocityY": 3.8151214372438793, + "timestamp": 0.5605637187989176 + }, + { + "x": 9.2967290739306, + "y": 5.711056988859694, + "heading": 0.6828474589270388, + "angularVelocity": -0.0000026930315748324016, + "velocityX": 1.6764436452933849, + "velocityY": 3.6665900908142905, + "timestamp": 0.6006039844274118 + }, + { + "x": 9.378040037181789, + "y": 5.850313712944271, + "heading": 0.6824260962947332, + "angularVelocity": -0.01052347245183362, + "velocityX": 2.03072986592535, + "velocityY": 3.477917089177032, + "timestamp": 0.640644250055906 }, { "x": 9.472442626953123, "y": 5.980258464813232, - "heading": -0.0801094140047848, - "angularVelocity": -2.2039370606335216e-7, - "velocityX": 2.3152056784619393, - "velocityY": 3.3006317315538127, - "timestamp": 0.652632228047877 - }, - { - "x": 9.690930195145093, - "y": 6.192521851895066, - "heading": -0.0801094275269848, - "angularVelocity": -1.7896807096367723e-7, - "velocityX": 2.891711304942813, - "velocityY": 2.809333460614483, - "timestamp": 0.7281887221621088 - }, - { - "x": 9.945264512695372, - "y": 6.36017218154714, - "heading": -0.0801094347291874, - "angularVelocity": -9.532208551658443e-8, - "velocityX": 3.3661476823654413, - "velocityY": 2.218873858792461, - "timestamp": 0.8037452162763407 - }, - { - "x": 10.226468274983382, - "y": 6.477291899741084, - "heading": -0.08010944009786584, - "angularVelocity": -7.105515558762335e-8, - "velocityX": 3.721768268692626, - "velocityY": 1.5500946618416913, - "timestamp": 0.8793017103905725 - }, - { - "x": 10.524615763044427, - "y": 6.5397471826115465, - "heading": -0.08010944490352531, - "angularVelocity": -6.360352642448382e-8, - "velocityX": 3.9460206770616324, - "velocityY": 0.8266037698366171, - "timestamp": 0.9548582045048043 - }, - { - "x": 10.828899774162563, - "y": 6.554024026243873, - "heading": -0.08010944967830837, - "angularVelocity": -6.319487302459263e-8, - "velocityX": 4.0272383556878255, - "velocityY": 0.18895587731666572, - "timestamp": 1.0304146986190361 - }, - { - "x": 11.133183845275457, - "y": 6.568299591143467, - "heading": -0.08010945445311723, - "angularVelocity": -6.319521471147181e-8, - "velocityX": 4.0272391497262205, - "velocityY": 0.18893895312309195, - "timestamp": 1.105971192733268 - }, - { - "x": 11.418815993462275, - "y": 6.581544555724133, - "heading": -0.1315821813078501, - "angularVelocity": -0.6812482164261439, - "velocityX": 3.7803785304672726, - "velocityY": 0.17529882422338566, - "timestamp": 1.1815276868475 - }, - { - "x": 11.647294799351972, - "y": 6.592156838882461, - "heading": -0.17650291196886067, - "angularVelocity": -0.594531696945813, - "velocityX": 3.023946631831091, - "velocityY": 0.1404549441148474, - "timestamp": 1.257084180961732 - }, - { - "x": 11.818643408754175, - "y": 6.600120231200814, - "heading": -0.21138621074303876, - "angularVelocity": -0.461684983972906, - "velocityX": 2.2678210709875604, - "velocityY": 0.10539653026134421, - "timestamp": 1.3326406750759638 - }, - { - "x": 11.932871296430273, - "y": 6.605430723246171, - "heading": -0.23512693570933194, - "angularVelocity": -0.3142115743274193, - "velocityX": 1.5118209098399886, - "velocityY": 0.07028505104178595, - "timestamp": 1.4081971691901958 + "heading": 0.6802910040496163, + "angularVelocity": -0.053323628392650334, + "velocityX": 2.3576913961339048, + "velocityY": 3.2453518933830967, + "timestamp": 0.6806845156844001 + }, + { + "x": 9.697571132700979, + "y": 6.190348552373689, + "heading": 0.6769837409938244, + "angularVelocity": -0.04315400893213956, + "velocityX": 2.9375339620804057, + "velocityY": 2.7413088593774586, + "timestamp": 0.7573231209410413 + }, + { + "x": 9.959401598982078, + "y": 6.354404479556993, + "heading": 0.6769835985523576, + "angularVelocity": -0.0000018586124631184994, + "velocityX": 3.4164304713571103, + "velocityY": 2.1406434346504692, + "timestamp": 0.8339617261976824 + }, + { + "x": 10.247599106550812, + "y": 6.465812536773354, + "heading": 0.676983552190923, + "angularVelocity": -6.049357816949861e-7, + "velocityX": 3.7604743275747707, + "velocityY": 1.4536806462406935, + "timestamp": 0.9106003314543235 + }, + { + "x": 10.551697598356373, + "y": 6.520526967813223, + "heading": 0.6769835217928931, + "angularVelocity": -3.9664122091915935e-7, + "velocityX": 3.9679544113207434, + "velocityY": 0.7139278025295573, + "timestamp": 0.9872389367109646 + }, + { + "x": 10.860221065632627, + "y": 6.537344592449922, + "heading": 0.6769834953565499, + "angularVelocity": -3.449481242526253e-7, + "velocityX": 4.025692615922479, + "velocityY": 0.21944064065877694, + "timestamp": 1.0638775419676059 + }, + { + "x": 11.1687445405045, + "y": 6.554162077742282, + "heading": 0.6769834689201766, + "angularVelocity": -3.4494851824762934e-7, + "velocityX": 4.025692715031963, + "velocityY": 0.21943882245931226, + "timestamp": 1.140516147224247 + }, + { + "x": 11.442585083138006, + "y": 6.576789374730947, + "heading": 0.6007167845106364, + "angularVelocity": -0.9951470822589282, + "velocityX": 3.5731410001068404, + "velocityY": 0.29524672210419306, + "timestamp": 1.2171547524808883 + }, + { + "x": 11.66406877710657, + "y": 6.593057077284222, + "heading": 0.3953817365180138, + "angularVelocity": -2.679263894547836, + "velocityX": 2.8899755316119893, + "velocityY": 0.21226511754568214, + "timestamp": 1.2937933577375296 + }, + { + "x": 11.829372604042955, + "y": 6.602362161514451, + "heading": 0.11020800066076368, + "angularVelocity": -3.721019385755826, + "velocityX": 2.1569263477960128, + "velocityY": 0.12141510403356627, + "timestamp": 1.3704319629941708 + }, + { + "x": 11.937154802881087, + "y": 6.606659009355426, + "heading": -0.1192351932122035, + "angularVelocity": -2.9938331093660486, + "velocityX": 1.4063695245653058, + "velocityY": 0.05606636272237923, + "timestamp": 1.447070568250812 }, { "x": 11.989983558654783, "y": 6.608086585998535, "heading": -0.2471764305163121, - "angularVelocity": -0.15947662670481846, - "velocityX": 0.7558881985466883, - "velocityY": 0.03515068801828196, - "timestamp": 1.4837536633044277 + "angularVelocity": -1.6694097821283331, + "velocityX": 0.6893230323906269, + "velocityY": 0.01862738287556901, + "timestamp": 1.5237091735074533 }, { "x": 11.989983558654783, "y": 6.608086585998535, "heading": -0.2471764305163121, - "angularVelocity": 4.5808750464576005e-27, - "velocityX": 4.658708906964682e-26, - "velocityY": 1.1001600166410631e-26, - "timestamp": 1.5593101574186596 + "angularVelocity": -4.255940633526814e-21, + "velocityX": -4.150602396756134e-21, + "velocityY": -2.2857309193220108e-20, + "timestamp": 1.6003477787640945 } ], "trajectoryWaypoints": [ @@ -3995,14 +4371,14 @@ "isStopPoint": true, "x": 9.165976524353027, "y": 4.114032745361328, - "heading": 0.019227672507796556, + "heading": 1.5776176711763843, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 17 }, { - "timestamp": 0.652632228047877, + "timestamp": 0.6806845156844001, "isStopPoint": false, "x": 9.472442626953125, "y": 5.980258464813232, @@ -4013,7 +4389,7 @@ "controlIntervalCount": 12 }, { - "timestamp": 1.5593101574186596, + "timestamp": 1.6003477787640945, "isStopPoint": true, "x": 11.989983558654785, "y": 6.608086585998535, diff --git a/src/main/deploy/choreo/Red 4 to 5.1 2.traj b/src/main/deploy/choreo/Red 4 to 5.1 2.traj new file mode 100644 index 00000000..df6244c1 --- /dev/null +++ b/src/main/deploy/choreo/Red 4 to 5.1 2.traj @@ -0,0 +1,140 @@ +{ + "samples": [ + { + "x": 9.111647605895996, + "y": 7.470791816711426, + "heading": -1.05718871216561e-30, + "angularVelocity": 3.118504998701382e-31, + "velocityX": 6.039100561599308e-21, + "velocityY": 1.8009343842569663e-19, + "timestamp": 0 + }, + { + "x": 9.143340108113769, + "y": 7.446687839925814, + "heading": -1.1700310711180642e-18, + "angularVelocity": -1.8565347315473353e-17, + "velocityX": 0.5028775532448245, + "velocityY": -0.3824673983179698, + "timestamp": 0.06302230436271758 + }, + { + "x": 9.20468145119787, + "y": 7.396006382726051, + "heading": 1.8056475843181295e-18, + "angularVelocity": 4.721627840095709e-17, + "velocityX": 0.9733275180014863, + "velocityY": -0.8041828636983419, + "timestamp": 0.12604460872543516 + }, + { + "x": 9.29159191314905, + "y": 7.314802210827562, + "heading": 6.5446419450669345e-18, + "angularVelocity": 7.519550919609085e-17, + "velocityX": 1.3790429091743241, + "velocityY": -1.2884989325681133, + "timestamp": 0.18906691308815274 + }, + { + "x": 9.394024992378341, + "y": 7.196931108324088, + "heading": 1.0466872659140665e-17, + "angularVelocity": 6.223559712325196e-17, + "velocityX": 1.6253464589248696, + "velocityY": -1.8703077219217359, + "timestamp": 0.2520892174508703 + }, + { + "x": 9.486406073668979, + "y": 7.040532454821936, + "heading": 9.387684387047981e-18, + "angularVelocity": -1.7123910513967864e-17, + "velocityX": 1.465847404737292, + "velocityY": -2.4816397160349983, + "timestamp": 0.3151115218135879 + }, + { + "x": 9.544986688827217, + "y": 6.8630875726594285, + "heading": 8.793699397125095e-18, + "angularVelocity": -9.424997001991433e-18, + "velocityX": 0.929521948627871, + "velocityY": -2.815588607181346, + "timestamp": 0.37813382617630553 + }, + { + "x": 9.564400672912598, + "y": 6.678473949432373, + "heading": 6.026195272855988e-18, + "angularVelocity": -4.391308904044169e-17, + "velocityX": 0.3080494164990886, + "velocityY": -2.9293378763945945, + "timestamp": 0.44115613053902314 + }, + { + "x": 9.533920060898456, + "y": 6.465337858341574, + "heading": 3.2791995300552913e-18, + "angularVelocity": -3.783392353465437e-17, + "velocityX": -0.41980447713101565, + "velocityY": -2.9354884749825843, + "timestamp": 0.5137628169242787 + }, + { + "x": 9.451829657549295, + "y": 6.26357893370136, + "heading": 6.745548569249782e-18, + "angularVelocity": 4.7741455890632045e-17, + "velocityX": -1.130617680491636, + "velocityY": -2.778792624830332, + "timestamp": 0.5863695033095343 + }, + { + "x": 9.332723735038355, + "y": 6.099540834625063, + "heading": 4.720380341741389e-18, + "angularVelocity": -2.7892309176467646e-17, + "velocityX": -1.640426363475722, + "velocityY": -2.2592698722790585, + "timestamp": 0.6589761896947899 + }, + { + "x": 9.221886486725465, + "y": 5.987700806646064, + "heading": -2.9444142095504363e-18, + "angularVelocity": -1.0556595929632475e-16, + "velocityX": -1.5265432679955857, + "velocityY": -1.5403543880980908, + "timestamp": 0.7315828760800455 + }, + { + "x": 9.14193306171042, + "y": 5.918746675175855, + "heading": -1.587123146449334e-18, + "angularVelocity": 1.8693746948193755e-17, + "velocityX": -1.1011854278925208, + "velocityY": -0.9496939593725585, + "timestamp": 0.804189562465301 + }, + { + "x": 9.100329399108888, + "y": 5.88615608215332, + "heading": 2.2037577276388683e-30, + "angularVelocity": 2.185918820959246e-17, + "velocityX": -0.5730004311286885, + "velocityY": -0.44886489998548856, + "timestamp": 0.8767962488505566 + }, + { + "x": 9.100329399108888, + "y": 5.88615608215332, + "heading": 1.055345960195893e-30, + "angularVelocity": -1.2817805459557936e-30, + "velocityX": -1.5709732296828935e-20, + "velocityY": 3.1336350844643724e-19, + "timestamp": 0.9494029352358122 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 4 to 5.1.traj b/src/main/deploy/choreo/Red 4 to 5.1.traj index df6244c1..0b123a63 100644 --- a/src/main/deploy/choreo/Red 4 to 5.1.traj +++ b/src/main/deploy/choreo/Red 4 to 5.1.traj @@ -1,140 +1,177 @@ { "samples": [ { - "x": 9.111647605895996, - "y": 7.470791816711426, - "heading": -1.05718871216561e-30, - "angularVelocity": 3.118504998701382e-31, - "velocityX": 6.039100561599308e-21, - "velocityY": 1.8009343842569663e-19, + "x": 9.570359230041504, + "y": 7.533205986022949, + "heading": 2.837553697468469e-27, + "angularVelocity": 1.2858073985093775e-27, + "velocityX": 3.929456586583098e-19, + "velocityY": 1.3369892836254632e-18, "timestamp": 0 }, { - "x": 9.143340108113769, - "y": 7.446687839925814, - "heading": -1.1700310711180642e-18, - "angularVelocity": -1.8565347315473353e-17, - "velocityX": 0.5028775532448245, - "velocityY": -0.3824673983179698, - "timestamp": 0.06302230436271758 + "x": 9.578782459095901, + "y": 7.491411310315301, + "heading": 4.923887190188839e-18, + "angularVelocity": 7.550337998258628e-17, + "velocityX": 0.12916263916652715, + "velocityY": -0.6408837492897412, + "timestamp": 0.06521412932371412 }, { - "x": 9.20468145119787, - "y": 7.396006382726051, - "heading": 1.8056475843181295e-18, - "angularVelocity": 4.721627840095709e-17, - "velocityX": 0.9733275180014863, - "velocityY": -0.8041828636983419, - "timestamp": 0.12604460872543516 + "x": 9.593047663479659, + "y": 7.407383744923941, + "heading": 8.7575474267071e-18, + "angularVelocity": 5.878573045312757e-17, + "velocityX": 0.21874407481462338, + "velocityY": -1.2884871156411322, + "timestamp": 0.13042825864742824 }, { - "x": 9.29159191314905, - "y": 7.314802210827562, - "heading": 6.5446419450669345e-18, - "angularVelocity": 7.519550919609085e-17, - "velocityX": 1.3790429091743241, - "velocityY": -1.2884989325681133, - "timestamp": 0.18906691308815274 + "x": 9.609050762578377, + "y": 7.280756587054724, + "heading": 1.5830775069210892e-17, + "angularVelocity": 1.0846158204666518e-16, + "velocityX": 0.24539312668395535, + "velocityY": -1.9417135394180687, + "timestamp": 0.19564238797114236 }, { - "x": 9.394024992378341, - "y": 7.196931108324088, - "heading": 1.0466872659140665e-17, - "angularVelocity": 6.223559712325196e-17, - "velocityX": 1.6253464589248696, - "velocityY": -1.8703077219217359, - "timestamp": 0.2520892174508703 + "x": 9.619581483588787, + "y": 7.111847063024372, + "heading": 1.351977297182082e-17, + "angularVelocity": -3.543713803053726e-17, + "velocityX": 0.16147913220059756, + "velocityY": -2.5900755830367284, + "timestamp": 0.2608565172948565 }, { - "x": 9.486406073668979, - "y": 7.040532454821936, - "heading": 9.387684387047981e-18, - "angularVelocity": -1.7123910513967864e-17, - "velocityX": 1.465847404737292, - "velocityY": -2.4816397160349983, - "timestamp": 0.3151115218135879 - }, - { - "x": 9.544986688827217, - "y": 6.8630875726594285, - "heading": 8.793699397125095e-18, - "angularVelocity": -9.424997001991433e-18, - "velocityX": 0.929521948627871, - "velocityY": -2.815588607181346, - "timestamp": 0.37813382617630553 + "x": 9.610988346334986, + "y": 6.904832116628309, + "heading": 1.1213563371751803e-17, + "angularVelocity": -3.536364929500549e-17, + "velocityX": -0.131768028537904, + "velocityY": -3.174387951550641, + "timestamp": 0.3260706466185706 }, { "x": 9.564400672912598, "y": 6.678473949432373, - "heading": 6.026195272855988e-18, - "angularVelocity": -4.391308904044169e-17, - "velocityX": 0.3080494164990886, - "velocityY": -2.9293378763945945, - "timestamp": 0.44115613053902314 - }, - { - "x": 9.533920060898456, - "y": 6.465337858341574, - "heading": 3.2791995300552913e-18, - "angularVelocity": -3.783392353465437e-17, - "velocityX": -0.41980447713101565, - "velocityY": -2.9354884749825843, - "timestamp": 0.5137628169242787 - }, - { - "x": 9.451829657549295, - "y": 6.26357893370136, - "heading": 6.745548569249782e-18, - "angularVelocity": 4.7741455890632045e-17, - "velocityX": -1.130617680491636, - "velocityY": -2.778792624830332, - "timestamp": 0.5863695033095343 - }, - { - "x": 9.332723735038355, - "y": 6.099540834625063, - "heading": 4.720380341741389e-18, - "angularVelocity": -2.7892309176467646e-17, - "velocityX": -1.640426363475722, - "velocityY": -2.2592698722790585, - "timestamp": 0.6589761896947899 - }, - { - "x": 9.221886486725465, - "y": 5.987700806646064, - "heading": -2.9444142095504363e-18, - "angularVelocity": -1.0556595929632475e-16, - "velocityX": -1.5265432679955857, - "velocityY": -1.5403543880980908, - "timestamp": 0.7315828760800455 - }, - { - "x": 9.14193306171042, - "y": 5.918746675175855, - "heading": -1.587123146449334e-18, - "angularVelocity": 1.8693746948193755e-17, - "velocityX": -1.1011854278925208, - "velocityY": -0.9496939593725585, - "timestamp": 0.804189562465301 - }, - { - "x": 9.100329399108888, - "y": 5.88615608215332, - "heading": 2.2037577276388683e-30, - "angularVelocity": 2.185918820959246e-17, - "velocityX": -0.5730004311286885, - "velocityY": -0.44886489998548856, - "timestamp": 0.8767962488505566 - }, - { - "x": 9.100329399108888, - "y": 5.88615608215332, - "heading": 1.055345960195893e-30, - "angularVelocity": -1.2817805459557936e-30, - "velocityX": -1.5709732296828935e-20, - "velocityY": 3.1336350844643724e-19, - "timestamp": 0.9494029352358122 + "heading": -3.0718939053161175e-18, + "angularVelocity": -2.1905463476658995e-16, + "velocityX": -0.7143800569832521, + "velocityY": -3.4709988394129123, + "timestamp": 0.3912847759422847 + }, + { + "x": 9.486888499519376, + "y": 6.477915488845292, + "heading": -2.382566973004584e-17, + "angularVelocity": -3.487118187109928e-16, + "velocityX": -1.3023852218818446, + "velocityY": -3.3698497120818054, + "timestamp": 0.4508003264037865 + }, + { + "x": 9.388684393644013, + "y": 6.306214692673101, + "heading": -2.812446729719055e-17, + "angularVelocity": -7.22298214874531e-17, + "velocityX": -1.6500579279508787, + "velocityY": -2.8849736722717, + "timestamp": 0.5103158768652882 + }, + { + "x": 9.305060350101293, + "y": 6.166892021614753, + "heading": -2.368886627734559e-17, + "angularVelocity": 7.452843840961614e-17, + "velocityX": -1.4050788893704838, + "velocityY": -2.3409456852536437, + "timestamp": 0.5698314273267899 + }, + { + "x": 9.246133814938752, + "y": 6.053083162641967, + "heading": -1.5812896843887468e-17, + "angularVelocity": 1.3233464831351022e-16, + "velocityX": -0.990103169770027, + "velocityY": -1.9122541603039365, + "timestamp": 0.6293469777882916 + }, + { + "x": 9.21502434791928, + "y": 5.961345379431105, + "heading": -1.0280341318353077e-17, + "angularVelocity": 9.295983120434094e-17, + "velocityX": -0.5227115733323473, + "velocityY": -1.5414086318533518, + "timestamp": 0.6888625282497933 + }, + { + "x": 9.213134765625, + "y": 5.889784812927246, + "heading": -5.475042515337732e-27, + "angularVelocity": 1.727337015993494e-16, + "velocityX": -0.03174938784280189, + "velocityY": -1.2023843507949883, + "timestamp": 0.748378078711295 + }, + { + "x": 9.279972650601232, + "y": 5.82484633107135, + "heading": 1.650291714944183e-17, + "angularVelocity": 1.818917451335665e-16, + "velocityX": 0.7366733685548789, + "velocityY": -0.7157385395219245, + "timestamp": 0.8391074138946035 + }, + { + "x": 9.414002802389295, + "y": 5.807817742851688, + "heading": 4.3811469472123005e-17, + "angularVelocity": 3.009892258719419e-16, + "velocityX": 1.4772526605344272, + "velocityY": -0.1876855835574857, + "timestamp": 0.929836749077912 + }, + { + "x": 9.566226598810443, + "y": 5.87128228242156, + "heading": 1.3002245043408661e-17, + "angularVelocity": -3.395729106505042e-16, + "velocityX": 1.6777792553378463, + "velocityY": 0.6994930519621869, + "timestamp": 1.0205660842612205 + }, + { + "x": 9.641109466552734, + "y": 5.9059624671936035, + "heading": 5.3076276692349445e-27, + "angularVelocity": -1.4330806011987793e-16, + "velocityX": 0.8253435075987224, + "velocityY": 0.38223783632907515, + "timestamp": 1.111295419444529 + }, + { + "x": 9.641109466552734, + "y": 5.9059624671936035, + "heading": 2.6375377363273866e-27, + "angularVelocity": -3.5878359006669536e-28, + "velocityX": -7.830786140194078e-19, + "velocityY": -5.392178903208723e-18, + "timestamp": 1.2020247546278375 } ], - "eventMarkers": [] + "eventMarkers": [ + { + "timestamp": 0.3912847759422847, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 4 to 5.traj b/src/main/deploy/choreo/Red 4 to 5.traj index df6244c1..0b123a63 100644 --- a/src/main/deploy/choreo/Red 4 to 5.traj +++ b/src/main/deploy/choreo/Red 4 to 5.traj @@ -1,140 +1,177 @@ { "samples": [ { - "x": 9.111647605895996, - "y": 7.470791816711426, - "heading": -1.05718871216561e-30, - "angularVelocity": 3.118504998701382e-31, - "velocityX": 6.039100561599308e-21, - "velocityY": 1.8009343842569663e-19, + "x": 9.570359230041504, + "y": 7.533205986022949, + "heading": 2.837553697468469e-27, + "angularVelocity": 1.2858073985093775e-27, + "velocityX": 3.929456586583098e-19, + "velocityY": 1.3369892836254632e-18, "timestamp": 0 }, { - "x": 9.143340108113769, - "y": 7.446687839925814, - "heading": -1.1700310711180642e-18, - "angularVelocity": -1.8565347315473353e-17, - "velocityX": 0.5028775532448245, - "velocityY": -0.3824673983179698, - "timestamp": 0.06302230436271758 + "x": 9.578782459095901, + "y": 7.491411310315301, + "heading": 4.923887190188839e-18, + "angularVelocity": 7.550337998258628e-17, + "velocityX": 0.12916263916652715, + "velocityY": -0.6408837492897412, + "timestamp": 0.06521412932371412 }, { - "x": 9.20468145119787, - "y": 7.396006382726051, - "heading": 1.8056475843181295e-18, - "angularVelocity": 4.721627840095709e-17, - "velocityX": 0.9733275180014863, - "velocityY": -0.8041828636983419, - "timestamp": 0.12604460872543516 + "x": 9.593047663479659, + "y": 7.407383744923941, + "heading": 8.7575474267071e-18, + "angularVelocity": 5.878573045312757e-17, + "velocityX": 0.21874407481462338, + "velocityY": -1.2884871156411322, + "timestamp": 0.13042825864742824 }, { - "x": 9.29159191314905, - "y": 7.314802210827562, - "heading": 6.5446419450669345e-18, - "angularVelocity": 7.519550919609085e-17, - "velocityX": 1.3790429091743241, - "velocityY": -1.2884989325681133, - "timestamp": 0.18906691308815274 + "x": 9.609050762578377, + "y": 7.280756587054724, + "heading": 1.5830775069210892e-17, + "angularVelocity": 1.0846158204666518e-16, + "velocityX": 0.24539312668395535, + "velocityY": -1.9417135394180687, + "timestamp": 0.19564238797114236 }, { - "x": 9.394024992378341, - "y": 7.196931108324088, - "heading": 1.0466872659140665e-17, - "angularVelocity": 6.223559712325196e-17, - "velocityX": 1.6253464589248696, - "velocityY": -1.8703077219217359, - "timestamp": 0.2520892174508703 + "x": 9.619581483588787, + "y": 7.111847063024372, + "heading": 1.351977297182082e-17, + "angularVelocity": -3.543713803053726e-17, + "velocityX": 0.16147913220059756, + "velocityY": -2.5900755830367284, + "timestamp": 0.2608565172948565 }, { - "x": 9.486406073668979, - "y": 7.040532454821936, - "heading": 9.387684387047981e-18, - "angularVelocity": -1.7123910513967864e-17, - "velocityX": 1.465847404737292, - "velocityY": -2.4816397160349983, - "timestamp": 0.3151115218135879 - }, - { - "x": 9.544986688827217, - "y": 6.8630875726594285, - "heading": 8.793699397125095e-18, - "angularVelocity": -9.424997001991433e-18, - "velocityX": 0.929521948627871, - "velocityY": -2.815588607181346, - "timestamp": 0.37813382617630553 + "x": 9.610988346334986, + "y": 6.904832116628309, + "heading": 1.1213563371751803e-17, + "angularVelocity": -3.536364929500549e-17, + "velocityX": -0.131768028537904, + "velocityY": -3.174387951550641, + "timestamp": 0.3260706466185706 }, { "x": 9.564400672912598, "y": 6.678473949432373, - "heading": 6.026195272855988e-18, - "angularVelocity": -4.391308904044169e-17, - "velocityX": 0.3080494164990886, - "velocityY": -2.9293378763945945, - "timestamp": 0.44115613053902314 - }, - { - "x": 9.533920060898456, - "y": 6.465337858341574, - "heading": 3.2791995300552913e-18, - "angularVelocity": -3.783392353465437e-17, - "velocityX": -0.41980447713101565, - "velocityY": -2.9354884749825843, - "timestamp": 0.5137628169242787 - }, - { - "x": 9.451829657549295, - "y": 6.26357893370136, - "heading": 6.745548569249782e-18, - "angularVelocity": 4.7741455890632045e-17, - "velocityX": -1.130617680491636, - "velocityY": -2.778792624830332, - "timestamp": 0.5863695033095343 - }, - { - "x": 9.332723735038355, - "y": 6.099540834625063, - "heading": 4.720380341741389e-18, - "angularVelocity": -2.7892309176467646e-17, - "velocityX": -1.640426363475722, - "velocityY": -2.2592698722790585, - "timestamp": 0.6589761896947899 - }, - { - "x": 9.221886486725465, - "y": 5.987700806646064, - "heading": -2.9444142095504363e-18, - "angularVelocity": -1.0556595929632475e-16, - "velocityX": -1.5265432679955857, - "velocityY": -1.5403543880980908, - "timestamp": 0.7315828760800455 - }, - { - "x": 9.14193306171042, - "y": 5.918746675175855, - "heading": -1.587123146449334e-18, - "angularVelocity": 1.8693746948193755e-17, - "velocityX": -1.1011854278925208, - "velocityY": -0.9496939593725585, - "timestamp": 0.804189562465301 - }, - { - "x": 9.100329399108888, - "y": 5.88615608215332, - "heading": 2.2037577276388683e-30, - "angularVelocity": 2.185918820959246e-17, - "velocityX": -0.5730004311286885, - "velocityY": -0.44886489998548856, - "timestamp": 0.8767962488505566 - }, - { - "x": 9.100329399108888, - "y": 5.88615608215332, - "heading": 1.055345960195893e-30, - "angularVelocity": -1.2817805459557936e-30, - "velocityX": -1.5709732296828935e-20, - "velocityY": 3.1336350844643724e-19, - "timestamp": 0.9494029352358122 + "heading": -3.0718939053161175e-18, + "angularVelocity": -2.1905463476658995e-16, + "velocityX": -0.7143800569832521, + "velocityY": -3.4709988394129123, + "timestamp": 0.3912847759422847 + }, + { + "x": 9.486888499519376, + "y": 6.477915488845292, + "heading": -2.382566973004584e-17, + "angularVelocity": -3.487118187109928e-16, + "velocityX": -1.3023852218818446, + "velocityY": -3.3698497120818054, + "timestamp": 0.4508003264037865 + }, + { + "x": 9.388684393644013, + "y": 6.306214692673101, + "heading": -2.812446729719055e-17, + "angularVelocity": -7.22298214874531e-17, + "velocityX": -1.6500579279508787, + "velocityY": -2.8849736722717, + "timestamp": 0.5103158768652882 + }, + { + "x": 9.305060350101293, + "y": 6.166892021614753, + "heading": -2.368886627734559e-17, + "angularVelocity": 7.452843840961614e-17, + "velocityX": -1.4050788893704838, + "velocityY": -2.3409456852536437, + "timestamp": 0.5698314273267899 + }, + { + "x": 9.246133814938752, + "y": 6.053083162641967, + "heading": -1.5812896843887468e-17, + "angularVelocity": 1.3233464831351022e-16, + "velocityX": -0.990103169770027, + "velocityY": -1.9122541603039365, + "timestamp": 0.6293469777882916 + }, + { + "x": 9.21502434791928, + "y": 5.961345379431105, + "heading": -1.0280341318353077e-17, + "angularVelocity": 9.295983120434094e-17, + "velocityX": -0.5227115733323473, + "velocityY": -1.5414086318533518, + "timestamp": 0.6888625282497933 + }, + { + "x": 9.213134765625, + "y": 5.889784812927246, + "heading": -5.475042515337732e-27, + "angularVelocity": 1.727337015993494e-16, + "velocityX": -0.03174938784280189, + "velocityY": -1.2023843507949883, + "timestamp": 0.748378078711295 + }, + { + "x": 9.279972650601232, + "y": 5.82484633107135, + "heading": 1.650291714944183e-17, + "angularVelocity": 1.818917451335665e-16, + "velocityX": 0.7366733685548789, + "velocityY": -0.7157385395219245, + "timestamp": 0.8391074138946035 + }, + { + "x": 9.414002802389295, + "y": 5.807817742851688, + "heading": 4.3811469472123005e-17, + "angularVelocity": 3.009892258719419e-16, + "velocityX": 1.4772526605344272, + "velocityY": -0.1876855835574857, + "timestamp": 0.929836749077912 + }, + { + "x": 9.566226598810443, + "y": 5.87128228242156, + "heading": 1.3002245043408661e-17, + "angularVelocity": -3.395729106505042e-16, + "velocityX": 1.6777792553378463, + "velocityY": 0.6994930519621869, + "timestamp": 1.0205660842612205 + }, + { + "x": 9.641109466552734, + "y": 5.9059624671936035, + "heading": 5.3076276692349445e-27, + "angularVelocity": -1.4330806011987793e-16, + "velocityX": 0.8253435075987224, + "velocityY": 0.38223783632907515, + "timestamp": 1.111295419444529 + }, + { + "x": 9.641109466552734, + "y": 5.9059624671936035, + "heading": 2.6375377363273866e-27, + "angularVelocity": -3.5878359006669536e-28, + "velocityX": -7.830786140194078e-19, + "velocityY": -5.392178903208723e-18, + "timestamp": 1.2020247546278375 } ], - "eventMarkers": [] + "eventMarkers": [ + { + "timestamp": 0.3912847759422847, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 4 to 6.traj b/src/main/deploy/choreo/Red 4 to 6.traj new file mode 100644 index 00000000..df6244c1 --- /dev/null +++ b/src/main/deploy/choreo/Red 4 to 6.traj @@ -0,0 +1,140 @@ +{ + "samples": [ + { + "x": 9.111647605895996, + "y": 7.470791816711426, + "heading": -1.05718871216561e-30, + "angularVelocity": 3.118504998701382e-31, + "velocityX": 6.039100561599308e-21, + "velocityY": 1.8009343842569663e-19, + "timestamp": 0 + }, + { + "x": 9.143340108113769, + "y": 7.446687839925814, + "heading": -1.1700310711180642e-18, + "angularVelocity": -1.8565347315473353e-17, + "velocityX": 0.5028775532448245, + "velocityY": -0.3824673983179698, + "timestamp": 0.06302230436271758 + }, + { + "x": 9.20468145119787, + "y": 7.396006382726051, + "heading": 1.8056475843181295e-18, + "angularVelocity": 4.721627840095709e-17, + "velocityX": 0.9733275180014863, + "velocityY": -0.8041828636983419, + "timestamp": 0.12604460872543516 + }, + { + "x": 9.29159191314905, + "y": 7.314802210827562, + "heading": 6.5446419450669345e-18, + "angularVelocity": 7.519550919609085e-17, + "velocityX": 1.3790429091743241, + "velocityY": -1.2884989325681133, + "timestamp": 0.18906691308815274 + }, + { + "x": 9.394024992378341, + "y": 7.196931108324088, + "heading": 1.0466872659140665e-17, + "angularVelocity": 6.223559712325196e-17, + "velocityX": 1.6253464589248696, + "velocityY": -1.8703077219217359, + "timestamp": 0.2520892174508703 + }, + { + "x": 9.486406073668979, + "y": 7.040532454821936, + "heading": 9.387684387047981e-18, + "angularVelocity": -1.7123910513967864e-17, + "velocityX": 1.465847404737292, + "velocityY": -2.4816397160349983, + "timestamp": 0.3151115218135879 + }, + { + "x": 9.544986688827217, + "y": 6.8630875726594285, + "heading": 8.793699397125095e-18, + "angularVelocity": -9.424997001991433e-18, + "velocityX": 0.929521948627871, + "velocityY": -2.815588607181346, + "timestamp": 0.37813382617630553 + }, + { + "x": 9.564400672912598, + "y": 6.678473949432373, + "heading": 6.026195272855988e-18, + "angularVelocity": -4.391308904044169e-17, + "velocityX": 0.3080494164990886, + "velocityY": -2.9293378763945945, + "timestamp": 0.44115613053902314 + }, + { + "x": 9.533920060898456, + "y": 6.465337858341574, + "heading": 3.2791995300552913e-18, + "angularVelocity": -3.783392353465437e-17, + "velocityX": -0.41980447713101565, + "velocityY": -2.9354884749825843, + "timestamp": 0.5137628169242787 + }, + { + "x": 9.451829657549295, + "y": 6.26357893370136, + "heading": 6.745548569249782e-18, + "angularVelocity": 4.7741455890632045e-17, + "velocityX": -1.130617680491636, + "velocityY": -2.778792624830332, + "timestamp": 0.5863695033095343 + }, + { + "x": 9.332723735038355, + "y": 6.099540834625063, + "heading": 4.720380341741389e-18, + "angularVelocity": -2.7892309176467646e-17, + "velocityX": -1.640426363475722, + "velocityY": -2.2592698722790585, + "timestamp": 0.6589761896947899 + }, + { + "x": 9.221886486725465, + "y": 5.987700806646064, + "heading": -2.9444142095504363e-18, + "angularVelocity": -1.0556595929632475e-16, + "velocityX": -1.5265432679955857, + "velocityY": -1.5403543880980908, + "timestamp": 0.7315828760800455 + }, + { + "x": 9.14193306171042, + "y": 5.918746675175855, + "heading": -1.587123146449334e-18, + "angularVelocity": 1.8693746948193755e-17, + "velocityX": -1.1011854278925208, + "velocityY": -0.9496939593725585, + "timestamp": 0.804189562465301 + }, + { + "x": 9.100329399108888, + "y": 5.88615608215332, + "heading": 2.2037577276388683e-30, + "angularVelocity": 2.185918820959246e-17, + "velocityX": -0.5730004311286885, + "velocityY": -0.44886489998548856, + "timestamp": 0.8767962488505566 + }, + { + "x": 9.100329399108888, + "y": 5.88615608215332, + "heading": 1.055345960195893e-30, + "angularVelocity": -1.2817805459557936e-30, + "velocityX": -1.5709732296828935e-20, + "velocityY": 3.1336350844643724e-19, + "timestamp": 0.9494029352358122 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 4 to CWS 2.traj b/src/main/deploy/choreo/Red 4 to CWS 2.traj new file mode 100644 index 00000000..d0eabd38 --- /dev/null +++ b/src/main/deploy/choreo/Red 4 to CWS 2.traj @@ -0,0 +1,122 @@ +{ + "samples": [ + { + "x": 9.1292085647583, + "y": 7.463241577148437, + "heading": -1.667402920110202e-23, + "angularVelocity": -7.105893464585413e-24, + "velocityX": 1.9107951304231947e-24, + "velocityY": -3.9513703650294445e-24, + "timestamp": 0 + }, + { + "x": 9.202228802975283, + "y": 7.43643053208001, + "heading": -0.010601683346708838, + "angularVelocity": -0.12032963165578175, + "velocityX": 0.8287833243759717, + "velocityY": -0.30430669091732365, + "timestamp": 0.08810534197458779 + }, + { + "x": 9.348269051180246, + "y": 7.382807931423719, + "heading": -0.03184320758536198, + "angularVelocity": -0.24109235334197815, + "velocityX": 1.6575640583415123, + "velocityY": -0.6086191762555923, + "timestamp": 0.17621068394917558 + }, + { + "x": 9.567328874222518, + "y": 7.302373052328402, + "heading": -0.06379484985933483, + "angularVelocity": -0.362652724090086, + "velocityX": 2.4863398533252936, + "velocityY": -0.9129398659903866, + "timestamp": 0.26431602592376335 + }, + { + "x": 9.859407635240762, + "y": 7.195124920858945, + "heading": -0.10655016324134052, + "angularVelocity": -0.4852749268521946, + "velocityX": 3.315108419901344, + "velocityY": -1.2172716099370104, + "timestamp": 0.35242136789835116 + }, + { + "x": 10.192837587328619, + "y": 7.072651571321672, + "heading": -0.10655016599578679, + "angularVelocity": -3.126310170419999e-8, + "velocityX": 3.7844464888863056, + "velocityY": -1.3900785899292845, + "timestamp": 0.44052670987293896 + }, + { + "x": 10.52626753919339, + "y": 6.950178221176961, + "heading": -0.1065501687502156, + "angularVelocity": -3.126290359484911e-8, + "velocityX": 3.7844464863542937, + "velocityY": -1.3900785968237475, + "timestamp": 0.5286320518475267 + }, + { + "x": 10.859697491223413, + "y": 6.82770487148224, + "heading": -0.10655017150466216, + "angularVelocity": -3.126310516103417e-8, + "velocityX": 3.7844464882298987, + "velocityY": -1.390078591716332, + "timestamp": 0.6167373938221145 + }, + { + "x": 11.151767702106236, + "y": 6.720442093033459, + "heading": -0.1504495662331604, + "angularVelocity": -0.4982603068626659, + "velocityX": 3.315011375440373, + "velocityY": -1.2174378538785793, + "timestamp": 0.7048427357967022 + }, + { + "x": 11.37082008036848, + "y": 6.6399944366452095, + "heading": -0.1834043776397133, + "angularVelocity": -0.3740387434857022, + "velocityX": 2.486255354702837, + "velocityY": -0.9130848888987183, + "timestamp": 0.79294807777129 + }, + { + "x": 11.516854922707223, + "y": 6.586362464735785, + "heading": -0.20538061595377177, + "angularVelocity": -0.2494313945276675, + "velocityX": 1.657502701491852, + "velocityY": -0.6087255404433256, + "timestamp": 0.8810534197458777 + }, + { + "x": 11.589872360229492, + "y": 6.55954647064209, + "heading": -0.2163660265240023, + "angularVelocity": -0.1246849546693668, + "velocityX": 0.8287515363521442, + "velocityY": -0.3043628625995168, + "timestamp": 0.9691587617204654 + }, + { + "x": 11.589872360229492, + "y": 6.55954647064209, + "heading": -0.2163660265240023, + "angularVelocity": 1.5342116317474738e-23, + "velocityX": 3.420153298016769e-24, + "velocityY": -5.557275176460609e-24, + "timestamp": 1.0572641036950532 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 4 to CWS.1 2.traj b/src/main/deploy/choreo/Red 4 to CWS.1 2.traj new file mode 100644 index 00000000..d0eabd38 --- /dev/null +++ b/src/main/deploy/choreo/Red 4 to CWS.1 2.traj @@ -0,0 +1,122 @@ +{ + "samples": [ + { + "x": 9.1292085647583, + "y": 7.463241577148437, + "heading": -1.667402920110202e-23, + "angularVelocity": -7.105893464585413e-24, + "velocityX": 1.9107951304231947e-24, + "velocityY": -3.9513703650294445e-24, + "timestamp": 0 + }, + { + "x": 9.202228802975283, + "y": 7.43643053208001, + "heading": -0.010601683346708838, + "angularVelocity": -0.12032963165578175, + "velocityX": 0.8287833243759717, + "velocityY": -0.30430669091732365, + "timestamp": 0.08810534197458779 + }, + { + "x": 9.348269051180246, + "y": 7.382807931423719, + "heading": -0.03184320758536198, + "angularVelocity": -0.24109235334197815, + "velocityX": 1.6575640583415123, + "velocityY": -0.6086191762555923, + "timestamp": 0.17621068394917558 + }, + { + "x": 9.567328874222518, + "y": 7.302373052328402, + "heading": -0.06379484985933483, + "angularVelocity": -0.362652724090086, + "velocityX": 2.4863398533252936, + "velocityY": -0.9129398659903866, + "timestamp": 0.26431602592376335 + }, + { + "x": 9.859407635240762, + "y": 7.195124920858945, + "heading": -0.10655016324134052, + "angularVelocity": -0.4852749268521946, + "velocityX": 3.315108419901344, + "velocityY": -1.2172716099370104, + "timestamp": 0.35242136789835116 + }, + { + "x": 10.192837587328619, + "y": 7.072651571321672, + "heading": -0.10655016599578679, + "angularVelocity": -3.126310170419999e-8, + "velocityX": 3.7844464888863056, + "velocityY": -1.3900785899292845, + "timestamp": 0.44052670987293896 + }, + { + "x": 10.52626753919339, + "y": 6.950178221176961, + "heading": -0.1065501687502156, + "angularVelocity": -3.126290359484911e-8, + "velocityX": 3.7844464863542937, + "velocityY": -1.3900785968237475, + "timestamp": 0.5286320518475267 + }, + { + "x": 10.859697491223413, + "y": 6.82770487148224, + "heading": -0.10655017150466216, + "angularVelocity": -3.126310516103417e-8, + "velocityX": 3.7844464882298987, + "velocityY": -1.390078591716332, + "timestamp": 0.6167373938221145 + }, + { + "x": 11.151767702106236, + "y": 6.720442093033459, + "heading": -0.1504495662331604, + "angularVelocity": -0.4982603068626659, + "velocityX": 3.315011375440373, + "velocityY": -1.2174378538785793, + "timestamp": 0.7048427357967022 + }, + { + "x": 11.37082008036848, + "y": 6.6399944366452095, + "heading": -0.1834043776397133, + "angularVelocity": -0.3740387434857022, + "velocityX": 2.486255354702837, + "velocityY": -0.9130848888987183, + "timestamp": 0.79294807777129 + }, + { + "x": 11.516854922707223, + "y": 6.586362464735785, + "heading": -0.20538061595377177, + "angularVelocity": -0.2494313945276675, + "velocityX": 1.657502701491852, + "velocityY": -0.6087255404433256, + "timestamp": 0.8810534197458777 + }, + { + "x": 11.589872360229492, + "y": 6.55954647064209, + "heading": -0.2163660265240023, + "angularVelocity": -0.1246849546693668, + "velocityX": 0.8287515363521442, + "velocityY": -0.3043628625995168, + "timestamp": 0.9691587617204654 + }, + { + "x": 11.589872360229492, + "y": 6.55954647064209, + "heading": -0.2163660265240023, + "angularVelocity": 1.5342116317474738e-23, + "velocityX": 3.420153298016769e-24, + "velocityY": -5.557275176460609e-24, + "timestamp": 1.0572641036950532 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 4 to CWS.1.traj b/src/main/deploy/choreo/Red 4 to CWS.1.traj index d0eabd38..b2486066 100644 --- a/src/main/deploy/choreo/Red 4 to CWS.1.traj +++ b/src/main/deploy/choreo/Red 4 to CWS.1.traj @@ -1,121 +1,112 @@ { "samples": [ { - "x": 9.1292085647583, - "y": 7.463241577148437, - "heading": -1.667402920110202e-23, - "angularVelocity": -7.105893464585413e-24, - "velocityX": 1.9107951304231947e-24, - "velocityY": -3.9513703650294445e-24, + "x": 9.9902982711792, + "y": 7.458646297454834, + "heading": 5.231965390296604e-25, + "angularVelocity": 6.579755902937532e-26, + "velocityX": -8.869878344137844e-27, + "velocityY": 1.2616478819313838e-26, "timestamp": 0 }, { - "x": 9.202228802975283, - "y": 7.43643053208001, - "heading": -0.010601683346708838, - "angularVelocity": -0.12032963165578175, - "velocityX": 0.8287833243759717, - "velocityY": -0.30430669091732365, - "timestamp": 0.08810534197458779 + "x": 10.050789739247952, + "y": 7.428504152821708, + "heading": -0.010882323813907078, + "angularVelocity": -0.13249687678312677, + "velocityX": 0.7365091067124155, + "velocityY": -0.36699330875732633, + "timestamp": 0.08213268175158164 }, { - "x": 9.348269051180246, - "y": 7.382807931423719, - "heading": -0.03184320758536198, - "angularVelocity": -0.24109235334197815, - "velocityX": 1.6575640583415123, - "velocityY": -0.6086191762555923, - "timestamp": 0.17621068394917558 + "x": 10.171772327451892, + "y": 7.368219305255567, + "heading": -0.03269948942306371, + "angularVelocity": -0.2656331821131178, + "velocityX": 1.473013977187104, + "velocityY": -0.7339934150510075, + "timestamp": 0.1642653635031633 }, { - "x": 9.567328874222518, - "y": 7.302373052328402, - "heading": -0.06379484985933483, - "angularVelocity": -0.362652724090086, - "velocityX": 2.4863398533252936, - "velocityY": -0.9129398659903866, - "timestamp": 0.26431602592376335 + "x": 10.353245366410322, + "y": 7.27779102201178, + "heading": -0.06555089127342774, + "angularVelocity": -0.39997965669410784, + "velocityX": 2.2095106976698102, + "velocityY": -1.1010024428192526, + "timestamp": 0.24639804525474493 }, { - "x": 9.859407635240762, - "y": 7.195124920858945, - "heading": -0.10655016324134052, - "angularVelocity": -0.4852749268521946, - "velocityX": 3.315108419901344, - "velocityY": -1.2172716099370104, - "timestamp": 0.35242136789835116 + "x": 10.595207857729005, + "y": 7.157218328320408, + "heading": -0.10957454059477183, + "angularVelocity": -0.536006476136965, + "velocityX": 2.9459952622820897, + "velocityY": -1.4680233388221322, + "timestamp": 0.3285307270063266 }, { - "x": 10.192837587328619, - "y": 7.072651571321672, - "heading": -0.10655016599578679, - "angularVelocity": -3.126310170419999e-8, - "velocityX": 3.7844464888863056, - "velocityY": -1.3900785899292845, - "timestamp": 0.44052670987293896 + "x": 10.89154952082099, + "y": 7.009468977921484, + "heading": -0.10957454363253621, + "angularVelocity": -3.698606117126844e-8, + "velocityX": 3.608084586697149, + "velocityY": -1.7989105828274894, + "timestamp": 0.4106634087579082 }, { - "x": 10.52626753919339, - "y": 6.950178221176961, - "heading": -0.1065501687502156, - "angularVelocity": -3.126290359484911e-8, - "velocityX": 3.7844464863542937, - "velocityY": -1.3900785968237475, - "timestamp": 0.5286320518475267 + "x": 11.187891183520787, + "y": 6.861719626735956, + "heading": -0.10957454667030299, + "angularVelocity": -3.6986090083451744e-8, + "velocityX": 3.608084581922121, + "velocityY": -1.7989105924047368, + "timestamp": 0.4927960905094898 }, { - "x": 10.859697491223413, - "y": 6.82770487148224, - "heading": -0.10655017150466216, - "angularVelocity": -3.126310516103417e-8, - "velocityX": 3.7844464882298987, - "velocityY": -1.390078591716332, - "timestamp": 0.6167373938221145 + "x": 11.429839524035081, + "y": 6.74113159453185, + "heading": -0.1554014401167673, + "angularVelocity": -0.5579617330050446, + "velocityX": 2.9458229702774172, + "velocityY": -1.4682100916762528, + "timestamp": 0.5749287722610714 }, { - "x": 11.151767702106236, - "y": 6.720442093033459, - "heading": -0.1504495662331604, - "angularVelocity": -0.4982603068626659, - "velocityX": 3.315011375440373, - "velocityY": -1.2174378538785793, - "timestamp": 0.7048427357967022 + "x": 11.611300274126046, + "y": 6.650689937948947, + "heading": -0.18982654217252887, + "angularVelocity": -0.41914011964060394, + "velocityX": 2.209361075531547, + "velocityY": -1.1011652688567102, + "timestamp": 0.657061454012653 }, { - "x": 11.37082008036848, - "y": 6.6399944366452095, - "heading": -0.1834043776397133, - "angularVelocity": -0.3740387434857022, - "velocityX": 2.486255354702837, - "velocityY": -0.9130848888987183, - "timestamp": 0.79294807777129 + "x": 11.732273970188768, + "y": 6.5903952606856615, + "heading": -0.21278890540487266, + "angularVelocity": -0.2795764456077998, + "velocityX": 1.4729057116218145, + "velocityY": -0.7341130957546612, + "timestamp": 0.7391941357642346 }, { - "x": 11.516854922707223, - "y": 6.586362464735785, - "heading": -0.20538061595377177, - "angularVelocity": -0.2494313945276675, - "velocityX": 1.657502701491852, - "velocityY": -0.6087255404433256, - "timestamp": 0.8810534197458777 + "x": 11.792760848999023, + "y": 6.560247898101807, + "heading": -0.22426516530504303, + "angularVelocity": -0.13972829883823154, + "velocityX": 0.7364532305569309, + "velocityY": -0.3670568395056038, + "timestamp": 0.8213268175158163 }, { - "x": 11.589872360229492, - "y": 6.55954647064209, - "heading": -0.2163660265240023, - "angularVelocity": -0.1246849546693668, - "velocityX": 0.8287515363521442, - "velocityY": -0.3043628625995168, - "timestamp": 0.9691587617204654 - }, - { - "x": 11.589872360229492, - "y": 6.55954647064209, - "heading": -0.2163660265240023, - "angularVelocity": 1.5342116317474738e-23, - "velocityX": 3.420153298016769e-24, - "velocityY": -5.557275176460609e-24, - "timestamp": 1.0572641036950532 + "x": 11.792760848999023, + "y": 6.560247898101807, + "heading": -0.22426516530504303, + "angularVelocity": 8.181375385736729e-26, + "velocityX": 3.520921012184917e-26, + "velocityY": 4.276560943665093e-27, + "timestamp": 0.9034594992673979 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red 4 to CWS.traj b/src/main/deploy/choreo/Red 4 to CWS.traj index d0eabd38..b2486066 100644 --- a/src/main/deploy/choreo/Red 4 to CWS.traj +++ b/src/main/deploy/choreo/Red 4 to CWS.traj @@ -1,121 +1,112 @@ { "samples": [ { - "x": 9.1292085647583, - "y": 7.463241577148437, - "heading": -1.667402920110202e-23, - "angularVelocity": -7.105893464585413e-24, - "velocityX": 1.9107951304231947e-24, - "velocityY": -3.9513703650294445e-24, + "x": 9.9902982711792, + "y": 7.458646297454834, + "heading": 5.231965390296604e-25, + "angularVelocity": 6.579755902937532e-26, + "velocityX": -8.869878344137844e-27, + "velocityY": 1.2616478819313838e-26, "timestamp": 0 }, { - "x": 9.202228802975283, - "y": 7.43643053208001, - "heading": -0.010601683346708838, - "angularVelocity": -0.12032963165578175, - "velocityX": 0.8287833243759717, - "velocityY": -0.30430669091732365, - "timestamp": 0.08810534197458779 + "x": 10.050789739247952, + "y": 7.428504152821708, + "heading": -0.010882323813907078, + "angularVelocity": -0.13249687678312677, + "velocityX": 0.7365091067124155, + "velocityY": -0.36699330875732633, + "timestamp": 0.08213268175158164 }, { - "x": 9.348269051180246, - "y": 7.382807931423719, - "heading": -0.03184320758536198, - "angularVelocity": -0.24109235334197815, - "velocityX": 1.6575640583415123, - "velocityY": -0.6086191762555923, - "timestamp": 0.17621068394917558 + "x": 10.171772327451892, + "y": 7.368219305255567, + "heading": -0.03269948942306371, + "angularVelocity": -0.2656331821131178, + "velocityX": 1.473013977187104, + "velocityY": -0.7339934150510075, + "timestamp": 0.1642653635031633 }, { - "x": 9.567328874222518, - "y": 7.302373052328402, - "heading": -0.06379484985933483, - "angularVelocity": -0.362652724090086, - "velocityX": 2.4863398533252936, - "velocityY": -0.9129398659903866, - "timestamp": 0.26431602592376335 + "x": 10.353245366410322, + "y": 7.27779102201178, + "heading": -0.06555089127342774, + "angularVelocity": -0.39997965669410784, + "velocityX": 2.2095106976698102, + "velocityY": -1.1010024428192526, + "timestamp": 0.24639804525474493 }, { - "x": 9.859407635240762, - "y": 7.195124920858945, - "heading": -0.10655016324134052, - "angularVelocity": -0.4852749268521946, - "velocityX": 3.315108419901344, - "velocityY": -1.2172716099370104, - "timestamp": 0.35242136789835116 + "x": 10.595207857729005, + "y": 7.157218328320408, + "heading": -0.10957454059477183, + "angularVelocity": -0.536006476136965, + "velocityX": 2.9459952622820897, + "velocityY": -1.4680233388221322, + "timestamp": 0.3285307270063266 }, { - "x": 10.192837587328619, - "y": 7.072651571321672, - "heading": -0.10655016599578679, - "angularVelocity": -3.126310170419999e-8, - "velocityX": 3.7844464888863056, - "velocityY": -1.3900785899292845, - "timestamp": 0.44052670987293896 + "x": 10.89154952082099, + "y": 7.009468977921484, + "heading": -0.10957454363253621, + "angularVelocity": -3.698606117126844e-8, + "velocityX": 3.608084586697149, + "velocityY": -1.7989105828274894, + "timestamp": 0.4106634087579082 }, { - "x": 10.52626753919339, - "y": 6.950178221176961, - "heading": -0.1065501687502156, - "angularVelocity": -3.126290359484911e-8, - "velocityX": 3.7844464863542937, - "velocityY": -1.3900785968237475, - "timestamp": 0.5286320518475267 + "x": 11.187891183520787, + "y": 6.861719626735956, + "heading": -0.10957454667030299, + "angularVelocity": -3.6986090083451744e-8, + "velocityX": 3.608084581922121, + "velocityY": -1.7989105924047368, + "timestamp": 0.4927960905094898 }, { - "x": 10.859697491223413, - "y": 6.82770487148224, - "heading": -0.10655017150466216, - "angularVelocity": -3.126310516103417e-8, - "velocityX": 3.7844464882298987, - "velocityY": -1.390078591716332, - "timestamp": 0.6167373938221145 + "x": 11.429839524035081, + "y": 6.74113159453185, + "heading": -0.1554014401167673, + "angularVelocity": -0.5579617330050446, + "velocityX": 2.9458229702774172, + "velocityY": -1.4682100916762528, + "timestamp": 0.5749287722610714 }, { - "x": 11.151767702106236, - "y": 6.720442093033459, - "heading": -0.1504495662331604, - "angularVelocity": -0.4982603068626659, - "velocityX": 3.315011375440373, - "velocityY": -1.2174378538785793, - "timestamp": 0.7048427357967022 + "x": 11.611300274126046, + "y": 6.650689937948947, + "heading": -0.18982654217252887, + "angularVelocity": -0.41914011964060394, + "velocityX": 2.209361075531547, + "velocityY": -1.1011652688567102, + "timestamp": 0.657061454012653 }, { - "x": 11.37082008036848, - "y": 6.6399944366452095, - "heading": -0.1834043776397133, - "angularVelocity": -0.3740387434857022, - "velocityX": 2.486255354702837, - "velocityY": -0.9130848888987183, - "timestamp": 0.79294807777129 + "x": 11.732273970188768, + "y": 6.5903952606856615, + "heading": -0.21278890540487266, + "angularVelocity": -0.2795764456077998, + "velocityX": 1.4729057116218145, + "velocityY": -0.7341130957546612, + "timestamp": 0.7391941357642346 }, { - "x": 11.516854922707223, - "y": 6.586362464735785, - "heading": -0.20538061595377177, - "angularVelocity": -0.2494313945276675, - "velocityX": 1.657502701491852, - "velocityY": -0.6087255404433256, - "timestamp": 0.8810534197458777 + "x": 11.792760848999023, + "y": 6.560247898101807, + "heading": -0.22426516530504303, + "angularVelocity": -0.13972829883823154, + "velocityX": 0.7364532305569309, + "velocityY": -0.3670568395056038, + "timestamp": 0.8213268175158163 }, { - "x": 11.589872360229492, - "y": 6.55954647064209, - "heading": -0.2163660265240023, - "angularVelocity": -0.1246849546693668, - "velocityX": 0.8287515363521442, - "velocityY": -0.3043628625995168, - "timestamp": 0.9691587617204654 - }, - { - "x": 11.589872360229492, - "y": 6.55954647064209, - "heading": -0.2163660265240023, - "angularVelocity": 1.5342116317474738e-23, - "velocityX": 3.420153298016769e-24, - "velocityY": -5.557275176460609e-24, - "timestamp": 1.0572641036950532 + "x": 11.792760848999023, + "y": 6.560247898101807, + "heading": -0.22426516530504303, + "angularVelocity": 8.181375385736729e-26, + "velocityX": 3.520921012184917e-26, + "velocityY": 4.276560943665093e-27, + "timestamp": 0.9034594992673979 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red 5 to 6.1 2.traj b/src/main/deploy/choreo/Red 5 to 6.1 2.traj new file mode 100644 index 00000000..fb5b3068 --- /dev/null +++ b/src/main/deploy/choreo/Red 5 to 6.1 2.traj @@ -0,0 +1,140 @@ +{ + "samples": [ + { + "x": 9.15098476409912, + "y": 5.797394275665283, + "heading": -4.065640793519508e-29, + "angularVelocity": -3.268176405529047e-29, + "velocityX": 1.157597527355568e-20, + "velocityY": 1.4018882473037506e-19, + "timestamp": 0 + }, + { + "x": 9.192684743588645, + "y": 5.767902935794363, + "heading": 8.806771020594398e-18, + "angularVelocity": 1.23382974768875e-16, + "velocityX": 0.5842172682176147, + "velocityY": -0.4131740645050891, + "timestamp": 0.07137751956005578 + }, + { + "x": 9.273759250876711, + "y": 5.70588047685832, + "heading": 7.597661113768503e-18, + "angularVelocity": -1.6939650399948386e-17, + "velocityX": 1.1358549272625345, + "velocityY": -0.8689354760200882, + "timestamp": 0.14275503912011156 + }, + { + "x": 9.38913435315891, + "y": 5.606014975885555, + "heading": -6.605907220253839e-18, + "angularVelocity": -1.9899218262115755e-16, + "velocityX": 1.6164067201176004, + "velocityY": -1.399116999136334, + "timestamp": 0.21413255868016734 + }, + { + "x": 9.523623783255955, + "y": 5.458786324088157, + "heading": -1.545595323632644e-17, + "angularVelocity": -1.2398926619047138e-16, + "velocityX": 1.8841987074633166, + "velocityY": -2.062675373212134, + "timestamp": 0.2855100782402231 + }, + { + "x": 9.632223213447787, + "y": 5.26753117493535, + "heading": -9.150147904566991e-18, + "angularVelocity": 8.834441437765331e-17, + "velocityX": 1.521479463860705, + "velocityY": -2.679487187725441, + "timestamp": 0.3568875978002789 + }, + { + "x": 9.691761227634146, + "y": 5.062077314595765, + "heading": -6.8541498963009875e-18, + "angularVelocity": 3.216696488819412e-17, + "velocityX": 0.8341283719759774, + "velocityY": -2.878411320621096, + "timestamp": 0.4282651173603347 + }, + { + "x": 9.70022678375244, + "y": 4.856142997741699, + "heading": -9.683146911978938e-18, + "angularVelocity": -3.963428214808329e-17, + "velocityX": 0.1186025540038291, + "velocityY": -2.8851425227921963, + "timestamp": 0.49964263692039046 + }, + { + "x": 9.65307144790573, + "y": 4.645942615589113, + "heading": -5.5462217098690175e-18, + "angularVelocity": 5.5158786569436616e-17, + "velocityX": -0.6287353341669664, + "velocityY": -2.8026607199750178, + "timestamp": 0.5746429324090815 + }, + { + "x": 9.551846573573934, + "y": 4.451755307762366, + "heading": -7.328868694792328e-18, + "angularVelocity": -2.3768530508516856e-17, + "velocityX": -1.3496596736354354, + "velocityY": -2.5891539034802524, + "timestamp": 0.6496432278977725 + }, + { + "x": 9.409128969103543, + "y": 4.29575550701389, + "heading": -1.2264974248358262e-17, + "angularVelocity": -6.58144800800035e-17, + "velocityX": -1.9028938958235035, + "velocityY": -2.0799891484682558, + "timestamp": 0.7246435233864635 + }, + { + "x": 9.274801512075, + "y": 4.195518932576719, + "heading": -1.0315851786469037e-17, + "angularVelocity": 2.598819788396303e-17, + "velocityX": -1.7910257040087993, + "velocityY": -1.3364823936217263, + "timestamp": 0.7996438188751546 + }, + { + "x": 9.178765638394157, + "y": 4.136679072340064, + "heading": -1.7595474741066158e-18, + "angularVelocity": 1.1408360752736715e-16, + "velocityX": -1.2804732708731046, + "velocityY": -0.7845283789014882, + "timestamp": 0.8746441143638456 + }, + { + "x": 9.1292085647583, + "y": 4.109771251678467, + "heading": 8.197183256276926e-29, + "angularVelocity": 2.3460541527021614e-17, + "velocityX": -0.6607583785231516, + "velocityY": -0.3587695286556818, + "timestamp": 0.9496444098525366 + }, + { + "x": 9.1292085647583, + "y": 4.109771251678467, + "heading": 4.0649014975521164e-29, + "angularVelocity": -8.983999410224412e-30, + "velocityX": 2.1617137820797256e-20, + "velocityY": 3.116448033899274e-19, + "timestamp": 1.0246447053412275 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 5 to 6.1.traj b/src/main/deploy/choreo/Red 5 to 6.1.traj index fb5b3068..0ebb9158 100644 --- a/src/main/deploy/choreo/Red 5 to 6.1.traj +++ b/src/main/deploy/choreo/Red 5 to 6.1.traj @@ -1,140 +1,186 @@ { "samples": [ { - "x": 9.15098476409912, - "y": 5.797394275665283, - "heading": -4.065640793519508e-29, - "angularVelocity": -3.268176405529047e-29, - "velocityX": 1.157597527355568e-20, - "velocityY": 1.4018882473037506e-19, + "x": 9.628934860229492, + "y": 5.792125701904297, + "heading": 1.9970068949058212e-28, + "angularVelocity": 6.405261716169362e-29, + "velocityX": 4.955467456720226e-23, + "velocityY": 1.462552204852727e-23, "timestamp": 0 }, { - "x": 9.192684743588645, - "y": 5.767902935794363, - "heading": 8.806771020594398e-18, - "angularVelocity": 1.23382974768875e-16, - "velocityX": 0.5842172682176147, - "velocityY": -0.4131740645050891, - "timestamp": 0.07137751956005578 + "x": 9.64209754532163, + "y": 5.757664815288066, + "heading": 3.6472115225969175e-18, + "angularVelocity": 6.012474053277396e-17, + "velocityX": 0.21698851877782935, + "velocityY": -0.5680920488702936, + "timestamp": 0.0606607444775193 }, { - "x": 9.273759250876711, - "y": 5.70588047685832, - "heading": 7.597661113768503e-18, - "angularVelocity": -1.6939650399948386e-17, - "velocityX": 1.1358549272625345, - "velocityY": -0.8689354760200882, - "timestamp": 0.14275503912011156 + "x": 9.666165788175904, + "y": 5.6879636437386, + "heading": 9.687140289512023e-18, + "angularVelocity": 9.956898516114172e-17, + "velocityX": 0.396768009716635, + "velocityY": -1.1490325769954366, + "timestamp": 0.1213214889550386 }, { - "x": 9.38913435315891, - "y": 5.606014975885555, - "heading": -6.605907220253839e-18, - "angularVelocity": -1.9899218262115755e-16, - "velocityX": 1.6164067201176004, - "velocityY": -1.399116999136334, - "timestamp": 0.21413255868016734 + "x": 9.69754267815799, + "y": 5.5821045909015545, + "heading": 1.7213238235683665e-17, + "angularVelocity": 1.240686709476666e-16, + "velocityX": 0.5172519765845578, + "velocityY": -1.7450997963975958, + "timestamp": 0.1819822334325579 }, { - "x": 9.523623783255955, - "y": 5.458786324088157, - "heading": -1.545595323632644e-17, - "angularVelocity": -1.2398926619047138e-16, - "velocityX": 1.8841987074633166, - "velocityY": -2.062675373212134, - "timestamp": 0.2855100782402231 + "x": 9.729912323289412, + "y": 5.439369761908001, + "heading": 2.899948078854106e-17, + "angularVelocity": 1.9429769044629593e-16, + "velocityX": 0.5336176700471545, + "velocityY": -2.3530016029798486, + "timestamp": 0.2426429779100772 }, { - "x": 9.632223213447787, - "y": 5.26753117493535, - "heading": -9.150147904566991e-18, - "angularVelocity": 8.834441437765331e-17, - "velocityX": 1.521479463860705, - "velocityY": -2.679487187725441, - "timestamp": 0.3568875978002789 + "x": 9.751348345266472, + "y": 5.261403357718118, + "heading": 4.0782917255307413e-17, + "angularVelocity": 1.9425143176374398e-16, + "velocityX": 0.35337551758868835, + "velocityY": -2.9337985499969643, + "timestamp": 0.3033037223875965 }, { - "x": 9.691761227634146, - "y": 5.062077314595765, - "heading": -6.8541498963009875e-18, - "angularVelocity": 3.216696488819412e-17, - "velocityX": 0.8341283719759774, - "velocityY": -2.878411320621096, - "timestamp": 0.4282651173603347 + "x": 9.744181294864156, + "y": 5.060141633962775, + "heading": 4.198809491304761e-17, + "angularVelocity": 1.9867505220008126e-17, + "velocityX": -0.11814972704420049, + "velocityY": -3.3178248221125908, + "timestamp": 0.3639644668651158 }, { "x": 9.70022678375244, "y": 4.856142997741699, - "heading": -9.683146911978938e-18, - "angularVelocity": -3.963428214808329e-17, - "velocityX": 0.1186025540038291, - "velocityY": -2.8851425227921963, - "timestamp": 0.49964263692039046 - }, - { - "x": 9.65307144790573, - "y": 4.645942615589113, - "heading": -5.5462217098690175e-18, - "angularVelocity": 5.5158786569436616e-17, - "velocityX": -0.6287353341669664, - "velocityY": -2.8026607199750178, - "timestamp": 0.5746429324090815 - }, - { - "x": 9.551846573573934, - "y": 4.451755307762366, - "heading": -7.328868694792328e-18, - "angularVelocity": -2.3768530508516856e-17, - "velocityX": -1.3496596736354354, - "velocityY": -2.5891539034802524, - "timestamp": 0.6496432278977725 - }, - { - "x": 9.409128969103543, - "y": 4.29575550701389, - "heading": -1.2264974248358262e-17, - "angularVelocity": -6.58144800800035e-17, - "velocityX": -1.9028938958235035, - "velocityY": -2.0799891484682558, - "timestamp": 0.7246435233864635 - }, - { - "x": 9.274801512075, - "y": 4.195518932576719, - "heading": -1.0315851786469037e-17, - "angularVelocity": 2.598819788396303e-17, - "velocityX": -1.7910257040087993, - "velocityY": -1.3364823936217263, - "timestamp": 0.7996438188751546 - }, - { - "x": 9.178765638394157, - "y": 4.136679072340064, - "heading": -1.7595474741066158e-18, - "angularVelocity": 1.1408360752736715e-16, - "velocityX": -1.2804732708731046, - "velocityY": -0.7845283789014882, - "timestamp": 0.8746441143638456 - }, - { - "x": 9.1292085647583, - "y": 4.109771251678467, - "heading": 8.197183256276926e-29, - "angularVelocity": 2.3460541527021614e-17, - "velocityX": -0.6607583785231516, - "velocityY": -0.3587695286556818, - "timestamp": 0.9496444098525366 - }, - { - "x": 9.1292085647583, - "y": 4.109771251678467, - "heading": 4.0649014975521164e-29, - "angularVelocity": -8.983999410224412e-30, - "velocityX": 2.1617137820797256e-20, - "velocityY": 3.116448033899274e-19, - "timestamp": 1.0246447053412275 + "heading": 4.117789366500127e-17, + "angularVelocity": -1.3356269448868408e-17, + "velocityX": -0.7245956423763401, + "velocityY": -3.3629431682408346, + "timestamp": 0.4246252113426351 + }, + { + "x": 9.62367202292937, + "y": 4.666896180193955, + "heading": 3.883641520117362e-17, + "angularVelocity": -3.959883629943507e-17, + "velocityX": -1.2946860234610496, + "velocityY": -3.2005221756203603, + "timestamp": 0.4837551930029131 + }, + { + "x": 9.520833726243332, + "y": 4.500838515926915, + "heading": 2.7882820869042386e-17, + "angularVelocity": -1.8524602958829465e-16, + "velocityX": -1.7391904038949337, + "velocityY": -2.8083496663519774, + "timestamp": 0.5428851746631911 + }, + { + "x": 9.422837551158624, + "y": 4.369495613480981, + "heading": 1.2414905453524928e-17, + "angularVelocity": -2.6159175059594473e-16, + "velocityX": -1.6573009551691247, + "velocityY": -2.221257283666062, + "timestamp": 0.6020151563234691 + }, + { + "x": 9.348748263717006, + "y": 4.263785158181079, + "heading": 8.266599657957546e-18, + "angularVelocity": -7.01557091454596e-17, + "velocityX": -1.2529901982260223, + "velocityY": -1.7877640467951625, + "timestamp": 0.6611451379837472 + }, + { + "x": 9.30366679578196, + "y": 4.177749774701769, + "heading": 8.751765261420785e-18, + "angularVelocity": 8.205069415908739e-18, + "velocityX": -0.7624130207591719, + "velocityY": -1.4550213117537047, + "timestamp": 0.7202751196440252 + }, + { + "x": 9.289485931396484, + "y": 4.1082587242126465, + "heading": -9.673823625011814e-28, + "angularVelocity": -1.4800892906397257e-16, + "velocityX": -0.23982527961788674, + "velocityY": -1.1752253009035645, + "timestamp": 0.7794051013043033 + }, + { + "x": 9.359415889552718, + "y": 4.032157790510569, + "heading": -1.916769609398054e-18, + "angularVelocity": -1.8826829061024516e-17, + "velocityX": 0.6868636491748086, + "velocityY": -0.7474759946436492, + "timestamp": 0.8812156400338537 + }, + { + "x": 9.52218848262578, + "y": 4.002726888178056, + "heading": 1.6766816122200447e-18, + "angularVelocity": 3.5295473979561707e-17, + "velocityX": 1.5987794103069355, + "velocityY": -0.28907520478496473, + "timestamp": 0.983026178763404 + }, + { + "x": 9.718168604909845, + "y": 4.071759502986741, + "heading": 1.7799244325302356e-18, + "angularVelocity": 1.0140681418433122e-18, + "velocityX": 1.9249492707691656, + "velocityY": 0.6780497939615223, + "timestamp": 1.0848367174929543 + }, + { + "x": 9.815460205078123, + "y": 4.1082587242126465, + "heading": 1.5656793258881755e-27, + "angularVelocity": -1.7482713005339804e-17, + "velocityX": 0.9556142358378643, + "velocityY": 0.35850140546709003, + "timestamp": 1.1866472562225046 + }, + { + "x": 9.815460205078123, + "y": 4.1082587242126465, + "heading": 7.676097442920992e-28, + "angularVelocity": -2.991815747657937e-28, + "velocityX": 3.353769132505619e-22, + "velocityY": 1.0669370220059503e-21, + "timestamp": 1.2884577949520548 } ], - "eventMarkers": [] + "eventMarkers": [ + { + "timestamp": 0.4246252113426351, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 5 to 6.traj b/src/main/deploy/choreo/Red 5 to 6.traj index fb5b3068..0ebb9158 100644 --- a/src/main/deploy/choreo/Red 5 to 6.traj +++ b/src/main/deploy/choreo/Red 5 to 6.traj @@ -1,140 +1,186 @@ { "samples": [ { - "x": 9.15098476409912, - "y": 5.797394275665283, - "heading": -4.065640793519508e-29, - "angularVelocity": -3.268176405529047e-29, - "velocityX": 1.157597527355568e-20, - "velocityY": 1.4018882473037506e-19, + "x": 9.628934860229492, + "y": 5.792125701904297, + "heading": 1.9970068949058212e-28, + "angularVelocity": 6.405261716169362e-29, + "velocityX": 4.955467456720226e-23, + "velocityY": 1.462552204852727e-23, "timestamp": 0 }, { - "x": 9.192684743588645, - "y": 5.767902935794363, - "heading": 8.806771020594398e-18, - "angularVelocity": 1.23382974768875e-16, - "velocityX": 0.5842172682176147, - "velocityY": -0.4131740645050891, - "timestamp": 0.07137751956005578 + "x": 9.64209754532163, + "y": 5.757664815288066, + "heading": 3.6472115225969175e-18, + "angularVelocity": 6.012474053277396e-17, + "velocityX": 0.21698851877782935, + "velocityY": -0.5680920488702936, + "timestamp": 0.0606607444775193 }, { - "x": 9.273759250876711, - "y": 5.70588047685832, - "heading": 7.597661113768503e-18, - "angularVelocity": -1.6939650399948386e-17, - "velocityX": 1.1358549272625345, - "velocityY": -0.8689354760200882, - "timestamp": 0.14275503912011156 + "x": 9.666165788175904, + "y": 5.6879636437386, + "heading": 9.687140289512023e-18, + "angularVelocity": 9.956898516114172e-17, + "velocityX": 0.396768009716635, + "velocityY": -1.1490325769954366, + "timestamp": 0.1213214889550386 }, { - "x": 9.38913435315891, - "y": 5.606014975885555, - "heading": -6.605907220253839e-18, - "angularVelocity": -1.9899218262115755e-16, - "velocityX": 1.6164067201176004, - "velocityY": -1.399116999136334, - "timestamp": 0.21413255868016734 + "x": 9.69754267815799, + "y": 5.5821045909015545, + "heading": 1.7213238235683665e-17, + "angularVelocity": 1.240686709476666e-16, + "velocityX": 0.5172519765845578, + "velocityY": -1.7450997963975958, + "timestamp": 0.1819822334325579 }, { - "x": 9.523623783255955, - "y": 5.458786324088157, - "heading": -1.545595323632644e-17, - "angularVelocity": -1.2398926619047138e-16, - "velocityX": 1.8841987074633166, - "velocityY": -2.062675373212134, - "timestamp": 0.2855100782402231 + "x": 9.729912323289412, + "y": 5.439369761908001, + "heading": 2.899948078854106e-17, + "angularVelocity": 1.9429769044629593e-16, + "velocityX": 0.5336176700471545, + "velocityY": -2.3530016029798486, + "timestamp": 0.2426429779100772 }, { - "x": 9.632223213447787, - "y": 5.26753117493535, - "heading": -9.150147904566991e-18, - "angularVelocity": 8.834441437765331e-17, - "velocityX": 1.521479463860705, - "velocityY": -2.679487187725441, - "timestamp": 0.3568875978002789 + "x": 9.751348345266472, + "y": 5.261403357718118, + "heading": 4.0782917255307413e-17, + "angularVelocity": 1.9425143176374398e-16, + "velocityX": 0.35337551758868835, + "velocityY": -2.9337985499969643, + "timestamp": 0.3033037223875965 }, { - "x": 9.691761227634146, - "y": 5.062077314595765, - "heading": -6.8541498963009875e-18, - "angularVelocity": 3.216696488819412e-17, - "velocityX": 0.8341283719759774, - "velocityY": -2.878411320621096, - "timestamp": 0.4282651173603347 + "x": 9.744181294864156, + "y": 5.060141633962775, + "heading": 4.198809491304761e-17, + "angularVelocity": 1.9867505220008126e-17, + "velocityX": -0.11814972704420049, + "velocityY": -3.3178248221125908, + "timestamp": 0.3639644668651158 }, { "x": 9.70022678375244, "y": 4.856142997741699, - "heading": -9.683146911978938e-18, - "angularVelocity": -3.963428214808329e-17, - "velocityX": 0.1186025540038291, - "velocityY": -2.8851425227921963, - "timestamp": 0.49964263692039046 - }, - { - "x": 9.65307144790573, - "y": 4.645942615589113, - "heading": -5.5462217098690175e-18, - "angularVelocity": 5.5158786569436616e-17, - "velocityX": -0.6287353341669664, - "velocityY": -2.8026607199750178, - "timestamp": 0.5746429324090815 - }, - { - "x": 9.551846573573934, - "y": 4.451755307762366, - "heading": -7.328868694792328e-18, - "angularVelocity": -2.3768530508516856e-17, - "velocityX": -1.3496596736354354, - "velocityY": -2.5891539034802524, - "timestamp": 0.6496432278977725 - }, - { - "x": 9.409128969103543, - "y": 4.29575550701389, - "heading": -1.2264974248358262e-17, - "angularVelocity": -6.58144800800035e-17, - "velocityX": -1.9028938958235035, - "velocityY": -2.0799891484682558, - "timestamp": 0.7246435233864635 - }, - { - "x": 9.274801512075, - "y": 4.195518932576719, - "heading": -1.0315851786469037e-17, - "angularVelocity": 2.598819788396303e-17, - "velocityX": -1.7910257040087993, - "velocityY": -1.3364823936217263, - "timestamp": 0.7996438188751546 - }, - { - "x": 9.178765638394157, - "y": 4.136679072340064, - "heading": -1.7595474741066158e-18, - "angularVelocity": 1.1408360752736715e-16, - "velocityX": -1.2804732708731046, - "velocityY": -0.7845283789014882, - "timestamp": 0.8746441143638456 - }, - { - "x": 9.1292085647583, - "y": 4.109771251678467, - "heading": 8.197183256276926e-29, - "angularVelocity": 2.3460541527021614e-17, - "velocityX": -0.6607583785231516, - "velocityY": -0.3587695286556818, - "timestamp": 0.9496444098525366 - }, - { - "x": 9.1292085647583, - "y": 4.109771251678467, - "heading": 4.0649014975521164e-29, - "angularVelocity": -8.983999410224412e-30, - "velocityX": 2.1617137820797256e-20, - "velocityY": 3.116448033899274e-19, - "timestamp": 1.0246447053412275 + "heading": 4.117789366500127e-17, + "angularVelocity": -1.3356269448868408e-17, + "velocityX": -0.7245956423763401, + "velocityY": -3.3629431682408346, + "timestamp": 0.4246252113426351 + }, + { + "x": 9.62367202292937, + "y": 4.666896180193955, + "heading": 3.883641520117362e-17, + "angularVelocity": -3.959883629943507e-17, + "velocityX": -1.2946860234610496, + "velocityY": -3.2005221756203603, + "timestamp": 0.4837551930029131 + }, + { + "x": 9.520833726243332, + "y": 4.500838515926915, + "heading": 2.7882820869042386e-17, + "angularVelocity": -1.8524602958829465e-16, + "velocityX": -1.7391904038949337, + "velocityY": -2.8083496663519774, + "timestamp": 0.5428851746631911 + }, + { + "x": 9.422837551158624, + "y": 4.369495613480981, + "heading": 1.2414905453524928e-17, + "angularVelocity": -2.6159175059594473e-16, + "velocityX": -1.6573009551691247, + "velocityY": -2.221257283666062, + "timestamp": 0.6020151563234691 + }, + { + "x": 9.348748263717006, + "y": 4.263785158181079, + "heading": 8.266599657957546e-18, + "angularVelocity": -7.01557091454596e-17, + "velocityX": -1.2529901982260223, + "velocityY": -1.7877640467951625, + "timestamp": 0.6611451379837472 + }, + { + "x": 9.30366679578196, + "y": 4.177749774701769, + "heading": 8.751765261420785e-18, + "angularVelocity": 8.205069415908739e-18, + "velocityX": -0.7624130207591719, + "velocityY": -1.4550213117537047, + "timestamp": 0.7202751196440252 + }, + { + "x": 9.289485931396484, + "y": 4.1082587242126465, + "heading": -9.673823625011814e-28, + "angularVelocity": -1.4800892906397257e-16, + "velocityX": -0.23982527961788674, + "velocityY": -1.1752253009035645, + "timestamp": 0.7794051013043033 + }, + { + "x": 9.359415889552718, + "y": 4.032157790510569, + "heading": -1.916769609398054e-18, + "angularVelocity": -1.8826829061024516e-17, + "velocityX": 0.6868636491748086, + "velocityY": -0.7474759946436492, + "timestamp": 0.8812156400338537 + }, + { + "x": 9.52218848262578, + "y": 4.002726888178056, + "heading": 1.6766816122200447e-18, + "angularVelocity": 3.5295473979561707e-17, + "velocityX": 1.5987794103069355, + "velocityY": -0.28907520478496473, + "timestamp": 0.983026178763404 + }, + { + "x": 9.718168604909845, + "y": 4.071759502986741, + "heading": 1.7799244325302356e-18, + "angularVelocity": 1.0140681418433122e-18, + "velocityX": 1.9249492707691656, + "velocityY": 0.6780497939615223, + "timestamp": 1.0848367174929543 + }, + { + "x": 9.815460205078123, + "y": 4.1082587242126465, + "heading": 1.5656793258881755e-27, + "angularVelocity": -1.7482713005339804e-17, + "velocityX": 0.9556142358378643, + "velocityY": 0.35850140546709003, + "timestamp": 1.1866472562225046 + }, + { + "x": 9.815460205078123, + "y": 4.1082587242126465, + "heading": 7.676097442920992e-28, + "angularVelocity": -2.991815747657937e-28, + "velocityX": 3.353769132505619e-22, + "velocityY": 1.0669370220059503e-21, + "timestamp": 1.2884577949520548 } ], - "eventMarkers": [] + "eventMarkers": [ + { + "timestamp": 0.4246252113426351, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 5 to 7.traj b/src/main/deploy/choreo/Red 5 to 7.traj new file mode 100644 index 00000000..fb5b3068 --- /dev/null +++ b/src/main/deploy/choreo/Red 5 to 7.traj @@ -0,0 +1,140 @@ +{ + "samples": [ + { + "x": 9.15098476409912, + "y": 5.797394275665283, + "heading": -4.065640793519508e-29, + "angularVelocity": -3.268176405529047e-29, + "velocityX": 1.157597527355568e-20, + "velocityY": 1.4018882473037506e-19, + "timestamp": 0 + }, + { + "x": 9.192684743588645, + "y": 5.767902935794363, + "heading": 8.806771020594398e-18, + "angularVelocity": 1.23382974768875e-16, + "velocityX": 0.5842172682176147, + "velocityY": -0.4131740645050891, + "timestamp": 0.07137751956005578 + }, + { + "x": 9.273759250876711, + "y": 5.70588047685832, + "heading": 7.597661113768503e-18, + "angularVelocity": -1.6939650399948386e-17, + "velocityX": 1.1358549272625345, + "velocityY": -0.8689354760200882, + "timestamp": 0.14275503912011156 + }, + { + "x": 9.38913435315891, + "y": 5.606014975885555, + "heading": -6.605907220253839e-18, + "angularVelocity": -1.9899218262115755e-16, + "velocityX": 1.6164067201176004, + "velocityY": -1.399116999136334, + "timestamp": 0.21413255868016734 + }, + { + "x": 9.523623783255955, + "y": 5.458786324088157, + "heading": -1.545595323632644e-17, + "angularVelocity": -1.2398926619047138e-16, + "velocityX": 1.8841987074633166, + "velocityY": -2.062675373212134, + "timestamp": 0.2855100782402231 + }, + { + "x": 9.632223213447787, + "y": 5.26753117493535, + "heading": -9.150147904566991e-18, + "angularVelocity": 8.834441437765331e-17, + "velocityX": 1.521479463860705, + "velocityY": -2.679487187725441, + "timestamp": 0.3568875978002789 + }, + { + "x": 9.691761227634146, + "y": 5.062077314595765, + "heading": -6.8541498963009875e-18, + "angularVelocity": 3.216696488819412e-17, + "velocityX": 0.8341283719759774, + "velocityY": -2.878411320621096, + "timestamp": 0.4282651173603347 + }, + { + "x": 9.70022678375244, + "y": 4.856142997741699, + "heading": -9.683146911978938e-18, + "angularVelocity": -3.963428214808329e-17, + "velocityX": 0.1186025540038291, + "velocityY": -2.8851425227921963, + "timestamp": 0.49964263692039046 + }, + { + "x": 9.65307144790573, + "y": 4.645942615589113, + "heading": -5.5462217098690175e-18, + "angularVelocity": 5.5158786569436616e-17, + "velocityX": -0.6287353341669664, + "velocityY": -2.8026607199750178, + "timestamp": 0.5746429324090815 + }, + { + "x": 9.551846573573934, + "y": 4.451755307762366, + "heading": -7.328868694792328e-18, + "angularVelocity": -2.3768530508516856e-17, + "velocityX": -1.3496596736354354, + "velocityY": -2.5891539034802524, + "timestamp": 0.6496432278977725 + }, + { + "x": 9.409128969103543, + "y": 4.29575550701389, + "heading": -1.2264974248358262e-17, + "angularVelocity": -6.58144800800035e-17, + "velocityX": -1.9028938958235035, + "velocityY": -2.0799891484682558, + "timestamp": 0.7246435233864635 + }, + { + "x": 9.274801512075, + "y": 4.195518932576719, + "heading": -1.0315851786469037e-17, + "angularVelocity": 2.598819788396303e-17, + "velocityX": -1.7910257040087993, + "velocityY": -1.3364823936217263, + "timestamp": 0.7996438188751546 + }, + { + "x": 9.178765638394157, + "y": 4.136679072340064, + "heading": -1.7595474741066158e-18, + "angularVelocity": 1.1408360752736715e-16, + "velocityX": -1.2804732708731046, + "velocityY": -0.7845283789014882, + "timestamp": 0.8746441143638456 + }, + { + "x": 9.1292085647583, + "y": 4.109771251678467, + "heading": 8.197183256276926e-29, + "angularVelocity": 2.3460541527021614e-17, + "velocityX": -0.6607583785231516, + "velocityY": -0.3587695286556818, + "timestamp": 0.9496444098525366 + }, + { + "x": 9.1292085647583, + "y": 4.109771251678467, + "heading": 4.0649014975521164e-29, + "angularVelocity": -8.983999410224412e-30, + "velocityX": 2.1617137820797256e-20, + "velocityY": 3.116448033899274e-19, + "timestamp": 1.0246447053412275 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 5 to CWS 2.traj b/src/main/deploy/choreo/Red 5 to CWS 2.traj new file mode 100644 index 00000000..22db1aa8 --- /dev/null +++ b/src/main/deploy/choreo/Red 5 to CWS 2.traj @@ -0,0 +1,122 @@ +{ + "samples": [ + { + "x": 8.976778030395508, + "y": 5.797394275665283, + "heading": 5.68893776700111e-24, + "angularVelocity": -2.9835246511928784e-24, + "velocityX": 6.5296719081595606e-24, + "velocityY": 1.747545176771818e-24, + "timestamp": 0 + }, + { + "x": 9.049243245948405, + "y": 5.822190512926668, + "heading": -0.011075196763805075, + "angularVelocity": -0.1266788603327554, + "velocityX": 0.8288621065414437, + "velocityY": 0.28362106279487775, + "timestamp": 0.08742734766253149 + }, + { + "x": 9.194173918876237, + "y": 5.871782400887453, + "heading": -0.033181848616755605, + "angularVelocity": -0.25285740039011523, + "velocityX": 1.657726979059941, + "velocityY": 0.5672354164535425, + "timestamp": 0.17485469532506298 + }, + { + "x": 9.411570535802515, + "y": 5.946169185203648, + "heading": -0.06622649901979259, + "angularVelocity": -0.3779669781426854, + "velocityX": 2.486597417611544, + "velocityY": 0.8508411418739135, + "timestamp": 0.2622820429875945 + }, + { + "x": 9.701433824736373, + "y": 6.045350005092207, + "heading": -0.11005848510673794, + "angularVelocity": -0.5013532636965756, + "velocityX": 3.315476183181578, + "velocityY": 1.1344370215987225, + "timestamp": 0.34970939065012596 + }, + { + "x": 10.034912241269211, + "y": 6.159512024287684, + "heading": -0.11005848780565615, + "angularVelocity": -3.0870411745699127e-8, + "velocityX": 3.814349004616508, + "velocityY": 1.3057930069678114, + "timestamp": 0.4371367383126574 + }, + { + "x": 10.368390657402738, + "y": 6.273674044649711, + "heading": -0.11005849050455542, + "angularVelocity": -3.087019508851263e-8, + "velocityX": 3.8143490000491562, + "velocityY": 1.305793020310885, + "timestamp": 0.5245640859751889 + }, + { + "x": 10.70186907395756, + "y": 6.387836063780976, + "heading": -0.11005849320347309, + "angularVelocity": -3.0870405575183246e-8, + "velocityX": 3.8143490048679434, + "velocityY": 1.3057930062333485, + "timestamp": 0.6119914336377204 + }, + { + "x": 10.99174259097438, + "y": 6.487006632069917, + "heading": -0.15169988410125665, + "angularVelocity": -0.47629708565011974, + "velocityX": 3.3155931726961243, + "velocityY": 1.1343197630990574, + "timestamp": 0.6994187813002518 + }, + { + "x": 11.209148113872045, + "y": 6.561384295183812, + "heading": -0.18284402599158536, + "angularVelocity": -0.3562288314011849, + "velocityX": 2.4866992847232186, + "velocityY": 0.8507368129362852, + "timestamp": 0.7868461289627833 + }, + { + "x": 11.354085215408697, + "y": 6.610969282515083, + "heading": -0.2035851968769597, + "angularVelocity": -0.2372389354122352, + "velocityX": 1.6578005099286304, + "velocityY": 0.5671564865797899, + "timestamp": 0.8742734766253147 + }, + { + "x": 11.426553726196287, + "y": 6.635761737823486, + "heading": -0.21396366633395164, + "angularVelocity": -0.11870964560257202, + "velocityX": 0.8288997976619157, + "velocityY": 0.2835778045572377, + "timestamp": 0.9617008242878462 + }, + { + "x": 11.426553726196287, + "y": 6.635761737823486, + "heading": -0.21396366633395164, + "angularVelocity": 1.2331869799730271e-23, + "velocityX": -5.618152052891591e-25, + "velocityY": -9.56708867463376e-25, + "timestamp": 1.0491281719503778 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 5 to CWS.1 2.traj b/src/main/deploy/choreo/Red 5 to CWS.1 2.traj new file mode 100644 index 00000000..22db1aa8 --- /dev/null +++ b/src/main/deploy/choreo/Red 5 to CWS.1 2.traj @@ -0,0 +1,122 @@ +{ + "samples": [ + { + "x": 8.976778030395508, + "y": 5.797394275665283, + "heading": 5.68893776700111e-24, + "angularVelocity": -2.9835246511928784e-24, + "velocityX": 6.5296719081595606e-24, + "velocityY": 1.747545176771818e-24, + "timestamp": 0 + }, + { + "x": 9.049243245948405, + "y": 5.822190512926668, + "heading": -0.011075196763805075, + "angularVelocity": -0.1266788603327554, + "velocityX": 0.8288621065414437, + "velocityY": 0.28362106279487775, + "timestamp": 0.08742734766253149 + }, + { + "x": 9.194173918876237, + "y": 5.871782400887453, + "heading": -0.033181848616755605, + "angularVelocity": -0.25285740039011523, + "velocityX": 1.657726979059941, + "velocityY": 0.5672354164535425, + "timestamp": 0.17485469532506298 + }, + { + "x": 9.411570535802515, + "y": 5.946169185203648, + "heading": -0.06622649901979259, + "angularVelocity": -0.3779669781426854, + "velocityX": 2.486597417611544, + "velocityY": 0.8508411418739135, + "timestamp": 0.2622820429875945 + }, + { + "x": 9.701433824736373, + "y": 6.045350005092207, + "heading": -0.11005848510673794, + "angularVelocity": -0.5013532636965756, + "velocityX": 3.315476183181578, + "velocityY": 1.1344370215987225, + "timestamp": 0.34970939065012596 + }, + { + "x": 10.034912241269211, + "y": 6.159512024287684, + "heading": -0.11005848780565615, + "angularVelocity": -3.0870411745699127e-8, + "velocityX": 3.814349004616508, + "velocityY": 1.3057930069678114, + "timestamp": 0.4371367383126574 + }, + { + "x": 10.368390657402738, + "y": 6.273674044649711, + "heading": -0.11005849050455542, + "angularVelocity": -3.087019508851263e-8, + "velocityX": 3.8143490000491562, + "velocityY": 1.305793020310885, + "timestamp": 0.5245640859751889 + }, + { + "x": 10.70186907395756, + "y": 6.387836063780976, + "heading": -0.11005849320347309, + "angularVelocity": -3.0870405575183246e-8, + "velocityX": 3.8143490048679434, + "velocityY": 1.3057930062333485, + "timestamp": 0.6119914336377204 + }, + { + "x": 10.99174259097438, + "y": 6.487006632069917, + "heading": -0.15169988410125665, + "angularVelocity": -0.47629708565011974, + "velocityX": 3.3155931726961243, + "velocityY": 1.1343197630990574, + "timestamp": 0.6994187813002518 + }, + { + "x": 11.209148113872045, + "y": 6.561384295183812, + "heading": -0.18284402599158536, + "angularVelocity": -0.3562288314011849, + "velocityX": 2.4866992847232186, + "velocityY": 0.8507368129362852, + "timestamp": 0.7868461289627833 + }, + { + "x": 11.354085215408697, + "y": 6.610969282515083, + "heading": -0.2035851968769597, + "angularVelocity": -0.2372389354122352, + "velocityX": 1.6578005099286304, + "velocityY": 0.5671564865797899, + "timestamp": 0.8742734766253147 + }, + { + "x": 11.426553726196287, + "y": 6.635761737823486, + "heading": -0.21396366633395164, + "angularVelocity": -0.11870964560257202, + "velocityX": 0.8288997976619157, + "velocityY": 0.2835778045572377, + "timestamp": 0.9617008242878462 + }, + { + "x": 11.426553726196287, + "y": 6.635761737823486, + "heading": -0.21396366633395164, + "angularVelocity": 1.2331869799730271e-23, + "velocityX": -5.618152052891591e-25, + "velocityY": -9.56708867463376e-25, + "timestamp": 1.0491281719503778 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 5 to CWS.1.traj b/src/main/deploy/choreo/Red 5 to CWS.1.traj index 22db1aa8..809dae25 100644 --- a/src/main/deploy/choreo/Red 5 to CWS.1.traj +++ b/src/main/deploy/choreo/Red 5 to CWS.1.traj @@ -1,121 +1,148 @@ { "samples": [ { - "x": 8.976778030395508, - "y": 5.797394275665283, - "heading": 5.68893776700111e-24, - "angularVelocity": -2.9835246511928784e-24, - "velocityX": 6.5296719081595606e-24, - "velocityY": 1.747545176771818e-24, + "x": 9.661816596984863, + "y": 5.803494453430176, + "heading": 1.3611922399510842e-21, + "angularVelocity": 3.0579268596230436e-22, + "velocityX": -1.736689415628626e-20, + "velocityY": 5.376921496421165e-21, "timestamp": 0 }, { - "x": 9.049243245948405, - "y": 5.822190512926668, - "heading": -0.011075196763805075, - "angularVelocity": -0.1266788603327554, - "velocityX": 0.8288621065414437, - "velocityY": 0.28362106279487775, - "timestamp": 0.08742734766253149 + "x": 9.678564574450153, + "y": 5.844997973970278, + "heading": 0.0029848399099582116, + "angularVelocity": 0.044669071267147434, + "velocityX": 0.250638768424957, + "velocityY": 0.6211132833818533, + "timestamp": 0.0668211768296481 }, { - "x": 9.194173918876237, - "y": 5.871782400887453, - "heading": -0.033181848616755605, - "angularVelocity": -0.25285740039011523, - "velocityX": 1.657726979059941, - "velocityY": 0.5672354164535425, - "timestamp": 0.17485469532506298 + "x": 9.716025235630987, + "y": 5.926178744283693, + "heading": 0.0082949313366775, + "angularVelocity": 0.0794671940641913, + "velocityX": 0.5606106171485411, + "velocityY": 1.214895848377748, + "timestamp": 0.1336423536592962 }, { - "x": 9.411570535802515, - "y": 5.946169185203648, - "heading": -0.06622649901979259, - "angularVelocity": -0.3779669781426854, - "velocityX": 2.486597417611544, - "velocityY": 0.8508411418739135, - "timestamp": 0.2622820429875945 + "x": 9.780171845440192, + "y": 6.043296248773044, + "heading": 0.014648503288511429, + "angularVelocity": 0.09508320944468747, + "velocityX": 0.9599742604353574, + "velocityY": 1.752700416924237, + "timestamp": 0.20046353048894427 }, { - "x": 9.701433824736373, - "y": 6.045350005092207, - "heading": -0.11005848510673794, - "angularVelocity": -0.5013532636965756, - "velocityX": 3.315476183181578, - "velocityY": 1.1344370215987225, - "timestamp": 0.34970939065012596 + "x": 9.879764902858732, + "y": 6.187745327513362, + "heading": 0.019081464409680428, + "angularVelocity": 0.0663406622195574, + "velocityX": 1.4904415358074388, + "velocityY": 2.1617260514459766, + "timestamp": 0.2672847073185924 }, { - "x": 10.034912241269211, - "y": 6.159512024287684, - "heading": -0.11005848780565615, - "angularVelocity": -3.0870411745699127e-8, - "velocityX": 3.814349004616508, - "velocityY": 1.3057930069678114, - "timestamp": 0.4371367383126574 + "x": 10.023311845369117, + "y": 6.3404966880582885, + "heading": 0.015505580615079386, + "angularVelocity": -0.053514229534108525, + "velocityX": 2.14822529809053, + "velocityY": 2.285972318857878, + "timestamp": 0.3341058841482405 }, { - "x": 10.368390657402738, - "y": 6.273674044649711, - "heading": -0.11005849050455542, - "angularVelocity": -3.087019508851263e-8, - "velocityX": 3.8143490000491562, - "velocityY": 1.305793020310885, - "timestamp": 0.5245640859751889 + "x": 10.208364486694336, + "y": 6.476681709289551, + "heading": -1.1002971311721428e-21, + "angularVelocity": -0.23204590746147513, + "velocityX": 2.7693711799631684, + "velocityY": 2.038051822663229, + "timestamp": 0.4009270609778886 }, { - "x": 10.70186907395756, - "y": 6.387836063780976, - "heading": -0.11005849320347309, - "angularVelocity": -3.0870405575183246e-8, - "velocityX": 3.8143490048679434, - "velocityY": 1.3057930062333485, - "timestamp": 0.6119914336377204 + "x": 10.40827692519543, + "y": 6.576214186706722, + "heading": -0.02372971218677946, + "angularVelocity": -0.3811944081806325, + "velocityX": 3.2113960372748394, + "velocityY": 1.5988910242982828, + "timestamp": 0.46317800610595805 }, { - "x": 10.99174259097438, - "y": 6.487006632069917, - "heading": -0.15169988410125665, - "angularVelocity": -0.47629708565011974, - "velocityX": 3.3155931726961243, - "velocityY": 1.1343197630990574, - "timestamp": 0.6994187813002518 + "x": 10.628550652151867, + "y": 6.642722598659207, + "heading": -0.055585275097017335, + "angularVelocity": -0.5117281809087577, + "velocityX": 3.5384800424135903, + "velocityY": 1.0683920029330198, + "timestamp": 0.5254289512340276 }, { - "x": 11.209148113872045, - "y": 6.561384295183812, - "heading": -0.18284402599158536, - "angularVelocity": -0.3562288314011849, - "velocityX": 2.4866992847232186, - "velocityY": 0.8507368129362852, - "timestamp": 0.7868461289627833 + "x": 10.849046714110486, + "y": 6.670407742335224, + "heading": -0.09244567690717581, + "angularVelocity": -0.5921259787192825, + "velocityX": 3.542051634778082, + "velocityY": 0.4447345116254647, + "timestamp": 0.587679896362097 }, { - "x": 11.354085215408697, - "y": 6.610969282515083, - "heading": -0.2035851968769597, - "angularVelocity": -0.2372389354122352, - "velocityX": 1.6578005099286304, - "velocityY": 0.5671564865797899, - "timestamp": 0.8742734766253147 + "x": 11.040961688225199, + "y": 6.671780771980835, + "heading": -0.12955345019997333, + "angularVelocity": -0.5960997574648119, + "velocityX": 3.0829246643653434, + "velocityY": 0.02205636615921845, + "timestamp": 0.6499308414901666 + }, + { + "x": 11.195452845995485, + "y": 6.662777946369766, + "heading": -0.16199848111936757, + "angularVelocity": -0.5211974027485814, + "velocityX": 2.4817479871651686, + "velocityY": -0.14462150883566724, + "timestamp": 0.7121817866182361 + }, + { + "x": 11.311201580164841, + "y": 6.651301472143275, + "heading": -0.18750887826283402, + "angularVelocity": -0.4097993547083289, + "velocityX": 1.8593891856799902, + "velocityY": -0.18435823268511917, + "timestamp": 0.7744327317463056 + }, + { + "x": 11.388172590876854, + "y": 6.641477967871726, + "heading": -0.20502458756132477, + "angularVelocity": -0.28137258418254585, + "velocityX": 1.2364633268336311, + "velocityY": -0.15780490158609067, + "timestamp": 0.8366836768743751 }, { "x": 11.426553726196287, "y": 6.635761737823486, "heading": -0.21396366633395164, - "angularVelocity": -0.11870964560257202, - "velocityX": 0.8288997976619157, - "velocityY": 0.2835778045572377, - "timestamp": 0.9617008242878462 + "angularVelocity": -0.14359747878905968, + "velocityX": 0.6165550617831983, + "velocityY": -0.0918255945708951, + "timestamp": 0.8989346220024446 }, { "x": 11.426553726196287, "y": 6.635761737823486, "heading": -0.21396366633395164, - "angularVelocity": 1.2331869799730271e-23, - "velocityX": -5.618152052891591e-25, - "velocityY": -9.56708867463376e-25, - "timestamp": 1.0491281719503778 + "angularVelocity": 6.005893376627867e-22, + "velocityX": -1.2386565547332063e-20, + "velocityY": -6.661443938725831e-20, + "timestamp": 0.9611855671305141 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red 5 to CWS.traj b/src/main/deploy/choreo/Red 5 to CWS.traj index 22db1aa8..809dae25 100644 --- a/src/main/deploy/choreo/Red 5 to CWS.traj +++ b/src/main/deploy/choreo/Red 5 to CWS.traj @@ -1,121 +1,148 @@ { "samples": [ { - "x": 8.976778030395508, - "y": 5.797394275665283, - "heading": 5.68893776700111e-24, - "angularVelocity": -2.9835246511928784e-24, - "velocityX": 6.5296719081595606e-24, - "velocityY": 1.747545176771818e-24, + "x": 9.661816596984863, + "y": 5.803494453430176, + "heading": 1.3611922399510842e-21, + "angularVelocity": 3.0579268596230436e-22, + "velocityX": -1.736689415628626e-20, + "velocityY": 5.376921496421165e-21, "timestamp": 0 }, { - "x": 9.049243245948405, - "y": 5.822190512926668, - "heading": -0.011075196763805075, - "angularVelocity": -0.1266788603327554, - "velocityX": 0.8288621065414437, - "velocityY": 0.28362106279487775, - "timestamp": 0.08742734766253149 + "x": 9.678564574450153, + "y": 5.844997973970278, + "heading": 0.0029848399099582116, + "angularVelocity": 0.044669071267147434, + "velocityX": 0.250638768424957, + "velocityY": 0.6211132833818533, + "timestamp": 0.0668211768296481 }, { - "x": 9.194173918876237, - "y": 5.871782400887453, - "heading": -0.033181848616755605, - "angularVelocity": -0.25285740039011523, - "velocityX": 1.657726979059941, - "velocityY": 0.5672354164535425, - "timestamp": 0.17485469532506298 + "x": 9.716025235630987, + "y": 5.926178744283693, + "heading": 0.0082949313366775, + "angularVelocity": 0.0794671940641913, + "velocityX": 0.5606106171485411, + "velocityY": 1.214895848377748, + "timestamp": 0.1336423536592962 }, { - "x": 9.411570535802515, - "y": 5.946169185203648, - "heading": -0.06622649901979259, - "angularVelocity": -0.3779669781426854, - "velocityX": 2.486597417611544, - "velocityY": 0.8508411418739135, - "timestamp": 0.2622820429875945 + "x": 9.780171845440192, + "y": 6.043296248773044, + "heading": 0.014648503288511429, + "angularVelocity": 0.09508320944468747, + "velocityX": 0.9599742604353574, + "velocityY": 1.752700416924237, + "timestamp": 0.20046353048894427 }, { - "x": 9.701433824736373, - "y": 6.045350005092207, - "heading": -0.11005848510673794, - "angularVelocity": -0.5013532636965756, - "velocityX": 3.315476183181578, - "velocityY": 1.1344370215987225, - "timestamp": 0.34970939065012596 + "x": 9.879764902858732, + "y": 6.187745327513362, + "heading": 0.019081464409680428, + "angularVelocity": 0.0663406622195574, + "velocityX": 1.4904415358074388, + "velocityY": 2.1617260514459766, + "timestamp": 0.2672847073185924 }, { - "x": 10.034912241269211, - "y": 6.159512024287684, - "heading": -0.11005848780565615, - "angularVelocity": -3.0870411745699127e-8, - "velocityX": 3.814349004616508, - "velocityY": 1.3057930069678114, - "timestamp": 0.4371367383126574 + "x": 10.023311845369117, + "y": 6.3404966880582885, + "heading": 0.015505580615079386, + "angularVelocity": -0.053514229534108525, + "velocityX": 2.14822529809053, + "velocityY": 2.285972318857878, + "timestamp": 0.3341058841482405 }, { - "x": 10.368390657402738, - "y": 6.273674044649711, - "heading": -0.11005849050455542, - "angularVelocity": -3.087019508851263e-8, - "velocityX": 3.8143490000491562, - "velocityY": 1.305793020310885, - "timestamp": 0.5245640859751889 + "x": 10.208364486694336, + "y": 6.476681709289551, + "heading": -1.1002971311721428e-21, + "angularVelocity": -0.23204590746147513, + "velocityX": 2.7693711799631684, + "velocityY": 2.038051822663229, + "timestamp": 0.4009270609778886 }, { - "x": 10.70186907395756, - "y": 6.387836063780976, - "heading": -0.11005849320347309, - "angularVelocity": -3.0870405575183246e-8, - "velocityX": 3.8143490048679434, - "velocityY": 1.3057930062333485, - "timestamp": 0.6119914336377204 + "x": 10.40827692519543, + "y": 6.576214186706722, + "heading": -0.02372971218677946, + "angularVelocity": -0.3811944081806325, + "velocityX": 3.2113960372748394, + "velocityY": 1.5988910242982828, + "timestamp": 0.46317800610595805 }, { - "x": 10.99174259097438, - "y": 6.487006632069917, - "heading": -0.15169988410125665, - "angularVelocity": -0.47629708565011974, - "velocityX": 3.3155931726961243, - "velocityY": 1.1343197630990574, - "timestamp": 0.6994187813002518 + "x": 10.628550652151867, + "y": 6.642722598659207, + "heading": -0.055585275097017335, + "angularVelocity": -0.5117281809087577, + "velocityX": 3.5384800424135903, + "velocityY": 1.0683920029330198, + "timestamp": 0.5254289512340276 }, { - "x": 11.209148113872045, - "y": 6.561384295183812, - "heading": -0.18284402599158536, - "angularVelocity": -0.3562288314011849, - "velocityX": 2.4866992847232186, - "velocityY": 0.8507368129362852, - "timestamp": 0.7868461289627833 + "x": 10.849046714110486, + "y": 6.670407742335224, + "heading": -0.09244567690717581, + "angularVelocity": -0.5921259787192825, + "velocityX": 3.542051634778082, + "velocityY": 0.4447345116254647, + "timestamp": 0.587679896362097 }, { - "x": 11.354085215408697, - "y": 6.610969282515083, - "heading": -0.2035851968769597, - "angularVelocity": -0.2372389354122352, - "velocityX": 1.6578005099286304, - "velocityY": 0.5671564865797899, - "timestamp": 0.8742734766253147 + "x": 11.040961688225199, + "y": 6.671780771980835, + "heading": -0.12955345019997333, + "angularVelocity": -0.5960997574648119, + "velocityX": 3.0829246643653434, + "velocityY": 0.02205636615921845, + "timestamp": 0.6499308414901666 + }, + { + "x": 11.195452845995485, + "y": 6.662777946369766, + "heading": -0.16199848111936757, + "angularVelocity": -0.5211974027485814, + "velocityX": 2.4817479871651686, + "velocityY": -0.14462150883566724, + "timestamp": 0.7121817866182361 + }, + { + "x": 11.311201580164841, + "y": 6.651301472143275, + "heading": -0.18750887826283402, + "angularVelocity": -0.4097993547083289, + "velocityX": 1.8593891856799902, + "velocityY": -0.18435823268511917, + "timestamp": 0.7744327317463056 + }, + { + "x": 11.388172590876854, + "y": 6.641477967871726, + "heading": -0.20502458756132477, + "angularVelocity": -0.28137258418254585, + "velocityX": 1.2364633268336311, + "velocityY": -0.15780490158609067, + "timestamp": 0.8366836768743751 }, { "x": 11.426553726196287, "y": 6.635761737823486, "heading": -0.21396366633395164, - "angularVelocity": -0.11870964560257202, - "velocityX": 0.8288997976619157, - "velocityY": 0.2835778045572377, - "timestamp": 0.9617008242878462 + "angularVelocity": -0.14359747878905968, + "velocityX": 0.6165550617831983, + "velocityY": -0.0918255945708951, + "timestamp": 0.8989346220024446 }, { "x": 11.426553726196287, "y": 6.635761737823486, "heading": -0.21396366633395164, - "angularVelocity": 1.2331869799730271e-23, - "velocityX": -5.618152052891591e-25, - "velocityY": -9.56708867463376e-25, - "timestamp": 1.0491281719503778 + "angularVelocity": 6.005893376627867e-22, + "velocityX": -1.2386565547332063e-20, + "velocityY": -6.661443938725831e-20, + "timestamp": 0.9611855671305141 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red 6 to CWS 2.traj b/src/main/deploy/choreo/Red 6 to CWS 2.traj new file mode 100644 index 00000000..f149f889 --- /dev/null +++ b/src/main/deploy/choreo/Red 6 to CWS 2.traj @@ -0,0 +1,203 @@ +{ + "samples": [ + { + "x": 9.165976524353027, + "y": 4.114032745361328, + "heading": 0.019227672507796556, + "angularVelocity": -3.1003593284430024e-27, + "velocityX": 1.318659078564603e-26, + "velocityY": 2.316078077174801e-28, + "timestamp": 0 + }, + { + "x": 9.16027596499261, + "y": 4.166403545411132, + "heading": 0.012282530601989756, + "angularVelocity": -0.09577565199197573, + "velocityX": -0.07861247428311936, + "velocityY": 0.7222095081912742, + "timestamp": 0.07251469200531965 + }, + { + "x": 9.149102494916367, + "y": 4.27117070053216, + "heading": -0.0014851703204856132, + "angularVelocity": -0.18986084808117745, + "velocityX": -0.15408560344467495, + "velocityY": 1.4447714280209942, + "timestamp": 0.1450293840106393 + }, + { + "x": 9.132833593950462, + "y": 4.428374277760363, + "heading": -0.021876491257742454, + "angularVelocity": -0.2812026138890714, + "velocityX": -0.22435316921309337, + "velocityY": 2.167885884651771, + "timestamp": 0.21754407601595896 + }, + { + "x": 9.112216918047768, + "y": 4.6380853538080355, + "heading": -0.0484894595052067, + "angularVelocity": -0.3670010519456103, + "velocityX": -0.28431032846671456, + "velocityY": 2.891980511098197, + "timestamp": 0.2900587680212786 + }, + { + "x": 9.089430616903869, + "y": 4.900448711996779, + "heading": -0.0801093822030687, + "angularVelocity": -0.4360485002893258, + "velocityX": -0.31423013066410976, + "velocityY": 3.618071744268, + "timestamp": 0.3625734600265983 + }, + { + "x": 9.109885650378216, + "y": 5.192087462357156, + "heading": -0.08010938704614348, + "angularVelocity": -6.678749717871197e-8, + "velocityX": 0.28208122945412023, + "velocityY": 4.02178844445733, + "timestamp": 0.435088152031918 + }, + { + "x": 9.182379800478888, + "y": 5.475312047806293, + "heading": -0.0801093912765566, + "angularVelocity": -5.8338703480250706e-8, + "velocityX": 0.9997167207902494, + "velocityY": 3.9057545114907084, + "timestamp": 0.5076028440372377 + }, + { + "x": 9.304556200250488, + "y": 5.740914171376622, + "heading": -0.08010939802300308, + "angularVelocity": -9.303558071395237e-8, + "velocityX": 1.6848502888578374, + "velocityY": 3.6627353192211642, + "timestamp": 0.5801175360425573 + }, + { + "x": 9.472442626953123, + "y": 5.980258464813232, + "heading": -0.0801094140047848, + "angularVelocity": -2.2039370606335216e-7, + "velocityX": 2.3152056784619393, + "velocityY": 3.3006317315538127, + "timestamp": 0.652632228047877 + }, + { + "x": 9.690930195145093, + "y": 6.192521851895066, + "heading": -0.0801094275269848, + "angularVelocity": -1.7896807096367723e-7, + "velocityX": 2.891711304942813, + "velocityY": 2.809333460614483, + "timestamp": 0.7281887221621088 + }, + { + "x": 9.945264512695372, + "y": 6.36017218154714, + "heading": -0.0801094347291874, + "angularVelocity": -9.532208551658443e-8, + "velocityX": 3.3661476823654413, + "velocityY": 2.218873858792461, + "timestamp": 0.8037452162763407 + }, + { + "x": 10.226468274983382, + "y": 6.477291899741084, + "heading": -0.08010944009786584, + "angularVelocity": -7.105515558762335e-8, + "velocityX": 3.721768268692626, + "velocityY": 1.5500946618416913, + "timestamp": 0.8793017103905725 + }, + { + "x": 10.524615763044427, + "y": 6.5397471826115465, + "heading": -0.08010944490352531, + "angularVelocity": -6.360352642448382e-8, + "velocityX": 3.9460206770616324, + "velocityY": 0.8266037698366171, + "timestamp": 0.9548582045048043 + }, + { + "x": 10.828899774162563, + "y": 6.554024026243873, + "heading": -0.08010944967830837, + "angularVelocity": -6.319487302459263e-8, + "velocityX": 4.0272383556878255, + "velocityY": 0.18895587731666572, + "timestamp": 1.0304146986190361 + }, + { + "x": 11.133183845275457, + "y": 6.568299591143467, + "heading": -0.08010945445311723, + "angularVelocity": -6.319521471147181e-8, + "velocityX": 4.0272391497262205, + "velocityY": 0.18893895312309195, + "timestamp": 1.105971192733268 + }, + { + "x": 11.418815993462275, + "y": 6.581544555724133, + "heading": -0.1315821813078501, + "angularVelocity": -0.6812482164261439, + "velocityX": 3.7803785304672726, + "velocityY": 0.17529882422338566, + "timestamp": 1.1815276868475 + }, + { + "x": 11.647294799351972, + "y": 6.592156838882461, + "heading": -0.17650291196886067, + "angularVelocity": -0.594531696945813, + "velocityX": 3.023946631831091, + "velocityY": 0.1404549441148474, + "timestamp": 1.257084180961732 + }, + { + "x": 11.818643408754175, + "y": 6.600120231200814, + "heading": -0.21138621074303876, + "angularVelocity": -0.461684983972906, + "velocityX": 2.2678210709875604, + "velocityY": 0.10539653026134421, + "timestamp": 1.3326406750759638 + }, + { + "x": 11.932871296430273, + "y": 6.605430723246171, + "heading": -0.23512693570933194, + "angularVelocity": -0.3142115743274193, + "velocityX": 1.5118209098399886, + "velocityY": 0.07028505104178595, + "timestamp": 1.4081971691901958 + }, + { + "x": 11.989983558654783, + "y": 6.608086585998535, + "heading": -0.2471764305163121, + "angularVelocity": -0.15947662670481846, + "velocityX": 0.7558881985466883, + "velocityY": 0.03515068801828196, + "timestamp": 1.4837536633044277 + }, + { + "x": 11.989983558654783, + "y": 6.608086585998535, + "heading": -0.2471764305163121, + "angularVelocity": 4.5808750464576005e-27, + "velocityX": 4.658708906964682e-26, + "velocityY": 1.1001600166410631e-26, + "timestamp": 1.5593101574186596 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 6 to CWS.1 2.traj b/src/main/deploy/choreo/Red 6 to CWS.1 2.traj new file mode 100644 index 00000000..f149f889 --- /dev/null +++ b/src/main/deploy/choreo/Red 6 to CWS.1 2.traj @@ -0,0 +1,203 @@ +{ + "samples": [ + { + "x": 9.165976524353027, + "y": 4.114032745361328, + "heading": 0.019227672507796556, + "angularVelocity": -3.1003593284430024e-27, + "velocityX": 1.318659078564603e-26, + "velocityY": 2.316078077174801e-28, + "timestamp": 0 + }, + { + "x": 9.16027596499261, + "y": 4.166403545411132, + "heading": 0.012282530601989756, + "angularVelocity": -0.09577565199197573, + "velocityX": -0.07861247428311936, + "velocityY": 0.7222095081912742, + "timestamp": 0.07251469200531965 + }, + { + "x": 9.149102494916367, + "y": 4.27117070053216, + "heading": -0.0014851703204856132, + "angularVelocity": -0.18986084808117745, + "velocityX": -0.15408560344467495, + "velocityY": 1.4447714280209942, + "timestamp": 0.1450293840106393 + }, + { + "x": 9.132833593950462, + "y": 4.428374277760363, + "heading": -0.021876491257742454, + "angularVelocity": -0.2812026138890714, + "velocityX": -0.22435316921309337, + "velocityY": 2.167885884651771, + "timestamp": 0.21754407601595896 + }, + { + "x": 9.112216918047768, + "y": 4.6380853538080355, + "heading": -0.0484894595052067, + "angularVelocity": -0.3670010519456103, + "velocityX": -0.28431032846671456, + "velocityY": 2.891980511098197, + "timestamp": 0.2900587680212786 + }, + { + "x": 9.089430616903869, + "y": 4.900448711996779, + "heading": -0.0801093822030687, + "angularVelocity": -0.4360485002893258, + "velocityX": -0.31423013066410976, + "velocityY": 3.618071744268, + "timestamp": 0.3625734600265983 + }, + { + "x": 9.109885650378216, + "y": 5.192087462357156, + "heading": -0.08010938704614348, + "angularVelocity": -6.678749717871197e-8, + "velocityX": 0.28208122945412023, + "velocityY": 4.02178844445733, + "timestamp": 0.435088152031918 + }, + { + "x": 9.182379800478888, + "y": 5.475312047806293, + "heading": -0.0801093912765566, + "angularVelocity": -5.8338703480250706e-8, + "velocityX": 0.9997167207902494, + "velocityY": 3.9057545114907084, + "timestamp": 0.5076028440372377 + }, + { + "x": 9.304556200250488, + "y": 5.740914171376622, + "heading": -0.08010939802300308, + "angularVelocity": -9.303558071395237e-8, + "velocityX": 1.6848502888578374, + "velocityY": 3.6627353192211642, + "timestamp": 0.5801175360425573 + }, + { + "x": 9.472442626953123, + "y": 5.980258464813232, + "heading": -0.0801094140047848, + "angularVelocity": -2.2039370606335216e-7, + "velocityX": 2.3152056784619393, + "velocityY": 3.3006317315538127, + "timestamp": 0.652632228047877 + }, + { + "x": 9.690930195145093, + "y": 6.192521851895066, + "heading": -0.0801094275269848, + "angularVelocity": -1.7896807096367723e-7, + "velocityX": 2.891711304942813, + "velocityY": 2.809333460614483, + "timestamp": 0.7281887221621088 + }, + { + "x": 9.945264512695372, + "y": 6.36017218154714, + "heading": -0.0801094347291874, + "angularVelocity": -9.532208551658443e-8, + "velocityX": 3.3661476823654413, + "velocityY": 2.218873858792461, + "timestamp": 0.8037452162763407 + }, + { + "x": 10.226468274983382, + "y": 6.477291899741084, + "heading": -0.08010944009786584, + "angularVelocity": -7.105515558762335e-8, + "velocityX": 3.721768268692626, + "velocityY": 1.5500946618416913, + "timestamp": 0.8793017103905725 + }, + { + "x": 10.524615763044427, + "y": 6.5397471826115465, + "heading": -0.08010944490352531, + "angularVelocity": -6.360352642448382e-8, + "velocityX": 3.9460206770616324, + "velocityY": 0.8266037698366171, + "timestamp": 0.9548582045048043 + }, + { + "x": 10.828899774162563, + "y": 6.554024026243873, + "heading": -0.08010944967830837, + "angularVelocity": -6.319487302459263e-8, + "velocityX": 4.0272383556878255, + "velocityY": 0.18895587731666572, + "timestamp": 1.0304146986190361 + }, + { + "x": 11.133183845275457, + "y": 6.568299591143467, + "heading": -0.08010945445311723, + "angularVelocity": -6.319521471147181e-8, + "velocityX": 4.0272391497262205, + "velocityY": 0.18893895312309195, + "timestamp": 1.105971192733268 + }, + { + "x": 11.418815993462275, + "y": 6.581544555724133, + "heading": -0.1315821813078501, + "angularVelocity": -0.6812482164261439, + "velocityX": 3.7803785304672726, + "velocityY": 0.17529882422338566, + "timestamp": 1.1815276868475 + }, + { + "x": 11.647294799351972, + "y": 6.592156838882461, + "heading": -0.17650291196886067, + "angularVelocity": -0.594531696945813, + "velocityX": 3.023946631831091, + "velocityY": 0.1404549441148474, + "timestamp": 1.257084180961732 + }, + { + "x": 11.818643408754175, + "y": 6.600120231200814, + "heading": -0.21138621074303876, + "angularVelocity": -0.461684983972906, + "velocityX": 2.2678210709875604, + "velocityY": 0.10539653026134421, + "timestamp": 1.3326406750759638 + }, + { + "x": 11.932871296430273, + "y": 6.605430723246171, + "heading": -0.23512693570933194, + "angularVelocity": -0.3142115743274193, + "velocityX": 1.5118209098399886, + "velocityY": 0.07028505104178595, + "timestamp": 1.4081971691901958 + }, + { + "x": 11.989983558654783, + "y": 6.608086585998535, + "heading": -0.2471764305163121, + "angularVelocity": -0.15947662670481846, + "velocityX": 0.7558881985466883, + "velocityY": 0.03515068801828196, + "timestamp": 1.4837536633044277 + }, + { + "x": 11.989983558654783, + "y": 6.608086585998535, + "heading": -0.2471764305163121, + "angularVelocity": 4.5808750464576005e-27, + "velocityX": 4.658708906964682e-26, + "velocityY": 1.1001600166410631e-26, + "timestamp": 1.5593101574186596 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red 6 to CWS.1.traj b/src/main/deploy/choreo/Red 6 to CWS.1.traj index f149f889..78cf1428 100644 --- a/src/main/deploy/choreo/Red 6 to CWS.1.traj +++ b/src/main/deploy/choreo/Red 6 to CWS.1.traj @@ -3,200 +3,272 @@ { "x": 9.165976524353027, "y": 4.114032745361328, - "heading": 0.019227672507796556, - "angularVelocity": -3.1003593284430024e-27, - "velocityX": 1.318659078564603e-26, - "velocityY": 2.316078077174801e-28, + "heading": 1.5776176711763843, + "angularVelocity": -5.58468760766859e-21, + "velocityX": -1.677172172736469e-20, + "velocityY": -6.242016627731418e-21, "timestamp": 0 }, { - "x": 9.16027596499261, - "y": 4.166403545411132, - "heading": 0.012282530601989756, - "angularVelocity": -0.09577565199197573, - "velocityX": -0.07861247428311936, - "velocityY": 0.7222095081912742, - "timestamp": 0.07251469200531965 + "x": 9.164796155364039, + "y": 4.128053853665708, + "heading": 1.5381182980542132, + "angularVelocity": -0.9864912857636485, + "velocityX": -0.029479549410119352, + "velocityY": 0.3501752069896988, + "timestamp": 0.04004026562849411 }, { - "x": 9.149102494916367, - "y": 4.27117070053216, - "heading": -0.0014851703204856132, - "angularVelocity": -0.18986084808117745, - "velocityX": -0.15408560344467495, - "velocityY": 1.4447714280209942, - "timestamp": 0.1450293840106393 + "x": 9.162495402237571, + "y": 4.156489712170952, + "heading": 1.4635174045753923, + "angularVelocity": -1.863146817530908, + "velocityX": -0.05746098559369431, + "velocityY": 0.7101815649578848, + "timestamp": 0.08008053125698822 }, { - "x": 9.132833593950462, - "y": 4.428374277760363, - "heading": -0.021876491257742454, - "angularVelocity": -0.2812026138890714, - "velocityX": -0.22435316921309337, - "velocityY": 2.167885884651771, - "timestamp": 0.21754407601595896 + "x": 9.159060655214276, + "y": 4.199811476239254, + "heading": 1.359950023881047, + "angularVelocity": -2.5865807598599746, + "velocityX": -0.08578232360400655, + "velocityY": 1.08195496178715, + "timestamp": 0.12012079688548233 }, { - "x": 9.112216918047768, - "y": 4.6380853538080355, - "heading": -0.0484894595052067, - "angularVelocity": -0.3670010519456103, - "velocityX": -0.28431032846671456, - "velocityY": 2.891980511098197, - "timestamp": 0.2900587680212786 + "x": 9.154362820944076, + "y": 4.258508481897705, + "heading": 1.235236776636678, + "angularVelocity": -3.114695801508823, + "velocityX": -0.1173277498610463, + "velocityY": 1.465949457059093, + "timestamp": 0.16016106251397644 }, { - "x": 9.089430616903869, - "y": 4.900448711996779, - "heading": -0.0801093822030687, - "angularVelocity": -0.4360485002893258, - "velocityX": -0.31423013066410976, - "velocityY": 3.618071744268, - "timestamp": 0.3625734600265983 + "x": 9.148245807581972, + "y": 4.333003352649315, + "heading": 1.0988055478710193, + "angularVelocity": -3.40735074116415, + "velocityX": -0.152771547995445, + "velocityY": 1.8604989148409636, + "timestamp": 0.20020132814247055 }, { - "x": 9.109885650378216, - "y": 5.192087462357156, - "heading": -0.08010938704614348, - "angularVelocity": -6.678749717871197e-8, - "velocityX": 0.28208122945412023, - "velocityY": 4.02178844445733, - "timestamp": 0.435088152031918 + "x": 9.140718433613461, + "y": 4.423506380828508, + "heading": 0.9635080390549446, + "angularVelocity": -3.379036244949183, + "velocityX": -0.1879951056831576, + "velocityY": 2.260300394080245, + "timestamp": 0.24024159377096466 }, { - "x": 9.182379800478888, - "y": 5.475312047806293, - "heading": -0.0801093912765566, - "angularVelocity": -5.8338703480250706e-8, - "velocityX": 0.9997167207902494, - "velocityY": 3.9057545114907084, - "timestamp": 0.5076028440372377 + "x": 9.132548850946765, + "y": 4.529323965266172, + "heading": 0.8487168791915694, + "angularVelocity": -2.866893065306883, + "velocityX": -0.2040341775602698, + "velocityY": 2.6427792817240476, + "timestamp": 0.28028185939945877 }, { - "x": 9.304556200250488, - "y": 5.740914171376622, - "heading": -0.08010939802300308, - "angularVelocity": -9.303558071395237e-8, - "velocityX": 1.6848502888578374, - "velocityY": 3.6627353192211642, - "timestamp": 0.5801175360425573 + "x": 9.124926332439184, + "y": 4.64950362338172, + "heading": 0.7639545864905695, + "angularVelocity": -2.1169263332928523, + "velocityX": -0.19037132715629668, + "velocityY": 3.001470050939277, + "timestamp": 0.3203221250279529 + }, + { + "x": 9.118434012961915, + "y": 4.7840493325662905, + "heading": 0.7087764113022048, + "angularVelocity": -1.378067161200295, + "velocityX": -0.1621447654096238, + "velocityY": 3.3602601549350606, + "timestamp": 0.360362390656447 + }, + { + "x": 9.114821619295993, + "y": 4.932875254785403, + "heading": 0.6828478055885947, + "angularVelocity": -0.6475632792795059, + "velocityX": -0.09021902350671948, + "velocityY": 3.7169064661081084, + "timestamp": 0.4004026562849411 + }, + { + "x": 9.119733054745478, + "y": 5.094229573217587, + "heading": 0.6828477010361423, + "angularVelocity": -0.0000026111827875269163, + "velocityX": 0.12266240926382038, + "velocityY": 4.029801398657725, + "timestamp": 0.4404429219134352 + }, + { + "x": 9.140665009898335, + "y": 5.254295817851585, + "heading": 0.6828476668832145, + "angularVelocity": -8.529645663166833e-7, + "velocityX": 0.522772634603688, + "velocityY": 3.9976319367908455, + "timestamp": 0.4804831875419293 + }, + { + "x": 9.177410010476544, + "y": 5.411487271920564, + "heading": 0.6828476246296896, + "angularVelocity": -0.0000010552758389959, + "velocityX": 0.9177012190492992, + "velocityY": 3.925834446937349, + "timestamp": 0.5205234531704235 + }, + { + "x": 9.229603825061764, + "y": 5.564245747672749, + "heading": 0.6828475667567384, + "angularVelocity": -0.0000014453688142033488, + "velocityX": 1.3035331750660322, + "velocityY": 3.8151214372438793, + "timestamp": 0.5605637187989176 + }, + { + "x": 9.2967290739306, + "y": 5.711056988859694, + "heading": 0.6828474589270388, + "angularVelocity": -0.0000026930315748324016, + "velocityX": 1.6764436452933849, + "velocityY": 3.6665900908142905, + "timestamp": 0.6006039844274118 + }, + { + "x": 9.378040037181789, + "y": 5.850313712944271, + "heading": 0.6824260962947332, + "angularVelocity": -0.01052347245183362, + "velocityX": 2.03072986592535, + "velocityY": 3.477917089177032, + "timestamp": 0.640644250055906 }, { "x": 9.472442626953123, "y": 5.980258464813232, - "heading": -0.0801094140047848, - "angularVelocity": -2.2039370606335216e-7, - "velocityX": 2.3152056784619393, - "velocityY": 3.3006317315538127, - "timestamp": 0.652632228047877 - }, - { - "x": 9.690930195145093, - "y": 6.192521851895066, - "heading": -0.0801094275269848, - "angularVelocity": -1.7896807096367723e-7, - "velocityX": 2.891711304942813, - "velocityY": 2.809333460614483, - "timestamp": 0.7281887221621088 - }, - { - "x": 9.945264512695372, - "y": 6.36017218154714, - "heading": -0.0801094347291874, - "angularVelocity": -9.532208551658443e-8, - "velocityX": 3.3661476823654413, - "velocityY": 2.218873858792461, - "timestamp": 0.8037452162763407 - }, - { - "x": 10.226468274983382, - "y": 6.477291899741084, - "heading": -0.08010944009786584, - "angularVelocity": -7.105515558762335e-8, - "velocityX": 3.721768268692626, - "velocityY": 1.5500946618416913, - "timestamp": 0.8793017103905725 - }, - { - "x": 10.524615763044427, - "y": 6.5397471826115465, - "heading": -0.08010944490352531, - "angularVelocity": -6.360352642448382e-8, - "velocityX": 3.9460206770616324, - "velocityY": 0.8266037698366171, - "timestamp": 0.9548582045048043 - }, - { - "x": 10.828899774162563, - "y": 6.554024026243873, - "heading": -0.08010944967830837, - "angularVelocity": -6.319487302459263e-8, - "velocityX": 4.0272383556878255, - "velocityY": 0.18895587731666572, - "timestamp": 1.0304146986190361 - }, - { - "x": 11.133183845275457, - "y": 6.568299591143467, - "heading": -0.08010945445311723, - "angularVelocity": -6.319521471147181e-8, - "velocityX": 4.0272391497262205, - "velocityY": 0.18893895312309195, - "timestamp": 1.105971192733268 - }, - { - "x": 11.418815993462275, - "y": 6.581544555724133, - "heading": -0.1315821813078501, - "angularVelocity": -0.6812482164261439, - "velocityX": 3.7803785304672726, - "velocityY": 0.17529882422338566, - "timestamp": 1.1815276868475 - }, - { - "x": 11.647294799351972, - "y": 6.592156838882461, - "heading": -0.17650291196886067, - "angularVelocity": -0.594531696945813, - "velocityX": 3.023946631831091, - "velocityY": 0.1404549441148474, - "timestamp": 1.257084180961732 - }, - { - "x": 11.818643408754175, - "y": 6.600120231200814, - "heading": -0.21138621074303876, - "angularVelocity": -0.461684983972906, - "velocityX": 2.2678210709875604, - "velocityY": 0.10539653026134421, - "timestamp": 1.3326406750759638 - }, - { - "x": 11.932871296430273, - "y": 6.605430723246171, - "heading": -0.23512693570933194, - "angularVelocity": -0.3142115743274193, - "velocityX": 1.5118209098399886, - "velocityY": 0.07028505104178595, - "timestamp": 1.4081971691901958 + "heading": 0.6802910040496163, + "angularVelocity": -0.053323628392650334, + "velocityX": 2.3576913961339048, + "velocityY": 3.2453518933830967, + "timestamp": 0.6806845156844001 + }, + { + "x": 9.697571132700979, + "y": 6.190348552373689, + "heading": 0.6769837409938244, + "angularVelocity": -0.04315400893213956, + "velocityX": 2.9375339620804057, + "velocityY": 2.7413088593774586, + "timestamp": 0.7573231209410413 + }, + { + "x": 9.959401598982078, + "y": 6.354404479556993, + "heading": 0.6769835985523576, + "angularVelocity": -0.0000018586124631184994, + "velocityX": 3.4164304713571103, + "velocityY": 2.1406434346504692, + "timestamp": 0.8339617261976824 + }, + { + "x": 10.247599106550812, + "y": 6.465812536773354, + "heading": 0.676983552190923, + "angularVelocity": -6.049357816949861e-7, + "velocityX": 3.7604743275747707, + "velocityY": 1.4536806462406935, + "timestamp": 0.9106003314543235 + }, + { + "x": 10.551697598356373, + "y": 6.520526967813223, + "heading": 0.6769835217928931, + "angularVelocity": -3.9664122091915935e-7, + "velocityX": 3.9679544113207434, + "velocityY": 0.7139278025295573, + "timestamp": 0.9872389367109646 + }, + { + "x": 10.860221065632627, + "y": 6.537344592449922, + "heading": 0.6769834953565499, + "angularVelocity": -3.449481242526253e-7, + "velocityX": 4.025692615922479, + "velocityY": 0.21944064065877694, + "timestamp": 1.0638775419676059 + }, + { + "x": 11.1687445405045, + "y": 6.554162077742282, + "heading": 0.6769834689201766, + "angularVelocity": -3.4494851824762934e-7, + "velocityX": 4.025692715031963, + "velocityY": 0.21943882245931226, + "timestamp": 1.140516147224247 + }, + { + "x": 11.442585083138006, + "y": 6.576789374730947, + "heading": 0.6007167845106364, + "angularVelocity": -0.9951470822589282, + "velocityX": 3.5731410001068404, + "velocityY": 0.29524672210419306, + "timestamp": 1.2171547524808883 + }, + { + "x": 11.66406877710657, + "y": 6.593057077284222, + "heading": 0.3953817365180138, + "angularVelocity": -2.679263894547836, + "velocityX": 2.8899755316119893, + "velocityY": 0.21226511754568214, + "timestamp": 1.2937933577375296 + }, + { + "x": 11.829372604042955, + "y": 6.602362161514451, + "heading": 0.11020800066076368, + "angularVelocity": -3.721019385755826, + "velocityX": 2.1569263477960128, + "velocityY": 0.12141510403356627, + "timestamp": 1.3704319629941708 + }, + { + "x": 11.937154802881087, + "y": 6.606659009355426, + "heading": -0.1192351932122035, + "angularVelocity": -2.9938331093660486, + "velocityX": 1.4063695245653058, + "velocityY": 0.05606636272237923, + "timestamp": 1.447070568250812 }, { "x": 11.989983558654783, "y": 6.608086585998535, "heading": -0.2471764305163121, - "angularVelocity": -0.15947662670481846, - "velocityX": 0.7558881985466883, - "velocityY": 0.03515068801828196, - "timestamp": 1.4837536633044277 + "angularVelocity": -1.6694097821283331, + "velocityX": 0.6893230323906269, + "velocityY": 0.01862738287556901, + "timestamp": 1.5237091735074533 }, { "x": 11.989983558654783, "y": 6.608086585998535, "heading": -0.2471764305163121, - "angularVelocity": 4.5808750464576005e-27, - "velocityX": 4.658708906964682e-26, - "velocityY": 1.1001600166410631e-26, - "timestamp": 1.5593101574186596 + "angularVelocity": -4.255940633526814e-21, + "velocityX": -4.150602396756134e-21, + "velocityY": -2.2857309193220108e-20, + "timestamp": 1.6003477787640945 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red 6 to CWS.traj b/src/main/deploy/choreo/Red 6 to CWS.traj index f149f889..78cf1428 100644 --- a/src/main/deploy/choreo/Red 6 to CWS.traj +++ b/src/main/deploy/choreo/Red 6 to CWS.traj @@ -3,200 +3,272 @@ { "x": 9.165976524353027, "y": 4.114032745361328, - "heading": 0.019227672507796556, - "angularVelocity": -3.1003593284430024e-27, - "velocityX": 1.318659078564603e-26, - "velocityY": 2.316078077174801e-28, + "heading": 1.5776176711763843, + "angularVelocity": -5.58468760766859e-21, + "velocityX": -1.677172172736469e-20, + "velocityY": -6.242016627731418e-21, "timestamp": 0 }, { - "x": 9.16027596499261, - "y": 4.166403545411132, - "heading": 0.012282530601989756, - "angularVelocity": -0.09577565199197573, - "velocityX": -0.07861247428311936, - "velocityY": 0.7222095081912742, - "timestamp": 0.07251469200531965 + "x": 9.164796155364039, + "y": 4.128053853665708, + "heading": 1.5381182980542132, + "angularVelocity": -0.9864912857636485, + "velocityX": -0.029479549410119352, + "velocityY": 0.3501752069896988, + "timestamp": 0.04004026562849411 }, { - "x": 9.149102494916367, - "y": 4.27117070053216, - "heading": -0.0014851703204856132, - "angularVelocity": -0.18986084808117745, - "velocityX": -0.15408560344467495, - "velocityY": 1.4447714280209942, - "timestamp": 0.1450293840106393 + "x": 9.162495402237571, + "y": 4.156489712170952, + "heading": 1.4635174045753923, + "angularVelocity": -1.863146817530908, + "velocityX": -0.05746098559369431, + "velocityY": 0.7101815649578848, + "timestamp": 0.08008053125698822 }, { - "x": 9.132833593950462, - "y": 4.428374277760363, - "heading": -0.021876491257742454, - "angularVelocity": -0.2812026138890714, - "velocityX": -0.22435316921309337, - "velocityY": 2.167885884651771, - "timestamp": 0.21754407601595896 + "x": 9.159060655214276, + "y": 4.199811476239254, + "heading": 1.359950023881047, + "angularVelocity": -2.5865807598599746, + "velocityX": -0.08578232360400655, + "velocityY": 1.08195496178715, + "timestamp": 0.12012079688548233 }, { - "x": 9.112216918047768, - "y": 4.6380853538080355, - "heading": -0.0484894595052067, - "angularVelocity": -0.3670010519456103, - "velocityX": -0.28431032846671456, - "velocityY": 2.891980511098197, - "timestamp": 0.2900587680212786 + "x": 9.154362820944076, + "y": 4.258508481897705, + "heading": 1.235236776636678, + "angularVelocity": -3.114695801508823, + "velocityX": -0.1173277498610463, + "velocityY": 1.465949457059093, + "timestamp": 0.16016106251397644 }, { - "x": 9.089430616903869, - "y": 4.900448711996779, - "heading": -0.0801093822030687, - "angularVelocity": -0.4360485002893258, - "velocityX": -0.31423013066410976, - "velocityY": 3.618071744268, - "timestamp": 0.3625734600265983 + "x": 9.148245807581972, + "y": 4.333003352649315, + "heading": 1.0988055478710193, + "angularVelocity": -3.40735074116415, + "velocityX": -0.152771547995445, + "velocityY": 1.8604989148409636, + "timestamp": 0.20020132814247055 }, { - "x": 9.109885650378216, - "y": 5.192087462357156, - "heading": -0.08010938704614348, - "angularVelocity": -6.678749717871197e-8, - "velocityX": 0.28208122945412023, - "velocityY": 4.02178844445733, - "timestamp": 0.435088152031918 + "x": 9.140718433613461, + "y": 4.423506380828508, + "heading": 0.9635080390549446, + "angularVelocity": -3.379036244949183, + "velocityX": -0.1879951056831576, + "velocityY": 2.260300394080245, + "timestamp": 0.24024159377096466 }, { - "x": 9.182379800478888, - "y": 5.475312047806293, - "heading": -0.0801093912765566, - "angularVelocity": -5.8338703480250706e-8, - "velocityX": 0.9997167207902494, - "velocityY": 3.9057545114907084, - "timestamp": 0.5076028440372377 + "x": 9.132548850946765, + "y": 4.529323965266172, + "heading": 0.8487168791915694, + "angularVelocity": -2.866893065306883, + "velocityX": -0.2040341775602698, + "velocityY": 2.6427792817240476, + "timestamp": 0.28028185939945877 }, { - "x": 9.304556200250488, - "y": 5.740914171376622, - "heading": -0.08010939802300308, - "angularVelocity": -9.303558071395237e-8, - "velocityX": 1.6848502888578374, - "velocityY": 3.6627353192211642, - "timestamp": 0.5801175360425573 + "x": 9.124926332439184, + "y": 4.64950362338172, + "heading": 0.7639545864905695, + "angularVelocity": -2.1169263332928523, + "velocityX": -0.19037132715629668, + "velocityY": 3.001470050939277, + "timestamp": 0.3203221250279529 + }, + { + "x": 9.118434012961915, + "y": 4.7840493325662905, + "heading": 0.7087764113022048, + "angularVelocity": -1.378067161200295, + "velocityX": -0.1621447654096238, + "velocityY": 3.3602601549350606, + "timestamp": 0.360362390656447 + }, + { + "x": 9.114821619295993, + "y": 4.932875254785403, + "heading": 0.6828478055885947, + "angularVelocity": -0.6475632792795059, + "velocityX": -0.09021902350671948, + "velocityY": 3.7169064661081084, + "timestamp": 0.4004026562849411 + }, + { + "x": 9.119733054745478, + "y": 5.094229573217587, + "heading": 0.6828477010361423, + "angularVelocity": -0.0000026111827875269163, + "velocityX": 0.12266240926382038, + "velocityY": 4.029801398657725, + "timestamp": 0.4404429219134352 + }, + { + "x": 9.140665009898335, + "y": 5.254295817851585, + "heading": 0.6828476668832145, + "angularVelocity": -8.529645663166833e-7, + "velocityX": 0.522772634603688, + "velocityY": 3.9976319367908455, + "timestamp": 0.4804831875419293 + }, + { + "x": 9.177410010476544, + "y": 5.411487271920564, + "heading": 0.6828476246296896, + "angularVelocity": -0.0000010552758389959, + "velocityX": 0.9177012190492992, + "velocityY": 3.925834446937349, + "timestamp": 0.5205234531704235 + }, + { + "x": 9.229603825061764, + "y": 5.564245747672749, + "heading": 0.6828475667567384, + "angularVelocity": -0.0000014453688142033488, + "velocityX": 1.3035331750660322, + "velocityY": 3.8151214372438793, + "timestamp": 0.5605637187989176 + }, + { + "x": 9.2967290739306, + "y": 5.711056988859694, + "heading": 0.6828474589270388, + "angularVelocity": -0.0000026930315748324016, + "velocityX": 1.6764436452933849, + "velocityY": 3.6665900908142905, + "timestamp": 0.6006039844274118 + }, + { + "x": 9.378040037181789, + "y": 5.850313712944271, + "heading": 0.6824260962947332, + "angularVelocity": -0.01052347245183362, + "velocityX": 2.03072986592535, + "velocityY": 3.477917089177032, + "timestamp": 0.640644250055906 }, { "x": 9.472442626953123, "y": 5.980258464813232, - "heading": -0.0801094140047848, - "angularVelocity": -2.2039370606335216e-7, - "velocityX": 2.3152056784619393, - "velocityY": 3.3006317315538127, - "timestamp": 0.652632228047877 - }, - { - "x": 9.690930195145093, - "y": 6.192521851895066, - "heading": -0.0801094275269848, - "angularVelocity": -1.7896807096367723e-7, - "velocityX": 2.891711304942813, - "velocityY": 2.809333460614483, - "timestamp": 0.7281887221621088 - }, - { - "x": 9.945264512695372, - "y": 6.36017218154714, - "heading": -0.0801094347291874, - "angularVelocity": -9.532208551658443e-8, - "velocityX": 3.3661476823654413, - "velocityY": 2.218873858792461, - "timestamp": 0.8037452162763407 - }, - { - "x": 10.226468274983382, - "y": 6.477291899741084, - "heading": -0.08010944009786584, - "angularVelocity": -7.105515558762335e-8, - "velocityX": 3.721768268692626, - "velocityY": 1.5500946618416913, - "timestamp": 0.8793017103905725 - }, - { - "x": 10.524615763044427, - "y": 6.5397471826115465, - "heading": -0.08010944490352531, - "angularVelocity": -6.360352642448382e-8, - "velocityX": 3.9460206770616324, - "velocityY": 0.8266037698366171, - "timestamp": 0.9548582045048043 - }, - { - "x": 10.828899774162563, - "y": 6.554024026243873, - "heading": -0.08010944967830837, - "angularVelocity": -6.319487302459263e-8, - "velocityX": 4.0272383556878255, - "velocityY": 0.18895587731666572, - "timestamp": 1.0304146986190361 - }, - { - "x": 11.133183845275457, - "y": 6.568299591143467, - "heading": -0.08010945445311723, - "angularVelocity": -6.319521471147181e-8, - "velocityX": 4.0272391497262205, - "velocityY": 0.18893895312309195, - "timestamp": 1.105971192733268 - }, - { - "x": 11.418815993462275, - "y": 6.581544555724133, - "heading": -0.1315821813078501, - "angularVelocity": -0.6812482164261439, - "velocityX": 3.7803785304672726, - "velocityY": 0.17529882422338566, - "timestamp": 1.1815276868475 - }, - { - "x": 11.647294799351972, - "y": 6.592156838882461, - "heading": -0.17650291196886067, - "angularVelocity": -0.594531696945813, - "velocityX": 3.023946631831091, - "velocityY": 0.1404549441148474, - "timestamp": 1.257084180961732 - }, - { - "x": 11.818643408754175, - "y": 6.600120231200814, - "heading": -0.21138621074303876, - "angularVelocity": -0.461684983972906, - "velocityX": 2.2678210709875604, - "velocityY": 0.10539653026134421, - "timestamp": 1.3326406750759638 - }, - { - "x": 11.932871296430273, - "y": 6.605430723246171, - "heading": -0.23512693570933194, - "angularVelocity": -0.3142115743274193, - "velocityX": 1.5118209098399886, - "velocityY": 0.07028505104178595, - "timestamp": 1.4081971691901958 + "heading": 0.6802910040496163, + "angularVelocity": -0.053323628392650334, + "velocityX": 2.3576913961339048, + "velocityY": 3.2453518933830967, + "timestamp": 0.6806845156844001 + }, + { + "x": 9.697571132700979, + "y": 6.190348552373689, + "heading": 0.6769837409938244, + "angularVelocity": -0.04315400893213956, + "velocityX": 2.9375339620804057, + "velocityY": 2.7413088593774586, + "timestamp": 0.7573231209410413 + }, + { + "x": 9.959401598982078, + "y": 6.354404479556993, + "heading": 0.6769835985523576, + "angularVelocity": -0.0000018586124631184994, + "velocityX": 3.4164304713571103, + "velocityY": 2.1406434346504692, + "timestamp": 0.8339617261976824 + }, + { + "x": 10.247599106550812, + "y": 6.465812536773354, + "heading": 0.676983552190923, + "angularVelocity": -6.049357816949861e-7, + "velocityX": 3.7604743275747707, + "velocityY": 1.4536806462406935, + "timestamp": 0.9106003314543235 + }, + { + "x": 10.551697598356373, + "y": 6.520526967813223, + "heading": 0.6769835217928931, + "angularVelocity": -3.9664122091915935e-7, + "velocityX": 3.9679544113207434, + "velocityY": 0.7139278025295573, + "timestamp": 0.9872389367109646 + }, + { + "x": 10.860221065632627, + "y": 6.537344592449922, + "heading": 0.6769834953565499, + "angularVelocity": -3.449481242526253e-7, + "velocityX": 4.025692615922479, + "velocityY": 0.21944064065877694, + "timestamp": 1.0638775419676059 + }, + { + "x": 11.1687445405045, + "y": 6.554162077742282, + "heading": 0.6769834689201766, + "angularVelocity": -3.4494851824762934e-7, + "velocityX": 4.025692715031963, + "velocityY": 0.21943882245931226, + "timestamp": 1.140516147224247 + }, + { + "x": 11.442585083138006, + "y": 6.576789374730947, + "heading": 0.6007167845106364, + "angularVelocity": -0.9951470822589282, + "velocityX": 3.5731410001068404, + "velocityY": 0.29524672210419306, + "timestamp": 1.2171547524808883 + }, + { + "x": 11.66406877710657, + "y": 6.593057077284222, + "heading": 0.3953817365180138, + "angularVelocity": -2.679263894547836, + "velocityX": 2.8899755316119893, + "velocityY": 0.21226511754568214, + "timestamp": 1.2937933577375296 + }, + { + "x": 11.829372604042955, + "y": 6.602362161514451, + "heading": 0.11020800066076368, + "angularVelocity": -3.721019385755826, + "velocityX": 2.1569263477960128, + "velocityY": 0.12141510403356627, + "timestamp": 1.3704319629941708 + }, + { + "x": 11.937154802881087, + "y": 6.606659009355426, + "heading": -0.1192351932122035, + "angularVelocity": -2.9938331093660486, + "velocityX": 1.4063695245653058, + "velocityY": 0.05606636272237923, + "timestamp": 1.447070568250812 }, { "x": 11.989983558654783, "y": 6.608086585998535, "heading": -0.2471764305163121, - "angularVelocity": -0.15947662670481846, - "velocityX": 0.7558881985466883, - "velocityY": 0.03515068801828196, - "timestamp": 1.4837536633044277 + "angularVelocity": -1.6694097821283331, + "velocityX": 0.6893230323906269, + "velocityY": 0.01862738287556901, + "timestamp": 1.5237091735074533 }, { "x": 11.989983558654783, "y": 6.608086585998535, "heading": -0.2471764305163121, - "angularVelocity": 4.5808750464576005e-27, - "velocityX": 4.658708906964682e-26, - "velocityY": 1.1001600166410631e-26, - "timestamp": 1.5593101574186596 + "angularVelocity": -4.255940633526814e-21, + "velocityX": -4.150602396756134e-21, + "velocityY": -2.2857309193220108e-20, + "timestamp": 1.6003477787640945 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red CWS to 5.1 2.traj b/src/main/deploy/choreo/Red CWS to 5.1 2.traj new file mode 100644 index 00000000..e5334c3c --- /dev/null +++ b/src/main/deploy/choreo/Red CWS to 5.1 2.traj @@ -0,0 +1,122 @@ +{ + "samples": [ + { + "x": 11.633423805236816, + "y": 6.701088905334473, + "heading": -0.23748596442563125, + "angularVelocity": 4.0633007276282394e-23, + "velocityX": 6.7184741034051145e-22, + "velocityY": 5.702159340164939e-22, + "timestamp": 0 + }, + { + "x": 11.558314016783294, + "y": 6.672811015849246, + "heading": -0.22610284681530854, + "angularVelocity": 0.1271903732506324, + "velocityX": -0.8392465364248116, + "velocityY": -0.31596575222103795, + "timestamp": 0.08949669160803514 + }, + { + "x": 11.40809470327327, + "y": 6.616254752098494, + "heading": -0.2032649088498646, + "angularVelocity": 0.25518192410358925, + "velocityX": -1.678490130479727, + "velocityY": -0.6319369234749451, + "timestamp": 0.17899338321607028 + }, + { + "x": 11.182766460817703, + "y": 6.531419518701983, + "heading": -0.16883330424227505, + "angularVelocity": 0.384724887467217, + "velocityX": -2.5177270572792247, + "velocityY": -0.947914739617524, + "timestamp": 0.2684900748241054 + }, + { + "x": 10.882330233029782, + "y": 6.418304523161176, + "heading": -0.12260874966451532, + "angularVelocity": 0.5164945625052538, + "velocityX": -3.3569534704515043, + "velocityY": -1.2639014234259063, + "timestamp": 0.35798676643214056 + }, + { + "x": 10.544678568903556, + "y": 6.291091383419362, + "heading": -0.12260874853394456, + "angularVelocity": 1.2632542470994776e-8, + "velocityX": -3.7727837463491594, + "velocityY": -1.4214284021947707, + "timestamp": 0.4474834580401757 + }, + { + "x": 10.20702690477794, + "y": 6.163878243477181, + "heading": -0.12260874740337606, + "angularVelocity": 1.2632517262639448e-8, + "velocityX": -3.772783744689189, + "velocityY": -1.4214284066008342, + "timestamp": 0.5369801496482108 + }, + { + "x": 9.869375240962672, + "y": 6.036665104106448, + "heading": -0.12260874627280524, + "angularVelocity": 1.263254313511497e-8, + "velocityX": -3.7727837462233933, + "velocityY": -1.421428402528582, + "timestamp": 0.626476841256246 + }, + { + "x": 9.568952859507139, + "y": 5.923536823964721, + "heading": -0.07364693995564293, + "angularVelocity": 0.5470795114035975, + "velocityX": -3.3567987476160837, + "velocityY": -1.2640498599150238, + "timestamp": 0.7159735328642811 + }, + { + "x": 9.34363659158264, + "y": 5.838690025801379, + "heading": -0.0368350275632345, + "angularVelocity": 0.4113214883253116, + "velocityX": -2.5175932510012293, + "velocityY": -0.948043960921556, + "timestamp": 0.8054702244723162 + }, + { + "x": 9.193425871877203, + "y": 5.782125253556939, + "heading": -0.012272886719585112, + "angularVelocity": 0.2744474728878599, + "velocityX": -1.6783941053799585, + "velocityY": -0.632031990730871, + "timestamp": 0.8949669160803513 + }, + { + "x": 9.11832046508789, + "y": 5.753842830657959, + "heading": -3.541695678797396e-22, + "angularVelocity": 0.13713229504992377, + "velocityX": -0.839197582807272, + "velocityY": -0.31601640745180465, + "timestamp": 0.9844636076883864 + }, + { + "x": 9.11832046508789, + "y": 5.753842830657959, + "heading": -1.742218903707832e-22, + "angularVelocity": 5.002572264403276e-23, + "velocityX": 8.398171768741014e-22, + "velocityY": 7.127695018988845e-22, + "timestamp": 1.0739602992964215 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red CWS to 5.1.traj b/src/main/deploy/choreo/Red CWS to 5.1.traj index e5334c3c..8cfbac08 100644 --- a/src/main/deploy/choreo/Red CWS to 5.1.traj +++ b/src/main/deploy/choreo/Red CWS to 5.1.traj @@ -3,120 +3,238 @@ { "x": 11.633423805236816, "y": 6.701088905334473, - "heading": -0.23748596442563125, - "angularVelocity": 4.0633007276282394e-23, - "velocityX": 6.7184741034051145e-22, - "velocityY": 5.702159340164939e-22, + "heading": -0.2489626838461611, + "angularVelocity": -3.9911915239081645e-28, + "velocityX": 3.750722112253487e-27, + "velocityY": 7.88866766822753e-27, "timestamp": 0 }, { - "x": 11.558314016783294, - "y": 6.672811015849246, - "heading": -0.22610284681530854, - "angularVelocity": 0.1271903732506324, - "velocityX": -0.8392465364248116, - "velocityY": -0.31596575222103795, - "timestamp": 0.08949669160803514 - }, - { - "x": 11.40809470327327, - "y": 6.616254752098494, - "heading": -0.2032649088498646, - "angularVelocity": 0.25518192410358925, - "velocityX": -1.678490130479727, - "velocityY": -0.6319369234749451, - "timestamp": 0.17899338321607028 - }, - { - "x": 11.182766460817703, - "y": 6.531419518701983, - "heading": -0.16883330424227505, - "angularVelocity": 0.384724887467217, - "velocityX": -2.5177270572792247, - "velocityY": -0.947914739617524, - "timestamp": 0.2684900748241054 - }, - { - "x": 10.882330233029782, - "y": 6.418304523161176, - "heading": -0.12260874966451532, - "angularVelocity": 0.5164945625052538, - "velocityX": -3.3569534704515043, - "velocityY": -1.2639014234259063, - "timestamp": 0.35798676643214056 - }, - { - "x": 10.544678568903556, - "y": 6.291091383419362, - "heading": -0.12260874853394456, - "angularVelocity": 1.2632542470994776e-8, - "velocityX": -3.7727837463491594, - "velocityY": -1.4214284021947707, - "timestamp": 0.4474834580401757 - }, - { - "x": 10.20702690477794, - "y": 6.163878243477181, - "heading": -0.12260874740337606, - "angularVelocity": 1.2632517262639448e-8, - "velocityX": -3.772783744689189, - "velocityY": -1.4214284066008342, - "timestamp": 0.5369801496482108 - }, - { - "x": 9.869375240962672, - "y": 6.036665104106448, - "heading": -0.12260874627280524, - "angularVelocity": 1.263254313511497e-8, - "velocityX": -3.7727837462233933, - "velocityY": -1.421428402528582, - "timestamp": 0.626476841256246 - }, - { - "x": 9.568952859507139, - "y": 5.923536823964721, - "heading": -0.07364693995564293, - "angularVelocity": 0.5470795114035975, - "velocityX": -3.3567987476160837, - "velocityY": -1.2640498599150238, - "timestamp": 0.7159735328642811 - }, - { - "x": 9.34363659158264, - "y": 5.838690025801379, - "heading": -0.0368350275632345, - "angularVelocity": 0.4113214883253116, - "velocityX": -2.5175932510012293, - "velocityY": -0.948043960921556, - "timestamp": 0.8054702244723162 - }, - { - "x": 9.193425871877203, - "y": 5.782125253556939, - "heading": -0.012272886719585112, - "angularVelocity": 0.2744474728878599, - "velocityX": -1.6783941053799585, - "velocityY": -0.632031990730871, - "timestamp": 0.8949669160803513 - }, - { - "x": 9.11832046508789, - "y": 5.753842830657959, - "heading": -3.541695678797396e-22, - "angularVelocity": 0.13713229504992377, - "velocityX": -0.839197582807272, - "velocityY": -0.31601640745180465, - "timestamp": 0.9844636076883864 - }, - { - "x": 9.11832046508789, - "y": 5.753842830657959, - "heading": -1.742218903707832e-22, - "angularVelocity": 5.002572264403276e-23, - "velocityX": 8.398171768741014e-22, - "velocityY": 7.127695018988845e-22, - "timestamp": 1.0739602992964215 + "x": 11.60880277589231, + "y": 6.695431678512334, + "heading": -0.22436926819218947, + "angularVelocity": 0.48466715646299596, + "velocityX": -0.485211344755504, + "velocityY": -0.11148805338511579, + "timestamp": 0.05074289711200875 + }, + { + "x": 11.559508022875812, + "y": 6.684033868840708, + "heading": -0.17658066697813565, + "angularVelocity": 0.9417791244470394, + "velocityX": -0.9714611467233332, + "velocityY": -0.22461881997920483, + "timestamp": 0.1014857942240175 + }, + { + "x": 11.485475054339716, + "y": 6.666795704493466, + "heading": -0.10751199441271837, + "angularVelocity": 1.361149569622653, + "velocityX": -1.4589819018941346, + "velocityY": -0.33971580907553484, + "timestamp": 0.15222869133602626 + }, + { + "x": 11.386615817361433, + "y": 6.643600374642708, + "heading": -0.020248136416301205, + "angularVelocity": 1.7197255766416497, + "velocityX": -1.9482379328886914, + "velocityY": -0.45711481154804423, + "timestamp": 0.202971588448035 + }, + { + "x": 11.26281279008371, + "y": 6.614317797925505, + "heading": 0.07980607344820692, + "angularVelocity": 1.9717874926149894, + "velocityX": -2.439809989651213, + "velocityY": -0.5770773523743771, + "timestamp": 0.2537144855600438 + }, + { + "x": 11.113964594314254, + "y": 6.578808862374169, + "heading": 0.18235735558472177, + "angularVelocity": 2.0209977745288255, + "velocityX": -2.9333799258819067, + "velocityY": -0.6997813994135037, + "timestamp": 0.3044573826720525 + }, + { + "x": 10.940530970314622, + "y": 6.536962266114887, + "heading": 0.2622073240603324, + "angularVelocity": 1.5736186347293408, + "velocityX": -3.4178896726530357, + "velocityY": -0.8246788938146384, + "timestamp": 0.35520027978406127 + }, + { + "x": 10.745529341112256, + "y": 6.489615298655598, + "heading": 0.27490923290973734, + "angularVelocity": 0.250318952450961, + "velocityX": -3.8429344854300362, + "velocityY": -0.9330757633876594, + "timestamp": 0.40594317689607 + }, + { + "x": 10.546411363954949, + "y": 6.442664173600506, + "heading": 0.2749092704823136, + "angularVelocity": 7.404499621771453e-7, + "velocityX": -3.924056143617072, + "velocityY": -0.9252748212513897, + "timestamp": 0.45668607400807876 + }, + { + "x": 10.349612345474712, + "y": 6.386784631613737, + "heading": 0.27490930845823947, + "angularVelocity": 7.483988509456561e-7, + "velocityX": -3.8783559804602334, + "velocityY": -1.1012288451607661, + "timestamp": 0.5074289711200876 + }, + { + "x": 10.161416053771973, + "y": 6.3065690994262695, + "heading": 0.2749093466778342, + "angularVelocity": 7.532008787450534e-7, + "velocityX": -3.70882039484896, + "velocityY": -1.5808228688717099, + "timestamp": 0.5581718682320963 + }, + { + "x": 9.999363853803423, + "y": 6.213952472147155, + "heading": 0.2662334616188675, + "angularVelocity": -0.18415006033277892, + "velocityX": -3.4396401287527363, + "velocityY": -1.9658348843200237, + "timestamp": 0.6052849945774712 + }, + { + "x": 9.858714919564335, + "y": 6.124248911568337, + "heading": 0.23140228534900956, + "angularVelocity": -0.7393093808829219, + "velocityX": -2.985344959025316, + "velocityY": -1.9040035662508, + "timestamp": 0.6523981209228461 + }, + { + "x": 9.739490758645776, + "y": 6.040365578797747, + "heading": 0.1893230151311145, + "angularVelocity": -0.8931538507850689, + "velocityX": -2.5305932797699513, + "velocityY": -1.7804662793052928, + "timestamp": 0.699511247268221 + }, + { + "x": 9.641470727591456, + "y": 5.9632238044536345, + "heading": 0.14586307693951026, + "angularVelocity": -0.9224592287298015, + "velocityX": -2.0805248697732264, + "velocityY": -1.637373282736633, + "timestamp": 0.7466243736135959 + }, + { + "x": 9.5645101355505, + "y": 5.893262282723749, + "heading": 0.103840530644284, + "angularVelocity": -0.8919498567590098, + "velocityX": -1.6335275964659406, + "velocityY": -1.4849687795501898, + "timestamp": 0.7937374999589708 + }, + { + "x": 9.50851272054425, + "y": 5.83073553526098, + "heading": 0.0648829602133342, + "angularVelocity": -0.8268941896439074, + "velocityX": -1.1885735324746936, + "velocityY": -1.3271619252010773, + "timestamp": 0.8408506263043457 + }, + { + "x": 9.473410925926867, + "y": 5.775809115188849, + "heading": 0.030031627756018166, + "angularVelocity": -0.7397372061838857, + "velocityX": -0.7450533925526411, + "velocityY": -1.165841121845301, + "timestamp": 0.8879637526497206 + }, + { + "x": 9.459155082702637, + "y": 5.7285990715026855, + "heading": -8.372965025783953e-28, + "angularVelocity": -0.637436529596096, + "velocityX": -0.3025875022541161, + "velocityY": -1.002057119709671, + "timestamp": 0.9350768789950955 + }, + { + "x": 9.516865915616641, + "y": 5.664716307428782, + "heading": -0.03919049606692322, + "angularVelocity": -0.40720727806946266, + "velocityX": 0.5996420955198722, + "velocityY": -0.6637712987778043, + "timestamp": 1.0313190096680105 + }, + { + "x": 9.660745822884815, + "y": 5.635065101552019, + "heading": -0.054962369362559035, + "angularVelocity": -0.16387701711672975, + "velocityX": 1.4949784077116863, + "velocityY": -0.3080896658193722, + "timestamp": 1.1275611403409254 + }, + { + "x": 9.837618900841257, + "y": 5.691130117547053, + "heading": -0.018915350923725704, + "angularVelocity": 0.3745450998102011, + "velocityX": 1.837792624911395, + "velocityY": 0.5825413008111203, + "timestamp": 1.2238032710138402 + }, + { + "x": 9.925745010375977, + "y": 5.720115661621094, + "heading": 6.588751442065098e-28, + "angularVelocity": 0.19653919537599127, + "velocityX": 0.9156708077694437, + "velocityY": 0.30117313354740216, + "timestamp": 1.320045401686755 + }, + { + "x": 9.925745010375977, + "y": 5.720115661621094, + "heading": 2.990444886128271e-28, + "angularVelocity": -2.5147012717587645e-28, + "velocityX": -1.474992207507212e-27, + "velocityY": -9.524307703496857e-27, + "timestamp": 1.41628753235967 } ], - "eventMarkers": [] + "eventMarkers": [ + { + "timestamp": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Red CWS to 5.traj b/src/main/deploy/choreo/Red CWS to 5.traj index e5334c3c..8cfbac08 100644 --- a/src/main/deploy/choreo/Red CWS to 5.traj +++ b/src/main/deploy/choreo/Red CWS to 5.traj @@ -3,120 +3,238 @@ { "x": 11.633423805236816, "y": 6.701088905334473, - "heading": -0.23748596442563125, - "angularVelocity": 4.0633007276282394e-23, - "velocityX": 6.7184741034051145e-22, - "velocityY": 5.702159340164939e-22, + "heading": -0.2489626838461611, + "angularVelocity": -3.9911915239081645e-28, + "velocityX": 3.750722112253487e-27, + "velocityY": 7.88866766822753e-27, "timestamp": 0 }, { - "x": 11.558314016783294, - "y": 6.672811015849246, - "heading": -0.22610284681530854, - "angularVelocity": 0.1271903732506324, - "velocityX": -0.8392465364248116, - "velocityY": -0.31596575222103795, - "timestamp": 0.08949669160803514 - }, - { - "x": 11.40809470327327, - "y": 6.616254752098494, - "heading": -0.2032649088498646, - "angularVelocity": 0.25518192410358925, - "velocityX": -1.678490130479727, - "velocityY": -0.6319369234749451, - "timestamp": 0.17899338321607028 - }, - { - "x": 11.182766460817703, - "y": 6.531419518701983, - "heading": -0.16883330424227505, - "angularVelocity": 0.384724887467217, - "velocityX": -2.5177270572792247, - "velocityY": -0.947914739617524, - "timestamp": 0.2684900748241054 - }, - { - "x": 10.882330233029782, - "y": 6.418304523161176, - "heading": -0.12260874966451532, - "angularVelocity": 0.5164945625052538, - "velocityX": -3.3569534704515043, - "velocityY": -1.2639014234259063, - "timestamp": 0.35798676643214056 - }, - { - "x": 10.544678568903556, - "y": 6.291091383419362, - "heading": -0.12260874853394456, - "angularVelocity": 1.2632542470994776e-8, - "velocityX": -3.7727837463491594, - "velocityY": -1.4214284021947707, - "timestamp": 0.4474834580401757 - }, - { - "x": 10.20702690477794, - "y": 6.163878243477181, - "heading": -0.12260874740337606, - "angularVelocity": 1.2632517262639448e-8, - "velocityX": -3.772783744689189, - "velocityY": -1.4214284066008342, - "timestamp": 0.5369801496482108 - }, - { - "x": 9.869375240962672, - "y": 6.036665104106448, - "heading": -0.12260874627280524, - "angularVelocity": 1.263254313511497e-8, - "velocityX": -3.7727837462233933, - "velocityY": -1.421428402528582, - "timestamp": 0.626476841256246 - }, - { - "x": 9.568952859507139, - "y": 5.923536823964721, - "heading": -0.07364693995564293, - "angularVelocity": 0.5470795114035975, - "velocityX": -3.3567987476160837, - "velocityY": -1.2640498599150238, - "timestamp": 0.7159735328642811 - }, - { - "x": 9.34363659158264, - "y": 5.838690025801379, - "heading": -0.0368350275632345, - "angularVelocity": 0.4113214883253116, - "velocityX": -2.5175932510012293, - "velocityY": -0.948043960921556, - "timestamp": 0.8054702244723162 - }, - { - "x": 9.193425871877203, - "y": 5.782125253556939, - "heading": -0.012272886719585112, - "angularVelocity": 0.2744474728878599, - "velocityX": -1.6783941053799585, - "velocityY": -0.632031990730871, - "timestamp": 0.8949669160803513 - }, - { - "x": 9.11832046508789, - "y": 5.753842830657959, - "heading": -3.541695678797396e-22, - "angularVelocity": 0.13713229504992377, - "velocityX": -0.839197582807272, - "velocityY": -0.31601640745180465, - "timestamp": 0.9844636076883864 - }, - { - "x": 9.11832046508789, - "y": 5.753842830657959, - "heading": -1.742218903707832e-22, - "angularVelocity": 5.002572264403276e-23, - "velocityX": 8.398171768741014e-22, - "velocityY": 7.127695018988845e-22, - "timestamp": 1.0739602992964215 + "x": 11.60880277589231, + "y": 6.695431678512334, + "heading": -0.22436926819218947, + "angularVelocity": 0.48466715646299596, + "velocityX": -0.485211344755504, + "velocityY": -0.11148805338511579, + "timestamp": 0.05074289711200875 + }, + { + "x": 11.559508022875812, + "y": 6.684033868840708, + "heading": -0.17658066697813565, + "angularVelocity": 0.9417791244470394, + "velocityX": -0.9714611467233332, + "velocityY": -0.22461881997920483, + "timestamp": 0.1014857942240175 + }, + { + "x": 11.485475054339716, + "y": 6.666795704493466, + "heading": -0.10751199441271837, + "angularVelocity": 1.361149569622653, + "velocityX": -1.4589819018941346, + "velocityY": -0.33971580907553484, + "timestamp": 0.15222869133602626 + }, + { + "x": 11.386615817361433, + "y": 6.643600374642708, + "heading": -0.020248136416301205, + "angularVelocity": 1.7197255766416497, + "velocityX": -1.9482379328886914, + "velocityY": -0.45711481154804423, + "timestamp": 0.202971588448035 + }, + { + "x": 11.26281279008371, + "y": 6.614317797925505, + "heading": 0.07980607344820692, + "angularVelocity": 1.9717874926149894, + "velocityX": -2.439809989651213, + "velocityY": -0.5770773523743771, + "timestamp": 0.2537144855600438 + }, + { + "x": 11.113964594314254, + "y": 6.578808862374169, + "heading": 0.18235735558472177, + "angularVelocity": 2.0209977745288255, + "velocityX": -2.9333799258819067, + "velocityY": -0.6997813994135037, + "timestamp": 0.3044573826720525 + }, + { + "x": 10.940530970314622, + "y": 6.536962266114887, + "heading": 0.2622073240603324, + "angularVelocity": 1.5736186347293408, + "velocityX": -3.4178896726530357, + "velocityY": -0.8246788938146384, + "timestamp": 0.35520027978406127 + }, + { + "x": 10.745529341112256, + "y": 6.489615298655598, + "heading": 0.27490923290973734, + "angularVelocity": 0.250318952450961, + "velocityX": -3.8429344854300362, + "velocityY": -0.9330757633876594, + "timestamp": 0.40594317689607 + }, + { + "x": 10.546411363954949, + "y": 6.442664173600506, + "heading": 0.2749092704823136, + "angularVelocity": 7.404499621771453e-7, + "velocityX": -3.924056143617072, + "velocityY": -0.9252748212513897, + "timestamp": 0.45668607400807876 + }, + { + "x": 10.349612345474712, + "y": 6.386784631613737, + "heading": 0.27490930845823947, + "angularVelocity": 7.483988509456561e-7, + "velocityX": -3.8783559804602334, + "velocityY": -1.1012288451607661, + "timestamp": 0.5074289711200876 + }, + { + "x": 10.161416053771973, + "y": 6.3065690994262695, + "heading": 0.2749093466778342, + "angularVelocity": 7.532008787450534e-7, + "velocityX": -3.70882039484896, + "velocityY": -1.5808228688717099, + "timestamp": 0.5581718682320963 + }, + { + "x": 9.999363853803423, + "y": 6.213952472147155, + "heading": 0.2662334616188675, + "angularVelocity": -0.18415006033277892, + "velocityX": -3.4396401287527363, + "velocityY": -1.9658348843200237, + "timestamp": 0.6052849945774712 + }, + { + "x": 9.858714919564335, + "y": 6.124248911568337, + "heading": 0.23140228534900956, + "angularVelocity": -0.7393093808829219, + "velocityX": -2.985344959025316, + "velocityY": -1.9040035662508, + "timestamp": 0.6523981209228461 + }, + { + "x": 9.739490758645776, + "y": 6.040365578797747, + "heading": 0.1893230151311145, + "angularVelocity": -0.8931538507850689, + "velocityX": -2.5305932797699513, + "velocityY": -1.7804662793052928, + "timestamp": 0.699511247268221 + }, + { + "x": 9.641470727591456, + "y": 5.9632238044536345, + "heading": 0.14586307693951026, + "angularVelocity": -0.9224592287298015, + "velocityX": -2.0805248697732264, + "velocityY": -1.637373282736633, + "timestamp": 0.7466243736135959 + }, + { + "x": 9.5645101355505, + "y": 5.893262282723749, + "heading": 0.103840530644284, + "angularVelocity": -0.8919498567590098, + "velocityX": -1.6335275964659406, + "velocityY": -1.4849687795501898, + "timestamp": 0.7937374999589708 + }, + { + "x": 9.50851272054425, + "y": 5.83073553526098, + "heading": 0.0648829602133342, + "angularVelocity": -0.8268941896439074, + "velocityX": -1.1885735324746936, + "velocityY": -1.3271619252010773, + "timestamp": 0.8408506263043457 + }, + { + "x": 9.473410925926867, + "y": 5.775809115188849, + "heading": 0.030031627756018166, + "angularVelocity": -0.7397372061838857, + "velocityX": -0.7450533925526411, + "velocityY": -1.165841121845301, + "timestamp": 0.8879637526497206 + }, + { + "x": 9.459155082702637, + "y": 5.7285990715026855, + "heading": -8.372965025783953e-28, + "angularVelocity": -0.637436529596096, + "velocityX": -0.3025875022541161, + "velocityY": -1.002057119709671, + "timestamp": 0.9350768789950955 + }, + { + "x": 9.516865915616641, + "y": 5.664716307428782, + "heading": -0.03919049606692322, + "angularVelocity": -0.40720727806946266, + "velocityX": 0.5996420955198722, + "velocityY": -0.6637712987778043, + "timestamp": 1.0313190096680105 + }, + { + "x": 9.660745822884815, + "y": 5.635065101552019, + "heading": -0.054962369362559035, + "angularVelocity": -0.16387701711672975, + "velocityX": 1.4949784077116863, + "velocityY": -0.3080896658193722, + "timestamp": 1.1275611403409254 + }, + { + "x": 9.837618900841257, + "y": 5.691130117547053, + "heading": -0.018915350923725704, + "angularVelocity": 0.3745450998102011, + "velocityX": 1.837792624911395, + "velocityY": 0.5825413008111203, + "timestamp": 1.2238032710138402 + }, + { + "x": 9.925745010375977, + "y": 5.720115661621094, + "heading": 6.588751442065098e-28, + "angularVelocity": 0.19653919537599127, + "velocityX": 0.9156708077694437, + "velocityY": 0.30117313354740216, + "timestamp": 1.320045401686755 + }, + { + "x": 9.925745010375977, + "y": 5.720115661621094, + "heading": 2.990444886128271e-28, + "angularVelocity": -2.5147012717587645e-28, + "velocityX": -1.474992207507212e-27, + "velocityY": -9.524307703496857e-27, + "timestamp": 1.41628753235967 } ], - "eventMarkers": [] + "eventMarkers": [ + { + "timestamp": 0, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Red CWS to 6.1 2.traj b/src/main/deploy/choreo/Red CWS to 6.1 2.traj new file mode 100644 index 00000000..5fe62072 --- /dev/null +++ b/src/main/deploy/choreo/Red CWS to 6.1 2.traj @@ -0,0 +1,257 @@ +{ + "samples": [ + { + "x": 11.584806442260742, + "y": 6.726798057556152, + "heading": -0.23521537604856152, + "angularVelocity": 1.679246454495359e-21, + "velocityX": 3.240973484928515e-21, + "velocityY": 9.774800818350314e-21, + "timestamp": 0 + }, + { + "x": 11.559846058604368, + "y": 6.7282627420674075, + "heading": -0.175905915948779, + "angularVelocity": 1.1294257669789647, + "velocityX": -0.47531878401457256, + "velocityY": 0.02789188140851985, + "timestamp": 0.052512933416089806 + }, + { + "x": 11.50927509086037, + "y": 6.730802663220887, + "heading": -0.06477184086460809, + "angularVelocity": 2.1163181687755372, + "velocityX": -0.9630192879018148, + "velocityY": 0.048367535161469424, + "timestamp": 0.10502586683217961 + }, + { + "x": 11.432308982244173, + "y": 6.73416856654449, + "heading": 0.08661428832321974, + "angularVelocity": 2.882835129173027, + "velocityX": -1.4656600499985364, + "velocityY": 0.06409665400140648, + "timestamp": 0.1575388002482694 + }, + { + "x": 11.328136267216523, + "y": 6.738372418011969, + "heading": 0.26104185351142656, + "angularVelocity": 3.3216115315082115, + "velocityX": -1.9837534917776496, + "velocityY": 0.08005364000769541, + "timestamp": 0.21005173366435922 + }, + { + "x": 11.196338566137927, + "y": 6.743423511543135, + "heading": 0.4326071308552776, + "angularVelocity": 3.267105190724006, + "velocityX": -2.5098141068274407, + "velocityY": 0.09618760946319428, + "timestamp": 0.262564667080449 + }, + { + "x": 11.039433341611105, + "y": 6.747254994852584, + "heading": 0.551089395188952, + "angularVelocity": 2.256249206169307, + "velocityX": -2.9879348632760006, + "velocityY": 0.07296266005527433, + "timestamp": 0.3150776004965388 + }, + { + "x": 10.857659948269687, + "y": 6.7497818385588255, + "heading": 0.6153346073386814, + "angularVelocity": 1.2234169369415742, + "velocityX": -3.461497606715688, + "velocityY": 0.04811850228015354, + "timestamp": 0.36759053391262864 + }, + { + "x": 10.650955539971601, + "y": 6.7509399404848525, + "heading": 0.6261370902534954, + "angularVelocity": 0.20571090228800812, + "velocityX": -3.936257124702039, + "velocityY": 0.02205365137457731, + "timestamp": 0.42010346732871845 + }, + { + "x": 10.440862907331802, + "y": 6.724782250121588, + "heading": 0.6261371223269522, + "angularVelocity": 6.107725221919589e-7, + "velocityX": -4.0007788362366865, + "velocityY": -0.49811900919924323, + "timestamp": 0.47261640074480826 + }, + { + "x": 10.235969605153617, + "y": 6.671473034783826, + "heading": 0.6261371641263257, + "angularVelocity": 7.959824540999628e-7, + "velocityX": -3.901768361609107, + "velocityY": -1.015163539150922, + "timestamp": 0.525129334160898 + }, + { + "x": 10.039769105618355, + "y": 6.591921175301338, + "heading": 0.62613723036689, + "angularVelocity": 0.0000012614142817927116, + "velocityX": -3.7362319484355355, + "velocityY": -1.5149003170727215, + "timestamp": 0.5776422675769878 + }, + { + "x": 9.855606702521321, + "y": 6.487483048858524, + "heading": 0.6261374583126166, + "angularVelocity": 0.0000043407540164823, + "velocityX": -3.506991347023285, + "velocityY": -1.9888077021943713, + "timestamp": 0.6301552009930775 + }, + { + "x": 9.687066510308895, + "y": 6.360242791536438, + "heading": 0.6279452088235031, + "angularVelocity": 0.0344248624726932, + "velocityX": -3.2094987129548294, + "velocityY": -2.423027034387871, + "timestamp": 0.6826681344091673 + }, + { + "x": 9.536823272705078, + "y": 6.212279796600342, + "heading": 0.6304013989449005, + "angularVelocity": 0.0467730511631427, + "velocityX": -2.86107112724699, + "velocityY": -2.817648630740343, + "timestamp": 0.735181067825257 + }, + { + "x": 9.40747136536471, + "y": 6.046554907280894, + "heading": 0.6312946793268718, + "angularVelocity": 0.01710285270996669, + "velocityX": -2.476587042148973, + "velocityY": -3.172988492315768, + "timestamp": 0.7874109733924533 + }, + { + "x": 9.300530012868803, + "y": 5.865158084455799, + "heading": 0.6312947993803139, + "angularVelocity": 0.0000022985575187704037, + "velocityX": -2.0475118868118565, + "velocityY": -3.4730451999709184, + "timestamp": 0.8396408789596495 + }, + { + "x": 9.217999283134946, + "y": 5.6714315991563975, + "heading": 0.631294847742659, + "angularVelocity": 9.259512276328381e-7, + "velocityX": -1.5801431926310214, + "velocityY": -3.7091103879204836, + "timestamp": 0.8918707845268457 + }, + { + "x": 9.161271222388658, + "y": 5.468643047045803, + "heading": 0.6312949083428273, + "angularVelocity": 0.00000116025804741646, + "velocityX": -1.0861222154286778, + "velocityY": -3.882613799668738, + "timestamp": 0.9441006900940419 + }, + { + "x": 9.131676641005107, + "y": 5.267072350448047, + "heading": 0.6455753422536832, + "angularVelocity": 0.273414890488045, + "velocityX": -0.5666213840932584, + "velocityY": -3.8592965928001113, + "timestamp": 0.996330595661238 + }, + { + "x": 9.117872673375993, + "y": 5.085803506042164, + "heading": 0.7054966882434258, + "angularVelocity": 1.1472612354745892, + "velocityX": -0.2642924102422165, + "velocityY": -3.4705949098965836, + "timestamp": 1.0485605012284342 + }, + { + "x": 9.112327598986647, + "y": 4.928252929148819, + "heading": 0.8165929141384998, + "angularVelocity": 2.127061588348913, + "velocityX": -0.10616665546460329, + "velocityY": -3.0164821318819866, + "timestamp": 1.1007904067956304 + }, + { + "x": 9.111030628381306, + "y": 4.796361729914607, + "heading": 0.967191729179588, + "angularVelocity": 2.8833828705153617, + "velocityX": -0.024831953864649082, + "velocityY": -2.5252046275387223, + "timestamp": 1.1530203123628266 + }, + { + "x": 9.111245919986635, + "y": 4.691776297697822, + "heading": 1.1173854949370192, + "angularVelocity": 2.875627748631485, + "velocityX": 0.004121998749548773, + "velocityY": -2.002405156222834, + "timestamp": 1.2052502179300228 + }, + { + "x": 9.11196538752937, + "y": 4.614168205295782, + "heading": 1.2463793467539295, + "angularVelocity": 2.469731668401231, + "velocityX": 0.013775011362305339, + "velocityY": -1.4858937912920456, + "timestamp": 1.257480123497219 + }, + { + "x": 9.112681926635897, + "y": 4.562954142521823, + "heading": 1.3406788657665367, + "angularVelocity": 1.8054698355004044, + "velocityX": 0.013718943174322498, + "velocityY": -0.9805505527490918, + "timestamp": 1.3097100290644152 + }, + { + "x": 9.113101959228516, + "y": 4.53757381439209, + "heading": 1.3909420707626845, + "angularVelocity": 0.9623453163529443, + "velocityX": 0.008041994105460236, + "velocityY": -0.48593478877875557, + "timestamp": 1.3619399346316114 + }, + { + "x": 9.113101959228516, + "y": 4.53757381439209, + "heading": 1.3909420707626845, + "angularVelocity": 1.3615947899255843e-21, + "velocityX": 7.756185722534006e-21, + "velocityY": 3.1945663211699707e-21, + "timestamp": 1.4141698401988076 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red CWS to 6.1.traj b/src/main/deploy/choreo/Red CWS to 6.1.traj index 5fe62072..c8147a8f 100644 --- a/src/main/deploy/choreo/Red CWS to 6.1.traj +++ b/src/main/deploy/choreo/Red CWS to 6.1.traj @@ -4,254 +4,264 @@ "x": 11.584806442260742, "y": 6.726798057556152, "heading": -0.23521537604856152, - "angularVelocity": 1.679246454495359e-21, - "velocityX": 3.240973484928515e-21, - "velocityY": 9.774800818350314e-21, + "angularVelocity": -1.366573471191084e-21, + "velocityX": -2.3464364057737896e-21, + "velocityY": 1.0451672994386824e-20, "timestamp": 0 }, { - "x": 11.559846058604368, - "y": 6.7282627420674075, - "heading": -0.175905915948779, - "angularVelocity": 1.1294257669789647, - "velocityX": -0.47531878401457256, - "velocityY": 0.02789188140851985, - "timestamp": 0.052512933416089806 + "x": 11.564450104674838, + "y": 6.730098062196231, + "heading": -0.17841273737044008, + "angularVelocity": 1.174862132661435, + "velocityX": -0.42103484531543234, + "velocityY": 0.06825476033261743, + "timestamp": 0.048348344115445675 }, { - "x": 11.50927509086037, - "y": 6.730802663220887, - "heading": -0.06477184086460809, - "angularVelocity": 2.1163181687755372, - "velocityX": -0.9630192879018148, - "velocityY": 0.048367535161469424, - "timestamp": 0.10502586683217961 + "x": 11.52300211540097, + "y": 6.736367992138528, + "heading": -0.07239159740120889, + "angularVelocity": 2.192859795075401, + "velocityX": -0.857278362520844, + "velocityY": 0.12968241326601065, + "timestamp": 0.09669668823089135 }, { - "x": 11.432308982244173, - "y": 6.73416856654449, - "heading": 0.08661428832321974, - "angularVelocity": 2.882835129173027, - "velocityX": -1.4656600499985364, - "velocityY": 0.06409665400140648, - "timestamp": 0.1575388002482694 + "x": 11.459608536886385, + "y": 6.745383256813499, + "heading": 0.07165765421788575, + "angularVelocity": 2.979404036571249, + "velocityX": -1.311184067921649, + "velocityY": 0.18646480742824664, + "timestamp": 0.14504503234633703 }, { - "x": 11.328136267216523, - "y": 6.738372418011969, - "heading": 0.26104185351142656, - "angularVelocity": 3.3216115315082115, - "velocityX": -1.9837534917776496, - "velocityY": 0.08005364000769541, - "timestamp": 0.21005173366435922 + "x": 11.373429519712674, + "y": 6.757049979355221, + "heading": 0.238364044928656, + "angularVelocity": 3.4480268923525164, + "velocityX": -1.7824605733739418, + "velocityY": 0.24130552462770832, + "timestamp": 0.1933933764617827 }, { - "x": 11.196338566137927, - "y": 6.743423511543135, - "heading": 0.4326071308552776, - "angularVelocity": 3.267105190724006, - "velocityX": -2.5098141068274407, - "velocityY": 0.09618760946319428, - "timestamp": 0.262564667080449 + "x": 11.263930858755637, + "y": 6.771011078683092, + "heading": 0.4063780100927094, + "angularVelocity": 3.4750717576360297, + "velocityX": -2.264786167145887, + "velocityY": 0.288760651133037, + "timestamp": 0.24174172057722837 }, { - "x": 11.039433341611105, - "y": 6.747254994852584, - "heading": 0.551089395188952, - "angularVelocity": 2.256249206169307, - "velocityX": -2.9879348632760006, - "velocityY": 0.07296266005527433, - "timestamp": 0.3150776004965388 + "x": 11.132329406214573, + "y": 6.785201936985669, + "heading": 0.5391063310201619, + "angularVelocity": 2.745250604871289, + "velocityX": -2.721943325024747, + "velocityY": 0.29351280922227524, + "timestamp": 0.29009006469267407 }, { - "x": 10.857659948269687, - "y": 6.7497818385588255, - "heading": 0.6153346073386814, - "angularVelocity": 1.2234169369415742, - "velocityX": -3.461497606715688, - "velocityY": 0.04811850228015354, - "timestamp": 0.36759053391262864 + "x": 10.979560638214958, + "y": 6.797464602241318, + "heading": 0.6268214972685803, + "angularVelocity": 1.8142331004961214, + "velocityX": -3.15975181352334, + "velocityY": 0.2536315458161054, + "timestamp": 0.33843840880811976 }, { - "x": 10.650955539971601, - "y": 6.7509399404848525, - "heading": 0.6261370902534954, - "angularVelocity": 0.20571090228800812, - "velocityX": -3.936257124702039, - "velocityY": 0.02205365137457731, - "timestamp": 0.42010346732871845 + "x": 10.805774130613937, + "y": 6.805172026883863, + "heading": 0.671009580464918, + "angularVelocity": 0.9139523597918021, + "velocityX": -3.5944665899214026, + "velocityY": 0.15941444910711128, + "timestamp": 0.38678675292356546 }, { - "x": 10.440862907331802, - "y": 6.724782250121588, - "heading": 0.6261371223269522, - "angularVelocity": 6.107725221919589e-7, - "velocityX": -4.0007788362366865, - "velocityY": -0.49811900919924323, - "timestamp": 0.47261640074480826 + "x": 10.613420175544595, + "y": 6.800914326974529, + "heading": 0.6764207825530686, + "angularVelocity": 0.1119211461561101, + "velocityX": -3.9785014065850066, + "velocityY": -0.0880629934086431, + "timestamp": 0.43513509703901115 }, { - "x": 10.235969605153617, - "y": 6.671473034783826, - "heading": 0.6261371641263257, - "angularVelocity": 7.959824540999628e-7, - "velocityX": -3.901768361609107, - "velocityY": -1.015163539150922, - "timestamp": 0.525129334160898 + "x": 10.420466599593615, + "y": 6.773266562186232, + "heading": 0.6764212702275126, + "angularVelocity": 0.000010086683483707061, + "velocityX": -3.9909035041660488, + "velocityY": -0.5718451230165833, + "timestamp": 0.48348344115445685 }, { - "x": 10.039769105618355, - "y": 6.591921175301338, - "heading": 0.62613723036689, - "angularVelocity": 0.0000012614142817927116, - "velocityX": -3.7362319484355355, - "velocityY": -1.5149003170727215, - "timestamp": 0.5776422675769878 + "x": 10.236338023860592, + "y": 6.7240814934873905, + "heading": 0.6866220877955258, + "angularVelocity": 0.21098587251831752, + "velocityX": -3.8083739805733132, + "velocityY": -1.017306168363344, + "timestamp": 0.5318317852699025 }, { - "x": 9.855606702521321, - "y": 6.487483048858524, - "heading": 0.6261374583126166, - "angularVelocity": 0.0000043407540164823, - "velocityX": -3.506991347023285, - "velocityY": -1.9888077021943713, - "timestamp": 0.6301552009930775 + "x": 10.065259656682233, + "y": 6.655935600538377, + "heading": 0.7131660865513336, + "angularVelocity": 0.549015674506377, + "velocityX": -3.538453494287846, + "velocityY": -1.409477288122344, + "timestamp": 0.5801801293853481 }, { - "x": 9.687066510308895, - "y": 6.360242791536438, - "heading": 0.6279452088235031, - "angularVelocity": 0.0344248624726932, - "velocityX": -3.2094987129548294, - "velocityY": -2.423027034387871, - "timestamp": 0.6826681344091673 + "x": 9.908917358821068, + "y": 6.57009894257851, + "heading": 0.7558905021068901, + "angularVelocity": 0.8836789829562635, + "velocityX": -3.233663959366384, + "velocityY": -1.7753794784569745, + "timestamp": 0.6285284735007938 }, { - "x": 9.536823272705078, - "y": 6.212279796600342, - "heading": 0.6304013989449005, - "angularVelocity": 0.0467730511631427, - "velocityX": -2.86107112724699, - "velocityY": -2.817648630740343, - "timestamp": 0.735181067825257 + "x": 9.768262559074747, + "y": 6.467223145022078, + "heading": 0.8117281749189443, + "angularVelocity": 1.1549035201438473, + "velocityX": -2.909195802249071, + "velocityY": -2.1278039494108354, + "timestamp": 0.6768768176162394 }, { - "x": 9.40747136536471, - "y": 6.046554907280894, - "heading": 0.6312946793268718, - "angularVelocity": 0.01710285270996669, - "velocityX": -2.476587042148973, - "velocityY": -3.172988492315768, - "timestamp": 0.7874109733924533 + "x": 9.644017833038875, + "y": 6.347779813464664, + "heading": 0.8760133154282924, + "angularVelocity": 1.3296244511673165, + "velocityX": -2.569782446719601, + "velocityY": -2.4704740926033413, + "timestamp": 0.725225161731685 }, { - "x": 9.300530012868803, - "y": 5.865158084455799, - "heading": 0.6312947993803139, - "angularVelocity": 0.0000022985575187704037, - "velocityX": -2.0475118868118565, - "velocityY": -3.4730451999709184, - "timestamp": 0.8396408789596495 + "x": 9.536823272705078, + "y": 6.212279796600342, + "heading": 0.9431692146044316, + "angularVelocity": 1.3890010176105514, + "velocityX": -2.2171299202690298, + "velocityY": -2.8025782339260843, + "timestamp": 0.7735735058471307 }, { - "x": 9.217999283134946, - "y": 5.6714315991563975, - "heading": 0.631294847742659, - "angularVelocity": 9.259512276328381e-7, - "velocityX": -1.5801431926310214, - "velocityY": -3.7091103879204836, - "timestamp": 0.8918707845268457 + "x": 9.430745921751914, + "y": 6.018891391860547, + "heading": 1.0161761837183603, + "angularVelocity": 1.2029114308437592, + "velocityX": -1.7478010601434386, + "velocityY": -3.1863961136520125, + "timestamp": 0.8342653965732162 }, { - "x": 9.161271222388658, - "y": 5.468643047045803, - "heading": 0.6312949083428273, - "angularVelocity": 0.00000116025804741646, - "velocityX": -1.0861222154286778, - "velocityY": -3.882613799668738, - "timestamp": 0.9441006900940419 + "x": 9.355607279894146, + "y": 5.806111268245478, + "heading": 1.0689458128041136, + "angularVelocity": 0.8694675426065267, + "velocityX": -1.2380342902298767, + "velocityY": -3.505906984763242, + "timestamp": 0.8949572872993017 }, { - "x": 9.131676641005107, - "y": 5.267072350448047, - "heading": 0.6455753422536832, - "angularVelocity": 0.273414890488045, - "velocityX": -0.5666213840932584, - "velocityY": -3.8592965928001113, - "timestamp": 0.996330595661238 + "x": 9.314003494910018, + "y": 5.578711557562336, + "heading": 1.103303372284574, + "angularVelocity": 0.5660980251138138, + "velocityX": -0.6854916610176038, + "velocityY": -3.746789034954244, + "timestamp": 0.9556491780253872 }, { - "x": 9.117872673375993, - "y": 5.085803506042164, - "heading": 0.7054966882434258, - "angularVelocity": 1.1472612354745892, - "velocityX": -0.2642924102422165, - "velocityY": -3.4705949098965836, - "timestamp": 1.0485605012284342 + "x": 9.308988486748726, + "y": 5.346516543470362, + "heading": 1.1325554053485514, + "angularVelocity": 0.481975972638548, + "velocityX": -0.08263061343597497, + "velocityY": -3.8257996466105637, + "timestamp": 1.0163410687514727 }, { - "x": 9.112327598986647, - "y": 4.928252929148819, - "heading": 0.8165929141384998, - "angularVelocity": 2.127061588348913, - "velocityX": -0.10616665546460329, - "velocityY": -3.0164821318819866, - "timestamp": 1.1007904067956304 + "x": 9.335752490116109, + "y": 5.1322569060829615, + "heading": 1.1866325336002834, + "angularVelocity": 0.8910107693924489, + "velocityX": 0.44098153883827, + "velocityY": -3.530284438729908, + "timestamp": 1.0770329594775583 }, { - "x": 9.111030628381306, - "y": 4.796361729914607, - "heading": 0.967191729179588, - "angularVelocity": 2.8833828705153617, - "velocityX": -0.024831953864649082, - "velocityY": -2.5252046275387223, - "timestamp": 1.1530203123628266 + "x": 9.37362677314899, + "y": 4.953188872464991, + "heading": 1.2470437020271732, + "angularVelocity": 0.9953746324947627, + "velocityX": 0.6240419037824511, + "velocityY": -2.9504441446082046, + "timestamp": 1.137724850203644 }, { - "x": 9.111245919986635, - "y": 4.691776297697822, - "heading": 1.1173854949370192, - "angularVelocity": 2.875627748631485, - "velocityX": 0.004121998749548773, - "velocityY": -2.002405156222834, - "timestamp": 1.2052502179300228 + "x": 9.410828568507965, + "y": 4.811020644344707, + "heading": 1.3015321362189178, + "angularVelocity": 0.8977877199058055, + "velocityX": 0.6129615491280562, + "velocityY": -2.3424583814978592, + "timestamp": 1.1984167409297295 }, { - "x": 9.11196538752937, - "y": 4.614168205295782, - "heading": 1.2463793467539295, - "angularVelocity": 2.469731668401231, - "velocityX": 0.013775011362305339, - "velocityY": -1.4858937912920456, - "timestamp": 1.257480123497219 + "x": 9.442024749275184, + "y": 4.705213488198149, + "heading": 1.345141357678761, + "angularVelocity": 0.7185345676024537, + "velocityX": 0.5140090446023834, + "velocityY": -1.7433491506154644, + "timestamp": 1.259108631655815 }, { - "x": 9.112681926635897, - "y": 4.562954142521823, - "heading": 1.3406788657665367, - "angularVelocity": 1.8054698355004044, - "velocityX": 0.013718943174322498, - "velocityY": -0.9805505527490918, - "timestamp": 1.3097100290644152 + "x": 9.464368518697883, + "y": 4.635140949017796, + "heading": 1.3754091619410362, + "angularVelocity": 0.49871249519774746, + "velocityX": 0.36815082139289257, + "velocityY": -1.154561809527337, + "timestamp": 1.3198005223819007 }, { - "x": 9.113101959228516, - "y": 4.53757381439209, + "x": 9.47612190246582, + "y": 4.60029935836792, "heading": 1.3909420707626845, - "angularVelocity": 0.9623453163529443, - "velocityX": 0.008041994105460236, - "velocityY": -0.48593478877875557, - "timestamp": 1.3619399346316114 + "angularVelocity": 0.2559305474886485, + "velocityX": 0.19365657631241665, + "velocityY": -0.5740732449269984, + "timestamp": 1.3804924131079863 }, { - "x": 9.113101959228516, - "y": 4.53757381439209, + "x": 9.47612190246582, + "y": 4.60029935836792, "heading": 1.3909420707626845, - "angularVelocity": 1.3615947899255843e-21, - "velocityX": 7.756185722534006e-21, - "velocityY": 3.1945663211699707e-21, - "timestamp": 1.4141698401988076 + "angularVelocity": -2.1753265335064506e-21, + "velocityX": 3.063926461949092e-22, + "velocityY": 1.462560101380818e-21, + "timestamp": 1.441184303834072 } ], - "eventMarkers": [] + "eventMarkers": [ + { + "timestamp": 0.7735735058471307, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Red CWS to 6.traj b/src/main/deploy/choreo/Red CWS to 6.traj index 5fe62072..c8147a8f 100644 --- a/src/main/deploy/choreo/Red CWS to 6.traj +++ b/src/main/deploy/choreo/Red CWS to 6.traj @@ -4,254 +4,264 @@ "x": 11.584806442260742, "y": 6.726798057556152, "heading": -0.23521537604856152, - "angularVelocity": 1.679246454495359e-21, - "velocityX": 3.240973484928515e-21, - "velocityY": 9.774800818350314e-21, + "angularVelocity": -1.366573471191084e-21, + "velocityX": -2.3464364057737896e-21, + "velocityY": 1.0451672994386824e-20, "timestamp": 0 }, { - "x": 11.559846058604368, - "y": 6.7282627420674075, - "heading": -0.175905915948779, - "angularVelocity": 1.1294257669789647, - "velocityX": -0.47531878401457256, - "velocityY": 0.02789188140851985, - "timestamp": 0.052512933416089806 + "x": 11.564450104674838, + "y": 6.730098062196231, + "heading": -0.17841273737044008, + "angularVelocity": 1.174862132661435, + "velocityX": -0.42103484531543234, + "velocityY": 0.06825476033261743, + "timestamp": 0.048348344115445675 }, { - "x": 11.50927509086037, - "y": 6.730802663220887, - "heading": -0.06477184086460809, - "angularVelocity": 2.1163181687755372, - "velocityX": -0.9630192879018148, - "velocityY": 0.048367535161469424, - "timestamp": 0.10502586683217961 + "x": 11.52300211540097, + "y": 6.736367992138528, + "heading": -0.07239159740120889, + "angularVelocity": 2.192859795075401, + "velocityX": -0.857278362520844, + "velocityY": 0.12968241326601065, + "timestamp": 0.09669668823089135 }, { - "x": 11.432308982244173, - "y": 6.73416856654449, - "heading": 0.08661428832321974, - "angularVelocity": 2.882835129173027, - "velocityX": -1.4656600499985364, - "velocityY": 0.06409665400140648, - "timestamp": 0.1575388002482694 + "x": 11.459608536886385, + "y": 6.745383256813499, + "heading": 0.07165765421788575, + "angularVelocity": 2.979404036571249, + "velocityX": -1.311184067921649, + "velocityY": 0.18646480742824664, + "timestamp": 0.14504503234633703 }, { - "x": 11.328136267216523, - "y": 6.738372418011969, - "heading": 0.26104185351142656, - "angularVelocity": 3.3216115315082115, - "velocityX": -1.9837534917776496, - "velocityY": 0.08005364000769541, - "timestamp": 0.21005173366435922 + "x": 11.373429519712674, + "y": 6.757049979355221, + "heading": 0.238364044928656, + "angularVelocity": 3.4480268923525164, + "velocityX": -1.7824605733739418, + "velocityY": 0.24130552462770832, + "timestamp": 0.1933933764617827 }, { - "x": 11.196338566137927, - "y": 6.743423511543135, - "heading": 0.4326071308552776, - "angularVelocity": 3.267105190724006, - "velocityX": -2.5098141068274407, - "velocityY": 0.09618760946319428, - "timestamp": 0.262564667080449 + "x": 11.263930858755637, + "y": 6.771011078683092, + "heading": 0.4063780100927094, + "angularVelocity": 3.4750717576360297, + "velocityX": -2.264786167145887, + "velocityY": 0.288760651133037, + "timestamp": 0.24174172057722837 }, { - "x": 11.039433341611105, - "y": 6.747254994852584, - "heading": 0.551089395188952, - "angularVelocity": 2.256249206169307, - "velocityX": -2.9879348632760006, - "velocityY": 0.07296266005527433, - "timestamp": 0.3150776004965388 + "x": 11.132329406214573, + "y": 6.785201936985669, + "heading": 0.5391063310201619, + "angularVelocity": 2.745250604871289, + "velocityX": -2.721943325024747, + "velocityY": 0.29351280922227524, + "timestamp": 0.29009006469267407 }, { - "x": 10.857659948269687, - "y": 6.7497818385588255, - "heading": 0.6153346073386814, - "angularVelocity": 1.2234169369415742, - "velocityX": -3.461497606715688, - "velocityY": 0.04811850228015354, - "timestamp": 0.36759053391262864 + "x": 10.979560638214958, + "y": 6.797464602241318, + "heading": 0.6268214972685803, + "angularVelocity": 1.8142331004961214, + "velocityX": -3.15975181352334, + "velocityY": 0.2536315458161054, + "timestamp": 0.33843840880811976 }, { - "x": 10.650955539971601, - "y": 6.7509399404848525, - "heading": 0.6261370902534954, - "angularVelocity": 0.20571090228800812, - "velocityX": -3.936257124702039, - "velocityY": 0.02205365137457731, - "timestamp": 0.42010346732871845 + "x": 10.805774130613937, + "y": 6.805172026883863, + "heading": 0.671009580464918, + "angularVelocity": 0.9139523597918021, + "velocityX": -3.5944665899214026, + "velocityY": 0.15941444910711128, + "timestamp": 0.38678675292356546 }, { - "x": 10.440862907331802, - "y": 6.724782250121588, - "heading": 0.6261371223269522, - "angularVelocity": 6.107725221919589e-7, - "velocityX": -4.0007788362366865, - "velocityY": -0.49811900919924323, - "timestamp": 0.47261640074480826 + "x": 10.613420175544595, + "y": 6.800914326974529, + "heading": 0.6764207825530686, + "angularVelocity": 0.1119211461561101, + "velocityX": -3.9785014065850066, + "velocityY": -0.0880629934086431, + "timestamp": 0.43513509703901115 }, { - "x": 10.235969605153617, - "y": 6.671473034783826, - "heading": 0.6261371641263257, - "angularVelocity": 7.959824540999628e-7, - "velocityX": -3.901768361609107, - "velocityY": -1.015163539150922, - "timestamp": 0.525129334160898 + "x": 10.420466599593615, + "y": 6.773266562186232, + "heading": 0.6764212702275126, + "angularVelocity": 0.000010086683483707061, + "velocityX": -3.9909035041660488, + "velocityY": -0.5718451230165833, + "timestamp": 0.48348344115445685 }, { - "x": 10.039769105618355, - "y": 6.591921175301338, - "heading": 0.62613723036689, - "angularVelocity": 0.0000012614142817927116, - "velocityX": -3.7362319484355355, - "velocityY": -1.5149003170727215, - "timestamp": 0.5776422675769878 + "x": 10.236338023860592, + "y": 6.7240814934873905, + "heading": 0.6866220877955258, + "angularVelocity": 0.21098587251831752, + "velocityX": -3.8083739805733132, + "velocityY": -1.017306168363344, + "timestamp": 0.5318317852699025 }, { - "x": 9.855606702521321, - "y": 6.487483048858524, - "heading": 0.6261374583126166, - "angularVelocity": 0.0000043407540164823, - "velocityX": -3.506991347023285, - "velocityY": -1.9888077021943713, - "timestamp": 0.6301552009930775 + "x": 10.065259656682233, + "y": 6.655935600538377, + "heading": 0.7131660865513336, + "angularVelocity": 0.549015674506377, + "velocityX": -3.538453494287846, + "velocityY": -1.409477288122344, + "timestamp": 0.5801801293853481 }, { - "x": 9.687066510308895, - "y": 6.360242791536438, - "heading": 0.6279452088235031, - "angularVelocity": 0.0344248624726932, - "velocityX": -3.2094987129548294, - "velocityY": -2.423027034387871, - "timestamp": 0.6826681344091673 + "x": 9.908917358821068, + "y": 6.57009894257851, + "heading": 0.7558905021068901, + "angularVelocity": 0.8836789829562635, + "velocityX": -3.233663959366384, + "velocityY": -1.7753794784569745, + "timestamp": 0.6285284735007938 }, { - "x": 9.536823272705078, - "y": 6.212279796600342, - "heading": 0.6304013989449005, - "angularVelocity": 0.0467730511631427, - "velocityX": -2.86107112724699, - "velocityY": -2.817648630740343, - "timestamp": 0.735181067825257 + "x": 9.768262559074747, + "y": 6.467223145022078, + "heading": 0.8117281749189443, + "angularVelocity": 1.1549035201438473, + "velocityX": -2.909195802249071, + "velocityY": -2.1278039494108354, + "timestamp": 0.6768768176162394 }, { - "x": 9.40747136536471, - "y": 6.046554907280894, - "heading": 0.6312946793268718, - "angularVelocity": 0.01710285270996669, - "velocityX": -2.476587042148973, - "velocityY": -3.172988492315768, - "timestamp": 0.7874109733924533 + "x": 9.644017833038875, + "y": 6.347779813464664, + "heading": 0.8760133154282924, + "angularVelocity": 1.3296244511673165, + "velocityX": -2.569782446719601, + "velocityY": -2.4704740926033413, + "timestamp": 0.725225161731685 }, { - "x": 9.300530012868803, - "y": 5.865158084455799, - "heading": 0.6312947993803139, - "angularVelocity": 0.0000022985575187704037, - "velocityX": -2.0475118868118565, - "velocityY": -3.4730451999709184, - "timestamp": 0.8396408789596495 + "x": 9.536823272705078, + "y": 6.212279796600342, + "heading": 0.9431692146044316, + "angularVelocity": 1.3890010176105514, + "velocityX": -2.2171299202690298, + "velocityY": -2.8025782339260843, + "timestamp": 0.7735735058471307 }, { - "x": 9.217999283134946, - "y": 5.6714315991563975, - "heading": 0.631294847742659, - "angularVelocity": 9.259512276328381e-7, - "velocityX": -1.5801431926310214, - "velocityY": -3.7091103879204836, - "timestamp": 0.8918707845268457 + "x": 9.430745921751914, + "y": 6.018891391860547, + "heading": 1.0161761837183603, + "angularVelocity": 1.2029114308437592, + "velocityX": -1.7478010601434386, + "velocityY": -3.1863961136520125, + "timestamp": 0.8342653965732162 }, { - "x": 9.161271222388658, - "y": 5.468643047045803, - "heading": 0.6312949083428273, - "angularVelocity": 0.00000116025804741646, - "velocityX": -1.0861222154286778, - "velocityY": -3.882613799668738, - "timestamp": 0.9441006900940419 + "x": 9.355607279894146, + "y": 5.806111268245478, + "heading": 1.0689458128041136, + "angularVelocity": 0.8694675426065267, + "velocityX": -1.2380342902298767, + "velocityY": -3.505906984763242, + "timestamp": 0.8949572872993017 }, { - "x": 9.131676641005107, - "y": 5.267072350448047, - "heading": 0.6455753422536832, - "angularVelocity": 0.273414890488045, - "velocityX": -0.5666213840932584, - "velocityY": -3.8592965928001113, - "timestamp": 0.996330595661238 + "x": 9.314003494910018, + "y": 5.578711557562336, + "heading": 1.103303372284574, + "angularVelocity": 0.5660980251138138, + "velocityX": -0.6854916610176038, + "velocityY": -3.746789034954244, + "timestamp": 0.9556491780253872 }, { - "x": 9.117872673375993, - "y": 5.085803506042164, - "heading": 0.7054966882434258, - "angularVelocity": 1.1472612354745892, - "velocityX": -0.2642924102422165, - "velocityY": -3.4705949098965836, - "timestamp": 1.0485605012284342 + "x": 9.308988486748726, + "y": 5.346516543470362, + "heading": 1.1325554053485514, + "angularVelocity": 0.481975972638548, + "velocityX": -0.08263061343597497, + "velocityY": -3.8257996466105637, + "timestamp": 1.0163410687514727 }, { - "x": 9.112327598986647, - "y": 4.928252929148819, - "heading": 0.8165929141384998, - "angularVelocity": 2.127061588348913, - "velocityX": -0.10616665546460329, - "velocityY": -3.0164821318819866, - "timestamp": 1.1007904067956304 + "x": 9.335752490116109, + "y": 5.1322569060829615, + "heading": 1.1866325336002834, + "angularVelocity": 0.8910107693924489, + "velocityX": 0.44098153883827, + "velocityY": -3.530284438729908, + "timestamp": 1.0770329594775583 }, { - "x": 9.111030628381306, - "y": 4.796361729914607, - "heading": 0.967191729179588, - "angularVelocity": 2.8833828705153617, - "velocityX": -0.024831953864649082, - "velocityY": -2.5252046275387223, - "timestamp": 1.1530203123628266 + "x": 9.37362677314899, + "y": 4.953188872464991, + "heading": 1.2470437020271732, + "angularVelocity": 0.9953746324947627, + "velocityX": 0.6240419037824511, + "velocityY": -2.9504441446082046, + "timestamp": 1.137724850203644 }, { - "x": 9.111245919986635, - "y": 4.691776297697822, - "heading": 1.1173854949370192, - "angularVelocity": 2.875627748631485, - "velocityX": 0.004121998749548773, - "velocityY": -2.002405156222834, - "timestamp": 1.2052502179300228 + "x": 9.410828568507965, + "y": 4.811020644344707, + "heading": 1.3015321362189178, + "angularVelocity": 0.8977877199058055, + "velocityX": 0.6129615491280562, + "velocityY": -2.3424583814978592, + "timestamp": 1.1984167409297295 }, { - "x": 9.11196538752937, - "y": 4.614168205295782, - "heading": 1.2463793467539295, - "angularVelocity": 2.469731668401231, - "velocityX": 0.013775011362305339, - "velocityY": -1.4858937912920456, - "timestamp": 1.257480123497219 + "x": 9.442024749275184, + "y": 4.705213488198149, + "heading": 1.345141357678761, + "angularVelocity": 0.7185345676024537, + "velocityX": 0.5140090446023834, + "velocityY": -1.7433491506154644, + "timestamp": 1.259108631655815 }, { - "x": 9.112681926635897, - "y": 4.562954142521823, - "heading": 1.3406788657665367, - "angularVelocity": 1.8054698355004044, - "velocityX": 0.013718943174322498, - "velocityY": -0.9805505527490918, - "timestamp": 1.3097100290644152 + "x": 9.464368518697883, + "y": 4.635140949017796, + "heading": 1.3754091619410362, + "angularVelocity": 0.49871249519774746, + "velocityX": 0.36815082139289257, + "velocityY": -1.154561809527337, + "timestamp": 1.3198005223819007 }, { - "x": 9.113101959228516, - "y": 4.53757381439209, + "x": 9.47612190246582, + "y": 4.60029935836792, "heading": 1.3909420707626845, - "angularVelocity": 0.9623453163529443, - "velocityX": 0.008041994105460236, - "velocityY": -0.48593478877875557, - "timestamp": 1.3619399346316114 + "angularVelocity": 0.2559305474886485, + "velocityX": 0.19365657631241665, + "velocityY": -0.5740732449269984, + "timestamp": 1.3804924131079863 }, { - "x": 9.113101959228516, - "y": 4.53757381439209, + "x": 9.47612190246582, + "y": 4.60029935836792, "heading": 1.3909420707626845, - "angularVelocity": 1.3615947899255843e-21, - "velocityX": 7.756185722534006e-21, - "velocityY": 3.1945663211699707e-21, - "timestamp": 1.4141698401988076 + "angularVelocity": -2.1753265335064506e-21, + "velocityX": 3.063926461949092e-22, + "velocityY": 1.462560101380818e-21, + "timestamp": 1.441184303834072 } ], - "eventMarkers": [] + "eventMarkers": [ + { + "timestamp": 0.7735735058471307, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Red CWS to 7.traj b/src/main/deploy/choreo/Red CWS to 7.traj new file mode 100644 index 00000000..e5334c3c --- /dev/null +++ b/src/main/deploy/choreo/Red CWS to 7.traj @@ -0,0 +1,122 @@ +{ + "samples": [ + { + "x": 11.633423805236816, + "y": 6.701088905334473, + "heading": -0.23748596442563125, + "angularVelocity": 4.0633007276282394e-23, + "velocityX": 6.7184741034051145e-22, + "velocityY": 5.702159340164939e-22, + "timestamp": 0 + }, + { + "x": 11.558314016783294, + "y": 6.672811015849246, + "heading": -0.22610284681530854, + "angularVelocity": 0.1271903732506324, + "velocityX": -0.8392465364248116, + "velocityY": -0.31596575222103795, + "timestamp": 0.08949669160803514 + }, + { + "x": 11.40809470327327, + "y": 6.616254752098494, + "heading": -0.2032649088498646, + "angularVelocity": 0.25518192410358925, + "velocityX": -1.678490130479727, + "velocityY": -0.6319369234749451, + "timestamp": 0.17899338321607028 + }, + { + "x": 11.182766460817703, + "y": 6.531419518701983, + "heading": -0.16883330424227505, + "angularVelocity": 0.384724887467217, + "velocityX": -2.5177270572792247, + "velocityY": -0.947914739617524, + "timestamp": 0.2684900748241054 + }, + { + "x": 10.882330233029782, + "y": 6.418304523161176, + "heading": -0.12260874966451532, + "angularVelocity": 0.5164945625052538, + "velocityX": -3.3569534704515043, + "velocityY": -1.2639014234259063, + "timestamp": 0.35798676643214056 + }, + { + "x": 10.544678568903556, + "y": 6.291091383419362, + "heading": -0.12260874853394456, + "angularVelocity": 1.2632542470994776e-8, + "velocityX": -3.7727837463491594, + "velocityY": -1.4214284021947707, + "timestamp": 0.4474834580401757 + }, + { + "x": 10.20702690477794, + "y": 6.163878243477181, + "heading": -0.12260874740337606, + "angularVelocity": 1.2632517262639448e-8, + "velocityX": -3.772783744689189, + "velocityY": -1.4214284066008342, + "timestamp": 0.5369801496482108 + }, + { + "x": 9.869375240962672, + "y": 6.036665104106448, + "heading": -0.12260874627280524, + "angularVelocity": 1.263254313511497e-8, + "velocityX": -3.7727837462233933, + "velocityY": -1.421428402528582, + "timestamp": 0.626476841256246 + }, + { + "x": 9.568952859507139, + "y": 5.923536823964721, + "heading": -0.07364693995564293, + "angularVelocity": 0.5470795114035975, + "velocityX": -3.3567987476160837, + "velocityY": -1.2640498599150238, + "timestamp": 0.7159735328642811 + }, + { + "x": 9.34363659158264, + "y": 5.838690025801379, + "heading": -0.0368350275632345, + "angularVelocity": 0.4113214883253116, + "velocityX": -2.5175932510012293, + "velocityY": -0.948043960921556, + "timestamp": 0.8054702244723162 + }, + { + "x": 9.193425871877203, + "y": 5.782125253556939, + "heading": -0.012272886719585112, + "angularVelocity": 0.2744474728878599, + "velocityX": -1.6783941053799585, + "velocityY": -0.632031990730871, + "timestamp": 0.8949669160803513 + }, + { + "x": 9.11832046508789, + "y": 5.753842830657959, + "heading": -3.541695678797396e-22, + "angularVelocity": 0.13713229504992377, + "velocityX": -0.839197582807272, + "velocityY": -0.31601640745180465, + "timestamp": 0.9844636076883864 + }, + { + "x": 9.11832046508789, + "y": 5.753842830657959, + "heading": -1.742218903707832e-22, + "angularVelocity": 5.002572264403276e-23, + "velocityX": 8.398171768741014e-22, + "velocityY": 7.127695018988845e-22, + "timestamp": 1.0739602992964215 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red CWS to 8.traj b/src/main/deploy/choreo/Red CWS to 8.traj new file mode 100644 index 00000000..5fe62072 --- /dev/null +++ b/src/main/deploy/choreo/Red CWS to 8.traj @@ -0,0 +1,257 @@ +{ + "samples": [ + { + "x": 11.584806442260742, + "y": 6.726798057556152, + "heading": -0.23521537604856152, + "angularVelocity": 1.679246454495359e-21, + "velocityX": 3.240973484928515e-21, + "velocityY": 9.774800818350314e-21, + "timestamp": 0 + }, + { + "x": 11.559846058604368, + "y": 6.7282627420674075, + "heading": -0.175905915948779, + "angularVelocity": 1.1294257669789647, + "velocityX": -0.47531878401457256, + "velocityY": 0.02789188140851985, + "timestamp": 0.052512933416089806 + }, + { + "x": 11.50927509086037, + "y": 6.730802663220887, + "heading": -0.06477184086460809, + "angularVelocity": 2.1163181687755372, + "velocityX": -0.9630192879018148, + "velocityY": 0.048367535161469424, + "timestamp": 0.10502586683217961 + }, + { + "x": 11.432308982244173, + "y": 6.73416856654449, + "heading": 0.08661428832321974, + "angularVelocity": 2.882835129173027, + "velocityX": -1.4656600499985364, + "velocityY": 0.06409665400140648, + "timestamp": 0.1575388002482694 + }, + { + "x": 11.328136267216523, + "y": 6.738372418011969, + "heading": 0.26104185351142656, + "angularVelocity": 3.3216115315082115, + "velocityX": -1.9837534917776496, + "velocityY": 0.08005364000769541, + "timestamp": 0.21005173366435922 + }, + { + "x": 11.196338566137927, + "y": 6.743423511543135, + "heading": 0.4326071308552776, + "angularVelocity": 3.267105190724006, + "velocityX": -2.5098141068274407, + "velocityY": 0.09618760946319428, + "timestamp": 0.262564667080449 + }, + { + "x": 11.039433341611105, + "y": 6.747254994852584, + "heading": 0.551089395188952, + "angularVelocity": 2.256249206169307, + "velocityX": -2.9879348632760006, + "velocityY": 0.07296266005527433, + "timestamp": 0.3150776004965388 + }, + { + "x": 10.857659948269687, + "y": 6.7497818385588255, + "heading": 0.6153346073386814, + "angularVelocity": 1.2234169369415742, + "velocityX": -3.461497606715688, + "velocityY": 0.04811850228015354, + "timestamp": 0.36759053391262864 + }, + { + "x": 10.650955539971601, + "y": 6.7509399404848525, + "heading": 0.6261370902534954, + "angularVelocity": 0.20571090228800812, + "velocityX": -3.936257124702039, + "velocityY": 0.02205365137457731, + "timestamp": 0.42010346732871845 + }, + { + "x": 10.440862907331802, + "y": 6.724782250121588, + "heading": 0.6261371223269522, + "angularVelocity": 6.107725221919589e-7, + "velocityX": -4.0007788362366865, + "velocityY": -0.49811900919924323, + "timestamp": 0.47261640074480826 + }, + { + "x": 10.235969605153617, + "y": 6.671473034783826, + "heading": 0.6261371641263257, + "angularVelocity": 7.959824540999628e-7, + "velocityX": -3.901768361609107, + "velocityY": -1.015163539150922, + "timestamp": 0.525129334160898 + }, + { + "x": 10.039769105618355, + "y": 6.591921175301338, + "heading": 0.62613723036689, + "angularVelocity": 0.0000012614142817927116, + "velocityX": -3.7362319484355355, + "velocityY": -1.5149003170727215, + "timestamp": 0.5776422675769878 + }, + { + "x": 9.855606702521321, + "y": 6.487483048858524, + "heading": 0.6261374583126166, + "angularVelocity": 0.0000043407540164823, + "velocityX": -3.506991347023285, + "velocityY": -1.9888077021943713, + "timestamp": 0.6301552009930775 + }, + { + "x": 9.687066510308895, + "y": 6.360242791536438, + "heading": 0.6279452088235031, + "angularVelocity": 0.0344248624726932, + "velocityX": -3.2094987129548294, + "velocityY": -2.423027034387871, + "timestamp": 0.6826681344091673 + }, + { + "x": 9.536823272705078, + "y": 6.212279796600342, + "heading": 0.6304013989449005, + "angularVelocity": 0.0467730511631427, + "velocityX": -2.86107112724699, + "velocityY": -2.817648630740343, + "timestamp": 0.735181067825257 + }, + { + "x": 9.40747136536471, + "y": 6.046554907280894, + "heading": 0.6312946793268718, + "angularVelocity": 0.01710285270996669, + "velocityX": -2.476587042148973, + "velocityY": -3.172988492315768, + "timestamp": 0.7874109733924533 + }, + { + "x": 9.300530012868803, + "y": 5.865158084455799, + "heading": 0.6312947993803139, + "angularVelocity": 0.0000022985575187704037, + "velocityX": -2.0475118868118565, + "velocityY": -3.4730451999709184, + "timestamp": 0.8396408789596495 + }, + { + "x": 9.217999283134946, + "y": 5.6714315991563975, + "heading": 0.631294847742659, + "angularVelocity": 9.259512276328381e-7, + "velocityX": -1.5801431926310214, + "velocityY": -3.7091103879204836, + "timestamp": 0.8918707845268457 + }, + { + "x": 9.161271222388658, + "y": 5.468643047045803, + "heading": 0.6312949083428273, + "angularVelocity": 0.00000116025804741646, + "velocityX": -1.0861222154286778, + "velocityY": -3.882613799668738, + "timestamp": 0.9441006900940419 + }, + { + "x": 9.131676641005107, + "y": 5.267072350448047, + "heading": 0.6455753422536832, + "angularVelocity": 0.273414890488045, + "velocityX": -0.5666213840932584, + "velocityY": -3.8592965928001113, + "timestamp": 0.996330595661238 + }, + { + "x": 9.117872673375993, + "y": 5.085803506042164, + "heading": 0.7054966882434258, + "angularVelocity": 1.1472612354745892, + "velocityX": -0.2642924102422165, + "velocityY": -3.4705949098965836, + "timestamp": 1.0485605012284342 + }, + { + "x": 9.112327598986647, + "y": 4.928252929148819, + "heading": 0.8165929141384998, + "angularVelocity": 2.127061588348913, + "velocityX": -0.10616665546460329, + "velocityY": -3.0164821318819866, + "timestamp": 1.1007904067956304 + }, + { + "x": 9.111030628381306, + "y": 4.796361729914607, + "heading": 0.967191729179588, + "angularVelocity": 2.8833828705153617, + "velocityX": -0.024831953864649082, + "velocityY": -2.5252046275387223, + "timestamp": 1.1530203123628266 + }, + { + "x": 9.111245919986635, + "y": 4.691776297697822, + "heading": 1.1173854949370192, + "angularVelocity": 2.875627748631485, + "velocityX": 0.004121998749548773, + "velocityY": -2.002405156222834, + "timestamp": 1.2052502179300228 + }, + { + "x": 9.11196538752937, + "y": 4.614168205295782, + "heading": 1.2463793467539295, + "angularVelocity": 2.469731668401231, + "velocityX": 0.013775011362305339, + "velocityY": -1.4858937912920456, + "timestamp": 1.257480123497219 + }, + { + "x": 9.112681926635897, + "y": 4.562954142521823, + "heading": 1.3406788657665367, + "angularVelocity": 1.8054698355004044, + "velocityX": 0.013718943174322498, + "velocityY": -0.9805505527490918, + "timestamp": 1.3097100290644152 + }, + { + "x": 9.113101959228516, + "y": 4.53757381439209, + "heading": 1.3909420707626845, + "angularVelocity": 0.9623453163529443, + "velocityX": 0.008041994105460236, + "velocityY": -0.48593478877875557, + "timestamp": 1.3619399346316114 + }, + { + "x": 9.113101959228516, + "y": 4.53757381439209, + "heading": 1.3909420707626845, + "angularVelocity": 1.3615947899255843e-21, + "velocityX": 7.756185722534006e-21, + "velocityY": 3.1945663211699707e-21, + "timestamp": 1.4141698401988076 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1) 2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1) 2.traj new file mode 100644 index 00000000..1eb15225 --- /dev/null +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1) 2.traj @@ -0,0 +1,897 @@ +{ + "samples": [ + { + "x": 15.198025703430176, + "y": 5.552863121032715, + "heading": 1.1788293001141444e-28, + "angularVelocity": 2.606876389920206e-29, + "velocityX": 1.6147849318653567e-22, + "velocityY": -1.7720524977117054e-24, + "timestamp": 0 + }, + { + "x": 15.10708649187578, + "y": 5.553873539056015, + "heading": 1.956931650032567e-18, + "angularVelocity": 2.0546044837018224e-17, + "velocityX": -0.9547809798230291, + "velocityY": 0.010608492132583473, + "timestamp": 0.09524614909196333 + }, + { + "x": 14.925208076423274, + "y": 5.555894375017549, + "heading": 4.8821720585032204e-18, + "angularVelocity": 3.0712427077008165e-17, + "velocityX": -1.909561879261876, + "velocityY": 0.02121698337202498, + "timestamp": 0.19049229818392666 + }, + { + "x": 14.65239049535407, + "y": 5.558925628491973, + "heading": 4.9529034125739184e-18, + "angularVelocity": 7.426164168014883e-19, + "velocityX": -2.864342376779887, + "velocityY": 0.03182547014575747, + "timestamp": 0.28573844727589 + }, + { + "x": 14.470512079901564, + "y": 5.560946464453506, + "heading": -5.155775683106669e-18, + "angularVelocity": -1.0613215539074661e-16, + "velocityX": -1.909561879261876, + "velocityY": 0.021216983372024975, + "timestamp": 0.3809845963678533 + }, + { + "x": 14.379572868347168, + "y": 5.561956882476807, + "heading": -2.3969600276869747e-28, + "angularVelocity": 5.413106704997615e-17, + "velocityX": -0.9547809798230291, + "velocityY": 0.010608492132583473, + "timestamp": 0.47623074545981664 + }, + { + "x": 14.379572868347168, + "y": 5.561956882476807, + "heading": -1.1788127589783305e-28, + "angularVelocity": 4.1297903821940984e-29, + "velocityX": -1.6147929435574846e-22, + "velocityY": 1.8251378962940275e-24, + "timestamp": 0.57147689455178 + }, + { + "x": 14.303949787857668, + "y": 5.561956882476807, + "heading": 4.2371272764586235e-25, + "angularVelocity": 4.879852542216884e-24, + "velocityX": -0.8707004020209776, + "velocityY": 4.615774993405339e-16, + "timestamp": 0.6583300535878512 + }, + { + "x": 14.152703635762082, + "y": 5.561956882476807, + "heading": 9.62666688048605e-25, + "angularVelocity": 6.205346660780305e-24, + "velocityX": -1.7414007017611408, + "velocityY": 9.23154942545405e-16, + "timestamp": 0.7451832126239225 + }, + { + "x": 13.925834474244276, + "y": 5.561956882476807, + "heading": -6.35334199578853e-25, + "angularVelocity": -1.8398880424876378e-23, + "velocityX": -2.6121002855357762, + "velocityY": 1.3847320055814999e-15, + "timestamp": 0.8320363716599938 + }, + { + "x": 13.774588363604606, + "y": 5.561956882476807, + "heading": -4.288440082411824e-25, + "angularVelocity": 2.3774632221708446e-24, + "velocityX": -1.7414002244507438, + "velocityY": 9.231546882646077e-16, + "timestamp": 0.918889530696065 + }, + { + "x": 13.717871079476808, + "y": 5.561956882476807, + "heading": -2.590171361320003e-25, + "angularVelocity": 3.910667216659344e-24, + "velocityX": -1.3060502198614803, + "velocityY": 6.923660434900599e-16, + "timestamp": 0.9623161028640279 + }, + { + "x": 13.680059555654978, + "y": 5.561956882476807, + "heading": -1.0277255516428025e-25, + "angularVelocity": 3.597902693388258e-24, + "velocityX": -0.8707001712128097, + "velocityY": 4.615773753780885e-16, + "timestamp": 1.0057426750319909 + }, + { + "x": 13.66115379333496, + "y": 5.561956882476807, + "heading": 4.1528332423630384e-27, + "angularVelocity": 2.4622111087440337e-24, + "velocityX": -0.43535009502700467, + "velocityY": 2.3078869268024055e-16, + "timestamp": 1.0491692471999539 + }, + { + "x": 13.66115379333496, + "y": 5.561956882476807, + "heading": 4.171558674345914e-27, + "angularVelocity": 4.311975603546073e-28, + "velocityX": -1.6145553776744748e-27, + "velocityY": 2.588288608432755e-27, + "timestamp": 1.0925958193679168 + }, + { + "x": 13.691513815926383, + "y": 5.525701060761086, + "heading": 0.023562463335368315, + "angularVelocity": 0.34182634637050774, + "velocityX": 0.4404401802325416, + "velocityY": -0.5259719620716655, + "timestamp": 1.1615269113845306 + }, + { + "x": 13.752233823940973, + "y": 5.453149387533433, + "heading": 0.06932129795634334, + "angularVelocity": 0.663834465438997, + "velocityX": 0.8808798212562146, + "velocityY": -1.0525246460648914, + "timestamp": 1.2304580034011443 + }, + { + "x": 13.843313276711916, + "y": 5.344233687044581, + "heading": 0.13498389635529368, + "angularVelocity": 0.9525831736877814, + "velocityX": 1.3213116186958351, + "velocityY": -1.580066372118415, + "timestamp": 1.299389095417758 + }, + { + "x": 13.964751433548663, + "y": 5.198837902534373, + "heading": 0.21647867376005756, + "angularVelocity": 1.1822644182848892, + "velocityX": 1.7617326707587837, + "velocityY": -2.109291761621367, + "timestamp": 1.3683201874343718 + }, + { + "x": 14.116535425780143, + "y": 5.016750311314966, + "heading": 0.30307237204373855, + "angularVelocity": 1.2562356949576599, + "velocityX": 2.2019670339024615, + "velocityY": -2.6415886632917585, + "timestamp": 1.4372512794509855 + }, + { + "x": 14.26793452721354, + "y": 4.8345584717676635, + "heading": 0.39204433398389704, + "angularVelocity": 1.290737740216195, + "velocityX": 2.196383330136527, + "velocityY": -2.6431010189624082, + "timestamp": 1.5061823714675993 + }, + { + "x": 14.389009217305178, + "y": 4.688953541309653, + "heading": 0.4689267567346741, + "angularVelocity": 1.1153518753517935, + "velocityX": 1.7564597709036147, + "velocityY": -2.1123258923987027, + "timestamp": 1.575113463484213 + }, + { + "x": 14.479788015237448, + "y": 4.5797997732912155, + "heading": 0.5284493552871049, + "angularVelocity": 0.8635087129924586, + "velocityX": 1.3169499463375665, + "velocityY": -1.583520075267771, + "timestamp": 1.6440445555008267 + }, + { + "x": 14.540294978830758, + "y": 4.507052135096341, + "heading": 0.568910729781111, + "angularVelocity": 0.5869829319438942, + "velocityX": 0.8777891343826024, + "velocityY": -1.0553675571734895, + "timestamp": 1.7129756475174405 + }, + { + "x": 14.570545196533203, + "y": 4.470687389373779, + "heading": 0.5894308145365931, + "angularVelocity": 0.29768982552222245, + "velocityX": 0.43884721418825445, + "velocityY": -0.5275521489460464, + "timestamp": 1.7819067395340542 + }, + { + "x": 14.570545196533203, + "y": 4.470687389373779, + "heading": 0.5894308145365931, + "angularVelocity": 3.863669117287725e-28, + "velocityX": -1.9871518401102085e-27, + "velocityY": 2.8088430719951805e-27, + "timestamp": 1.850837831550668 + }, + { + "x": 14.45709086591278, + "y": 4.4278439544867165, + "heading": 0.488089493217441, + "angularVelocity": -0.9146778399911556, + "velocityX": -1.0240064044825004, + "velocityY": -0.38669261432744145, + "timestamp": 1.961632383346229 + }, + { + "x": 14.230046021105487, + "y": 4.34248670484011, + "heading": 0.2854587836923866, + "angularVelocity": -1.8288869465255828, + "velocityX": -2.0492419629643726, + "velocityY": -0.7704101714686167, + "timestamp": 2.0724269351417903 + }, + { + "x": 14.002349455992734, + "y": 4.258200754530428, + "heading": 0.09515293619351321, + "angularVelocity": -1.7176462598091191, + "velocityX": -2.055124204418461, + "velocityY": -0.760740929438543, + "timestamp": 2.1832214869373514 + }, + { + "x": 13.888501167358198, + "y": 4.216057777427628, + "heading": 8.28079525180121e-11, + "angularVelocity": -0.8588232414738415, + "velocityX": -1.027562157068967, + "velocityY": -0.3803704823009896, + "timestamp": 2.2940160387329125 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 8.268158682804007e-19, + "angularVelocity": -7.474009357788098e-10, + "velocityX": -5.490850602427426e-10, + "velocityY": -2.0617274991432905e-10, + "timestamp": 2.4048105905284736 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 5.905827430465516e-19, + "angularVelocity": -6.406293808708934e-10, + "velocityX": -4.706443421676888e-10, + "velocityY": -1.7671950173476352e-10, + "timestamp": 2.4048105908972253 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 3.9372180312859395e-19, + "angularVelocity": -5.33857823518088e-10, + "velocityX": -3.922036224873799e-10, + "velocityY": -1.472662529525917e-10, + "timestamp": 2.404810591265977 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 2.3623304942893484e-19, + "angularVelocity": -4.270862637180886e-10, + "velocityX": -3.137629012016172e-10, + "velocityY": -1.1781300356773835e-10, + "timestamp": 2.4048105916347287 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 1.1811648285083364e-19, + "angularVelocity": -3.203147014685827e-10, + "velocityX": -2.353221783101984e-10, + "velocityY": -8.835975358012759e-11, + "timestamp": 2.4048105920034804 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 3.937210429839709e-20, + "angularVelocity": -2.1354313676727285e-10, + "velocityX": -1.568814538129234e-10, + "velocityY": -5.890650298968465e-11, + "timestamp": 2.404810592372232 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": -8.532341534406571e-26, + "angularVelocity": -1.0677156961184637e-10, + "velocityX": -7.84407277095909e-11, + "velocityY": -2.945325179633351e-11, + "timestamp": 2.4048105927409837 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": -8.510879482431963e-26, + "angularVelocity": -1.6866441387712243e-26, + "velocityX": -2.0583941335553829e-25, + "velocityY": 9.841185153624076e-26, + "timestamp": 2.4048105931097354 + }, + { + "x": 13.899986336038381, + "y": 4.272873358599484, + "heading": -0.023038991913601637, + "angularVelocity": -0.3021063684546837, + "velocityX": 0.15060305730606005, + "velocityY": 0.7450130183968425, + "timestamp": 2.481071785587899 + }, + { + "x": 13.922959662828976, + "y": 4.386578192081037, + "heading": -0.0663634759005527, + "angularVelocity": -0.5681065634969774, + "velocityX": 0.3012453129050349, + "velocityY": 1.4909920732502444, + "timestamp": 2.557332978066063 + }, + { + "x": 13.957441252375148, + "y": 4.557273568298352, + "heading": -0.12543465168712403, + "angularVelocity": -0.7745902452743002, + "velocityX": 0.45215119808209714, + "velocityY": 2.238299332470985, + "timestamp": 2.6335941705442267 + }, + { + "x": 14.00350258986825, + "y": 4.785079242541037, + "heading": -0.19129776367156157, + "angularVelocity": -0.8636517453263874, + "velocityX": 0.6039944563716563, + "velocityY": 2.987176922363417, + "timestamp": 2.7098553630223905 + }, + { + "x": 14.06127377629267, + "y": 5.069635038422791, + "heading": -0.23370464392529416, + "angularVelocity": -0.556074182368382, + "velocityX": 0.757543706662608, + "velocityY": 3.731331580780533, + "timestamp": 2.7861165555005543 + }, + { + "x": 14.121438982529087, + "y": 5.371150739294933, + "heading": -0.23370465578542146, + "angularVelocity": -1.5551982461065506e-7, + "velocityX": 0.7889360798238878, + "velocityY": 3.953723920045926, + "timestamp": 2.862377747978718 + }, + { + "x": 14.181604184628418, + "y": 5.672666440992599, + "heading": -0.23370466764554898, + "angularVelocity": -1.5551982762801367e-7, + "velocityX": 0.7889360255749569, + "velocityY": 3.9537239308708902, + "timestamp": 2.938638940456882 + }, + { + "x": 14.238820898893755, + "y": 5.957011236905596, + "heading": -0.27574816661054075, + "angularVelocity": -0.5513092255544053, + "velocityX": 0.7502730078830246, + "velocityY": 3.7285647742056445, + "timestamp": 3.0149001329350456 + }, + { + "x": 14.284420707366433, + "y": 6.1847837505193715, + "heading": -0.34721144483836985, + "angularVelocity": -0.9370857693877743, + "velocityX": 0.5979425051048811, + "velocityY": 2.98674209269669, + "timestamp": 3.0911613254132093 + }, + { + "x": 14.318439308265718, + "y": 6.35544203945297, + "heading": -0.41225183766095985, + "angularVelocity": -0.8528635693864027, + "velocityX": 0.44608010698268263, + "velocityY": 2.2378130132500083, + "timestamp": 3.167422517891373 + }, + { + "x": 14.341028135354547, + "y": 6.469114800114249, + "heading": -0.4602003337578672, + "angularVelocity": -0.6287404450256496, + "velocityX": 0.2962034339457177, + "velocityY": 1.490571507832474, + "timestamp": 3.243683710369537 + }, + { + "x": 14.352291107177734, + "y": 6.525911808013916, + "heading": -0.48570563852984416, + "angularVelocity": -0.33444670799345433, + "velocityX": 0.14768942704917307, + "velocityY": 0.7447694699493517, + "timestamp": 3.3199449028477006 + }, + { + "x": 14.352291107177734, + "y": 6.525911808013916, + "heading": -0.48570563852984416, + "angularVelocity": 1.0539627829247847e-26, + "velocityX": -5.1337066004257686e-26, + "velocityY": -2.752712125666017e-26, + "timestamp": 3.3962060953258644 + }, + { + "x": 14.281244102466694, + "y": 6.578011637665101, + "heading": -0.49633092681747565, + "angularVelocity": -0.11332344447203622, + "velocityX": -0.7577480323661817, + "velocityY": 0.555667949202982, + "timestamp": 3.489966820100281 + }, + { + "x": 14.139150572999801, + "y": 6.682211842382296, + "heading": -0.5176048283391019, + "angularVelocity": -0.22689565991314806, + "velocityX": -1.515490945795936, + "velocityY": 1.1113417154986245, + "timestamp": 3.5837275448746975 + }, + { + "x": 13.926011946794734, + "y": 6.838513890829568, + "heading": -0.5495863998363797, + "angularVelocity": -0.3410977418767151, + "velocityX": -2.273218628779447, + "velocityY": 1.6670311457524134, + "timestamp": 3.677488269649114 + }, + { + "x": 13.783920159167808, + "y": 6.942715781556149, + "heading": -0.5709265094735697, + "angularVelocity": -0.22760179903187838, + "velocityX": -1.5154723682948443, + "velocityY": 1.111359697541642, + "timestamp": 3.7712489944235306 + }, + { + "x": 13.738450910492517, + "y": 6.97606035494089, + "heading": -0.577756226947398, + "angularVelocity": -0.1821053451622054, + "velocityX": -1.2123771233598004, + "velocityY": 0.8890887608138573, + "timestamp": 3.808753206437433 + }, + { + "x": 13.704348988985252, + "y": 7.001068805104032, + "heading": -0.582878916551458, + "angularVelocity": -0.13658971430091627, + "velocityX": -0.9092824425859869, + "velocityY": 0.6668171072057679, + "timestamp": 3.846257418451336 + }, + { + "x": 13.681614380767433, + "y": 7.017741113917072, + "heading": -0.5862941684873881, + "angularVelocity": -0.09106315671061506, + "velocityX": -0.6061881318661277, + "velocityY": 0.44454497022520045, + "timestamp": 3.8837616304652385 + }, + { + "x": 13.670247077941896, + "y": 7.0260772705078125, + "heading": -0.5880017968591525, + "angularVelocity": -0.04553164244942512, + "velocityX": -0.3030940317136526, + "velocityY": 0.2222725433519379, + "timestamp": 3.921265842479141 + }, + { + "x": 13.670247077941896, + "y": 7.0260772705078125, + "heading": -0.5880017968591525, + "angularVelocity": 4.3220894143993686e-27, + "velocityX": -2.1854050012083175e-26, + "velocityY": 3.319166050857422e-26, + "timestamp": 3.9587700544930438 + }, + { + "x": 13.645024237315694, + "y": 7.029213812474465, + "heading": -0.5586050184455363, + "angularVelocity": 0.5751667660211687, + "velocityX": -0.49350100438627437, + "velocityY": 0.061368449088751095, + "timestamp": 4.009880063190129 + }, + { + "x": 13.59445850581916, + "y": 7.035391208480669, + "heading": -0.5016300369058596, + "angularVelocity": 1.1147519437406412, + "velocityX": -0.9893508685592758, + "velocityY": 0.1208647027007058, + "timestamp": 4.060990071887214 + }, + { + "x": 13.518403853674121, + "y": 7.044499236825847, + "heading": -0.41964577392325547, + "angularVelocity": 1.6040745261559681, + "velocityX": -1.4880578987140245, + "velocityY": 0.17820439826490078, + "timestamp": 4.112100080584299 + }, + { + "x": 13.416671986943191, + "y": 7.056423820145038, + "heading": -0.3169282187933418, + "angularVelocity": 2.0097346439264405, + "velocityX": -1.9904490201491905, + "velocityY": 0.2333120972423329, + "timestamp": 4.163210089281384 + }, + { + "x": 13.289036124529042, + "y": 7.071060906808593, + "heading": -0.20104341684391944, + "angularVelocity": 2.2673602471140164, + "velocityX": -2.497277258757889, + "velocityY": 0.28638395955486035, + "timestamp": 4.21432009797847 + }, + { + "x": 13.135342270967683, + "y": 7.08828685694279, + "heading": -0.08640408959186506, + "angularVelocity": 2.2429917383010047, + "velocityX": -3.007118517084588, + "velocityY": 0.3370367286824492, + "timestamp": 4.265430106675555 + }, + { + "x": 12.956762188284944, + "y": 7.1075748581915015, + "heading": -0.010726520537828105, + "angularVelocity": 1.48068002693087, + "velocityX": -3.4940335021489295, + "velocityY": 0.3773820772175376, + "timestamp": 4.31654011537264 + }, + { + "x": 12.755389428383719, + "y": 7.12888688951605, + "heading": -1.8456979469985217e-7, + "angularVelocity": 0.20986762165519152, + "velocityX": -3.9399868056119236, + "velocityY": 0.41698351982012405, + "timestamp": 4.367650124069725 + }, + { + "x": 12.55069190372683, + "y": 7.152531606779918, + "heading": -1.385534968264371e-7, + "angularVelocity": 9.003382908060325e-7, + "velocityX": -4.00503795391767, + "velocityY": 0.4626240117469566, + "timestamp": 4.418760132766811 + }, + { + "x": 12.345994384979965, + "y": 7.176176375208352, + "heading": -9.253727372935047e-8, + "angularVelocity": 9.003368277593551e-7, + "velocityX": -4.005037838284301, + "velocityY": 0.46262501281442936, + "timestamp": 4.469870141463896 + }, + { + "x": 12.141296845030876, + "y": 7.1998209600841285, + "heading": -4.652103543469606e-8, + "angularVelocity": 9.003371251094857e-7, + "velocityX": -4.005038253119366, + "velocityY": 0.46262142148932034, + "timestamp": 4.520980150160981 + }, + { + "x": 11.93593692779541, + "y": 7.216775417327881, + "heading": -7.736119630596452e-26, + "angularVelocity": 9.102138039226991e-7, + "velocityX": -4.017998088252668, + "velocityY": 0.33172479668779814, + "timestamp": 4.5720901588580665 + }, + { + "x": 11.603934607227266, + "y": 7.244185435917073, + "heading": 2.6079666498891626e-17, + "angularVelocity": 3.1562448816546114e-16, + "velocityX": -4.017998550432804, + "velocityY": 0.33172483484526066, + "timestamp": 4.654718939431514 + }, + { + "x": 11.271932286659125, + "y": 7.271595454506265, + "heading": 1.288603670926865e-16, + "angularVelocity": 1.2438849984287554e-15, + "velocityX": -4.017998550432804, + "velocityY": 0.33172483484526066, + "timestamp": 4.737347720004961 + }, + { + "x": 10.93992996609098, + "y": 7.299005473095457, + "heading": 2.271885871201871e-16, + "angularVelocity": 1.1899996507881914e-15, + "velocityX": -4.017998550432804, + "velocityY": 0.3317248348452606, + "timestamp": 4.8199765005784085 + }, + { + "x": 10.607927645522839, + "y": 7.326415491684649, + "heading": 2.5614842689098653e-16, + "angularVelocity": 3.504812677828607e-16, + "velocityX": -4.017998550432804, + "velocityY": 0.3317248348452606, + "timestamp": 4.902605281151856 + }, + { + "x": 10.275925324954695, + "y": 7.3538255102738415, + "heading": 2.977295806911055e-16, + "angularVelocity": 5.032284575841044e-16, + "velocityX": -4.017998550432804, + "velocityY": 0.3317248348452606, + "timestamp": 4.985234061725303 + }, + { + "x": 9.943923004386694, + "y": 7.381235528863022, + "heading": 2.9677600290999526e-16, + "angularVelocity": -1.1540504102827907e-17, + "velocityX": -4.017998550431105, + "velocityY": 0.3317248348451205, + "timestamp": 5.067862842298751 + }, + { + "x": 9.671068786342074, + "y": 7.4037622966758025, + "heading": 1.766717126358e-16, + "angularVelocity": -1.453540636092317e-15, + "velocityX": -3.302169246006048, + "velocityY": 0.2726261679821815, + "timestamp": 5.150491622872198 + }, + { + "x": 9.466428105073092, + "y": 7.420657373999625, + "heading": 8.769803938528749e-17, + "angularVelocity": -1.0767879258667162e-15, + "velocityX": -2.476627149145434, + "velocityY": 0.20446964370732854, + "timestamp": 5.233120403445645 + }, + { + "x": 9.330000979086366, + "y": 7.4319207593065935, + "heading": 3.000420973683897e-17, + "angularVelocity": -6.982292277353821e-16, + "velocityX": -1.6510848283118424, + "velocityY": 0.13631310094132928, + "timestamp": 5.315749184019093 + }, + { + "x": 9.26178741455078, + "y": 7.437552452087402, + "heading": -5.153249325171924e-25, + "angularVelocity": -3.631205742599535e-16, + "velocityX": -0.8255424328204009, + "velocityY": 0.06815655201159886, + "timestamp": 5.39837796459254 + }, + { + "x": 9.26178741455078, + "y": 7.437552452087402, + "heading": -5.204793542360701e-25, + "angularVelocity": -6.317967395111268e-26, + "velocityX": -1.2129880403241932e-23, + "velocityY": 1.4316814179344083e-22, + "timestamp": 5.481006745165987 + }, + { + "x": 9.329755853999478, + "y": 7.419457395378352, + "heading": -0.013376630543289742, + "angularVelocity": -0.15963467895053327, + "velocityX": 0.8111250419190595, + "velocityY": -0.21594366077412155, + "timestamp": 5.564802012154527 + }, + { + "x": 9.46569267412481, + "y": 7.383267297606982, + "heading": -0.04017390350981554, + "angularVelocity": -0.31979458899738056, + "velocityX": 1.6222493824612316, + "velocityY": -0.4318871348225413, + "timestamp": 5.648597279143067 + }, + { + "x": 9.669597656216824, + "y": 7.32898221699986, + "heading": -0.08046638000926655, + "angularVelocity": -0.4808443000123345, + "velocityX": 2.4333711129520146, + "velocityY": -0.6478299140039275, + "timestamp": 5.732392546131607 + }, + { + "x": 9.94147049419242, + "y": 7.256602235044674, + "heading": -0.13433954177465304, + "angularVelocity": -0.6429141370568642, + "velocityX": 3.2444891906935323, + "velocityY": -0.8637717207236001, + "timestamp": 5.816187813120147 + }, + { + "x": 10.267933929791393, + "y": 7.169688743599082, + "heading": -0.1343395455343343, + "angularVelocity": -4.4867465828483885e-8, + "velocityX": 3.8959650984061396, + "velocityY": -1.0372124174684092, + "timestamp": 5.899983080108687 + }, + { + "x": 10.594397365390467, + "y": 7.082775252153461, + "heading": -0.13433954929395236, + "angularVelocity": -4.486671145978699e-8, + "velocityX": 3.8959650984073204, + "velocityY": -1.0372124174687236, + "timestamp": 5.9837783470972274 + }, + { + "x": 10.92086080098954, + "y": 6.99586176070784, + "heading": -0.1343395530535705, + "angularVelocity": -4.486671228851664e-8, + "velocityX": 3.8959650984073204, + "velocityY": -1.0372124174687236, + "timestamp": 6.0675736140857675 + }, + { + "x": 11.247324236588515, + "y": 6.908948269262247, + "heading": -0.13433955681325235, + "angularVelocity": -4.486747226873845e-8, + "velocityX": 3.895965098406137, + "velocityY": -1.0372124174684085, + "timestamp": 6.1513688810743075 + }, + { + "x": 11.519194528616957, + "y": 6.836568965107796, + "heading": -0.18900176869601656, + "angularVelocity": -0.6523305414163806, + "velocityX": 3.244458807746568, + "velocityY": -0.8637636319525063, + "timestamp": 6.2351641480628475 + }, + { + "x": 11.723097253083957, + "y": 6.7822844855421724, + "heading": -0.2300018718976995, + "angularVelocity": -0.4892890096918049, + "velocityX": 2.4333441707977026, + "velocityY": -0.6478227412659024, + "timestamp": 6.3189594150513875 + }, + { + "x": 11.859032415291114, + "y": 6.746094829153945, + "heading": -0.2573341284430376, + "angularVelocity": -0.32617900184119325, + "velocityX": 1.6222295971173368, + "velocityY": -0.4318818674231029, + "timestamp": 6.4027546820399275 + }, + { + "x": 11.927, + "y": 6.728, + "heading": -0.271, + "angularVelocity": -0.16308643731431074, + "velocityX": 0.8111148415838417, + "velocityY": -0.21594094516602882, + "timestamp": 6.4865499490284675 + }, + { + "x": 11.927, + "y": 6.728, + "heading": -0.271, + "angularVelocity": -1.104828910146849e-25, + "velocityX": -2.8779219726051263e-23, + "velocityY": -1.0811880313092697e-22, + "timestamp": 6.5703452160170075 + } + ], + "eventMarkers": [ + { + "timestamp": 0.918889530696065, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + }, + { + "timestamp": 2.4048105905284736, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + }, + { + "timestamp": 3.7712489944235306, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).1 2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).1 2.traj new file mode 100644 index 00000000..d94e4138 --- /dev/null +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).1 2.traj @@ -0,0 +1,68 @@ +{ + "samples": [ + { + "x": 15.198025703430176, + "y": 5.552863121032715, + "heading": 1.1788293001141444e-28, + "angularVelocity": 2.606876389920206e-29, + "velocityX": 1.6147849318653567e-22, + "velocityY": -1.7720524977117054e-24, + "timestamp": 0 + }, + { + "x": 15.10708649187578, + "y": 5.553873539056015, + "heading": 1.956931650032567e-18, + "angularVelocity": 2.0546044837018224e-17, + "velocityX": -0.9547809798230291, + "velocityY": 0.010608492132583473, + "timestamp": 0.09524614909196333 + }, + { + "x": 14.925208076423274, + "y": 5.555894375017549, + "heading": 4.8821720585032204e-18, + "angularVelocity": 3.0712427077008165e-17, + "velocityX": -1.909561879261876, + "velocityY": 0.02121698337202498, + "timestamp": 0.19049229818392666 + }, + { + "x": 14.65239049535407, + "y": 5.558925628491973, + "heading": 4.9529034125739184e-18, + "angularVelocity": 7.426164168014883e-19, + "velocityX": -2.864342376779887, + "velocityY": 0.03182547014575747, + "timestamp": 0.28573844727589 + }, + { + "x": 14.470512079901564, + "y": 5.560946464453506, + "heading": -5.155775683106669e-18, + "angularVelocity": -1.0613215539074661e-16, + "velocityX": -1.909561879261876, + "velocityY": 0.021216983372024975, + "timestamp": 0.3809845963678533 + }, + { + "x": 14.379572868347168, + "y": 5.561956882476807, + "heading": -2.3969600276869747e-28, + "angularVelocity": 5.413106704997615e-17, + "velocityX": -0.9547809798230291, + "velocityY": 0.010608492132583473, + "timestamp": 0.47623074545981664 + }, + { + "x": 14.379572868347168, + "y": 5.561956882476807, + "heading": -1.1788127589783305e-28, + "angularVelocity": 4.1297903821940984e-29, + "velocityX": -1.6147929435574846e-22, + "velocityY": 1.8251378962940275e-24, + "timestamp": 0.57147689455178 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).1.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).1.traj index d94e4138..cfa30ca8 100644 --- a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).1.traj +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).1.traj @@ -3,65 +3,65 @@ { "x": 15.198025703430176, "y": 5.552863121032715, - "heading": 1.1788293001141444e-28, - "angularVelocity": 2.606876389920206e-29, - "velocityX": 1.6147849318653567e-22, - "velocityY": -1.7720524977117054e-24, + "heading": -9.080591826526709e-28, + "angularVelocity": -1.1603484864380112e-28, + "velocityX": -3.747628767048725e-22, + "velocityY": 4.167074864344212e-24, "timestamp": 0 }, { - "x": 15.10708649187578, - "y": 5.553873539056015, - "heading": 1.956931650032567e-18, - "angularVelocity": 2.0546044837018224e-17, - "velocityX": -0.9547809798230291, - "velocityY": 0.010608492132583473, - "timestamp": 0.09524614909196333 + "x": 15.107086496476764, + "y": 5.553873539004894, + "heading": -2.0171687380914284e-18, + "angularVelocity": -2.117848210218587e-17, + "velocityX": -0.95478099189959, + "velocityY": 0.010608492266763213, + "timestamp": 0.09524614306835108 }, { - "x": 14.925208076423274, - "y": 5.555894375017549, - "heading": 4.8821720585032204e-18, - "angularVelocity": 3.0712427077008165e-17, - "velocityX": -1.909561879261876, - "velocityY": 0.02121698337202498, - "timestamp": 0.19049229818392666 + "x": 14.925208085625242, + "y": 5.555894374915306, + "heading": -4.5506349702476365e-18, + "angularVelocity": -2.659914774113284e-17, + "velocityX": -1.9095619517212512, + "velocityY": 0.021216984177111247, + "timestamp": 0.19049228613670216 }, { - "x": 14.65239049535407, - "y": 5.558925628491973, - "heading": 4.9529034125739184e-18, - "angularVelocity": 7.426164168014883e-19, - "velocityX": -2.864342376779887, - "velocityY": 0.03182547014575747, - "timestamp": 0.28573844727589 + "x": 14.652390486152102, + "y": 5.558925628594215, + "heading": -6.069838807935888e-18, + "angularVelocity": -1.5950292451107106e-17, + "velocityX": -2.8643427511532806, + "velocityY": 0.03182547430538356, + "timestamp": 0.2857384292050532 }, { - "x": 14.470512079901564, - "y": 5.560946464453506, - "heading": -5.155775683106669e-18, - "angularVelocity": -1.0613215539074661e-16, - "velocityX": -1.909561879261876, - "velocityY": 0.021216983372024975, - "timestamp": 0.3809845963678533 + "x": 14.47051207530058, + "y": 5.560946464504627, + "heading": -5.0495636395818696e-18, + "angularVelocity": 1.0711984090840416e-17, + "velocityX": -1.9095619517212512, + "velocityY": 0.021216984177111247, + "timestamp": 0.3809845722734043 }, { "x": 14.379572868347168, "y": 5.561956882476807, - "heading": -2.3969600276869747e-28, - "angularVelocity": 5.413106704997615e-17, - "velocityX": -0.9547809798230291, - "velocityY": 0.010608492132583473, - "timestamp": 0.47623074545981664 + "heading": 1.854505670800523e-27, + "angularVelocity": 5.3015938280259253e-17, + "velocityX": -0.95478099189959, + "velocityY": 0.01060849226676321, + "timestamp": 0.47623071534175543 }, { "x": 14.379572868347168, "y": 5.561956882476807, - "heading": -1.1788127589783305e-28, - "angularVelocity": 4.1297903821940984e-29, - "velocityX": -1.6147929435574846e-22, - "velocityY": 1.8251378962940275e-24, - "timestamp": 0.57147689455178 + "heading": 9.080546216345213e-28, + "angularVelocity": -4.029142807111623e-28, + "velocityX": 3.747658167813594e-22, + "velocityY": -4.159522027499615e-24, + "timestamp": 0.5714768584101065 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).2 2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).2 2.traj new file mode 100644 index 00000000..9b8db6e8 --- /dev/null +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).2 2.traj @@ -0,0 +1,96 @@ +{ + "samples": [ + { + "x": 14.379572868347168, + "y": 5.561956882476807, + "heading": -1.1788127589783305e-28, + "angularVelocity": 4.1297903821940984e-29, + "velocityX": -1.6147929435574846e-22, + "velocityY": 1.8251378962940275e-24, + "timestamp": 0 + }, + { + "x": 14.303949787857668, + "y": 5.561956882476807, + "heading": 4.2371272764586235e-25, + "angularVelocity": 4.879852542216884e-24, + "velocityX": -0.8707004020209776, + "velocityY": 4.615774993405339e-16, + "timestamp": 0.08685315903607127 + }, + { + "x": 14.152703635762082, + "y": 5.561956882476807, + "heading": 9.62666688048605e-25, + "angularVelocity": 6.205346660780305e-24, + "velocityX": -1.7414007017611408, + "velocityY": 9.23154942545405e-16, + "timestamp": 0.17370631807214254 + }, + { + "x": 13.925834474244276, + "y": 5.561956882476807, + "heading": -6.35334199578853e-25, + "angularVelocity": -1.8398880424876378e-23, + "velocityX": -2.6121002855357762, + "velocityY": 1.3847320055814999e-15, + "timestamp": 0.2605594771082138 + }, + { + "x": 13.774588363604606, + "y": 5.561956882476807, + "heading": -4.288440082411824e-25, + "angularVelocity": 2.3774632221708446e-24, + "velocityX": -1.7414002244507438, + "velocityY": 9.231546882646077e-16, + "timestamp": 0.3474126361442851 + }, + { + "x": 13.717871079476808, + "y": 5.561956882476807, + "heading": -2.590171361320003e-25, + "angularVelocity": 3.910667216659344e-24, + "velocityX": -1.3060502198614803, + "velocityY": 6.923660434900599e-16, + "timestamp": 0.39083920831224794 + }, + { + "x": 13.680059555654978, + "y": 5.561956882476807, + "heading": -1.0277255516428025e-25, + "angularVelocity": 3.597902693388258e-24, + "velocityX": -0.8707001712128097, + "velocityY": 4.615773753780885e-16, + "timestamp": 0.4342657804802109 + }, + { + "x": 13.66115379333496, + "y": 5.561956882476807, + "heading": 4.1528332423630384e-27, + "angularVelocity": 2.4622111087440337e-24, + "velocityX": -0.43535009502700467, + "velocityY": 2.3078869268024055e-16, + "timestamp": 0.4776923526481739 + }, + { + "x": 13.66115379333496, + "y": 5.561956882476807, + "heading": 4.171558674345914e-27, + "angularVelocity": 4.311975603546073e-28, + "velocityX": -1.6145553776744748e-27, + "velocityY": 2.588288608432755e-27, + "timestamp": 0.5211189248161369 + } + ], + "eventMarkers": [ + { + "timestamp": 0.3474126361442851, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).2.traj index 9b8db6e8..40f60b67 100644 --- a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).2.traj +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).2.traj @@ -3,88 +3,88 @@ { "x": 14.379572868347168, "y": 5.561956882476807, - "heading": -1.1788127589783305e-28, - "angularVelocity": 4.1297903821940984e-29, - "velocityX": -1.6147929435574846e-22, - "velocityY": 1.8251378962940275e-24, + "heading": 9.080546216345213e-28, + "angularVelocity": -4.029142807111623e-28, + "velocityX": 3.747658167813594e-22, + "velocityY": -4.159522027499615e-24, "timestamp": 0 }, { - "x": 14.303949787857668, + "x": 14.303949799853617, "y": 5.561956882476807, - "heading": 4.2371272764586235e-25, - "angularVelocity": 4.879852542216884e-24, - "velocityX": -0.8707004020209776, - "velocityY": 4.615774993405339e-16, - "timestamp": 0.08685315903607127 + "heading": -6.436370661864032e-21, + "angularVelocity": -7.410637050778398e-20, + "velocityX": -0.8707003739387539, + "velocityY": 2.6828250389840444e-16, + "timestamp": 0.08685314805994404 }, { - "x": 14.152703635762082, + "x": 14.15270366641151, "y": 5.561956882476807, - "heading": 9.62666688048605e-25, - "angularVelocity": 6.205346660780305e-24, - "velocityX": -1.7414007017611408, - "velocityY": 9.23154942545405e-16, - "timestamp": 0.17370631807214254 + "heading": -2.0042157210092705e-20, + "angularVelocity": -1.5665277370046843e-19, + "velocityX": -1.741400707061555, + "velocityY": 5.368432878740444e-16, + "timestamp": 0.17370629611988808 }, { - "x": 13.925834474244276, + "x": 13.925834492835799, "y": 5.561956882476807, - "heading": -6.35334199578853e-25, - "angularVelocity": -1.8398880424876378e-23, - "velocityX": -2.6121002855357762, - "velocityY": 1.3847320055814999e-15, - "timestamp": 0.2605594771082138 + "heading": -1.7073389524704635e-20, + "angularVelocity": 3.4181463213745686e-20, + "velocityX": -2.612100754472715, + "velocityY": 8.033174756016209e-16, + "timestamp": 0.2605594441798321 }, { - "x": 13.774588363604606, + "x": 13.774588375936995, "y": 5.561956882476807, - "heading": -4.288440082411824e-25, - "angularVelocity": 2.3774632221708446e-24, - "velocityX": -1.7414002244507438, - "velocityY": 9.231546882646077e-16, - "timestamp": 0.3474126361442851 + "heading": -7.883060482060434e-21, + "angularVelocity": 1.0581457607411195e-19, + "velocityX": -1.7414005165871203, + "velocityY": 5.360485261048027e-16, + "timestamp": 0.34741259223977616 }, { - "x": 13.717871079476808, + "x": 13.717871085037839, "y": 5.561956882476807, - "heading": -2.590171361320003e-25, - "angularVelocity": 3.910667216659344e-24, - "velocityX": -1.3060502198614803, - "velocityY": 6.923660434900599e-16, - "timestamp": 0.39083920831224794 + "heading": -3.134968058954825e-21, + "angularVelocity": 1.0933611158328031e-19, + "velocityX": -1.3060504080011666, + "velocityY": 4.021698310203676e-16, + "timestamp": 0.39083916333664437 }, { - "x": 13.680059555654978, + "x": 13.680059557344757, "y": 5.561956882476807, - "heading": -1.0277255516428025e-25, - "angularVelocity": 3.597902693388258e-24, - "velocityX": -0.8707001712128097, - "velocityY": 4.615773753780885e-16, - "timestamp": 0.4342657804802109 + "heading": 7.130869940497685e-22, + "angularVelocity": 8.861061225492244e-20, + "velocityX": -0.8707002818329597, + "velocityY": 2.681499176641714e-16, + "timestamp": 0.4342657344335127 }, { "x": 13.66115379333496, "y": 5.561956882476807, - "heading": 4.1528332423630384e-27, - "angularVelocity": 2.4622111087440337e-24, - "velocityX": -0.43535009502700467, - "velocityY": 2.3078869268024055e-16, - "timestamp": 0.4776923526481739 + "heading": 2.9039653211073e-21, + "angularVelocity": 5.0450179964504564e-20, + "velocityX": -0.4353501446758437, + "velocityY": 1.3407690569526743e-16, + "timestamp": 0.4776923055303809 }, { "x": 13.66115379333496, "y": 5.561956882476807, - "heading": 4.171558674345914e-27, - "angularVelocity": 4.311975603546073e-28, - "velocityX": -1.6145553776744748e-27, - "velocityY": 2.588288608432755e-27, - "timestamp": 0.5211189248161369 + "heading": 2.9289616731652808e-21, + "angularVelocity": 5.756004088616709e-22, + "velocityX": -2.138357283045396e-22, + "velocityY": 2.150198010410122e-22, + "timestamp": 0.5211188766272491 } ], "eventMarkers": [ { - "timestamp": 0.3474126361442851, + "timestamp": 0.34741259223977616, "command": { "type": "named", "data": { diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).3 2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).3 2.traj new file mode 100644 index 00000000..35cf47dc --- /dev/null +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).3 2.traj @@ -0,0 +1,113 @@ +{ + "samples": [ + { + "x": 13.66115379333496, + "y": 5.561956882476807, + "heading": 4.171558674345914e-27, + "angularVelocity": 4.311975603546073e-28, + "velocityX": -1.6145553776744748e-27, + "velocityY": 2.588288608432755e-27, + "timestamp": 0 + }, + { + "x": 13.691513815926383, + "y": 5.525701060761086, + "heading": 0.023562463335368315, + "angularVelocity": 0.34182634637050774, + "velocityX": 0.4404401802325416, + "velocityY": -0.5259719620716655, + "timestamp": 0.06893109201661374 + }, + { + "x": 13.752233823940973, + "y": 5.453149387533433, + "heading": 0.06932129795634334, + "angularVelocity": 0.663834465438997, + "velocityX": 0.8808798212562146, + "velocityY": -1.0525246460648914, + "timestamp": 0.13786218403322748 + }, + { + "x": 13.843313276711916, + "y": 5.344233687044581, + "heading": 0.13498389635529368, + "angularVelocity": 0.9525831736877814, + "velocityX": 1.3213116186958351, + "velocityY": -1.580066372118415, + "timestamp": 0.2067932760498412 + }, + { + "x": 13.964751433548663, + "y": 5.198837902534373, + "heading": 0.21647867376005756, + "angularVelocity": 1.1822644182848892, + "velocityX": 1.7617326707587837, + "velocityY": -2.109291761621367, + "timestamp": 0.27572436806645495 + }, + { + "x": 14.116535425780143, + "y": 5.016750311314966, + "heading": 0.30307237204373855, + "angularVelocity": 1.2562356949576599, + "velocityX": 2.2019670339024615, + "velocityY": -2.6415886632917585, + "timestamp": 0.3446554600830687 + }, + { + "x": 14.26793452721354, + "y": 4.8345584717676635, + "heading": 0.39204433398389704, + "angularVelocity": 1.290737740216195, + "velocityX": 2.196383330136527, + "velocityY": -2.6431010189624082, + "timestamp": 0.4135865520996824 + }, + { + "x": 14.389009217305178, + "y": 4.688953541309653, + "heading": 0.4689267567346741, + "angularVelocity": 1.1153518753517935, + "velocityX": 1.7564597709036147, + "velocityY": -2.1123258923987027, + "timestamp": 0.48251764411629616 + }, + { + "x": 14.479788015237448, + "y": 4.5797997732912155, + "heading": 0.5284493552871049, + "angularVelocity": 0.8635087129924586, + "velocityX": 1.3169499463375665, + "velocityY": -1.583520075267771, + "timestamp": 0.5514487361329099 + }, + { + "x": 14.540294978830758, + "y": 4.507052135096341, + "heading": 0.568910729781111, + "angularVelocity": 0.5869829319438942, + "velocityX": 0.8777891343826024, + "velocityY": -1.0553675571734895, + "timestamp": 0.6203798281495236 + }, + { + "x": 14.570545196533203, + "y": 4.470687389373779, + "heading": 0.5894308145365931, + "angularVelocity": 0.29768982552222245, + "velocityX": 0.43884721418825445, + "velocityY": -0.5275521489460464, + "timestamp": 0.6893109201661374 + }, + { + "x": 14.570545196533203, + "y": 4.470687389373779, + "heading": 0.5894308145365931, + "angularVelocity": 3.863669117287725e-28, + "velocityX": -1.9871518401102085e-27, + "velocityY": 2.8088430719951805e-27, + "timestamp": 0.7582420121827511 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).3.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).3.traj index 35cf47dc..d138940b 100644 --- a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).3.traj +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).3.traj @@ -3,110 +3,110 @@ { "x": 13.66115379333496, "y": 5.561956882476807, - "heading": 4.171558674345914e-27, - "angularVelocity": 4.311975603546073e-28, - "velocityX": -1.6145553776744748e-27, - "velocityY": 2.588288608432755e-27, + "heading": 2.9289616731652808e-21, + "angularVelocity": 5.756004088616709e-22, + "velocityX": -2.138357283045396e-22, + "velocityY": 2.150198010410122e-22, "timestamp": 0 }, { - "x": 13.691513815926383, - "y": 5.525701060761086, - "heading": 0.023562463335368315, - "angularVelocity": 0.34182634637050774, - "velocityX": 0.4404401802325416, - "velocityY": -0.5259719620716655, - "timestamp": 0.06893109201661374 + "x": 13.69151381424092, + "y": 5.525701061208603, + "heading": 0.023562396214272266, + "angularVelocity": 0.3418254011415353, + "velocityX": 0.44044019251996336, + "velocityY": -0.5259719994527794, + "timestamp": 0.06893108626680466 }, { - "x": 13.752233823940973, - "y": 5.453149387533433, - "heading": 0.06932129795634334, - "angularVelocity": 0.663834465438997, - "velocityX": 0.8808798212562146, - "velocityY": -1.0525246460648914, - "timestamp": 0.13786218403322748 + "x": 13.75223381956499, + "y": 5.45314938890248, + "heading": 0.06932112146119182, + "angularVelocity": 0.6638329340960312, + "velocityX": 0.880879855701796, + "velocityY": -1.05252472049121, + "timestamp": 0.13786217253360933 }, { - "x": 13.843313276711916, - "y": 5.344233687044581, - "heading": 0.13498389635529368, - "angularVelocity": 0.9525831736877814, - "velocityX": 1.3213116186958351, - "velocityY": -1.580066372118415, - "timestamp": 0.2067932760498412 + "x": 13.843313269738008, + "y": 5.344233689629052, + "heading": 0.13498360807083573, + "angularVelocity": 0.9525816313918327, + "velocityX": 1.3213116912228196, + "velocityY": -1.580066486285422, + "timestamp": 0.206793258800414 }, { - "x": 13.964751433548663, - "y": 5.198837902534373, - "heading": 0.21647867376005756, - "angularVelocity": 1.1822644182848892, - "velocityX": 1.7617326707587837, - "velocityY": -2.109291761621367, - "timestamp": 0.27572436806645495 + "x": 13.964751426359838, + "y": 5.198837905171001, + "heading": 0.21647833458525872, + "angularVelocity": 1.182263778623618, + "velocityX": 1.7617328145938624, + "velocityY": -2.1092919368089142, + "timestamp": 0.27572434506721866 }, { - "x": 14.116535425780143, - "y": 5.016750311314966, - "heading": 0.30307237204373855, - "angularVelocity": 1.2562356949576599, - "velocityX": 2.2019670339024615, - "velocityY": -2.6415886632917585, - "timestamp": 0.3446554600830687 + "x": 14.116535429050689, + "y": 5.0167503033742005, + "heading": 0.3030721451467355, + "angularVelocity": 1.2562374285863827, + "velocityX": 2.2019673693137145, + "velocityY": -2.64158903708572, + "timestamp": 0.3446554313340233 }, { - "x": 14.26793452721354, - "y": 4.8345584717676635, - "heading": 0.39204433398389704, - "angularVelocity": 1.290737740216195, - "velocityX": 2.196383330136527, - "velocityY": -2.6431010189624082, - "timestamp": 0.4135865520996824 + "x": 14.267934537167369, + "y": 4.834558459237534, + "heading": 0.3920443693842881, + "angularVelocity": 1.290741653093595, + "velocityX": 2.1963836103013628, + "velocityY": -2.6431013060127135, + "timestamp": 0.413586517600828 }, { - "x": 14.389009217305178, - "y": 4.688953541309653, - "heading": 0.4689267567346741, - "angularVelocity": 1.1153518753517935, - "velocityX": 1.7564597709036147, - "velocityY": -2.1123258923987027, - "timestamp": 0.48251764411629616 + "x": 14.389009225126475, + "y": 4.688953532451243, + "heading": 0.4689268613880663, + "angularVelocity": 1.115352973057713, + "velocityX": 1.756459886479585, + "velocityY": -2.1123260153294563, + "timestamp": 0.48251760386763265 }, { - "x": 14.479788015237448, - "y": 4.5797997732912155, - "heading": 0.5284493552871049, - "angularVelocity": 0.8635087129924586, - "velocityX": 1.3169499463375665, - "velocityY": -1.583520075267771, - "timestamp": 0.5514487361329099 + "x": 14.47978801964584, + "y": 4.5797997685436185, + "heading": 0.5284494394989407, + "angularVelocity": 0.8635084884704485, + "velocityX": 1.3169500066776294, + "velocityY": -1.5835201477187593, + "timestamp": 0.5514486901344373 }, { - "x": 14.540294978830758, - "y": 4.507052135096341, - "heading": 0.568910729781111, - "angularVelocity": 0.5869829319438942, - "velocityX": 0.8777891343826024, - "velocityY": -1.0553675571734895, - "timestamp": 0.6203798281495236 + "x": 14.540294980406806, + "y": 4.507052133449136, + "heading": 0.5689107661779228, + "angularVelocity": 0.5869822872422525, + "velocityX": 0.8777891665128892, + "velocityY": -1.0553676002276473, + "timestamp": 0.620379776401242 }, { "x": 14.570545196533203, "y": 4.470687389373779, "heading": 0.5894308145365931, - "angularVelocity": 0.29768982552222245, - "velocityX": 0.43884721418825445, - "velocityY": -0.5275521489460464, - "timestamp": 0.6893109201661374 + "angularVelocity": 0.2976893223363026, + "velocityX": 0.43884722793008496, + "velocityY": -0.5275521690548106, + "timestamp": 0.6893108626680466 }, { "x": 14.570545196533203, "y": 4.470687389373779, "heading": 0.5894308145365931, - "angularVelocity": 3.863669117287725e-28, - "velocityX": -1.9871518401102085e-27, - "velocityY": 2.8088430719951805e-27, - "timestamp": 0.7582420121827511 + "angularVelocity": 4.237174098560341e-22, + "velocityX": -2.826594351845562e-22, + "velocityY": 3.573387212832748e-22, + "timestamp": 0.7582419489348513 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).4 2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).4 2.traj new file mode 100644 index 00000000..e143392d --- /dev/null +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).4 2.traj @@ -0,0 +1,132 @@ +{ + "samples": [ + { + "x": 14.570545196533203, + "y": 4.470687389373779, + "heading": 0.5894308145365931, + "angularVelocity": 3.863669117287725e-28, + "velocityX": -1.9871518401102085e-27, + "velocityY": 2.8088430719951805e-27, + "timestamp": 0 + }, + { + "x": 14.45709086591278, + "y": 4.4278439544867165, + "heading": 0.488089493217441, + "angularVelocity": -0.9146778399911556, + "velocityX": -1.0240064044825004, + "velocityY": -0.38669261432744145, + "timestamp": 0.11079455179556108 + }, + { + "x": 14.230046021105487, + "y": 4.34248670484011, + "heading": 0.2854587836923866, + "angularVelocity": -1.8288869465255828, + "velocityX": -2.0492419629643726, + "velocityY": -0.7704101714686167, + "timestamp": 0.22158910359112238 + }, + { + "x": 14.002349455992734, + "y": 4.258200754530428, + "heading": 0.09515293619351321, + "angularVelocity": -1.7176462598091191, + "velocityX": -2.055124204418461, + "velocityY": -0.760740929438543, + "timestamp": 0.33238365538668346 + }, + { + "x": 13.888501167358198, + "y": 4.216057777427628, + "heading": 8.28079525180121e-11, + "angularVelocity": -0.8588232414738415, + "velocityX": -1.027562157068967, + "velocityY": -0.3803704823009896, + "timestamp": 0.44317820718224454 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 8.268158682804007e-19, + "angularVelocity": -7.474009357788098e-10, + "velocityX": -5.490850602427426e-10, + "velocityY": -2.0617274991432905e-10, + "timestamp": 0.5539727589778056 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 5.905827430465516e-19, + "angularVelocity": -6.406293808708934e-10, + "velocityX": -4.706443421676888e-10, + "velocityY": -1.7671950173476352e-10, + "timestamp": 0.5539727593465573 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 3.9372180312859395e-19, + "angularVelocity": -5.33857823518088e-10, + "velocityX": -3.922036224873799e-10, + "velocityY": -1.472662529525917e-10, + "timestamp": 0.553972759715309 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 2.3623304942893484e-19, + "angularVelocity": -4.270862637180886e-10, + "velocityX": -3.137629012016172e-10, + "velocityY": -1.1781300356773835e-10, + "timestamp": 0.5539727600840607 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 1.1811648285083364e-19, + "angularVelocity": -3.203147014685827e-10, + "velocityX": -2.353221783101984e-10, + "velocityY": -8.835975358012759e-11, + "timestamp": 0.5539727604528124 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": 3.937210429839709e-20, + "angularVelocity": -2.1354313676727285e-10, + "velocityX": -1.568814538129234e-10, + "velocityY": -5.890650298968465e-11, + "timestamp": 0.5539727608215641 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": -8.532341534406571e-26, + "angularVelocity": -1.0677156961184637e-10, + "velocityX": -7.84407277095909e-11, + "velocityY": -2.945325179633351e-11, + "timestamp": 0.5539727611903158 + }, + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": -8.510879482431963e-26, + "angularVelocity": -1.6866441387712243e-26, + "velocityX": -2.0583941335553829e-25, + "velocityY": 9.841185153624076e-26, + "timestamp": 0.5539727615590675 + } + ], + "eventMarkers": [ + { + "timestamp": 0.5539727589778056, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).4.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).4.traj index e143392d..5342c0bc 100644 --- a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).4.traj +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).4.traj @@ -4,123 +4,123 @@ "x": 14.570545196533203, "y": 4.470687389373779, "heading": 0.5894308145365931, - "angularVelocity": 3.863669117287725e-28, - "velocityX": -1.9871518401102085e-27, - "velocityY": 2.8088430719951805e-27, + "angularVelocity": 4.237174098560341e-22, + "velocityX": -2.826594351845562e-22, + "velocityY": 3.573387212832748e-22, "timestamp": 0 }, { - "x": 14.45709086591278, - "y": 4.4278439544867165, - "heading": 0.488089493217441, - "angularVelocity": -0.9146778399911556, - "velocityX": -1.0240064044825004, - "velocityY": -0.38669261432744145, - "timestamp": 0.11079455179556108 + "x": 14.53016602593679, + "y": 4.455446506235451, + "heading": 0.5540064540349209, + "angularVelocity": -0.5360505707818769, + "velocityX": -0.6110280366214283, + "velocityY": -0.2306289793187704, + "timestamp": 0.06608398989297326 }, { - "x": 14.230046021105487, - "y": 4.34248670484011, - "heading": 0.2854587836923866, - "angularVelocity": -1.8288869465255828, - "velocityX": -2.0492419629643726, - "velocityY": -0.7704101714686167, - "timestamp": 0.22158910359112238 + "x": 14.44938649587058, + "y": 4.425019141773473, + "heading": 0.4829804666462581, + "angularVelocity": -1.0747835822820835, + "velocityX": -1.2223767087464938, + "velocityY": -0.4604347363295905, + "timestamp": 0.13216797978594652 }, { - "x": 14.002349455992734, - "y": 4.258200754530428, - "heading": 0.09515293619351321, - "angularVelocity": -1.7176462598091191, - "velocityX": -2.055124204418461, - "velocityY": -0.760740929438543, - "timestamp": 0.33238365538668346 + "x": 14.328164393235157, + "y": 4.379500232629268, + "heading": 0.37640102935392167, + "angularVelocity": -1.6127875672299412, + "velocityX": -1.8343641603927023, + "velocityY": -0.6888038875667982, + "timestamp": 0.19825196967892 }, { - "x": 13.888501167358198, - "y": 4.216057777427628, - "heading": 8.28079525180121e-11, - "angularVelocity": -0.8588232414738415, - "velocityX": -1.027562157068967, - "velocityY": -0.3803704823009896, - "timestamp": 0.44317820718224454 + "x": 14.16632561176535, + "y": 4.319179292971803, + "heading": 0.23626694546085536, + "angularVelocity": -2.120545144444536, + "velocityX": -2.448986233003096, + "velocityY": -0.9127920356376347, + "timestamp": 0.26433595957189326 }, { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 8.268158682804007e-19, - "angularVelocity": -7.474009357788098e-10, - "velocityX": -5.490850602427426e-10, - "velocityY": -2.0617274991432905e-10, - "timestamp": 0.5539727589778056 + "x": 14.0448416017969, + "y": 4.274051603899093, + "heading": 0.1325451584186999, + "angularVelocity": -1.569544865710116, + "velocityX": -1.838327409788673, + "velocityY": -0.6828838444197568, + "timestamp": 0.3304199494648665 }, { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 5.905827430465516e-19, - "angularVelocity": -6.406293808708934e-10, - "velocityX": -4.706443421676888e-10, - "velocityY": -1.7671950173476352e-10, - "timestamp": 0.5539727593465573 + "x": 14.000178586698231, + "y": 4.2574722138771985, + "heading": 0.0945578464623197, + "angularVelocity": -1.340385412793797, + "velocityX": -1.5759381447780825, + "velocityY": -0.585005134448155, + "timestamp": 0.3587605383684733 }, { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 3.9372180312859395e-19, - "angularVelocity": -5.33857823518088e-10, - "velocityX": -3.922036224873799e-10, - "velocityY": -1.472662529525917e-10, - "timestamp": 0.553972759715309 + "x": 13.96295541639372, + "y": 4.243662024781544, + "heading": 0.06298966033692675, + "angularVelocity": -1.1138860322473925, + "velocityX": -1.31342261203944, + "velocityY": -0.4872936530227246, + "timestamp": 0.3871011272720801 }, { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 2.3623304942893484e-19, - "angularVelocity": -4.270862637180886e-10, - "velocityX": -3.137629012016172e-10, - "velocityY": -1.1781300356773835e-10, - "timestamp": 0.5539727600840607 + "x": 13.933174653940528, + "y": 4.2326176532188775, + "heading": 0.03778230634920248, + "angularVelocity": -0.8894435494428381, + "velocityX": -1.0508166416190299, + "velocityY": -0.3897015549053042, + "timestamp": 0.4154417161756869 }, { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 1.1811648285083364e-19, - "angularVelocity": -3.203147014685827e-10, - "velocityX": -2.353221783101984e-10, - "velocityY": -8.835975358012759e-11, - "timestamp": 0.5539727604528124 + "x": 13.910838071206133, + "y": 4.2243366192852125, + "heading": 0.018894460026481222, + "angularVelocity": -0.6664592040399531, + "velocityX": -0.788148150709412, + "velocityY": -0.2921969604029915, + "timestamp": 0.4437823050792937 }, { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 3.937210429839709e-20, - "angularVelocity": -2.1354313676727285e-10, - "velocityX": -1.568814538129234e-10, - "velocityY": -5.890650298968465e-11, - "timestamp": 0.5539727608215641 + "x": 13.895946755796466, + "y": 4.218817098366333, + "heading": 0.006302298120624352, + "angularVelocity": -0.4443154638982944, + "velocityX": -0.5254412835356537, + "velocityY": -0.19475674756274655, + "timestamp": 0.4721228939829005 }, { "x": 13.888501167297363, "y": 4.216057777404785, - "heading": -8.532341534406571e-26, - "angularVelocity": -1.0677156961184637e-10, - "velocityX": -7.84407277095909e-11, - "velocityY": -2.945325179633351e-11, - "timestamp": 0.5539727611903158 + "heading": -7.359942498471454e-23, + "angularVelocity": -0.22237710522036122, + "velocityX": -0.26271819983798683, + "velocityY": -0.0973628660622863, + "timestamp": 0.5004634828865073 }, { "x": 13.888501167297363, "y": 4.216057777404785, - "heading": -8.510879482431963e-26, - "angularVelocity": -1.6866441387712243e-26, - "velocityX": -2.0583941335553829e-25, - "velocityY": 9.841185153624076e-26, - "timestamp": 0.5539727615590675 + "heading": 2.117727432484989e-22, + "angularVelocity": 5.073032730422753e-22, + "velocityX": 1.319201559585301e-22, + "velocityY": -1.2833790442104632e-22, + "timestamp": 0.5288040717901141 } ], "eventMarkers": [ { - "timestamp": 0.5539727589778056, + "timestamp": 0.3304199494648665, "command": { "type": "named", "data": { diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).5 2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).5 2.traj new file mode 100644 index 00000000..c34ca353 --- /dev/null +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).5 2.traj @@ -0,0 +1,131 @@ +{ + "samples": [ + { + "x": 13.888501167297363, + "y": 4.216057777404785, + "heading": -8.510879482431963e-26, + "angularVelocity": -1.6866441387712243e-26, + "velocityX": -2.0583941335553829e-25, + "velocityY": 9.841185153624076e-26, + "timestamp": 0 + }, + { + "x": 13.899986336038381, + "y": 4.272873358599484, + "heading": -0.023038991913601637, + "angularVelocity": -0.3021063684546837, + "velocityX": 0.15060305730606005, + "velocityY": 0.7450130183968425, + "timestamp": 0.07626119247816376 + }, + { + "x": 13.922959662828976, + "y": 4.386578192081037, + "heading": -0.0663634759005527, + "angularVelocity": -0.5681065634969774, + "velocityX": 0.3012453129050349, + "velocityY": 1.4909920732502444, + "timestamp": 0.15252238495632753 + }, + { + "x": 13.957441252375148, + "y": 4.557273568298352, + "heading": -0.12543465168712403, + "angularVelocity": -0.7745902452743002, + "velocityX": 0.45215119808209714, + "velocityY": 2.238299332470985, + "timestamp": 0.2287835774344913 + }, + { + "x": 14.00350258986825, + "y": 4.785079242541037, + "heading": -0.19129776367156157, + "angularVelocity": -0.8636517453263874, + "velocityX": 0.6039944563716563, + "velocityY": 2.987176922363417, + "timestamp": 0.30504476991265506 + }, + { + "x": 14.06127377629267, + "y": 5.069635038422791, + "heading": -0.23370464392529416, + "angularVelocity": -0.556074182368382, + "velocityX": 0.757543706662608, + "velocityY": 3.731331580780533, + "timestamp": 0.3813059623908188 + }, + { + "x": 14.121438982529087, + "y": 5.371150739294933, + "heading": -0.23370465578542146, + "angularVelocity": -1.5551982461065506e-7, + "velocityX": 0.7889360798238878, + "velocityY": 3.953723920045926, + "timestamp": 0.4575671548689826 + }, + { + "x": 14.181604184628418, + "y": 5.672666440992599, + "heading": -0.23370466764554898, + "angularVelocity": -1.5551982762801367e-7, + "velocityX": 0.7889360255749569, + "velocityY": 3.9537239308708902, + "timestamp": 0.5338283473471463 + }, + { + "x": 14.238820898893755, + "y": 5.957011236905596, + "heading": -0.27574816661054075, + "angularVelocity": -0.5513092255544053, + "velocityX": 0.7502730078830246, + "velocityY": 3.7285647742056445, + "timestamp": 0.6100895398253101 + }, + { + "x": 14.284420707366433, + "y": 6.1847837505193715, + "heading": -0.34721144483836985, + "angularVelocity": -0.9370857693877743, + "velocityX": 0.5979425051048811, + "velocityY": 2.98674209269669, + "timestamp": 0.6863507323034739 + }, + { + "x": 14.318439308265718, + "y": 6.35544203945297, + "heading": -0.41225183766095985, + "angularVelocity": -0.8528635693864027, + "velocityX": 0.44608010698268263, + "velocityY": 2.2378130132500083, + "timestamp": 0.7626119247816376 + }, + { + "x": 14.341028135354547, + "y": 6.469114800114249, + "heading": -0.4602003337578672, + "angularVelocity": -0.6287404450256496, + "velocityX": 0.2962034339457177, + "velocityY": 1.490571507832474, + "timestamp": 0.8388731172598014 + }, + { + "x": 14.352291107177734, + "y": 6.525911808013916, + "heading": -0.48570563852984416, + "angularVelocity": -0.33444670799345433, + "velocityX": 0.14768942704917307, + "velocityY": 0.7447694699493517, + "timestamp": 0.9151343097379652 + }, + { + "x": 14.352291107177734, + "y": 6.525911808013916, + "heading": -0.48570563852984416, + "angularVelocity": 1.0539627829247847e-26, + "velocityX": -5.1337066004257686e-26, + "velocityY": -2.752712125666017e-26, + "timestamp": 0.9913955022161289 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).5.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).5.traj index c34ca353..30661804 100644 --- a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).5.traj +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).5.traj @@ -3,128 +3,137 @@ { "x": 13.888501167297363, "y": 4.216057777404785, - "heading": -8.510879482431963e-26, - "angularVelocity": -1.6866441387712243e-26, - "velocityX": -2.0583941335553829e-25, - "velocityY": 9.841185153624076e-26, + "heading": 2.117727432484989e-22, + "angularVelocity": 5.073032730422753e-22, + "velocityX": 1.319201559585301e-22, + "velocityY": -1.2833790442104632e-22, "timestamp": 0 }, { - "x": 13.899986336038381, - "y": 4.272873358599484, - "heading": -0.023038991913601637, - "angularVelocity": -0.3021063684546837, - "velocityX": 0.15060305730606005, - "velocityY": 0.7450130183968425, - "timestamp": 0.07626119247816376 + "x": 13.90055456208268, + "y": 4.2672774930951745, + "heading": -0.024639723698078066, + "angularVelocity": -0.3387505660309093, + "velocityX": 0.16571185440844172, + "velocityY": 0.7041762275692574, + "timestamp": 0.07273707018936726 }, { - "x": 13.922959662828976, - "y": 4.386578192081037, - "heading": -0.0663634759005527, - "angularVelocity": -0.5681065634969774, - "velocityX": 0.3012453129050349, - "velocityY": 1.4909920732502444, - "timestamp": 0.15252238495632753 + "x": 13.924664422654004, + "y": 4.369780859826108, + "heading": -0.07184553791598466, + "angularVelocity": -0.648992516402005, + "velocityX": 0.33146592938861846, + "velocityY": 1.4092314477895682, + "timestamp": 0.14547414037873452 }, { - "x": 13.957441252375148, - "y": 4.557273568298352, - "heading": -0.12543465168712403, - "angularVelocity": -0.7745902452743002, - "velocityX": 0.45215119808209714, - "velocityY": 2.238299332470985, - "timestamp": 0.2287835774344913 + "x": 13.960850027131373, + "y": 4.523662093222596, + "heading": -0.1381918964160448, + "angularVelocity": -0.9121395504016173, + "velocityX": 0.4974850428146382, + "velocityY": 2.1155819583586095, + "timestamp": 0.21821121056810178 }, { - "x": 14.00350258986825, - "y": 4.785079242541037, - "heading": -0.19129776367156157, - "angularVelocity": -0.8636517453263874, - "velocityX": 0.6039944563716563, - "velocityY": 2.987176922363417, - "timestamp": 0.30504476991265506 + "x": 14.00918910605255, + "y": 4.729054591196495, + "heading": -0.21706345100390917, + "angularVelocity": -1.0843377989040044, + "velocityX": 0.6645728071714858, + "velocityY": 2.8237664431516296, + "timestamp": 0.29094828075746904 }, { - "x": 14.06127377629267, - "y": 5.069635038422791, - "heading": -0.23370464392529416, - "angularVelocity": -0.556074182368382, - "velocityX": 0.757543706662608, - "velocityY": 3.731331580780533, - "timestamp": 0.3813059623908188 + "x": 14.069933310565814, + "y": 4.985967503680229, + "heading": -0.2879560349605607, + "angularVelocity": -0.9746417304420771, + "velocityX": 0.8351203087382914, + "velocityY": 3.532076722569039, + "timestamp": 0.3636853509468363 }, { - "x": 14.121438982529087, - "y": 5.371150739294933, - "heading": -0.23370465578542146, - "angularVelocity": -1.5551982461065506e-7, - "velocityX": 0.7889360798238878, - "velocityY": 3.953723920045926, - "timestamp": 0.4575671548689826 + "x": 14.136089967453417, + "y": 5.271659493582205, + "heading": -0.2879560398332574, + "angularVelocity": -6.699055461306423e-8, + "velocityX": 0.909531504573521, + "velocityY": 3.9277357358248457, + "timestamp": 0.43642242113620355 }, { - "x": 14.181604184628418, - "y": 5.672666440992599, - "heading": -0.23370466764554898, - "angularVelocity": -1.5551982762801367e-7, - "velocityX": 0.7889360255749569, - "velocityY": 3.9537239308708902, - "timestamp": 0.5338283473471463 + "x": 14.202246205709509, + "y": 5.557351580424848, + "heading": -0.2879560447059434, + "angularVelocity": -6.699040765465796e-8, + "velocityX": 0.9095257491655405, + "velocityY": 3.9277370685794617, + "timestamp": 0.5091594913255708 }, { - "x": 14.238820898893755, - "y": 5.957011236905596, - "heading": -0.27574816661054075, - "angularVelocity": -0.5513092255544053, - "velocityX": 0.7502730078830246, - "velocityY": 3.7285647742056445, - "timestamp": 0.6100895398253101 + "x": 14.268402835983203, + "y": 5.843043576489709, + "heading": -0.2879560495786414, + "angularVelocity": -6.699057305241701e-8, + "velocityX": 0.9095311386815427, + "velocityY": 3.9277358205530803, + "timestamp": 0.5818965615149381 }, { - "x": 14.284420707366433, - "y": 6.1847837505193715, - "heading": -0.34721144483836985, - "angularVelocity": -0.9370857693877743, - "velocityX": 0.5979425051048811, - "velocityY": 2.98674209269669, - "timestamp": 0.6863507323034739 + "x": 14.328385114782021, + "y": 6.0998732839416245, + "heading": -0.35678639279969876, + "angularVelocity": -0.9462897397690232, + "velocityX": 0.8246452413144653, + "velocityY": 3.5309328074841777, + "timestamp": 0.6546336317043053 }, { - "x": 14.318439308265718, - "y": 6.35544203945297, - "heading": -0.41225183766095985, - "angularVelocity": -0.8528635693864027, - "velocityX": 0.44608010698268263, - "velocityY": 2.2378130132500083, - "timestamp": 0.7626119247816376 + "x": 14.376005525808099, + "y": 6.305305162261247, + "heading": -0.44145888026224256, + "angularVelocity": -1.1640898821206782, + "velocityX": 0.654692454646584, + "velocityY": 2.8243078499696304, + "timestamp": 0.7273707018936726 }, { - "x": 14.341028135354547, - "y": 6.469114800114249, - "heading": -0.4602003337578672, - "angularVelocity": -0.6287404450256496, - "velocityX": 0.2962034339457177, - "velocityY": 1.490571507832474, - "timestamp": 0.8388731172598014 + "x": 14.41149328213472, + "y": 6.459199589883294, + "heading": -0.5144824922677588, + "angularVelocity": -1.0039394192727746, + "velocityX": 0.48789092321468885, + "velocityY": 2.1157633545232315, + "timestamp": 0.8001077720830398 }, { - "x": 14.352291107177734, - "y": 6.525911808013916, - "heading": -0.48570563852984416, - "angularVelocity": -0.33444670799345433, - "velocityX": 0.14768942704917307, - "velocityY": 0.7447694699493517, - "timestamp": 0.9151343097379652 + "x": 14.435047453373745, + "y": 6.561702808468181, + "heading": -0.5669658281865239, + "angularVelocity": -0.7215486653796684, + "velocityX": 0.3238262302523786, + "velocityY": 1.4092294110558068, + "timestamp": 0.8728448422724071 }, { - "x": 14.352291107177734, - "y": 6.525911808013916, - "heading": -0.48570563852984416, - "angularVelocity": 1.0539627829247847e-26, - "velocityX": -5.1337066004257686e-26, - "velocityY": -2.752712125666017e-26, - "timestamp": 0.9913955022161289 + "x": 14.446789741516112, + "y": 6.612919330596924, + "heading": -0.5944409566225068, + "angularVelocity": -0.37773212977169646, + "velocityX": 0.16143471426325046, + "velocityY": 0.7041323220113681, + "timestamp": 0.9455819124617744 + }, + { + "x": 14.446789741516112, + "y": 6.612919330596924, + "heading": -0.5944409566225068, + "angularVelocity": -2.2907035566312707e-22, + "velocityX": -2.4801938086703075e-22, + "velocityY": 8.637122930206518e-23, + "timestamp": 1.0183189826511416 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).6 2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).6 2.traj new file mode 100644 index 00000000..2805b76e --- /dev/null +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).6 2.traj @@ -0,0 +1,105 @@ +{ + "samples": [ + { + "x": 14.352291107177734, + "y": 6.525911808013916, + "heading": -0.48570563852984416, + "angularVelocity": 1.0539627829247847e-26, + "velocityX": -5.1337066004257686e-26, + "velocityY": -2.752712125666017e-26, + "timestamp": 0 + }, + { + "x": 14.281244102466694, + "y": 6.578011637665101, + "heading": -0.49633092681747565, + "angularVelocity": -0.11332344447203622, + "velocityX": -0.7577480323661817, + "velocityY": 0.555667949202982, + "timestamp": 0.09376072477441655 + }, + { + "x": 14.139150572999801, + "y": 6.682211842382296, + "heading": -0.5176048283391019, + "angularVelocity": -0.22689565991314806, + "velocityX": -1.515490945795936, + "velocityY": 1.1113417154986245, + "timestamp": 0.1875214495488331 + }, + { + "x": 13.926011946794734, + "y": 6.838513890829568, + "heading": -0.5495863998363797, + "angularVelocity": -0.3410977418767151, + "velocityX": -2.273218628779447, + "velocityY": 1.6670311457524134, + "timestamp": 0.28128217432324965 + }, + { + "x": 13.783920159167808, + "y": 6.942715781556149, + "heading": -0.5709265094735697, + "angularVelocity": -0.22760179903187838, + "velocityX": -1.5154723682948443, + "velocityY": 1.111359697541642, + "timestamp": 0.3750428990976662 + }, + { + "x": 13.738450910492517, + "y": 6.97606035494089, + "heading": -0.577756226947398, + "angularVelocity": -0.1821053451622054, + "velocityX": -1.2123771233598004, + "velocityY": 0.8890887608138573, + "timestamp": 0.41254711111156883 + }, + { + "x": 13.704348988985252, + "y": 7.001068805104032, + "heading": -0.582878916551458, + "angularVelocity": -0.13658971430091627, + "velocityX": -0.9092824425859869, + "velocityY": 0.6668171072057679, + "timestamp": 0.45005132312547147 + }, + { + "x": 13.681614380767433, + "y": 7.017741113917072, + "heading": -0.5862941684873881, + "angularVelocity": -0.09106315671061506, + "velocityX": -0.6061881318661277, + "velocityY": 0.44454497022520045, + "timestamp": 0.4875555351393741 + }, + { + "x": 13.670247077941896, + "y": 7.0260772705078125, + "heading": -0.5880017968591525, + "angularVelocity": -0.04553164244942512, + "velocityX": -0.3030940317136526, + "velocityY": 0.2222725433519379, + "timestamp": 0.5250597471532767 + }, + { + "x": 13.670247077941896, + "y": 7.0260772705078125, + "heading": -0.5880017968591525, + "angularVelocity": 4.3220894143993686e-27, + "velocityX": -2.1854050012083175e-26, + "velocityY": 3.319166050857422e-26, + "timestamp": 0.5625639591671794 + } + ], + "eventMarkers": [ + { + "timestamp": 0.3750428990976662, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).6.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).6.traj index 2805b76e..70bc3f62 100644 --- a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).6.traj +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).6.traj @@ -1,99 +1,99 @@ { "samples": [ { - "x": 14.352291107177734, - "y": 6.525911808013916, - "heading": -0.48570563852984416, - "angularVelocity": 1.0539627829247847e-26, - "velocityX": -5.1337066004257686e-26, - "velocityY": -2.752712125666017e-26, + "x": 14.446789741516112, + "y": 6.612919330596924, + "heading": -0.5944409566225068, + "angularVelocity": -2.2907035566312707e-22, + "velocityX": -2.4801938086703075e-22, + "velocityY": 8.637122930206518e-23, "timestamp": 0 }, { - "x": 14.281244102466694, - "y": 6.578011637665101, - "heading": -0.49633092681747565, - "angularVelocity": -0.11332344447203622, - "velocityX": -0.7577480323661817, - "velocityY": 0.555667949202982, - "timestamp": 0.09376072477441655 + "x": 14.374878621007513, + "y": 6.6681554053884105, + "heading": -0.5937701281033373, + "angularVelocity": 0.0070535118408139325, + "velocityX": -0.7561186286794916, + "velocityY": 0.5807867382623025, + "timestamp": 0.09510560615890062 }, { - "x": 14.139150572999801, - "y": 6.682211842382296, - "heading": -0.5176048283391019, - "angularVelocity": -0.22689565991314806, - "velocityX": -1.515490945795936, - "velocityY": 1.1113417154986245, - "timestamp": 0.1875214495488331 + "x": 14.231056382755842, + "y": 6.778627552624006, + "heading": -0.5924285241888535, + "angularVelocity": 0.014106465104088719, + "velocityX": -1.5122372282804897, + "velocityY": 1.1615734518427945, + "timestamp": 0.19021121231780125 }, { - "x": 13.926011946794734, - "y": 6.838513890829568, - "heading": -0.5495863998363797, - "angularVelocity": -0.3410977418767151, - "velocityX": -2.273218628779447, - "velocityY": 1.6670311457524134, - "timestamp": 0.28128217432324965 + "x": 14.015323043710728, + "y": 6.944335758664245, + "heading": -0.5904163161957059, + "angularVelocity": 0.021157617036641953, + "velocityX": -2.268355649662441, + "velocityY": 1.7423600220093918, + "timestamp": 0.28531681847670187 }, { - "x": 13.783920159167808, - "y": 6.942715781556149, - "heading": -0.5709265094735697, - "angularVelocity": -0.22760179903187838, - "velocityX": -1.5154723682948443, - "velocityY": 1.111359697541642, - "timestamp": 0.3750428990976662 + "x": 13.871500813115434, + "y": 7.0548078992888055, + "heading": -0.5890749095880986, + "angularVelocity": 0.014104390495824683, + "velocityX": -1.512237147776525, + "velocityY": 1.161573382330228, + "timestamp": 0.3804224246356025 }, { - "x": 13.738450910492517, - "y": 6.97606035494089, - "heading": -0.577756226947398, - "angularVelocity": -0.1821053451622054, - "velocityX": -1.2123771233598004, - "velocityY": 0.8890887608138573, - "timestamp": 0.41254711111156883 + "x": 13.82547769782775, + "y": 7.0901589854242735, + "heading": -0.5886456630608746, + "angularVelocity": 0.011283417909435272, + "velocityX": -1.209789737015094, + "velocityY": 0.9292587199213779, + "timestamp": 0.41846466774574465 }, { - "x": 13.704348988985252, - "y": 7.001068805104032, - "heading": -0.582878916551458, - "angularVelocity": -0.13658971430091627, - "velocityX": -0.9092824425859869, - "velocityY": 0.6668171072057679, - "timestamp": 0.45005132312547147 + "x": 13.790960360957698, + "y": 7.116672300328794, + "heading": -0.5883237297397991, + "angularVelocity": 0.008462522048014776, + "velocityX": -0.9073423133887173, + "velocityY": 0.696944047903743, + "timestamp": 0.4565069108558868 }, { - "x": 13.681614380767433, - "y": 7.017741113917072, - "heading": -0.5862941684873881, - "angularVelocity": -0.09106315671061506, - "velocityX": -0.6061881318661277, - "velocityY": 0.44454497022520045, - "timestamp": 0.4875555351393741 + "x": 13.767948802831496, + "y": 7.134347843758721, + "heading": -0.588109107903455, + "angularVelocity": 0.005641671436740162, + "velocityX": -0.6048948811871482, + "velocityY": 0.46462936948148964, + "timestamp": 0.49454915396602894 }, { - "x": 13.670247077941896, - "y": 7.0260772705078125, + "x": 13.75644302368164, + "y": 7.143185615539551, "heading": -0.5880017968591525, - "angularVelocity": -0.04553164244942512, - "velocityX": -0.3030940317136526, - "velocityY": 0.2222725433519379, - "timestamp": 0.5250597471532767 + "angularVelocity": 0.0028208390339083305, + "velocityX": -0.3024474428740401, + "velocityY": 0.2323146864721631, + "timestamp": 0.5325913970761711 }, { - "x": 13.670247077941896, - "y": 7.0260772705078125, + "x": 13.75644302368164, + "y": 7.143185615539551, "heading": -0.5880017968591525, - "angularVelocity": 4.3220894143993686e-27, - "velocityX": -2.1854050012083175e-26, - "velocityY": 3.319166050857422e-26, - "timestamp": 0.5625639591671794 + "angularVelocity": -2.989139878822297e-21, + "velocityX": -3.903367328466188e-21, + "velocityY": -2.1704144058469085e-20, + "timestamp": 0.5706336401863132 } ], "eventMarkers": [ { - "timestamp": 0.3750428990976662, + "timestamp": 0.3804224246356025, "command": { "type": "named", "data": { diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).7 2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).7 2.traj new file mode 100644 index 00000000..2786ceef --- /dev/null +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).7 2.traj @@ -0,0 +1,221 @@ +{ + "samples": [ + { + "x": 13.670247077941896, + "y": 7.0260772705078125, + "heading": -0.5880017968591525, + "angularVelocity": 4.3220894143993686e-27, + "velocityX": -2.1854050012083175e-26, + "velocityY": 3.319166050857422e-26, + "timestamp": 0 + }, + { + "x": 13.645024237315694, + "y": 7.029213812474465, + "heading": -0.5586050184455363, + "angularVelocity": 0.5751667660211687, + "velocityX": -0.49350100438627437, + "velocityY": 0.061368449088751095, + "timestamp": 0.05111000869708482 + }, + { + "x": 13.59445850581916, + "y": 7.035391208480669, + "heading": -0.5016300369058596, + "angularVelocity": 1.1147519437406412, + "velocityX": -0.9893508685592758, + "velocityY": 0.1208647027007058, + "timestamp": 0.10222001739417008 + }, + { + "x": 13.518403853674121, + "y": 7.044499236825847, + "heading": -0.41964577392325547, + "angularVelocity": 1.6040745261559681, + "velocityX": -1.4880578987140245, + "velocityY": 0.17820439826490078, + "timestamp": 0.15333002609125534 + }, + { + "x": 13.416671986943191, + "y": 7.056423820145038, + "heading": -0.3169282187933418, + "angularVelocity": 2.0097346439264405, + "velocityX": -1.9904490201491905, + "velocityY": 0.2333120972423329, + "timestamp": 0.2044400347883406 + }, + { + "x": 13.289036124529042, + "y": 7.071060906808593, + "heading": -0.20104341684391944, + "angularVelocity": 2.2673602471140164, + "velocityX": -2.497277258757889, + "velocityY": 0.28638395955486035, + "timestamp": 0.25555004348542587 + }, + { + "x": 13.135342270967683, + "y": 7.08828685694279, + "heading": -0.08640408959186506, + "angularVelocity": 2.2429917383010047, + "velocityX": -3.007118517084588, + "velocityY": 0.3370367286824492, + "timestamp": 0.30666005218251113 + }, + { + "x": 12.956762188284944, + "y": 7.1075748581915015, + "heading": -0.010726520537828105, + "angularVelocity": 1.48068002693087, + "velocityX": -3.4940335021489295, + "velocityY": 0.3773820772175376, + "timestamp": 0.3577700608795964 + }, + { + "x": 12.755389428383719, + "y": 7.12888688951605, + "heading": -1.8456979469985217e-7, + "angularVelocity": 0.20986762165519152, + "velocityX": -3.9399868056119236, + "velocityY": 0.41698351982012405, + "timestamp": 0.40888006957668166 + }, + { + "x": 12.55069190372683, + "y": 7.152531606779918, + "heading": -1.385534968264371e-7, + "angularVelocity": 9.003382908060325e-7, + "velocityX": -4.00503795391767, + "velocityY": 0.4626240117469566, + "timestamp": 0.4599900782737669 + }, + { + "x": 12.345994384979965, + "y": 7.176176375208352, + "heading": -9.253727372935047e-8, + "angularVelocity": 9.003368277593551e-7, + "velocityX": -4.005037838284301, + "velocityY": 0.46262501281442936, + "timestamp": 0.5111000869708522 + }, + { + "x": 12.141296845030876, + "y": 7.1998209600841285, + "heading": -4.652103543469606e-8, + "angularVelocity": 9.003371251094857e-7, + "velocityX": -4.005038253119366, + "velocityY": 0.46262142148932034, + "timestamp": 0.5622100956679374 + }, + { + "x": 11.93593692779541, + "y": 7.216775417327881, + "heading": -7.736119630596452e-26, + "angularVelocity": 9.102138039226991e-7, + "velocityX": -4.017998088252668, + "velocityY": 0.33172479668779814, + "timestamp": 0.6133201043650227 + }, + { + "x": 11.603934607227266, + "y": 7.244185435917073, + "heading": 2.6079666498891626e-17, + "angularVelocity": 3.1562448816546114e-16, + "velocityX": -4.017998550432804, + "velocityY": 0.33172483484526066, + "timestamp": 0.6959488849384701 + }, + { + "x": 11.271932286659125, + "y": 7.271595454506265, + "heading": 1.288603670926865e-16, + "angularVelocity": 1.2438849984287554e-15, + "velocityX": -4.017998550432804, + "velocityY": 0.33172483484526066, + "timestamp": 0.7785776655119174 + }, + { + "x": 10.93992996609098, + "y": 7.299005473095457, + "heading": 2.271885871201871e-16, + "angularVelocity": 1.1899996507881914e-15, + "velocityX": -4.017998550432804, + "velocityY": 0.3317248348452606, + "timestamp": 0.8612064460853648 + }, + { + "x": 10.607927645522839, + "y": 7.326415491684649, + "heading": 2.5614842689098653e-16, + "angularVelocity": 3.504812677828607e-16, + "velocityX": -4.017998550432804, + "velocityY": 0.3317248348452606, + "timestamp": 0.9438352266588121 + }, + { + "x": 10.275925324954695, + "y": 7.3538255102738415, + "heading": 2.977295806911055e-16, + "angularVelocity": 5.032284575841044e-16, + "velocityX": -4.017998550432804, + "velocityY": 0.3317248348452606, + "timestamp": 1.0264640072322595 + }, + { + "x": 9.943923004386694, + "y": 7.381235528863022, + "heading": 2.9677600290999526e-16, + "angularVelocity": -1.1540504102827907e-17, + "velocityX": -4.017998550431105, + "velocityY": 0.3317248348451205, + "timestamp": 1.1090927878057069 + }, + { + "x": 9.671068786342074, + "y": 7.4037622966758025, + "heading": 1.766717126358e-16, + "angularVelocity": -1.453540636092317e-15, + "velocityX": -3.302169246006048, + "velocityY": 0.2726261679821815, + "timestamp": 1.1917215683791542 + }, + { + "x": 9.466428105073092, + "y": 7.420657373999625, + "heading": 8.769803938528749e-17, + "angularVelocity": -1.0767879258667162e-15, + "velocityX": -2.476627149145434, + "velocityY": 0.20446964370732854, + "timestamp": 1.2743503489526016 + }, + { + "x": 9.330000979086366, + "y": 7.4319207593065935, + "heading": 3.000420973683897e-17, + "angularVelocity": -6.982292277353821e-16, + "velocityX": -1.6510848283118424, + "velocityY": 0.13631310094132928, + "timestamp": 1.356979129526049 + }, + { + "x": 9.26178741455078, + "y": 7.437552452087402, + "heading": -5.153249325171924e-25, + "angularVelocity": -3.631205742599535e-16, + "velocityX": -0.8255424328204009, + "velocityY": 0.06815655201159886, + "timestamp": 1.4396079100994963 + }, + { + "x": 9.26178741455078, + "y": 7.437552452087402, + "heading": -5.204793542360701e-25, + "angularVelocity": -6.317967395111268e-26, + "velocityX": -1.2129880403241932e-23, + "velocityY": 1.4316814179344083e-22, + "timestamp": 1.5222366906729436 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).7.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).7.traj index 2786ceef..828b78f1 100644 --- a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).7.traj +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).7.traj @@ -1,221 +1,231 @@ { "samples": [ { - "x": 13.670247077941896, - "y": 7.0260772705078125, + "x": 13.75644302368164, + "y": 7.143185615539551, "heading": -0.5880017968591525, - "angularVelocity": 4.3220894143993686e-27, - "velocityX": -2.1854050012083175e-26, - "velocityY": 3.319166050857422e-26, + "angularVelocity": -2.989139878822297e-21, + "velocityX": -3.903367328466188e-21, + "velocityY": -2.1704144058469085e-20, "timestamp": 0 }, { - "x": 13.645024237315694, - "y": 7.029213812474465, - "heading": -0.5586050184455363, - "angularVelocity": 0.5751667660211687, - "velocityX": -0.49350100438627437, - "velocityY": 0.061368449088751095, - "timestamp": 0.05111000869708482 - }, - { - "x": 13.59445850581916, - "y": 7.035391208480669, - "heading": -0.5016300369058596, - "angularVelocity": 1.1147519437406412, - "velocityX": -0.9893508685592758, - "velocityY": 0.1208647027007058, - "timestamp": 0.10222001739417008 - }, - { - "x": 13.518403853674121, - "y": 7.044499236825847, - "heading": -0.41964577392325547, - "angularVelocity": 1.6040745261559681, - "velocityX": -1.4880578987140245, - "velocityY": 0.17820439826490078, - "timestamp": 0.15333002609125534 - }, - { - "x": 13.416671986943191, - "y": 7.056423820145038, - "heading": -0.3169282187933418, - "angularVelocity": 2.0097346439264405, - "velocityX": -1.9904490201491905, - "velocityY": 0.2333120972423329, - "timestamp": 0.2044400347883406 - }, - { - "x": 13.289036124529042, - "y": 7.071060906808593, - "heading": -0.20104341684391944, - "angularVelocity": 2.2673602471140164, - "velocityX": -2.497277258757889, - "velocityY": 0.28638395955486035, - "timestamp": 0.25555004348542587 - }, - { - "x": 13.135342270967683, - "y": 7.08828685694279, - "heading": -0.08640408959186506, - "angularVelocity": 2.2429917383010047, - "velocityX": -3.007118517084588, - "velocityY": 0.3370367286824492, - "timestamp": 0.30666005218251113 - }, - { - "x": 12.956762188284944, - "y": 7.1075748581915015, - "heading": -0.010726520537828105, - "angularVelocity": 1.48068002693087, - "velocityX": -3.4940335021489295, - "velocityY": 0.3773820772175376, - "timestamp": 0.3577700608795964 - }, - { - "x": 12.755389428383719, - "y": 7.12888688951605, - "heading": -1.8456979469985217e-7, - "angularVelocity": 0.20986762165519152, - "velocityX": -3.9399868056119236, - "velocityY": 0.41698351982012405, - "timestamp": 0.40888006957668166 - }, - { - "x": 12.55069190372683, - "y": 7.152531606779918, - "heading": -1.385534968264371e-7, - "angularVelocity": 9.003382908060325e-7, - "velocityX": -4.00503795391767, - "velocityY": 0.4626240117469566, - "timestamp": 0.4599900782737669 - }, - { - "x": 12.345994384979965, - "y": 7.176176375208352, - "heading": -9.253727372935047e-8, - "angularVelocity": 9.003368277593551e-7, - "velocityX": -4.005037838284301, - "velocityY": 0.46262501281442936, - "timestamp": 0.5111000869708522 - }, - { - "x": 12.141296845030876, - "y": 7.1998209600841285, - "heading": -4.652103543469606e-8, - "angularVelocity": 9.003371251094857e-7, - "velocityX": -4.005038253119366, - "velocityY": 0.46262142148932034, - "timestamp": 0.5622100956679374 - }, - { - "x": 11.93593692779541, - "y": 7.216775417327881, - "heading": -7.736119630596452e-26, - "angularVelocity": 9.102138039226991e-7, - "velocityX": -4.017998088252668, - "velocityY": 0.33172479668779814, - "timestamp": 0.6133201043650227 - }, - { - "x": 11.603934607227266, - "y": 7.244185435917073, - "heading": 2.6079666498891626e-17, - "angularVelocity": 3.1562448816546114e-16, - "velocityX": -4.017998550432804, - "velocityY": 0.33172483484526066, - "timestamp": 0.6959488849384701 - }, - { - "x": 11.271932286659125, - "y": 7.271595454506265, - "heading": 1.288603670926865e-16, - "angularVelocity": 1.2438849984287554e-15, - "velocityX": -4.017998550432804, - "velocityY": 0.33172483484526066, - "timestamp": 0.7785776655119174 - }, - { - "x": 10.93992996609098, - "y": 7.299005473095457, - "heading": 2.271885871201871e-16, - "angularVelocity": 1.1899996507881914e-15, - "velocityX": -4.017998550432804, - "velocityY": 0.3317248348452606, - "timestamp": 0.8612064460853648 - }, - { - "x": 10.607927645522839, - "y": 7.326415491684649, - "heading": 2.5614842689098653e-16, - "angularVelocity": 3.504812677828607e-16, - "velocityX": -4.017998550432804, - "velocityY": 0.3317248348452606, - "timestamp": 0.9438352266588121 - }, - { - "x": 10.275925324954695, - "y": 7.3538255102738415, - "heading": 2.977295806911055e-16, - "angularVelocity": 5.032284575841044e-16, - "velocityX": -4.017998550432804, - "velocityY": 0.3317248348452606, - "timestamp": 1.0264640072322595 - }, - { - "x": 9.943923004386694, - "y": 7.381235528863022, - "heading": 2.9677600290999526e-16, - "angularVelocity": -1.1540504102827907e-17, - "velocityX": -4.017998550431105, - "velocityY": 0.3317248348451205, - "timestamp": 1.1090927878057069 - }, - { - "x": 9.671068786342074, - "y": 7.4037622966758025, - "heading": 1.766717126358e-16, - "angularVelocity": -1.453540636092317e-15, - "velocityX": -3.302169246006048, - "velocityY": 0.2726261679821815, - "timestamp": 1.1917215683791542 - }, - { - "x": 9.466428105073092, - "y": 7.420657373999625, - "heading": 8.769803938528749e-17, - "angularVelocity": -1.0767879258667162e-15, - "velocityX": -2.476627149145434, - "velocityY": 0.20446964370732854, - "timestamp": 1.2743503489526016 - }, - { - "x": 9.330000979086366, - "y": 7.4319207593065935, - "heading": 3.000420973683897e-17, - "angularVelocity": -6.982292277353821e-16, - "velocityX": -1.6510848283118424, - "velocityY": 0.13631310094132928, - "timestamp": 1.356979129526049 + "x": 13.7295528779877, + "y": 7.145512508632667, + "heading": -0.5593356739909306, + "angularVelocity": 0.5453918054970608, + "velocityX": -0.5116026739128892, + "velocityY": 0.04427066858979402, + "timestamp": 0.05256060428355003 + }, + { + "x": 13.675696703739712, + "y": 7.150081094861737, + "heading": -0.5031004725393259, + "angularVelocity": 1.0699116233183206, + "velocityX": -1.024649069052714, + "velocityY": 0.08692035206491855, + "timestamp": 0.10512120856710006 + }, + { + "x": 13.594778136456949, + "y": 7.156781935585546, + "heading": -0.4208691999200477, + "angularVelocity": 1.5645039424520941, + "velocityX": -1.5395288616970724, + "velocityY": 0.12748789354969425, + "timestamp": 0.15768181285065008 + }, + { + "x": 13.486657017941033, + "y": 7.165480150396445, + "heading": -0.31559001044646323, + "angularVelocity": 2.003005690452726, + "velocityX": -2.0570752560726255, + "velocityY": 0.16548924673647072, + "timestamp": 0.2102424171342001 + }, + { + "x": 13.351125165106843, + "y": 7.1760168934848485, + "heading": -0.19335854854743198, + "angularVelocity": 2.3255338016973006, + "velocityX": -2.5785824703048017, + "velocityY": 0.20046845412127823, + "timestamp": 0.26280302141775014 + }, + { + "x": 13.187951040267704, + "y": 7.1881615298982275, + "heading": -0.06830550460961216, + "angularVelocity": 2.3792162522179776, + "velocityX": -3.1044948410193456, + "velocityY": 0.23105968013346895, + "timestamp": 0.31536362570130017 + }, + { + "x": 12.999485780940217, + "y": 7.200902665906217, + "heading": -7.219474003499003e-8, + "angularVelocity": 1.299555690919823, + "velocityX": -3.58567527707191, + "velocityY": 0.24240847649419411, + "timestamp": 0.3679242299848502 + }, + { + "x": 12.78823863885508, + "y": 7.217612049538315, + "heading": -5.77661422338363e-8, + "angularVelocity": 2.745135448469375e-7, + "velocityX": -4.019115551745117, + "velocityY": 0.31790699250631954, + "timestamp": 0.4204848342684002 + }, + { + "x": 12.576998356930917, + "y": 7.234407936932018, + "heading": -4.3339952330086093e-8, + "angularVelocity": 2.7446773301816857e-7, + "velocityX": -4.018985032679267, + "velocityY": 0.3195527833564148, + "timestamp": 0.47304543855195025 + }, + { + "x": 12.365757195669019, + "y": 7.251192761311872, + "heading": -2.8913607283949816e-8, + "angularVelocity": 2.7447068470390713e-7, + "velocityX": -4.019001762656892, + "velocityY": 0.31934230225557386, + "timestamp": 0.5256060428355003 + }, + { + "x": 12.154511938095094, + "y": 7.267925952676431, + "heading": -1.4486541183922857e-8, + "angularVelocity": 2.7448440322751447e-7, + "velocityX": -4.019079697682207, + "velocityY": 0.31835995024503166, + "timestamp": 0.5781666471190503 + }, + { + "x": 11.942968368530272, + "y": 7.280330657958984, + "heading": 2.0785063958808085e-20, + "angularVelocity": 2.756159557415578e-7, + "velocityX": -4.024755279136523, + "velocityY": 0.23600766109220464, + "timestamp": 0.6307272514026003 + }, + { + "x": 11.610157071294928, + "y": 7.29984638261751, + "heading": 5.720799166605624e-18, + "angularVelocity": 6.893144194831854e-17, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 0.7134183125065823 + }, + { + "x": 11.277345774059585, + "y": 7.319362107276035, + "heading": 4.27914940906101e-18, + "angularVelocity": -1.7434167161617802e-17, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 0.7961093736105642 + }, + { + "x": 10.944534476824241, + "y": 7.3388778319345604, + "heading": -2.8918690030091486e-17, + "angularVelocity": -4.01468296820159e-16, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 0.8788004347145462 + }, + { + "x": 10.611723179588898, + "y": 7.358393556593086, + "heading": -7.60328295215413e-17, + "angularVelocity": -5.697609740032433e-16, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 0.9614914958185281 + }, + { + "x": 10.278911882353555, + "y": 7.377909281251611, + "heading": -1.4437311459656085e-16, + "angularVelocity": -8.264531158162608e-16, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 1.04418255692251 + }, + { + "x": 9.946100585118234, + "y": 7.3974250059101365, + "heading": -1.6062472011936515e-16, + "angularVelocity": -1.9653400658253493e-16, + "velocityX": -4.024755430539445, + "velocityY": 0.236007669970322, + "timestamp": 1.126873618026492 + }, + { + "x": 9.672375324044506, + "y": 7.413475983961583, + "heading": 2.0694001348599773e-12, + "angularVelocity": 2.5027623686889767e-11, + "velocityX": -3.310215849443808, + "velocityY": 0.19410777703363524, + "timestamp": 1.209564679130474 + }, + { + "x": 9.467081371147623, + "y": 7.425514217916012, + "heading": 1.4773324091984537e-12, + "angularVelocity": -7.159996712547157e-12, + "velocityX": -2.482661972842874, + "velocityY": 0.14558083780410894, + "timestamp": 1.2922557402344559 + }, + { + "x": 9.3302187338275, + "y": 7.433539707339499, + "heading": 5.797823580481929e-13, + "angularVelocity": -1.0854257269980322e-11, + "velocityX": -1.655108006753246, + "velocityY": 0.09705389332705425, + "timestamp": 1.3749468013384378 }, { "x": 9.26178741455078, "y": 7.437552452087402, - "heading": -5.153249325171924e-25, - "angularVelocity": -3.631205742599535e-16, - "velocityX": -0.8255424328204009, - "velocityY": 0.06815655201159886, - "timestamp": 1.4396079100994963 + "heading": -4.962355351536534e-26, + "angularVelocity": -7.011427236605464e-12, + "velocityX": -0.8275540108340242, + "velocityY": 0.04852694710082174, + "timestamp": 1.4576378624424198 }, { "x": 9.26178741455078, "y": 7.437552452087402, - "heading": -5.204793542360701e-25, - "angularVelocity": -6.317967395111268e-26, - "velocityX": -1.2129880403241932e-23, - "velocityY": 1.4316814179344083e-22, - "timestamp": 1.5222366906729436 + "heading": -2.4523596562464635e-26, + "angularVelocity": 6.97031270768781e-27, + "velocityX": 1.5654039367177217e-19, + "velocityY": 1.335931106132843e-18, + "timestamp": 1.5403289235464017 } ], - "eventMarkers": [] + "eventMarkers": [ + { + "timestamp": 0.6307272514026003, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + } + ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).8 2.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).8 2.traj new file mode 100644 index 00000000..d2e7c60b --- /dev/null +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).8 2.traj @@ -0,0 +1,131 @@ +{ + "samples": [ + { + "x": 9.26178741455078, + "y": 7.437552452087402, + "heading": -5.204793542360701e-25, + "angularVelocity": -6.317967395111268e-26, + "velocityX": -1.2129880403241932e-23, + "velocityY": 1.4316814179344083e-22, + "timestamp": 0 + }, + { + "x": 9.329755853999478, + "y": 7.419457395378352, + "heading": -0.013376630543289742, + "angularVelocity": -0.15963467895053327, + "velocityX": 0.8111250419190595, + "velocityY": -0.21594366077412155, + "timestamp": 0.08379526698854001 + }, + { + "x": 9.46569267412481, + "y": 7.383267297606982, + "heading": -0.04017390350981554, + "angularVelocity": -0.31979458899738056, + "velocityX": 1.6222493824612316, + "velocityY": -0.4318871348225413, + "timestamp": 0.16759053397708001 + }, + { + "x": 9.669597656216824, + "y": 7.32898221699986, + "heading": -0.08046638000926655, + "angularVelocity": -0.4808443000123345, + "velocityX": 2.4333711129520146, + "velocityY": -0.6478299140039275, + "timestamp": 0.25138580096562 + }, + { + "x": 9.94147049419242, + "y": 7.256602235044674, + "heading": -0.13433954177465304, + "angularVelocity": -0.6429141370568642, + "velocityX": 3.2444891906935323, + "velocityY": -0.8637717207236001, + "timestamp": 0.33518106795416003 + }, + { + "x": 10.267933929791393, + "y": 7.169688743599082, + "heading": -0.1343395455343343, + "angularVelocity": -4.4867465828483885e-8, + "velocityX": 3.8959650984061396, + "velocityY": -1.0372124174684092, + "timestamp": 0.41897633494270003 + }, + { + "x": 10.594397365390467, + "y": 7.082775252153461, + "heading": -0.13433954929395236, + "angularVelocity": -4.486671145978699e-8, + "velocityX": 3.8959650984073204, + "velocityY": -1.0372124174687236, + "timestamp": 0.50277160193124 + }, + { + "x": 10.92086080098954, + "y": 6.99586176070784, + "heading": -0.1343395530535705, + "angularVelocity": -4.486671228851664e-8, + "velocityX": 3.8959650984073204, + "velocityY": -1.0372124174687236, + "timestamp": 0.58656686891978 + }, + { + "x": 11.247324236588515, + "y": 6.908948269262247, + "heading": -0.13433955681325235, + "angularVelocity": -4.486747226873845e-8, + "velocityX": 3.895965098406137, + "velocityY": -1.0372124174684085, + "timestamp": 0.6703621359083201 + }, + { + "x": 11.519194528616957, + "y": 6.836568965107796, + "heading": -0.18900176869601656, + "angularVelocity": -0.6523305414163806, + "velocityX": 3.244458807746568, + "velocityY": -0.8637636319525063, + "timestamp": 0.7541574028968601 + }, + { + "x": 11.723097253083957, + "y": 6.7822844855421724, + "heading": -0.2300018718976995, + "angularVelocity": -0.4892890096918049, + "velocityX": 2.4333441707977026, + "velocityY": -0.6478227412659024, + "timestamp": 0.8379526698854001 + }, + { + "x": 11.859032415291114, + "y": 6.746094829153945, + "heading": -0.2573341284430376, + "angularVelocity": -0.32617900184119325, + "velocityX": 1.6222295971173368, + "velocityY": -0.4318818674231029, + "timestamp": 0.9217479368739401 + }, + { + "x": 11.927, + "y": 6.728, + "heading": -0.271, + "angularVelocity": -0.16308643731431074, + "velocityX": 0.8111148415838417, + "velocityY": -0.21594094516602882, + "timestamp": 1.00554320386248 + }, + { + "x": 11.927, + "y": 6.728, + "heading": -0.271, + "angularVelocity": -1.104828910146849e-25, + "velocityX": -2.8779219726051263e-23, + "velocityY": -1.0811880313092697e-22, + "timestamp": 1.08933847085102 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).8.traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).8.traj index d2e7c60b..fa78f22f 100644 --- a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).8.traj +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).8.traj @@ -3,128 +3,65 @@ { "x": 9.26178741455078, "y": 7.437552452087402, - "heading": -5.204793542360701e-25, - "angularVelocity": -6.317967395111268e-26, - "velocityX": -1.2129880403241932e-23, - "velocityY": 1.4316814179344083e-22, + "heading": -2.4523596562464635e-26, + "angularVelocity": 6.97031270768781e-27, + "velocityX": 1.5654039367177217e-19, + "velocityY": 1.335931106132843e-18, "timestamp": 0 }, { - "x": 9.329755853999478, - "y": 7.419457395378352, - "heading": -0.013376630543289742, - "angularVelocity": -0.15963467895053327, - "velocityX": 0.8111250419190595, - "velocityY": -0.21594366077412155, - "timestamp": 0.08379526698854001 + "x": 9.361038211199778, + "y": 7.437553617689382, + "heading": -1.6076494634632555e-20, + "angularVelocity": -1.6157169576201665e-19, + "velocityX": 0.9974900839285512, + "velocityY": 0.000011714529816125984, + "timestamp": 0.09950053464001041 }, { - "x": 9.46569267412481, - "y": 7.383267297606982, - "heading": -0.04017390350981554, - "angularVelocity": -0.31979458899738056, - "velocityX": 1.6222493824612316, - "velocityY": -0.4318871348225413, - "timestamp": 0.16759053397708001 + "x": 9.559539801305808, + "y": 7.437555948893304, + "heading": -2.775286477301083e-20, + "angularVelocity": -1.173498230999548e-19, + "velocityX": 1.9949801357771932, + "velocityY": 0.000023429059255504534, + "timestamp": 0.19900106928002081 }, { - "x": 9.669597656216824, - "y": 7.32898221699986, - "heading": -0.08046638000926655, - "angularVelocity": -0.4808443000123345, - "velocityX": 2.4333711129520146, - "velocityY": -0.6478299140039275, - "timestamp": 0.25138580096562 + "x": 9.857292168909034, + "y": 7.437559445698981, + "heading": -1.615205586446462e-20, + "angularVelocity": 1.1659041783583904e-19, + "velocityX": 2.9924700272263003, + "velocityY": 0.000035143586811149936, + "timestamp": 0.2985016039200312 }, { - "x": 9.94147049419242, - "y": 7.256602235044674, - "heading": -0.13433954177465304, - "angularVelocity": -0.6429141370568642, - "velocityX": 3.2444891906935323, - "velocityY": -0.8637717207236001, - "timestamp": 0.33518106795416003 + "x": 10.055793759015064, + "y": 7.437561776902903, + "heading": -5.186936203377308e-21, + "angularVelocity": 1.1020161550669728e-19, + "velocityX": 1.9949801357771932, + "velocityY": 0.000023429059255504534, + "timestamp": 0.39800213856004163 }, { - "x": 10.267933929791393, - "y": 7.169688743599082, - "heading": -0.1343395455343343, - "angularVelocity": -4.4867465828483885e-8, - "velocityX": 3.8959650984061396, - "velocityY": -1.0372124174684092, - "timestamp": 0.41897633494270003 + "x": 10.155044555664062, + "y": 7.437562942504883, + "heading": 4.39993089223087e-32, + "angularVelocity": 5.2129731987967565e-20, + "velocityX": 0.9974900839285512, + "velocityY": 0.000011714529816125986, + "timestamp": 0.49750267320005204 }, { - "x": 10.594397365390467, - "y": 7.082775252153461, - "heading": -0.13433954929395236, - "angularVelocity": -4.486671145978699e-8, - "velocityX": 3.8959650984073204, - "velocityY": -1.0372124174687236, - "timestamp": 0.50277160193124 - }, - { - "x": 10.92086080098954, - "y": 6.99586176070784, - "heading": -0.1343395530535705, - "angularVelocity": -4.486671228851664e-8, - "velocityX": 3.8959650984073204, - "velocityY": -1.0372124174687236, - "timestamp": 0.58656686891978 - }, - { - "x": 11.247324236588515, - "y": 6.908948269262247, - "heading": -0.13433955681325235, - "angularVelocity": -4.486747226873845e-8, - "velocityX": 3.895965098406137, - "velocityY": -1.0372124174684085, - "timestamp": 0.6703621359083201 - }, - { - "x": 11.519194528616957, - "y": 6.836568965107796, - "heading": -0.18900176869601656, - "angularVelocity": -0.6523305414163806, - "velocityX": 3.244458807746568, - "velocityY": -0.8637636319525063, - "timestamp": 0.7541574028968601 - }, - { - "x": 11.723097253083957, - "y": 6.7822844855421724, - "heading": -0.2300018718976995, - "angularVelocity": -0.4892890096918049, - "velocityX": 2.4333441707977026, - "velocityY": -0.6478227412659024, - "timestamp": 0.8379526698854001 - }, - { - "x": 11.859032415291114, - "y": 6.746094829153945, - "heading": -0.2573341284430376, - "angularVelocity": -0.32617900184119325, - "velocityX": 1.6222295971173368, - "velocityY": -0.4318818674231029, - "timestamp": 0.9217479368739401 - }, - { - "x": 11.927, - "y": 6.728, - "heading": -0.271, - "angularVelocity": -0.16308643731431074, - "velocityX": 0.8111148415838417, - "velocityY": -0.21594094516602882, - "timestamp": 1.00554320386248 - }, - { - "x": 11.927, - "y": 6.728, - "heading": -0.271, - "angularVelocity": -1.104828910146849e-25, - "velocityX": -2.8779219726051263e-23, - "velocityY": -1.0811880313092697e-22, - "timestamp": 1.08933847085102 + "x": 10.155044555664062, + "y": 7.437562942504883, + "heading": 2.1939125534073598e-32, + "angularVelocity": -1.2166553134589197e-33, + "velocityX": 1.4750876026512068e-22, + "velocityY": 2.5957646699888015e-19, + "timestamp": 0.5970032078400624 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).traj b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).traj index 1eb15225..ec42ffda 100644 --- a/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).traj +++ b/src/main/deploy/choreo/Red Six Piece 2 to 4 (1).traj @@ -3,871 +3,826 @@ { "x": 15.198025703430176, "y": 5.552863121032715, - "heading": 1.1788293001141444e-28, - "angularVelocity": 2.606876389920206e-29, - "velocityX": 1.6147849318653567e-22, - "velocityY": -1.7720524977117054e-24, + "heading": -9.080591826526709e-28, + "angularVelocity": -1.1603484864380112e-28, + "velocityX": -3.747628767048725e-22, + "velocityY": 4.167074864344212e-24, "timestamp": 0 }, { - "x": 15.10708649187578, - "y": 5.553873539056015, - "heading": 1.956931650032567e-18, - "angularVelocity": 2.0546044837018224e-17, - "velocityX": -0.9547809798230291, - "velocityY": 0.010608492132583473, - "timestamp": 0.09524614909196333 + "x": 15.107086496476764, + "y": 5.553873539004894, + "heading": -2.0171687380914284e-18, + "angularVelocity": -2.117848210218587e-17, + "velocityX": -0.95478099189959, + "velocityY": 0.010608492266763213, + "timestamp": 0.09524614306835108 }, { - "x": 14.925208076423274, - "y": 5.555894375017549, - "heading": 4.8821720585032204e-18, - "angularVelocity": 3.0712427077008165e-17, - "velocityX": -1.909561879261876, - "velocityY": 0.02121698337202498, - "timestamp": 0.19049229818392666 + "x": 14.925208085625242, + "y": 5.555894374915306, + "heading": -4.5506349702476365e-18, + "angularVelocity": -2.659914774113284e-17, + "velocityX": -1.9095619517212512, + "velocityY": 0.021216984177111247, + "timestamp": 0.19049228613670216 }, { - "x": 14.65239049535407, - "y": 5.558925628491973, - "heading": 4.9529034125739184e-18, - "angularVelocity": 7.426164168014883e-19, - "velocityX": -2.864342376779887, - "velocityY": 0.03182547014575747, - "timestamp": 0.28573844727589 + "x": 14.652390486152102, + "y": 5.558925628594215, + "heading": -6.069838807935888e-18, + "angularVelocity": -1.5950292451107106e-17, + "velocityX": -2.8643427511532806, + "velocityY": 0.03182547430538356, + "timestamp": 0.2857384292050532 }, { - "x": 14.470512079901564, - "y": 5.560946464453506, - "heading": -5.155775683106669e-18, - "angularVelocity": -1.0613215539074661e-16, - "velocityX": -1.909561879261876, - "velocityY": 0.021216983372024975, - "timestamp": 0.3809845963678533 + "x": 14.47051207530058, + "y": 5.560946464504627, + "heading": -5.0495636395818696e-18, + "angularVelocity": 1.0711984090840416e-17, + "velocityX": -1.9095619517212512, + "velocityY": 0.021216984177111247, + "timestamp": 0.3809845722734043 }, { "x": 14.379572868347168, "y": 5.561956882476807, - "heading": -2.3969600276869747e-28, - "angularVelocity": 5.413106704997615e-17, - "velocityX": -0.9547809798230291, - "velocityY": 0.010608492132583473, - "timestamp": 0.47623074545981664 + "heading": 1.854505670800523e-27, + "angularVelocity": 5.3015938280259253e-17, + "velocityX": -0.95478099189959, + "velocityY": 0.01060849226676321, + "timestamp": 0.47623071534175543 }, { "x": 14.379572868347168, "y": 5.561956882476807, - "heading": -1.1788127589783305e-28, - "angularVelocity": 4.1297903821940984e-29, - "velocityX": -1.6147929435574846e-22, - "velocityY": 1.8251378962940275e-24, - "timestamp": 0.57147689455178 + "heading": 9.080546216345213e-28, + "angularVelocity": -4.029142807111623e-28, + "velocityX": 3.747658167813594e-22, + "velocityY": -4.159522027499615e-24, + "timestamp": 0.5714768584101065 }, { - "x": 14.303949787857668, + "x": 14.303949799853617, "y": 5.561956882476807, - "heading": 4.2371272764586235e-25, - "angularVelocity": 4.879852542216884e-24, - "velocityX": -0.8707004020209776, - "velocityY": 4.615774993405339e-16, - "timestamp": 0.6583300535878512 + "heading": -6.436370661864032e-21, + "angularVelocity": -7.410637050778398e-20, + "velocityX": -0.8707003739387539, + "velocityY": 2.6828250389840444e-16, + "timestamp": 0.6583300064700506 }, { - "x": 14.152703635762082, + "x": 14.15270366641151, "y": 5.561956882476807, - "heading": 9.62666688048605e-25, - "angularVelocity": 6.205346660780305e-24, - "velocityX": -1.7414007017611408, - "velocityY": 9.23154942545405e-16, - "timestamp": 0.7451832126239225 + "heading": -2.0042157210092705e-20, + "angularVelocity": -1.5665277370046843e-19, + "velocityX": -1.741400707061555, + "velocityY": 5.368432878740444e-16, + "timestamp": 0.7451831545299946 }, { - "x": 13.925834474244276, + "x": 13.925834492835799, "y": 5.561956882476807, - "heading": -6.35334199578853e-25, - "angularVelocity": -1.8398880424876378e-23, - "velocityX": -2.6121002855357762, - "velocityY": 1.3847320055814999e-15, - "timestamp": 0.8320363716599938 + "heading": -1.7073389524704635e-20, + "angularVelocity": 3.4181463213745686e-20, + "velocityX": -2.612100754472715, + "velocityY": 8.033174756016209e-16, + "timestamp": 0.8320363025899387 }, { - "x": 13.774588363604606, + "x": 13.774588375936995, "y": 5.561956882476807, - "heading": -4.288440082411824e-25, - "angularVelocity": 2.3774632221708446e-24, - "velocityX": -1.7414002244507438, - "velocityY": 9.231546882646077e-16, - "timestamp": 0.918889530696065 + "heading": -7.883060482060434e-21, + "angularVelocity": 1.0581457607411195e-19, + "velocityX": -1.7414005165871203, + "velocityY": 5.360485261048027e-16, + "timestamp": 0.9188894506498827 }, { - "x": 13.717871079476808, + "x": 13.717871085037839, "y": 5.561956882476807, - "heading": -2.590171361320003e-25, - "angularVelocity": 3.910667216659344e-24, - "velocityX": -1.3060502198614803, - "velocityY": 6.923660434900599e-16, - "timestamp": 0.9623161028640279 + "heading": -3.134968058954825e-21, + "angularVelocity": 1.0933611158328031e-19, + "velocityX": -1.3060504080011666, + "velocityY": 4.021698310203676e-16, + "timestamp": 0.9623160217467509 }, { - "x": 13.680059555654978, + "x": 13.680059557344757, "y": 5.561956882476807, - "heading": -1.0277255516428025e-25, - "angularVelocity": 3.597902693388258e-24, - "velocityX": -0.8707001712128097, - "velocityY": 4.615773753780885e-16, - "timestamp": 1.0057426750319909 + "heading": 7.130869940497685e-22, + "angularVelocity": 8.861061225492244e-20, + "velocityX": -0.8707002818329597, + "velocityY": 2.681499176641714e-16, + "timestamp": 1.0057425928436192 }, { "x": 13.66115379333496, "y": 5.561956882476807, - "heading": 4.1528332423630384e-27, - "angularVelocity": 2.4622111087440337e-24, - "velocityX": -0.43535009502700467, - "velocityY": 2.3078869268024055e-16, - "timestamp": 1.0491692471999539 + "heading": 2.9039653211073e-21, + "angularVelocity": 5.0450179964504564e-20, + "velocityX": -0.4353501446758437, + "velocityY": 1.3407690569526743e-16, + "timestamp": 1.0491691639404874 }, { "x": 13.66115379333496, "y": 5.561956882476807, - "heading": 4.171558674345914e-27, - "angularVelocity": 4.311975603546073e-28, - "velocityX": -1.6145553776744748e-27, - "velocityY": 2.588288608432755e-27, - "timestamp": 1.0925958193679168 - }, - { - "x": 13.691513815926383, - "y": 5.525701060761086, - "heading": 0.023562463335368315, - "angularVelocity": 0.34182634637050774, - "velocityX": 0.4404401802325416, - "velocityY": -0.5259719620716655, - "timestamp": 1.1615269113845306 - }, - { - "x": 13.752233823940973, - "y": 5.453149387533433, - "heading": 0.06932129795634334, - "angularVelocity": 0.663834465438997, - "velocityX": 0.8808798212562146, - "velocityY": -1.0525246460648914, - "timestamp": 1.2304580034011443 - }, - { - "x": 13.843313276711916, - "y": 5.344233687044581, - "heading": 0.13498389635529368, - "angularVelocity": 0.9525831736877814, - "velocityX": 1.3213116186958351, - "velocityY": -1.580066372118415, - "timestamp": 1.299389095417758 - }, - { - "x": 13.964751433548663, - "y": 5.198837902534373, - "heading": 0.21647867376005756, - "angularVelocity": 1.1822644182848892, - "velocityX": 1.7617326707587837, - "velocityY": -2.109291761621367, - "timestamp": 1.3683201874343718 - }, - { - "x": 14.116535425780143, - "y": 5.016750311314966, - "heading": 0.30307237204373855, - "angularVelocity": 1.2562356949576599, - "velocityX": 2.2019670339024615, - "velocityY": -2.6415886632917585, - "timestamp": 1.4372512794509855 - }, - { - "x": 14.26793452721354, - "y": 4.8345584717676635, - "heading": 0.39204433398389704, - "angularVelocity": 1.290737740216195, - "velocityX": 2.196383330136527, - "velocityY": -2.6431010189624082, - "timestamp": 1.5061823714675993 - }, - { - "x": 14.389009217305178, - "y": 4.688953541309653, - "heading": 0.4689267567346741, - "angularVelocity": 1.1153518753517935, - "velocityX": 1.7564597709036147, - "velocityY": -2.1123258923987027, - "timestamp": 1.575113463484213 - }, - { - "x": 14.479788015237448, - "y": 4.5797997732912155, - "heading": 0.5284493552871049, - "angularVelocity": 0.8635087129924586, - "velocityX": 1.3169499463375665, - "velocityY": -1.583520075267771, - "timestamp": 1.6440445555008267 - }, - { - "x": 14.540294978830758, - "y": 4.507052135096341, - "heading": 0.568910729781111, - "angularVelocity": 0.5869829319438942, - "velocityX": 0.8777891343826024, - "velocityY": -1.0553675571734895, - "timestamp": 1.7129756475174405 + "heading": 2.9289616731652808e-21, + "angularVelocity": 5.756004088616709e-22, + "velocityX": -2.138357283045396e-22, + "velocityY": 2.150198010410122e-22, + "timestamp": 1.0925957350373556 + }, + { + "x": 13.69151381424092, + "y": 5.525701061208603, + "heading": 0.023562396214272266, + "angularVelocity": 0.3418254011415353, + "velocityX": 0.44044019251996336, + "velocityY": -0.5259719994527794, + "timestamp": 1.1615268213041603 + }, + { + "x": 13.75223381956499, + "y": 5.45314938890248, + "heading": 0.06932112146119182, + "angularVelocity": 0.6638329340960312, + "velocityX": 0.880879855701796, + "velocityY": -1.05252472049121, + "timestamp": 1.230457907570965 + }, + { + "x": 13.843313269738008, + "y": 5.344233689629052, + "heading": 0.13498360807083573, + "angularVelocity": 0.9525816313918327, + "velocityX": 1.3213116912228196, + "velocityY": -1.580066486285422, + "timestamp": 1.2993889938377696 + }, + { + "x": 13.964751426359838, + "y": 5.198837905171001, + "heading": 0.21647833458525872, + "angularVelocity": 1.182263778623618, + "velocityX": 1.7617328145938624, + "velocityY": -2.1092919368089142, + "timestamp": 1.3683200801045743 + }, + { + "x": 14.116535429050689, + "y": 5.0167503033742005, + "heading": 0.3030721451467355, + "angularVelocity": 1.2562374285863827, + "velocityX": 2.2019673693137145, + "velocityY": -2.64158903708572, + "timestamp": 1.437251166371379 + }, + { + "x": 14.267934537167369, + "y": 4.834558459237534, + "heading": 0.3920443693842881, + "angularVelocity": 1.290741653093595, + "velocityX": 2.1963836103013628, + "velocityY": -2.6431013060127135, + "timestamp": 1.5061822526381836 + }, + { + "x": 14.389009225126475, + "y": 4.688953532451243, + "heading": 0.4689268613880663, + "angularVelocity": 1.115352973057713, + "velocityX": 1.756459886479585, + "velocityY": -2.1123260153294563, + "timestamp": 1.5751133389049883 + }, + { + "x": 14.47978801964584, + "y": 4.5797997685436185, + "heading": 0.5284494394989407, + "angularVelocity": 0.8635084884704485, + "velocityX": 1.3169500066776294, + "velocityY": -1.5835201477187593, + "timestamp": 1.644044425171793 + }, + { + "x": 14.540294980406806, + "y": 4.507052133449136, + "heading": 0.5689107661779228, + "angularVelocity": 0.5869822872422525, + "velocityX": 0.8777891665128892, + "velocityY": -1.0553676002276473, + "timestamp": 1.7129755114385976 }, { "x": 14.570545196533203, "y": 4.470687389373779, "heading": 0.5894308145365931, - "angularVelocity": 0.29768982552222245, - "velocityX": 0.43884721418825445, - "velocityY": -0.5275521489460464, - "timestamp": 1.7819067395340542 + "angularVelocity": 0.2976893223363026, + "velocityX": 0.43884722793008496, + "velocityY": -0.5275521690548106, + "timestamp": 1.7819065977054023 }, { "x": 14.570545196533203, "y": 4.470687389373779, "heading": 0.5894308145365931, - "angularVelocity": 3.863669117287725e-28, - "velocityX": -1.9871518401102085e-27, - "velocityY": 2.8088430719951805e-27, - "timestamp": 1.850837831550668 - }, - { - "x": 14.45709086591278, - "y": 4.4278439544867165, - "heading": 0.488089493217441, - "angularVelocity": -0.9146778399911556, - "velocityX": -1.0240064044825004, - "velocityY": -0.38669261432744145, - "timestamp": 1.961632383346229 - }, - { - "x": 14.230046021105487, - "y": 4.34248670484011, - "heading": 0.2854587836923866, - "angularVelocity": -1.8288869465255828, - "velocityX": -2.0492419629643726, - "velocityY": -0.7704101714686167, - "timestamp": 2.0724269351417903 - }, - { - "x": 14.002349455992734, - "y": 4.258200754530428, - "heading": 0.09515293619351321, - "angularVelocity": -1.7176462598091191, - "velocityX": -2.055124204418461, - "velocityY": -0.760740929438543, - "timestamp": 2.1832214869373514 - }, - { - "x": 13.888501167358198, - "y": 4.216057777427628, - "heading": 8.28079525180121e-11, - "angularVelocity": -0.8588232414738415, - "velocityX": -1.027562157068967, - "velocityY": -0.3803704823009896, - "timestamp": 2.2940160387329125 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 8.268158682804007e-19, - "angularVelocity": -7.474009357788098e-10, - "velocityX": -5.490850602427426e-10, - "velocityY": -2.0617274991432905e-10, - "timestamp": 2.4048105905284736 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 5.905827430465516e-19, - "angularVelocity": -6.406293808708934e-10, - "velocityX": -4.706443421676888e-10, - "velocityY": -1.7671950173476352e-10, - "timestamp": 2.4048105908972253 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 3.9372180312859395e-19, - "angularVelocity": -5.33857823518088e-10, - "velocityX": -3.922036224873799e-10, - "velocityY": -1.472662529525917e-10, - "timestamp": 2.404810591265977 + "angularVelocity": 4.237174098560341e-22, + "velocityX": -2.826594351845562e-22, + "velocityY": 3.573387212832748e-22, + "timestamp": 1.850837683972207 + }, + { + "x": 14.53016602593679, + "y": 4.455446506235451, + "heading": 0.5540064540349209, + "angularVelocity": -0.5360505707818769, + "velocityX": -0.6110280366214283, + "velocityY": -0.2306289793187704, + "timestamp": 1.9169216738651802 + }, + { + "x": 14.44938649587058, + "y": 4.425019141773473, + "heading": 0.4829804666462581, + "angularVelocity": -1.0747835822820835, + "velocityX": -1.2223767087464938, + "velocityY": -0.4604347363295905, + "timestamp": 1.9830056637581535 + }, + { + "x": 14.328164393235157, + "y": 4.379500232629268, + "heading": 0.37640102935392167, + "angularVelocity": -1.6127875672299412, + "velocityX": -1.8343641603927023, + "velocityY": -0.6888038875667982, + "timestamp": 2.049089653651127 + }, + { + "x": 14.16632561176535, + "y": 4.319179292971803, + "heading": 0.23626694546085536, + "angularVelocity": -2.120545144444536, + "velocityX": -2.448986233003096, + "velocityY": -0.9127920356376347, + "timestamp": 2.1151736435441 + }, + { + "x": 14.0448416017969, + "y": 4.274051603899093, + "heading": 0.1325451584186999, + "angularVelocity": -1.569544865710116, + "velocityX": -1.838327409788673, + "velocityY": -0.6828838444197568, + "timestamp": 2.1812576334370735 + }, + { + "x": 14.000178586698231, + "y": 4.2574722138771985, + "heading": 0.0945578464623197, + "angularVelocity": -1.340385412793797, + "velocityX": -1.5759381447780825, + "velocityY": -0.585005134448155, + "timestamp": 2.2095982223406803 + }, + { + "x": 13.96295541639372, + "y": 4.243662024781544, + "heading": 0.06298966033692675, + "angularVelocity": -1.1138860322473925, + "velocityX": -1.31342261203944, + "velocityY": -0.4872936530227246, + "timestamp": 2.237938811244287 + }, + { + "x": 13.933174653940528, + "y": 4.2326176532188775, + "heading": 0.03778230634920248, + "angularVelocity": -0.8894435494428381, + "velocityX": -1.0508166416190299, + "velocityY": -0.3897015549053042, + "timestamp": 2.266279400147894 + }, + { + "x": 13.910838071206133, + "y": 4.2243366192852125, + "heading": 0.018894460026481222, + "angularVelocity": -0.6664592040399531, + "velocityX": -0.788148150709412, + "velocityY": -0.2921969604029915, + "timestamp": 2.2946199890515007 + }, + { + "x": 13.895946755796466, + "y": 4.218817098366333, + "heading": 0.006302298120624352, + "angularVelocity": -0.4443154638982944, + "velocityX": -0.5254412835356537, + "velocityY": -0.19475674756274655, + "timestamp": 2.3229605779551075 }, { "x": 13.888501167297363, "y": 4.216057777404785, - "heading": 2.3623304942893484e-19, - "angularVelocity": -4.270862637180886e-10, - "velocityX": -3.137629012016172e-10, - "velocityY": -1.1781300356773835e-10, - "timestamp": 2.4048105916347287 + "heading": -7.359942498471454e-23, + "angularVelocity": -0.22237710522036122, + "velocityX": -0.26271819983798683, + "velocityY": -0.0973628660622863, + "timestamp": 2.3513011668587143 }, { "x": 13.888501167297363, "y": 4.216057777404785, - "heading": 1.1811648285083364e-19, - "angularVelocity": -3.203147014685827e-10, - "velocityX": -2.353221783101984e-10, - "velocityY": -8.835975358012759e-11, - "timestamp": 2.4048105920034804 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": 3.937210429839709e-20, - "angularVelocity": -2.1354313676727285e-10, - "velocityX": -1.568814538129234e-10, - "velocityY": -5.890650298968465e-11, - "timestamp": 2.404810592372232 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": -8.532341534406571e-26, - "angularVelocity": -1.0677156961184637e-10, - "velocityX": -7.84407277095909e-11, - "velocityY": -2.945325179633351e-11, - "timestamp": 2.4048105927409837 - }, - { - "x": 13.888501167297363, - "y": 4.216057777404785, - "heading": -8.510879482431963e-26, - "angularVelocity": -1.6866441387712243e-26, - "velocityX": -2.0583941335553829e-25, - "velocityY": 9.841185153624076e-26, - "timestamp": 2.4048105931097354 - }, - { - "x": 13.899986336038381, - "y": 4.272873358599484, - "heading": -0.023038991913601637, - "angularVelocity": -0.3021063684546837, - "velocityX": 0.15060305730606005, - "velocityY": 0.7450130183968425, - "timestamp": 2.481071785587899 - }, - { - "x": 13.922959662828976, - "y": 4.386578192081037, - "heading": -0.0663634759005527, - "angularVelocity": -0.5681065634969774, - "velocityX": 0.3012453129050349, - "velocityY": 1.4909920732502444, - "timestamp": 2.557332978066063 - }, - { - "x": 13.957441252375148, - "y": 4.557273568298352, - "heading": -0.12543465168712403, - "angularVelocity": -0.7745902452743002, - "velocityX": 0.45215119808209714, - "velocityY": 2.238299332470985, - "timestamp": 2.6335941705442267 - }, - { - "x": 14.00350258986825, - "y": 4.785079242541037, - "heading": -0.19129776367156157, - "angularVelocity": -0.8636517453263874, - "velocityX": 0.6039944563716563, - "velocityY": 2.987176922363417, - "timestamp": 2.7098553630223905 - }, - { - "x": 14.06127377629267, - "y": 5.069635038422791, - "heading": -0.23370464392529416, - "angularVelocity": -0.556074182368382, - "velocityX": 0.757543706662608, - "velocityY": 3.731331580780533, - "timestamp": 2.7861165555005543 - }, - { - "x": 14.121438982529087, - "y": 5.371150739294933, - "heading": -0.23370465578542146, - "angularVelocity": -1.5551982461065506e-7, - "velocityX": 0.7889360798238878, - "velocityY": 3.953723920045926, - "timestamp": 2.862377747978718 - }, - { - "x": 14.181604184628418, - "y": 5.672666440992599, - "heading": -0.23370466764554898, - "angularVelocity": -1.5551982762801367e-7, - "velocityX": 0.7889360255749569, - "velocityY": 3.9537239308708902, - "timestamp": 2.938638940456882 - }, - { - "x": 14.238820898893755, - "y": 5.957011236905596, - "heading": -0.27574816661054075, - "angularVelocity": -0.5513092255544053, - "velocityX": 0.7502730078830246, - "velocityY": 3.7285647742056445, - "timestamp": 3.0149001329350456 - }, - { - "x": 14.284420707366433, - "y": 6.1847837505193715, - "heading": -0.34721144483836985, - "angularVelocity": -0.9370857693877743, - "velocityX": 0.5979425051048811, - "velocityY": 2.98674209269669, - "timestamp": 3.0911613254132093 - }, - { - "x": 14.318439308265718, - "y": 6.35544203945297, - "heading": -0.41225183766095985, - "angularVelocity": -0.8528635693864027, - "velocityX": 0.44608010698268263, - "velocityY": 2.2378130132500083, - "timestamp": 3.167422517891373 - }, - { - "x": 14.341028135354547, - "y": 6.469114800114249, - "heading": -0.4602003337578672, - "angularVelocity": -0.6287404450256496, - "velocityX": 0.2962034339457177, - "velocityY": 1.490571507832474, - "timestamp": 3.243683710369537 - }, - { - "x": 14.352291107177734, - "y": 6.525911808013916, - "heading": -0.48570563852984416, - "angularVelocity": -0.33444670799345433, - "velocityX": 0.14768942704917307, - "velocityY": 0.7447694699493517, - "timestamp": 3.3199449028477006 - }, - { - "x": 14.352291107177734, - "y": 6.525911808013916, - "heading": -0.48570563852984416, - "angularVelocity": 1.0539627829247847e-26, - "velocityX": -5.1337066004257686e-26, - "velocityY": -2.752712125666017e-26, - "timestamp": 3.3962060953258644 - }, - { - "x": 14.281244102466694, - "y": 6.578011637665101, - "heading": -0.49633092681747565, - "angularVelocity": -0.11332344447203622, - "velocityX": -0.7577480323661817, - "velocityY": 0.555667949202982, - "timestamp": 3.489966820100281 - }, - { - "x": 14.139150572999801, - "y": 6.682211842382296, - "heading": -0.5176048283391019, - "angularVelocity": -0.22689565991314806, - "velocityX": -1.515490945795936, - "velocityY": 1.1113417154986245, - "timestamp": 3.5837275448746975 - }, - { - "x": 13.926011946794734, - "y": 6.838513890829568, - "heading": -0.5495863998363797, - "angularVelocity": -0.3410977418767151, - "velocityX": -2.273218628779447, - "velocityY": 1.6670311457524134, - "timestamp": 3.677488269649114 - }, - { - "x": 13.783920159167808, - "y": 6.942715781556149, - "heading": -0.5709265094735697, - "angularVelocity": -0.22760179903187838, - "velocityX": -1.5154723682948443, - "velocityY": 1.111359697541642, - "timestamp": 3.7712489944235306 - }, - { - "x": 13.738450910492517, - "y": 6.97606035494089, - "heading": -0.577756226947398, - "angularVelocity": -0.1821053451622054, - "velocityX": -1.2123771233598004, - "velocityY": 0.8890887608138573, - "timestamp": 3.808753206437433 - }, - { - "x": 13.704348988985252, - "y": 7.001068805104032, - "heading": -0.582878916551458, - "angularVelocity": -0.13658971430091627, - "velocityX": -0.9092824425859869, - "velocityY": 0.6668171072057679, - "timestamp": 3.846257418451336 - }, - { - "x": 13.681614380767433, - "y": 7.017741113917072, - "heading": -0.5862941684873881, - "angularVelocity": -0.09106315671061506, - "velocityX": -0.6061881318661277, - "velocityY": 0.44454497022520045, - "timestamp": 3.8837616304652385 - }, - { - "x": 13.670247077941896, - "y": 7.0260772705078125, + "heading": 2.117727432484989e-22, + "angularVelocity": 5.073032730422753e-22, + "velocityX": 1.319201559585301e-22, + "velocityY": -1.2833790442104632e-22, + "timestamp": 2.379641755762321 + }, + { + "x": 13.90055456208268, + "y": 4.2672774930951745, + "heading": -0.024639723698078066, + "angularVelocity": -0.3387505660309093, + "velocityX": 0.16571185440844172, + "velocityY": 0.7041762275692574, + "timestamp": 2.4523788259516883 + }, + { + "x": 13.924664422654004, + "y": 4.369780859826108, + "heading": -0.07184553791598466, + "angularVelocity": -0.648992516402005, + "velocityX": 0.33146592938861846, + "velocityY": 1.4092314477895682, + "timestamp": 2.5251158961410556 + }, + { + "x": 13.960850027131373, + "y": 4.523662093222596, + "heading": -0.1381918964160448, + "angularVelocity": -0.9121395504016173, + "velocityX": 0.4974850428146382, + "velocityY": 2.1155819583586095, + "timestamp": 2.597852966330423 + }, + { + "x": 14.00918910605255, + "y": 4.729054591196495, + "heading": -0.21706345100390917, + "angularVelocity": -1.0843377989040044, + "velocityX": 0.6645728071714858, + "velocityY": 2.8237664431516296, + "timestamp": 2.67059003651979 + }, + { + "x": 14.069933310565814, + "y": 4.985967503680229, + "heading": -0.2879560349605607, + "angularVelocity": -0.9746417304420771, + "velocityX": 0.8351203087382914, + "velocityY": 3.532076722569039, + "timestamp": 2.7433271067091574 + }, + { + "x": 14.136089967453417, + "y": 5.271659493582205, + "heading": -0.2879560398332574, + "angularVelocity": -6.699055461306423e-8, + "velocityX": 0.909531504573521, + "velocityY": 3.9277357358248457, + "timestamp": 2.8160641768985246 + }, + { + "x": 14.202246205709509, + "y": 5.557351580424848, + "heading": -0.2879560447059434, + "angularVelocity": -6.699040765465796e-8, + "velocityX": 0.9095257491655405, + "velocityY": 3.9277370685794617, + "timestamp": 2.888801247087892 + }, + { + "x": 14.268402835983203, + "y": 5.843043576489709, + "heading": -0.2879560495786414, + "angularVelocity": -6.699057305241701e-8, + "velocityX": 0.9095311386815427, + "velocityY": 3.9277358205530803, + "timestamp": 2.961538317277259 + }, + { + "x": 14.328385114782021, + "y": 6.0998732839416245, + "heading": -0.35678639279969876, + "angularVelocity": -0.9462897397690232, + "velocityX": 0.8246452413144653, + "velocityY": 3.5309328074841777, + "timestamp": 3.0342753874666264 + }, + { + "x": 14.376005525808099, + "y": 6.305305162261247, + "heading": -0.44145888026224256, + "angularVelocity": -1.1640898821206782, + "velocityX": 0.654692454646584, + "velocityY": 2.8243078499696304, + "timestamp": 3.1070124576559937 + }, + { + "x": 14.41149328213472, + "y": 6.459199589883294, + "heading": -0.5144824922677588, + "angularVelocity": -1.0039394192727746, + "velocityX": 0.48789092321468885, + "velocityY": 2.1157633545232315, + "timestamp": 3.179749527845361 + }, + { + "x": 14.435047453373745, + "y": 6.561702808468181, + "heading": -0.5669658281865239, + "angularVelocity": -0.7215486653796684, + "velocityX": 0.3238262302523786, + "velocityY": 1.4092294110558068, + "timestamp": 3.252486598034728 + }, + { + "x": 14.446789741516112, + "y": 6.612919330596924, + "heading": -0.5944409566225068, + "angularVelocity": -0.37773212977169646, + "velocityX": 0.16143471426325046, + "velocityY": 0.7041323220113681, + "timestamp": 3.3252236682240954 + }, + { + "x": 14.446789741516112, + "y": 6.612919330596924, + "heading": -0.5944409566225068, + "angularVelocity": -2.2907035566312707e-22, + "velocityX": -2.4801938086703075e-22, + "velocityY": 8.637122930206518e-23, + "timestamp": 3.3979607384134627 + }, + { + "x": 14.374878621007513, + "y": 6.6681554053884105, + "heading": -0.5937701281033373, + "angularVelocity": 0.0070535118408139325, + "velocityX": -0.7561186286794916, + "velocityY": 0.5807867382623025, + "timestamp": 3.4930663445723633 + }, + { + "x": 14.231056382755842, + "y": 6.778627552624006, + "heading": -0.5924285241888535, + "angularVelocity": 0.014106465104088719, + "velocityX": -1.5122372282804897, + "velocityY": 1.1615734518427945, + "timestamp": 3.588171950731264 + }, + { + "x": 14.015323043710728, + "y": 6.944335758664245, + "heading": -0.5904163161957059, + "angularVelocity": 0.021157617036641953, + "velocityX": -2.268355649662441, + "velocityY": 1.7423600220093918, + "timestamp": 3.6832775568901646 + }, + { + "x": 13.871500813115434, + "y": 7.0548078992888055, + "heading": -0.5890749095880986, + "angularVelocity": 0.014104390495824683, + "velocityX": -1.512237147776525, + "velocityY": 1.161573382330228, + "timestamp": 3.778383163049065 + }, + { + "x": 13.82547769782775, + "y": 7.0901589854242735, + "heading": -0.5886456630608746, + "angularVelocity": 0.011283417909435272, + "velocityX": -1.209789737015094, + "velocityY": 0.9292587199213779, + "timestamp": 3.8164254061592073 + }, + { + "x": 13.790960360957698, + "y": 7.116672300328794, + "heading": -0.5883237297397991, + "angularVelocity": 0.008462522048014776, + "velocityX": -0.9073423133887173, + "velocityY": 0.696944047903743, + "timestamp": 3.8544676492693495 + }, + { + "x": 13.767948802831496, + "y": 7.134347843758721, + "heading": -0.588109107903455, + "angularVelocity": 0.005641671436740162, + "velocityX": -0.6048948811871482, + "velocityY": 0.46462936948148964, + "timestamp": 3.8925098923794916 + }, + { + "x": 13.75644302368164, + "y": 7.143185615539551, "heading": -0.5880017968591525, - "angularVelocity": -0.04553164244942512, - "velocityX": -0.3030940317136526, - "velocityY": 0.2222725433519379, - "timestamp": 3.921265842479141 + "angularVelocity": 0.0028208390339083305, + "velocityX": -0.3024474428740401, + "velocityY": 0.2323146864721631, + "timestamp": 3.930552135489634 }, { - "x": 13.670247077941896, - "y": 7.0260772705078125, + "x": 13.75644302368164, + "y": 7.143185615539551, "heading": -0.5880017968591525, - "angularVelocity": 4.3220894143993686e-27, - "velocityX": -2.1854050012083175e-26, - "velocityY": 3.319166050857422e-26, - "timestamp": 3.9587700544930438 - }, - { - "x": 13.645024237315694, - "y": 7.029213812474465, - "heading": -0.5586050184455363, - "angularVelocity": 0.5751667660211687, - "velocityX": -0.49350100438627437, - "velocityY": 0.061368449088751095, - "timestamp": 4.009880063190129 - }, - { - "x": 13.59445850581916, - "y": 7.035391208480669, - "heading": -0.5016300369058596, - "angularVelocity": 1.1147519437406412, - "velocityX": -0.9893508685592758, - "velocityY": 0.1208647027007058, - "timestamp": 4.060990071887214 - }, - { - "x": 13.518403853674121, - "y": 7.044499236825847, - "heading": -0.41964577392325547, - "angularVelocity": 1.6040745261559681, - "velocityX": -1.4880578987140245, - "velocityY": 0.17820439826490078, - "timestamp": 4.112100080584299 - }, - { - "x": 13.416671986943191, - "y": 7.056423820145038, - "heading": -0.3169282187933418, - "angularVelocity": 2.0097346439264405, - "velocityX": -1.9904490201491905, - "velocityY": 0.2333120972423329, - "timestamp": 4.163210089281384 - }, - { - "x": 13.289036124529042, - "y": 7.071060906808593, - "heading": -0.20104341684391944, - "angularVelocity": 2.2673602471140164, - "velocityX": -2.497277258757889, - "velocityY": 0.28638395955486035, - "timestamp": 4.21432009797847 - }, - { - "x": 13.135342270967683, - "y": 7.08828685694279, - "heading": -0.08640408959186506, - "angularVelocity": 2.2429917383010047, - "velocityX": -3.007118517084588, - "velocityY": 0.3370367286824492, - "timestamp": 4.265430106675555 - }, - { - "x": 12.956762188284944, - "y": 7.1075748581915015, - "heading": -0.010726520537828105, - "angularVelocity": 1.48068002693087, - "velocityX": -3.4940335021489295, - "velocityY": 0.3773820772175376, - "timestamp": 4.31654011537264 - }, - { - "x": 12.755389428383719, - "y": 7.12888688951605, - "heading": -1.8456979469985217e-7, - "angularVelocity": 0.20986762165519152, - "velocityX": -3.9399868056119236, - "velocityY": 0.41698351982012405, - "timestamp": 4.367650124069725 - }, - { - "x": 12.55069190372683, - "y": 7.152531606779918, - "heading": -1.385534968264371e-7, - "angularVelocity": 9.003382908060325e-7, - "velocityX": -4.00503795391767, - "velocityY": 0.4626240117469566, - "timestamp": 4.418760132766811 - }, - { - "x": 12.345994384979965, - "y": 7.176176375208352, - "heading": -9.253727372935047e-8, - "angularVelocity": 9.003368277593551e-7, - "velocityX": -4.005037838284301, - "velocityY": 0.46262501281442936, - "timestamp": 4.469870141463896 - }, - { - "x": 12.141296845030876, - "y": 7.1998209600841285, - "heading": -4.652103543469606e-8, - "angularVelocity": 9.003371251094857e-7, - "velocityX": -4.005038253119366, - "velocityY": 0.46262142148932034, - "timestamp": 4.520980150160981 - }, - { - "x": 11.93593692779541, - "y": 7.216775417327881, - "heading": -7.736119630596452e-26, - "angularVelocity": 9.102138039226991e-7, - "velocityX": -4.017998088252668, - "velocityY": 0.33172479668779814, - "timestamp": 4.5720901588580665 - }, - { - "x": 11.603934607227266, - "y": 7.244185435917073, - "heading": 2.6079666498891626e-17, - "angularVelocity": 3.1562448816546114e-16, - "velocityX": -4.017998550432804, - "velocityY": 0.33172483484526066, - "timestamp": 4.654718939431514 - }, - { - "x": 11.271932286659125, - "y": 7.271595454506265, - "heading": 1.288603670926865e-16, - "angularVelocity": 1.2438849984287554e-15, - "velocityX": -4.017998550432804, - "velocityY": 0.33172483484526066, - "timestamp": 4.737347720004961 - }, - { - "x": 10.93992996609098, - "y": 7.299005473095457, - "heading": 2.271885871201871e-16, - "angularVelocity": 1.1899996507881914e-15, - "velocityX": -4.017998550432804, - "velocityY": 0.3317248348452606, - "timestamp": 4.8199765005784085 - }, - { - "x": 10.607927645522839, - "y": 7.326415491684649, - "heading": 2.5614842689098653e-16, - "angularVelocity": 3.504812677828607e-16, - "velocityX": -4.017998550432804, - "velocityY": 0.3317248348452606, - "timestamp": 4.902605281151856 - }, - { - "x": 10.275925324954695, - "y": 7.3538255102738415, - "heading": 2.977295806911055e-16, - "angularVelocity": 5.032284575841044e-16, - "velocityX": -4.017998550432804, - "velocityY": 0.3317248348452606, - "timestamp": 4.985234061725303 - }, - { - "x": 9.943923004386694, - "y": 7.381235528863022, - "heading": 2.9677600290999526e-16, - "angularVelocity": -1.1540504102827907e-17, - "velocityX": -4.017998550431105, - "velocityY": 0.3317248348451205, - "timestamp": 5.067862842298751 - }, - { - "x": 9.671068786342074, - "y": 7.4037622966758025, - "heading": 1.766717126358e-16, - "angularVelocity": -1.453540636092317e-15, - "velocityX": -3.302169246006048, - "velocityY": 0.2726261679821815, - "timestamp": 5.150491622872198 - }, - { - "x": 9.466428105073092, - "y": 7.420657373999625, - "heading": 8.769803938528749e-17, - "angularVelocity": -1.0767879258667162e-15, - "velocityX": -2.476627149145434, - "velocityY": 0.20446964370732854, - "timestamp": 5.233120403445645 - }, - { - "x": 9.330000979086366, - "y": 7.4319207593065935, - "heading": 3.000420973683897e-17, - "angularVelocity": -6.982292277353821e-16, - "velocityX": -1.6510848283118424, - "velocityY": 0.13631310094132928, - "timestamp": 5.315749184019093 + "angularVelocity": -2.989139878822297e-21, + "velocityX": -3.903367328466188e-21, + "velocityY": -2.1704144058469085e-20, + "timestamp": 3.968594378599776 + }, + { + "x": 13.7295528779877, + "y": 7.145512508632667, + "heading": -0.5593356739909306, + "angularVelocity": 0.5453918054970608, + "velocityX": -0.5116026739128892, + "velocityY": 0.04427066858979402, + "timestamp": 4.021154982883326 + }, + { + "x": 13.675696703739712, + "y": 7.150081094861737, + "heading": -0.5031004725393259, + "angularVelocity": 1.0699116233183206, + "velocityX": -1.024649069052714, + "velocityY": 0.08692035206491855, + "timestamp": 4.073715587166876 + }, + { + "x": 13.594778136456949, + "y": 7.156781935585546, + "heading": -0.4208691999200477, + "angularVelocity": 1.5645039424520941, + "velocityX": -1.5395288616970724, + "velocityY": 0.12748789354969425, + "timestamp": 4.126276191450426 + }, + { + "x": 13.486657017941033, + "y": 7.165480150396445, + "heading": -0.31559001044646323, + "angularVelocity": 2.003005690452726, + "velocityX": -2.0570752560726255, + "velocityY": 0.16548924673647072, + "timestamp": 4.178836795733976 + }, + { + "x": 13.351125165106843, + "y": 7.1760168934848485, + "heading": -0.19335854854743198, + "angularVelocity": 2.3255338016973006, + "velocityX": -2.5785824703048017, + "velocityY": 0.20046845412127823, + "timestamp": 4.231397400017526 + }, + { + "x": 13.187951040267704, + "y": 7.1881615298982275, + "heading": -0.06830550460961216, + "angularVelocity": 2.3792162522179776, + "velocityX": -3.1044948410193456, + "velocityY": 0.23105968013346895, + "timestamp": 4.283958004301076 + }, + { + "x": 12.999485780940217, + "y": 7.200902665906217, + "heading": -7.219474003499003e-8, + "angularVelocity": 1.299555690919823, + "velocityX": -3.58567527707191, + "velocityY": 0.24240847649419411, + "timestamp": 4.336518608584626 + }, + { + "x": 12.78823863885508, + "y": 7.217612049538315, + "heading": -5.77661422338363e-8, + "angularVelocity": 2.745135448469375e-7, + "velocityX": -4.019115551745117, + "velocityY": 0.31790699250631954, + "timestamp": 4.389079212868176 + }, + { + "x": 12.576998356930917, + "y": 7.234407936932018, + "heading": -4.3339952330086093e-8, + "angularVelocity": 2.7446773301816857e-7, + "velocityX": -4.018985032679267, + "velocityY": 0.3195527833564148, + "timestamp": 4.441639817151726 + }, + { + "x": 12.365757195669019, + "y": 7.251192761311872, + "heading": -2.8913607283949816e-8, + "angularVelocity": 2.7447068470390713e-7, + "velocityX": -4.019001762656892, + "velocityY": 0.31934230225557386, + "timestamp": 4.494200421435276 + }, + { + "x": 12.154511938095094, + "y": 7.267925952676431, + "heading": -1.4486541183922857e-8, + "angularVelocity": 2.7448440322751447e-7, + "velocityX": -4.019079697682207, + "velocityY": 0.31835995024503166, + "timestamp": 4.546761025718826 + }, + { + "x": 11.942968368530272, + "y": 7.280330657958984, + "heading": 2.0785063958808085e-20, + "angularVelocity": 2.756159557415578e-7, + "velocityX": -4.024755279136523, + "velocityY": 0.23600766109220464, + "timestamp": 4.599321630002376 + }, + { + "x": 11.610157071294928, + "y": 7.29984638261751, + "heading": 5.720799166605624e-18, + "angularVelocity": 6.893144194831854e-17, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 4.682012691106358 + }, + { + "x": 11.277345774059585, + "y": 7.319362107276035, + "heading": 4.27914940906101e-18, + "angularVelocity": -1.7434167161617802e-17, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 4.76470375221034 + }, + { + "x": 10.944534476824241, + "y": 7.3388778319345604, + "heading": -2.8918690030091486e-17, + "angularVelocity": -4.01468296820159e-16, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 4.847394813314322 + }, + { + "x": 10.611723179588898, + "y": 7.358393556593086, + "heading": -7.60328295215413e-17, + "angularVelocity": -5.697609740032433e-16, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 4.930085874418304 + }, + { + "x": 10.278911882353555, + "y": 7.377909281251611, + "heading": -1.4437311459656085e-16, + "angularVelocity": -8.264531158162608e-16, + "velocityX": -4.024755430539708, + "velocityY": 0.2360076699703374, + "timestamp": 5.012776935522286 + }, + { + "x": 9.946100585118234, + "y": 7.3974250059101365, + "heading": -1.6062472011936515e-16, + "angularVelocity": -1.9653400658253493e-16, + "velocityX": -4.024755430539445, + "velocityY": 0.236007669970322, + "timestamp": 5.095467996626268 + }, + { + "x": 9.672375324044506, + "y": 7.413475983961583, + "heading": 2.0694001348599773e-12, + "angularVelocity": 2.5027623686889767e-11, + "velocityX": -3.310215849443808, + "velocityY": 0.19410777703363524, + "timestamp": 5.17815905773025 + }, + { + "x": 9.467081371147623, + "y": 7.425514217916012, + "heading": 1.4773324091984537e-12, + "angularVelocity": -7.159996712547157e-12, + "velocityX": -2.482661972842874, + "velocityY": 0.14558083780410894, + "timestamp": 5.260850118834232 + }, + { + "x": 9.3302187338275, + "y": 7.433539707339499, + "heading": 5.797823580481929e-13, + "angularVelocity": -1.0854257269980322e-11, + "velocityX": -1.655108006753246, + "velocityY": 0.09705389332705425, + "timestamp": 5.343541179938214 }, { "x": 9.26178741455078, "y": 7.437552452087402, - "heading": -5.153249325171924e-25, - "angularVelocity": -3.631205742599535e-16, - "velocityX": -0.8255424328204009, - "velocityY": 0.06815655201159886, - "timestamp": 5.39837796459254 + "heading": -4.962355351536534e-26, + "angularVelocity": -7.011427236605464e-12, + "velocityX": -0.8275540108340242, + "velocityY": 0.04852694710082174, + "timestamp": 5.426232241042196 }, { "x": 9.26178741455078, "y": 7.437552452087402, - "heading": -5.204793542360701e-25, - "angularVelocity": -6.317967395111268e-26, - "velocityX": -1.2129880403241932e-23, - "velocityY": 1.4316814179344083e-22, - "timestamp": 5.481006745165987 - }, - { - "x": 9.329755853999478, - "y": 7.419457395378352, - "heading": -0.013376630543289742, - "angularVelocity": -0.15963467895053327, - "velocityX": 0.8111250419190595, - "velocityY": -0.21594366077412155, - "timestamp": 5.564802012154527 - }, - { - "x": 9.46569267412481, - "y": 7.383267297606982, - "heading": -0.04017390350981554, - "angularVelocity": -0.31979458899738056, - "velocityX": 1.6222493824612316, - "velocityY": -0.4318871348225413, - "timestamp": 5.648597279143067 - }, - { - "x": 9.669597656216824, - "y": 7.32898221699986, - "heading": -0.08046638000926655, - "angularVelocity": -0.4808443000123345, - "velocityX": 2.4333711129520146, - "velocityY": -0.6478299140039275, - "timestamp": 5.732392546131607 - }, - { - "x": 9.94147049419242, - "y": 7.256602235044674, - "heading": -0.13433954177465304, - "angularVelocity": -0.6429141370568642, - "velocityX": 3.2444891906935323, - "velocityY": -0.8637717207236001, - "timestamp": 5.816187813120147 - }, - { - "x": 10.267933929791393, - "y": 7.169688743599082, - "heading": -0.1343395455343343, - "angularVelocity": -4.4867465828483885e-8, - "velocityX": 3.8959650984061396, - "velocityY": -1.0372124174684092, - "timestamp": 5.899983080108687 - }, - { - "x": 10.594397365390467, - "y": 7.082775252153461, - "heading": -0.13433954929395236, - "angularVelocity": -4.486671145978699e-8, - "velocityX": 3.8959650984073204, - "velocityY": -1.0372124174687236, - "timestamp": 5.9837783470972274 - }, - { - "x": 10.92086080098954, - "y": 6.99586176070784, - "heading": -0.1343395530535705, - "angularVelocity": -4.486671228851664e-8, - "velocityX": 3.8959650984073204, - "velocityY": -1.0372124174687236, - "timestamp": 6.0675736140857675 - }, - { - "x": 11.247324236588515, - "y": 6.908948269262247, - "heading": -0.13433955681325235, - "angularVelocity": -4.486747226873845e-8, - "velocityX": 3.895965098406137, - "velocityY": -1.0372124174684085, - "timestamp": 6.1513688810743075 - }, - { - "x": 11.519194528616957, - "y": 6.836568965107796, - "heading": -0.18900176869601656, - "angularVelocity": -0.6523305414163806, - "velocityX": 3.244458807746568, - "velocityY": -0.8637636319525063, - "timestamp": 6.2351641480628475 - }, - { - "x": 11.723097253083957, - "y": 6.7822844855421724, - "heading": -0.2300018718976995, - "angularVelocity": -0.4892890096918049, - "velocityX": 2.4333441707977026, - "velocityY": -0.6478227412659024, - "timestamp": 6.3189594150513875 - }, - { - "x": 11.859032415291114, - "y": 6.746094829153945, - "heading": -0.2573341284430376, - "angularVelocity": -0.32617900184119325, - "velocityX": 1.6222295971173368, - "velocityY": -0.4318818674231029, - "timestamp": 6.4027546820399275 - }, - { - "x": 11.927, - "y": 6.728, - "heading": -0.271, - "angularVelocity": -0.16308643731431074, - "velocityX": 0.8111148415838417, - "velocityY": -0.21594094516602882, - "timestamp": 6.4865499490284675 - }, - { - "x": 11.927, - "y": 6.728, - "heading": -0.271, - "angularVelocity": -1.104828910146849e-25, - "velocityX": -2.8779219726051263e-23, - "velocityY": -1.0811880313092697e-22, - "timestamp": 6.5703452160170075 + "heading": -2.4523596562464635e-26, + "angularVelocity": 6.97031270768781e-27, + "velocityX": 1.5654039367177217e-19, + "velocityY": 1.335931106132843e-18, + "timestamp": 5.508923302146178 + }, + { + "x": 9.361038211199778, + "y": 7.437553617689382, + "heading": -1.6076494634632555e-20, + "angularVelocity": -1.6157169576201665e-19, + "velocityX": 0.9974900839285512, + "velocityY": 0.000011714529816125984, + "timestamp": 5.608423836786188 + }, + { + "x": 9.559539801305808, + "y": 7.437555948893304, + "heading": -2.775286477301083e-20, + "angularVelocity": -1.173498230999548e-19, + "velocityX": 1.9949801357771932, + "velocityY": 0.000023429059255504534, + "timestamp": 5.7079243714261985 + }, + { + "x": 9.857292168909034, + "y": 7.437559445698981, + "heading": -1.615205586446462e-20, + "angularVelocity": 1.1659041783583904e-19, + "velocityX": 2.9924700272263003, + "velocityY": 0.000035143586811149936, + "timestamp": 5.807424906066209 + }, + { + "x": 10.055793759015064, + "y": 7.437561776902903, + "heading": -5.186936203377308e-21, + "angularVelocity": 1.1020161550669728e-19, + "velocityX": 1.9949801357771932, + "velocityY": 0.000023429059255504534, + "timestamp": 5.906925440706219 + }, + { + "x": 10.155044555664062, + "y": 7.437562942504883, + "heading": 4.39993089223087e-32, + "angularVelocity": 5.2129731987967565e-20, + "velocityX": 0.9974900839285512, + "velocityY": 0.000011714529816125986, + "timestamp": 6.00642597534623 + }, + { + "x": 10.155044555664062, + "y": 7.437562942504883, + "heading": 2.1939125534073598e-32, + "angularVelocity": -1.2166553134589197e-33, + "velocityX": 1.4750876026512068e-22, + "velocityY": 2.5957646699888015e-19, + "timestamp": 6.10592650998624 } ], "eventMarkers": [ { - "timestamp": 0.918889530696065, + "timestamp": 0.9188894506498827, + "command": { + "type": "named", + "data": { + "name": "intakeFloor" + } + } + }, + { + "timestamp": 2.1812576334370735, "command": { "type": "named", "data": { @@ -876,7 +831,7 @@ } }, { - "timestamp": 2.4048105905284736, + "timestamp": 3.778383163049065, "command": { "type": "named", "data": { @@ -885,7 +840,7 @@ } }, { - "timestamp": 3.7712489944235306, + "timestamp": 4.599321630002376, "command": { "type": "named", "data": { diff --git a/src/main/deploy/pathplanner/autos/Red Six Piece 2 to 4.1.auto b/src/main/deploy/pathplanner/autos/Red Six Piece 2 to 4.1.auto index 1f524d35..34966ba0 100644 --- a/src/main/deploy/pathplanner/autos/Red Six Piece 2 to 4.1.auto +++ b/src/main/deploy/pathplanner/autos/Red Six Piece 2 to 4.1.auto @@ -5,12 +5,18 @@ "x": 15.198025703430176, "y": 5.552863121032715 }, - "rotation": 6.754194366290117e-27 + "rotation": 8.407083341517454e-36 }, "command": { "type": "sequential", "data": { "commands": [ + { + "type": "named", + "data": { + "name": "homeClimber" + } + }, { "type": "named", "data": { @@ -77,6 +83,12 @@ "pathName": "Red Six Piece 2 to 4 (1).7" } }, + { + "type": "path", + "data": { + "pathName": "Red Six Piece 2 to 4 (1).8" + } + }, { "type": "named", "data": {