From e63e3717217957447053b52dbc01b05ed0ab2ce3 Mon Sep 17 00:00:00 2001 From: Sean Maizel Date: Sun, 24 Mar 2024 16:20:20 -0400 Subject: [PATCH] end of comp changes! --- choreo.chor | 3327 +++++++++-------- src/main/deploy/choreo/2 piece left.1.traj | 345 +- src/main/deploy/choreo/2 piece left.2.traj | 209 +- src/main/deploy/choreo/2 piece left.3.traj | 357 +- src/main/deploy/choreo/2 piece left.4.traj | 292 ++ src/main/deploy/choreo/2 piece left.traj | 1172 +++--- src/main/deploy/choreo/2 piece right.1.traj | 363 +- src/main/deploy/choreo/2 piece right.2.traj | 209 +- src/main/deploy/choreo/2 piece right.3.traj | 364 +- src/main/deploy/choreo/2 piece right.4.traj | 575 ++- src/main/deploy/choreo/2 piece right.5.traj | 184 + src/main/deploy/choreo/2 piece right.traj | 1281 ++++--- src/main/deploy/choreo/Minor Trolling.traj | 310 -- src/main/deploy/choreo/source exit.1.traj | 382 ++ src/main/deploy/choreo/source exit.traj | 382 ++ .../autos/1 piece source side.auto | 2 +- .../pathplanner/autos/2 piece amp side.auto | 28 +- .../autos/2 piece source side.auto | 18 +- src/main/java/frc/robot/RobotContainer.java | 5 +- .../robot/commands/arm/SetArmPosition.java | 10 +- .../robot/commands/intake/IntakeWithArm.java | 5 +- .../commands/scoring/SetScoringPosition.java | 8 +- .../frc/robot/constants/ArmConstants.java | 12 +- .../frc/robot/constants/ScoringConstants.java | 2 +- src/main/java/frc/robot/subsystems/Arm.java | 26 +- 25 files changed, 5633 insertions(+), 4235 deletions(-) create mode 100644 src/main/deploy/choreo/2 piece left.4.traj create mode 100644 src/main/deploy/choreo/2 piece right.5.traj delete mode 100644 src/main/deploy/choreo/Minor Trolling.traj create mode 100644 src/main/deploy/choreo/source exit.1.traj create mode 100644 src/main/deploy/choreo/source exit.traj diff --git a/choreo.chor b/choreo.chor index 7968ed0..f4a35b1 100644 --- a/choreo.chor +++ b/choreo.chor @@ -297,11 +297,11 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 21 + "controlIntervalCount": 31 }, { - "x": 3.296103000640869, - "y": 7.606926441192627, + "x": 6.970706462860107, + "y": 7.47182035446167, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -314,568 +314,658 @@ "x": 0.6235248446464539, "y": 6.866034507751465, "heading": 1.3037, - "angularVelocity": -1.8232741591844444e-18, - "velocityX": 7.12403473371085e-18, - "velocityY": -2.6478556818989226e-17, + "angularVelocity": -4.322191282064769e-19, + "velocityX": -7.217119807256306e-20, + "velocityY": -5.737284120005055e-18, "timestamp": 0 }, { - "x": 0.6371647021269348, - "y": 6.867787221776129, - "heading": 1.30369672515582, - "angularVelocity": -0.00006476812380618724, - "velocityX": 0.26976183581085494, - "velocityY": 0.03466424437509273, - "timestamp": 0.05056259140393951 - }, - { - "x": 0.6644444154536387, - "y": 6.871292657837969, - "heading": 1.3036900282942052, - "angularVelocity": -0.00013244696185552165, - "velocityX": 0.5395236393002303, - "velocityY": 0.06932864721736587, - "timestamp": 0.10112518280787902 - }, - { - "x": 0.7053639821708213, - "y": 6.876550827985717, - "heading": 1.303679688712071, - "angularVelocity": -0.0002044907479422861, - "velocityX": 0.8092853942211979, - "velocityY": 0.10399328835306255, - "timestamp": 0.15168777421181853 - }, - { - "x": 0.7599233981740878, - "y": 6.8835617523794905, - "heading": 1.303665338742152, - "angularVelocity": -0.00028380606137315504, - "velocityX": 1.0790470679676354, - "velocityY": 0.1386583282048117, - "timestamp": 0.20225036561575804 - }, - { - "x": 0.8281226552122382, - "y": 6.892325471630372, - "heading": 1.303646243766396, - "angularVelocity": -0.0003776502593285092, - "velocityX": 1.3488085785262331, - "velocityY": 0.17332417124092056, - "timestamp": 0.2528129570196975 - }, - { - "x": 0.9099617281962122, - "y": 6.902842109861635, - "heading": 1.3036202050692922, - "angularVelocity": -0.0005149794814836402, - "velocityX": 1.618569592886762, - "velocityY": 0.20799246912103908, - "timestamp": 0.303375548423637 - }, - { - "x": 0.9952174677554063, - "y": 6.913794817693266, - "heading": 1.2650372111423984, - "angularVelocity": -0.7630739021791462, - "velocityX": 1.686142604481928, - "velocityY": 0.21661682139925917, - "timestamp": 0.3539381398275765 - }, - { - "x": 1.0804727987833038, - "y": 6.9247507050181865, - "heading": 1.186135227119023, - "angularVelocity": -1.5604814119006516, - "velocityX": 1.6861345247675534, - "velocityY": 0.21667970372394155, - "timestamp": 0.404500731231516 - }, - { - "x": 1.1657282386661474, - "y": 6.935705745230898, - "heading": 1.0686962835514935, - "angularVelocity": -2.322644870578755, - "velocityX": 1.6861366776426912, - "velocityY": 0.21666294998988928, - "timestamp": 0.45506332263545546 - }, - { - "x": 1.2509836774698653, - "y": 6.946660793838387, - "heading": 0.9157151714041298, - "angularVelocity": -3.0255789487768316, - "velocityX": 1.6861366563003188, - "velocityY": 0.21666311601734475, - "timestamp": 0.5056259140393949 - }, - { - "x": 1.336239105659407, - "y": 6.957615925046211, - "heading": 0.7330874583136245, - "angularVelocity": -3.61191363060312, - "velocityX": 1.6861364463787976, - "velocityY": 0.21666474964277663, - "timestamp": 0.5561885054433344 - }, - { - "x": 1.4214945304345679, - "y": 6.96857108283117, - "heading": 0.5355420616291058, - "angularVelocity": -3.9069476306376054, - "velocityX": 1.6861363788509953, - "velocityY": 0.21666527527119953, - "timestamp": 0.6067510968472739 - }, - { - "x": 1.5067500117706687, - "y": 6.979525800477682, - "heading": 0.35218632067901057, - "angularVelocity": -3.6263121778170837, - "velocityX": 1.68613749748313, - "velocityY": 0.21665657044742084, - "timestamp": 0.6573136882512134 - }, - { - "x": 1.5920056795979993, - "y": 6.990479066662584, - "heading": 0.19830964964916573, - "angularVelocity": -3.043290835324073, - "velocityX": 1.6861411858073423, - "velocityY": 0.21662786421283695, - "timestamp": 0.7078762796551529 - }, - { - "x": 1.677260793789504, - "y": 7.001436641396165, - "heading": 0.07990683643812377, - "angularVelocity": -2.341707770971106, - "velocityX": 1.686130236292877, - "velocityY": 0.21671307639360837, - "timestamp": 0.7584388710590924 + "x": 0.6479605438901133, + "y": 6.869173153614982, + "heading": 1.3036976853929116, + "angularVelocity": -0.00003420125047541537, + "velocityX": 0.3610683967062306, + "velocityY": 0.04637746677385236, + "timestamp": 0.06767609535082215 + }, + { + "x": 0.6968319343921952, + "y": 6.875450489923095, + "heading": 1.3036923603900246, + "angularVelocity": -0.00007868366015476742, + "velocityX": 0.7221366754204779, + "velocityY": 0.0927555922895904, + "timestamp": 0.1353521907016443 + }, + { + "x": 0.7701389910336393, + "y": 6.884866659045138, + "heading": 1.3036818951032874, + "angularVelocity": -0.0001546378626426958, + "velocityX": 1.0832045829687984, + "velocityY": 0.13913582149253725, + "timestamp": 0.20302828605246648 + }, + { + "x": 0.8641137016555179, + "y": 6.896935578985996, + "heading": 1.278165357348036, + "angularVelocity": -0.377039154268251, + "velocityX": 1.3885953398275834, + "velocityY": 0.17833357374260927, + "timestamp": 0.2707043814032886 + }, + { + "x": 0.9580882030461527, + "y": 6.909006127999201, + "heading": 1.179816306963164, + "angularVelocity": -1.4532317485966955, + "velocityX": 1.3885922481710244, + "velocityY": 0.17835764534927084, + "timestamp": 0.33838047675411076 + }, + { + "x": 1.052062741771001, + "y": 6.921076386345661, + "heading": 1.012746961397015, + "angularVelocity": -2.4686611232531686, + "velocityX": 1.3885927998313032, + "velocityY": 0.17835335038004493, + "timestamp": 0.4060565721049329 + }, + { + "x": 1.1460372822563507, + "y": 6.933146630985501, + "heading": 0.7854621166033714, + "angularVelocity": -3.358421368955095, + "velocityX": 1.3885928258449378, + "velocityY": 0.17835314784740675, + "timestamp": 0.47373266745575504 + }, + { + "x": 1.2400118226682206, + "y": 6.945216876197425, + "heading": 0.5235178410871962, + "angularVelocity": -3.870558343508111, + "velocityX": 1.388592824759185, + "velocityY": 0.17835315630066892, + "timestamp": 0.5414087628065772 + }, + { + "x": 1.3339863634408795, + "y": 6.9572871186003775, + "heading": 0.29518267414695665, + "angularVelocity": -3.3739412085846014, + "velocityX": 1.3885928300902979, + "velocityY": 0.17835311479456636, + "timestamp": 0.6090848581573994 + }, + { + "x": 1.4279609034991196, + "y": 6.9693573665655375, + "heading": 0.1268000189106551, + "angularVelocity": -2.4880669365369346, + "velocityX": 1.388592819533854, + "velocityY": 0.1783531969832136, + "timestamp": 0.6767609535082216 + }, + { + "x": 1.5219354277832027, + "y": 6.981427737341889, + "heading": 0.027024429838266126, + "angularVelocity": -1.4743106639821368, + "velocityX": 1.38859258645071, + "velocityY": 0.1783550116740703, + "timestamp": 0.7444370488590438 + }, + { + "x": 1.615910044720068, + "y": 6.993497386738595, + "heading": 0.000018019004787293556, + "angularVelocity": -0.3990539154701806, + "velocityX": 1.3885939555128846, + "velocityY": 0.17834435237639756, + "timestamp": 0.812113144209866 + }, + { + "x": 1.6892171360383907, + "y": 7.002913285881713, + "heading": 0.000007615738994617179, + "angularVelocity": -0.00015372142465942866, + "velocityX": 1.083205095363587, + "velocityY": 0.13913183221207334, + "timestamp": 0.8797892395606882 + }, + { + "x": 1.7380885418555654, + "y": 7.0091905029584325, + "heading": 0.0000023106612364746317, + "angularVelocity": -0.00007838924114270227, + "velocityX": 0.7221369017203642, + "velocityY": 0.09275383049478103, + "timestamp": 0.9474653349115104 }, { "x": 1.7625242471694946, "y": 7.0123291015625, - "heading": -3.350712590761302e-17, - "angularVelocity": -1.5803548477125327, - "velocityX": 1.6862951643207778, - "velocityY": 0.21542527516670237, - "timestamp": 0.8090014624630318 - }, - { - "x": 1.894716655716008, - "y": 7.013428377195941, - "heading": -0.044649055027065385, - "angularVelocity": -0.5741688805746129, - "velocityX": 1.6999411788133891, - "velocityY": 0.01413624229031357, - "timestamp": 0.886764393436201 - }, - { - "x": 2.02690952861163, - "y": 7.014470317880742, - "heading": -0.00008383158388879456, - "angularVelocity": 0.5730908401401845, - "velocityX": 1.6999471501560728, - "velocityY": 0.013398937922744884, - "timestamp": 0.9645273244093701 - }, - { - "x": 2.1570160896928328, - "y": 7.015495222736415, - "heading": -0.0000539532537599614, - "angularVelocity": 0.00038422330222239603, - "velocityX": 1.673118019768238, - "velocityY": 0.013179864015495759, - "timestamp": 1.042290255382539 - }, - { - "x": 2.2545960183213234, - "y": 7.01626391842549, - "heading": -0.00002792078971867657, - "angularVelocity": 0.00033476701193603026, - "velocityX": 1.254838615357224, - "velocityY": 0.009885117233306755, - "timestamp": 1.1200531863557082 - }, - { - "x": 2.319649306326726, - "y": 7.016776387311738, - "heading": -0.000009516691596222152, - "angularVelocity": 0.00023666929592428788, - "velocityX": 0.8365591058784542, - "velocityY": 0.006590143656300914, - "timestamp": 1.1978161173288773 + "heading": 9.925554069905119e-19, + "angularVelocity": -0.00003414294551807602, + "velocityX": 0.36106848640215494, + "velocityY": 0.04637676845564921, + "timestamp": 1.0151414302623325 }, { - "x": 2.3521759510040283, - "y": 7.017032623291016, - "heading": 5.0938422434116983e-17, - "angularVelocity": 0.000122380824348686, - "velocityX": 0.4182795616143265, - "velocityY": 0.0032950915824648324, - "timestamp": 1.2755790483020464 + "x": 1.7625242471694946, + "y": 7.0123291015625, + "heading": 4.930172708842328e-19, + "angularVelocity": -9.633639100834258e-20, + "velocityX": -4.2792851430038895e-18, + "velocityY": -3.4270302349667407e-18, + "timestamp": 1.0828175256131547 + }, + { + "x": 1.83126410757397, + "y": 7.012877424284074, + "heading": -8.433317163645367e-17, + "angularVelocity": -7.503648365992863e-16, + "velocityX": 0.6080666216218581, + "velocityY": 0.00485041347050172, + "timestamp": 1.1958641194569832 + }, + { + "x": 1.9443071033303654, + "y": 7.013779143355416, + "heading": -9.171422334116161e-15, + "angularVelocity": -8.038357329937531e-14, + "velocityX": 0.9999681716422337, + "velocityY": 0.007976525790660745, + "timestamp": 1.3089107133008118 + }, + { + "x": 2.0573500990867615, + "y": 7.014680862426758, + "heading": 4.721221727458621e-16, + "angularVelocity": 8.530592721947881e-14, + "velocityX": 0.9999681716422396, + "velocityY": 0.007976525790660303, + "timestamp": 1.4219573071446403 + }, + { + "x": 2.1703930948431576, + "y": 7.0155825814981, + "heading": 9.192667005316373e-15, + "angularVelocity": 7.714115512942701e-14, + "velocityX": 0.9999681716422396, + "velocityY": 0.00797652579066031, + "timestamp": 1.5350039009884688 + }, + { + "x": 2.283436090599553, + "y": 7.0164843005694415, + "heading": 8.651637279707332e-17, + "angularVelocity": -8.055218934866491e-14, + "velocityX": 0.9999681716422337, + "velocityY": 0.007976525790660681, + "timestamp": 1.6480504948322974 }, { "x": 2.3521759510040283, "y": 7.017032623291016, - "heading": 2.5675724430541464e-17, - "angularVelocity": -4.4489652123602634e-18, - "velocityX": -3.43042618417509e-18, - "velocityY": 7.376106260350096e-18, - "timestamp": 1.3533419792752155 - }, - { - "x": 2.289226350907918, - "y": 7.0088843634816955, - "heading": 3.162897756010949e-9, - "angularVelocity": 2.911644384649021e-8, - "velocityX": -0.5794902817179733, - "velocityY": -0.07500980729353342, - "timestamp": 1.4619712386123833 - }, - { - "x": 2.181495855279594, - "y": 6.9949396193307365, - "heading": 0.012994469205035452, - "angularVelocity": 0.1196221544860667, - "velocityX": -0.9917263201983659, - "velocityY": -0.12837005642905872, - "timestamp": 1.5706004979495511 - }, - { - "x": 2.0737653596512815, - "y": 6.980994875179695, - "heading": 0.05695780222793572, - "angularVelocity": 0.40470986630264244, - "velocityX": -0.9917263201982718, - "velocityY": -0.1283700564298165, - "timestamp": 1.679229757286719 - }, - { - "x": 1.9660348640229757, - "y": 6.967050131028595, - "heading": 0.1246841048489742, - "angularVelocity": 0.6234628039838408, - "velocityX": -0.9917263201982012, - "velocityY": -0.12837005643036203, - "timestamp": 1.7878590166238868 - }, - { - "x": 1.858304368394671, - "y": 6.953105386877488, - "heading": 0.21021952796955365, - "angularVelocity": 0.7874068519153866, - "velocityX": -0.9917263201981937, - "velocityY": -0.12837005643041954, - "timestamp": 1.8964882759610546 - }, - { - "x": 1.7505738727663673, - "y": 6.939160642726373, - "heading": 0.30861254552236733, - "angularVelocity": 0.905769017971645, - "velocityX": -0.9917263201981846, - "velocityY": -0.1283700564304895, - "timestamp": 2.0051175352982225 - }, - { - "x": 1.642843377138065, - "y": 6.9252158985752486, - "heading": 0.4156678969757047, - "angularVelocity": 0.9855111975039318, - "velocityX": -0.9917263201981728, - "velocityY": -0.12837005643058202, - "timestamp": 2.1137467946353903 - }, - { - "x": 1.5351128815097632, - "y": 6.911271154424119, - "heading": 0.5277146421232866, - "angularVelocity": 1.0314600857196923, - "velocityX": -0.9917263201981663, - "velocityY": -0.12837005643063132, - "timestamp": 2.222376053972558 - }, - { - "x": 1.4273823858814612, - "y": 6.897326410272993, - "heading": 0.6413892672091689, - "angularVelocity": 1.0464457345976608, - "velocityX": -0.9917263201981706, - "velocityY": -0.12837005643059782, - "timestamp": 2.331005313309726 - }, - { - "x": 1.3196518902531575, - "y": 6.883381666121877, - "heading": 0.7534304016146443, - "angularVelocity": 1.0314084353435349, - "velocityX": -0.9917263201981834, - "velocityY": -0.12837005643049992, - "timestamp": 2.4396345726468938 - }, - { - "x": 1.2119213946248526, - "y": 6.869436921970773, - "heading": 0.8604745712214651, - "angularVelocity": 0.9854082616413032, - "velocityX": -0.9917263201981971, - "velocityY": -0.12837005643039434, - "timestamp": 2.5482638319840616 - }, - { - "x": 1.1041908989965465, - "y": 6.855492177819676, - "heading": 0.9588506837381976, - "angularVelocity": 0.9056133965839597, - "velocityX": -0.9917263201982054, - "velocityY": -0.12837005643033003, - "timestamp": 2.6568930913212294 - }, - { - "x": 0.9964604033682406, - "y": 6.8415474336685795, - "heading": 1.0443629237764005, - "angularVelocity": 0.7871934372008065, - "velocityX": -0.9917263201982054, - "velocityY": -0.12837005643032937, - "timestamp": 2.7655223506583972 - }, - { - "x": 0.8887299077399343, - "y": 6.827602689517485, - "heading": 1.1120586417031153, - "angularVelocity": 0.6231812528206429, - "velocityX": -0.991726320198208, - "velocityY": -0.12837005643030963, - "timestamp": 2.874151609995565 - }, - { - "x": 0.7809994121116139, - "y": 6.813657945366499, - "heading": 1.1559822740636867, - "angularVelocity": 0.4043443969754001, - "velocityX": -0.9917263201983385, - "velocityY": -0.12837005642930135, - "timestamp": 2.982780869332733 - }, - { - "x": 0.6732689164832739, - "y": 6.799713201215669, - "heading": 1.1689257207199144, - "angularVelocity": 0.11915248925755068, - "velocityX": -0.9917263201985188, - "velocityY": -0.12837005642787808, - "timestamp": 3.0914101286699007 + "heading": -6.215056253707531e-20, + "angularVelocity": -7.658658295322792e-16, + "velocityX": 0.6080666216218581, + "velocityY": 0.0048504134705017595, + "timestamp": 1.761097088676126 }, { - "x": 0.6103193163871765, + "x": 2.3521759510040283, + "y": 7.017032623291016, + "heading": -6.764077311276182e-20, + "angularVelocity": -4.856600085384507e-20, + "velocityX": -1.0267091726602369e-19, + "velocityY": 4.493167408339835e-21, + "timestamp": 1.8741436825199544 + }, + { + "x": 2.3142846244738164, + "y": 7.01212793182799, + "heading": 6.683917732209663e-8, + "angularVelocity": 7.930688018171609e-7, + "velocityX": -0.4495930401075561, + "velocityY": -0.058195775856320804, + "timestamp": 1.9584228486768576 + }, + { + "x": 2.2385019738243845, + "y": 7.002318549276381, + "heading": 2.2158259200811548e-7, + "angularVelocity": 0.0000018360814628605592, + "velocityX": -0.8991860516078994, + "velocityY": -0.11639154726979174, + "timestamp": 2.0427020148337607 + }, + { + "x": 2.124828006289812, + "y": 6.987604476750434, + "heading": 5.265338458246272e-7, + "angularVelocity": 0.0000036183468313963867, + "velocityX": -1.348778977273516, + "velocityY": -0.17458730546233459, + "timestamp": 2.126981180990664 + }, + { + "x": 2.0078133931582736, + "y": 6.972457986338443, + "heading": 0.04540585981844075, + "angularVelocity": 0.5387491992987151, + "velocityX": -1.3884168349944475, + "velocityY": -0.17971808576977197, + "timestamp": 2.2112603471475674 + }, + { + "x": 1.8907987799711805, + "y": 6.957311496355622, + "heading": 0.12790574879755856, + "angularVelocity": 0.9788882916274608, + "velocityX": -1.388416835653617, + "velocityY": -0.17971808067752473, + "timestamp": 2.2955395133044707 + }, + { + "x": 1.773784166758088, + "y": 6.942165006573663, + "heading": 0.23868110863736844, + "angularVelocity": 1.314386044512811, + "velocityX": -1.388416835962111, + "velocityY": -0.17971807829424596, + "timestamp": 2.379818679461374 + }, + { + "x": 1.6567695535340663, + "y": 6.927018516876137, + "heading": 0.36923521647113, + "angularVelocity": 1.549067388620196, + "velocityX": -1.3884168360917915, + "velocityY": -0.1797180772923964, + "timestamp": 2.4640978456182774 + }, + { + "x": 1.5397549403057158, + "y": 6.911872027212054, + "heading": 0.5114515305873049, + "angularVelocity": 1.6874432982809748, + "velocityX": -1.388416836143153, + "velocityY": -0.17971807689559988, + "timestamp": 2.5483770117751807 + }, + { + "x": 1.4227403270757022, + "y": 6.896725537560817, + "heading": 0.6575137656671957, + "angularVelocity": 1.733076414258367, + "velocityX": -1.3884168361628832, + "velocityY": -0.17971807674317355, + "timestamp": 2.632656177932084 + }, + { + "x": 1.305725713846615, + "y": 6.8815790479024255, + "heading": 0.7997212587593243, + "angularVelocity": 1.6873386339321397, + "velocityX": -1.388416836151895, + "velocityY": -0.1797180768280647, + "timestamp": 2.7169353440889874 + }, + { + "x": 1.1887111006243336, + "y": 6.866432558191455, + "heading": 0.9302631260100628, + "angularVelocity": 1.5489221500804566, + "velocityX": -1.3884168360711413, + "velocityY": -0.17971807745192941, + "timestamp": 2.8012145102458907 + }, + { + "x": 1.0716964874224564, + "y": 6.851286068322852, + "heading": 1.0410285148792768, + "angularVelocity": 1.3142677356703014, + "velocityX": -1.3884168358290385, + "velocityY": -0.17971807932230188, + "timestamp": 2.885493676402794 + }, + { + "x": 0.9546818742710608, + "y": 6.836139578064252, + "heading": 1.1235226807914271, + "angularVelocity": 0.9788203855573323, + "velocityX": -1.3884168352300577, + "velocityY": -0.17971808394974448, + "timestamp": 2.9697728425596974 + }, + { + "x": 0.8376672612285322, + "y": 6.820993086964611, + "heading": 1.1689251973993973, + "angularVelocity": 0.538715778505021, + "velocityX": -1.3884168339383145, + "velocityY": -0.1797180939289554, + "timestamp": 3.0540520087166008 + }, + { + "x": 0.7239932936157987, + "y": 6.8062790150425, + "heading": 1.168925502319645, + "angularVelocity": 0.000003617978933894318, + "velocityX": -1.3487789782009216, + "velocityY": -0.1745872982976388, + "timestamp": 3.138331174873504 + }, + { + "x": 0.6482106429312192, + "y": 6.796469632762423, + "heading": 1.1689256570497522, + "angularVelocity": 0.0000018359235639903555, + "velocityX": -0.899186052024937, + "velocityY": -0.11639154404795703, + "timestamp": 3.2226103410304074 + }, + { + "x": 0.6103193163871763, "y": 6.79156494140625, "heading": 1.1689257238838953, - "angularVelocity": 2.912641665287057e-8, - "velocityX": -0.5794902817178565, - "velocityY": -0.07500980729443667, - "timestamp": 3.2000393880070686 + "angularVelocity": 7.930090686887444e-7, + "velocityX": -0.4495930402716653, + "velocityY": -0.05819577458848967, + "timestamp": 3.306889507187311 }, { - "x": 0.6103193163871765, + "x": 0.6103193163871763, "y": 6.79156494140625, "heading": 1.1689257238838953, - "angularVelocity": 4.3622376722558945e-19, - "velocityX": -5.6270263381275445e-18, - "velocityY": -6.414998799232099e-19, - "timestamp": 3.3086686473442364 - }, - { - "x": 0.6557794938984264, - "y": 6.805365933173805, - "heading": 1.1689257265280788, - "angularVelocity": 2.813571130767411e-8, - "velocityX": 0.4837237542621103, - "velocityY": 0.14685089051159525, - "timestamp": 3.402648273340889 - }, - { - "x": 0.7466998461856557, - "y": 6.832967915878576, - "heading": 1.1689257334168288, - "angularVelocity": 7.330046251495819e-8, - "velocityX": 0.9674474794192922, - "velocityY": 0.2937017721878802, - "timestamp": 3.496627899337542 - }, - { - "x": 0.8830803650430104, - "y": 6.874370887029686, - "heading": 1.1689257494213645, - "angularVelocity": 1.7029792885174175e-7, - "velocityX": 1.4511711172612278, - "velocityY": 0.4405526273597229, - "timestamp": 3.5906075253341947 - }, - { - "x": 1.0359561926852463, - "y": 6.920781573342344, - "heading": 1.1411189880762886, - "angularVelocity": -0.295880740641233, - "velocityX": 1.6266911686548022, - "velocityY": 0.4938377421752201, - "timestamp": 3.6845871513308475 - }, - { - "x": 1.188832020320688, - "y": 6.96719225967738, - "heading": 1.0851032009960204, - "angularVelocity": -0.5960418174282094, - "velocityX": 1.6266911685825045, - "velocityY": 0.4938377424133325, - "timestamp": 3.7785667773275002 - }, - { - "x": 1.3417078479593374, - "y": 7.013602946001849, - "heading": 1.0076693097111147, - "angularVelocity": -0.8239433862788942, - "velocityX": 1.6266911686166408, - "velocityY": 0.4938377423008883, - "timestamp": 3.872546403324153 - }, - { - "x": 1.4945836755981692, - "y": 7.0600136323257185, - "heading": 0.9144858910263077, - "angularVelocity": -0.9915278731597208, - "velocityX": 1.6266911686185797, - "velocityY": 0.4938377422945011, - "timestamp": 3.966526029320806 - }, - { - "x": 1.6474595032368191, - "y": 7.106424318650186, - "heading": 0.810295986033273, - "angularVelocity": -1.1086435372358867, - "velocityX": 1.6266911686166439, - "velocityY": 0.4938377423008778, - "timestamp": 4.060505655317458 - }, - { - "x": 1.8003353308752952, - "y": 7.152835004975228, - "heading": 0.6991463049745296, - "angularVelocity": -1.1826997594425672, - "velocityX": 1.6266911686147918, - "velocityY": 0.4938377423069784, - "timestamp": 4.154485281314111 - }, - { - "x": 1.9532111585137473, - "y": 7.199245691300347, - "heading": 0.5846243537681264, - "angularVelocity": -1.2185827512282525, - "velocityX": 1.6266911686145398, - "velocityY": 0.49383774230780825, - "timestamp": 4.248464907310764 - }, - { - "x": 2.1060869861523015, - "y": 7.24565637762513, - "heading": 0.47009178756221137, - "angularVelocity": -1.2186957012362925, - "velocityX": 1.6266911686156262, - "velocityY": 0.49383774230423, - "timestamp": 4.342444533307416 - }, - { - "x": 2.258962813791129, - "y": 7.2920670639490135, - "heading": 0.3589127090659498, - "angularVelocity": -1.1830125659387238, - "velocityX": 1.626691168618534, - "velocityY": 0.49383774229465205, - "timestamp": 4.436424159304069 - }, - { - "x": 2.411838641430174, - "y": 7.33847775027218, - "heading": 0.25468045110617066, - "angularVelocity": -1.1090941983903169, - "velocityX": 1.6266911686208465, - "velocityY": 0.4938377422870341, - "timestamp": 4.530403785300722 - }, - { - "x": 2.56471446906933, - "y": 7.38488843659498, - "heading": 0.1614473718452329, - "angularVelocity": -0.9920562916930376, - "velocityX": 1.6266911686220302, - "velocityY": 0.4938377422831351, - "timestamp": 4.624383411297375 - }, - { - "x": 2.7175902967080585, - "y": 7.43129912291919, - "heading": 0.083958784465868, - "angularVelocity": -0.8245253857695168, - "velocityX": 1.6266911686174785, - "velocityY": 0.4938377422981283, - "timestamp": 4.7183630372940275 - }, - { - "x": 2.87046612433849, - "y": 7.477709809270727, - "heading": 0.027881129348107082, - "angularVelocity": -0.5967001307257632, - "velocityX": 1.6266911685292, - "velocityY": 0.493837742588916, - "timestamp": 4.81234266329068 - }, - { - "x": 3.02334195198337, - "y": 7.524120495574675, - "heading": -2.5550742755505775e-8, - "angularVelocity": -0.29667233299952617, - "velocityX": 1.6266911686829375, - "velocityY": 0.4938377420825434, - "timestamp": 4.906322289287333 - }, - { - "x": 3.159722470841706, - "y": 7.565523466722555, - "heading": -9.539534540603276e-9, - "angularVelocity": 1.7036892885524613e-7, - "velocityX": 1.4511711172716637, - "velocityY": 0.44055262732534617, - "timestamp": 5.000301915283986 - }, - { - "x": 3.2506428231294207, - "y": 7.5931254494257265, - "heading": -2.646376503470658e-9, - "angularVelocity": 7.334736613811233e-8, - "velocityX": 0.9674474794244572, - "velocityY": 0.2937017721708666, - "timestamp": 5.094281541280639 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 7.567002470257346e-19, - "angularVelocity": 2.8159044862445686e-8, - "velocityX": 0.48372375426422565, - "velocityY": 0.1468508905046274, - "timestamp": 5.188261167277291 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 3.6938220334538545e-19, - "angularVelocity": -1.9084817747204875e-19, - "velocityX": 4.2439574212322616e-19, - "velocityY": 1.26437464030373e-19, - "timestamp": 5.282240793273944 + "angularVelocity": -1.1197773464222718e-17, + "velocityX": 1.4630555201274963e-16, + "velocityY": 1.5632470182389935e-17, + "timestamp": 3.391168673344214 + }, + { + "x": 0.6716933262156661, + "y": 6.798129007626276, + "heading": 1.1689262102304376, + "angularVelocity": 0.000004540173449016303, + "velocityX": 0.5729425946972534, + "velocityY": 0.06127729216936612, + "timestamp": 3.4982893698965807 + }, + { + "x": 0.7944413423015934, + "y": 6.811257139685496, + "heading": 1.1689274078355216, + "angularVelocity": 0.00001117995982505779, + "velocityX": 1.1458851560577887, + "velocityY": 0.12255458078358349, + "timestamp": 3.6054100664489472 + }, + { + "x": 0.9785633539293457, + "y": 6.830949336441548, + "heading": 1.1689299826958874, + "angularVelocity": 0.000024037001706306202, + "velocityX": 1.7188276173852506, + "velocityY": 0.1838318587335218, + "timestamp": 3.712530763001314 + }, + { + "x": 1.2128924819066704, + "y": 6.856011279048505, + "heading": 1.1639992699084272, + "angularVelocity": -0.046029506399353645, + "velocityX": 2.1875243115392906, + "velocityY": 0.23395985475790373, + "timestamp": 3.8196514595536804 + }, + { + "x": 1.4472216098815815, + "y": 6.88107322166846, + "heading": 1.1410466957992857, + "angularVelocity": -0.2142683426066124, + "velocityX": 2.1875243115167615, + "velocityY": 0.23395985487924773, + "timestamp": 3.926772156106047 + }, + { + "x": 1.6815507378566321, + "y": 6.906135164287114, + "heading": 1.1053748993971433, + "angularVelocity": -0.3330056427023327, + "velocityX": 2.1875243115180623, + "velocityY": 0.23395985486708806, + "timestamp": 4.033892852658413 + }, + { + "x": 1.9158798658322573, + "y": 6.931197106900394, + "heading": 1.060764506700477, + "angularVelocity": -0.4164498003880933, + "velocityX": 2.1875243115234255, + "velocityY": 0.23395985481693854, + "timestamp": 4.141013549210779 + }, + { + "x": 2.15020899380867, + "y": 6.956259049506311, + "heading": 1.0098936328394574, + "angularVelocity": -0.47489304586580033, + "velocityX": 2.187524311530778, + "velocityY": 0.23395985474819475, + "timestamp": 4.248134245763145 + }, + { + "x": 2.3845381217856128, + "y": 6.981320992107275, + "heading": 0.9546528217127299, + "angularVelocity": -0.5156875646315714, + "velocityX": 2.1875243115357237, + "velocityY": 0.23395985470195227, + "timestamp": 4.355254942315511 + }, + { + "x": 2.618867249763438, + "y": 7.006382934699983, + "heading": 0.896375656527792, + "angularVelocity": -0.5440327318675406, + "velocityX": 2.187524311543967, + "velocityY": 0.23395985462487984, + "timestamp": 4.4623756388678775 + }, + { + "x": 2.853196377741319, + "y": 7.0314448772921745, + "heading": 0.8360053257441781, + "angularVelocity": -0.5635729856751034, + "velocityX": 2.1875243115444807, + "velocityY": 0.2339598546200722, + "timestamp": 4.569496335420244 + }, + { + "x": 3.0875255057190256, + "y": 7.056506819885997, + "heading": 0.7742143737208824, + "angularVelocity": -0.5768348602278626, + "velocityX": 2.187524311542854, + "velocityY": 0.23395985463528263, + "timestamp": 4.67661703197261 + }, + { + "x": 3.321854633695538, + "y": 7.08156876249098, + "heading": 0.7114910094502581, + "angularVelocity": -0.585539174868618, + "velocityX": 2.1875243115317105, + "velocityY": 0.2339598547394751, + "timestamp": 4.783737728524976 + }, + { + "x": 3.5561837616716114, + "y": 7.106630705100066, + "heading": 0.6482025434058349, + "angularVelocity": -0.5908145492079041, + "velocityX": 2.187524311527614, + "velocityY": 0.23395985477777761, + "timestamp": 4.890858425077342 + }, + { + "x": 3.7905128896459725, + "y": 7.131692647725167, + "heading": 0.5846437426209585, + "angularVelocity": -0.5933381954233768, + "velocityX": 2.1875243115116247, + "velocityY": 0.2339598549272787, + "timestamp": 4.997979121629708 + }, + { + "x": 4.024842017618935, + "y": 7.156754590363345, + "heading": 0.5210758768415071, + "angularVelocity": -0.5934228195424006, + "velocityX": 2.1875243114985685, + "velocityY": 0.23395985504935324, + "timestamp": 5.105099818182074 + }, + { + "x": 4.259171145591179, + "y": 7.181816533008237, + "heading": 0.45776123122730383, + "angularVelocity": -0.5910589424075633, + "velocityX": 2.187524311491864, + "velocityY": 0.23395985511203934, + "timestamp": 5.21222051473444 + }, + { + "x": 4.493500273561688, + "y": 7.206878475669362, + "heading": 0.39499750155382707, + "angularVelocity": -0.585915996567426, + "velocityX": 2.187524311475657, + "velocityY": 0.23395985526357388, + "timestamp": 5.319341211286806 + }, + { + "x": 4.727829401532455, + "y": 7.231940418328061, + "heading": 0.3331565162700674, + "angularVelocity": -0.5773019339313977, + "velocityX": 2.18752431147808, + "velocityY": 0.23395985524092341, + "timestamp": 5.4264619078391725 + }, + { + "x": 4.962158529503271, + "y": 7.257002360986307, + "heading": 0.27273200291903427, + "angularVelocity": -0.5640787942551723, + "velocityX": 2.1875243114785317, + "velocityY": 0.23395985523669777, + "timestamp": 5.533582604391539 + }, + { + "x": 5.1964876574745045, + "y": 7.282064303640645, + "heading": 0.21440226305549417, + "angularVelocity": -0.544523530380754, + "velocityX": 2.1875243114824334, + "velocityY": 0.23395985520021775, + "timestamp": 5.640703300943905 + }, + { + "x": 5.430816785446193, + "y": 7.307126246290732, + "heading": 0.1591156180940034, + "angularVelocity": -0.5161154355868443, + "velocityX": 2.187524311486677, + "velocityY": 0.2339598551605356, + "timestamp": 5.747823997496271 + }, + { + "x": 5.665145913418822, + "y": 7.332188188932028, + "heading": 0.10820921967960505, + "angularVelocity": -0.4752246769559841, + "velocityX": 2.1875243114954563, + "velocityY": 0.2339598550784526, + "timestamp": 5.854944694048637 + }, + { + "x": 5.899475041392132, + "y": 7.357250131566959, + "heading": 0.06357495743964915, + "angularVelocity": -0.4166726288802297, + "velocityX": 2.1875243115018086, + "velocityY": 0.23395985501905772, + "timestamp": 5.962065390601003 + }, + { + "x": 6.133804169366042, + "y": 7.382312074196284, + "heading": 0.02788975594931021, + "angularVelocity": -0.3331307827418212, + "velocityX": 2.1875243115074072, + "velocityY": 0.23395985496671218, + "timestamp": 6.069186087153369 + }, + { + "x": 6.368133297340411, + "y": 7.40737401682131, + "heading": 0.004930772411512917, + "angularVelocity": -0.21432817631627044, + "velocityX": 2.1875243115117, + "velocityY": 0.23395985492657667, + "timestamp": 6.176306783705735 + }, + { + "x": 6.602462425316754, + "y": 7.432435959437442, + "heading": -0.000004257837407987995, + "angularVelocity": -0.046069811042615996, + "velocityX": 2.1875243115301304, + "velocityY": 0.23395985484355303, + "timestamp": 6.283427480258101 + }, + { + "x": 6.786584436945245, + "y": 7.452128156186601, + "heading": -0.0000016835872093411556, + "angularVelocity": 0.000024031305634360455, + "velocityX": 1.7188276173921362, + "velocityY": 0.1838318586691832, + "timestamp": 6.3905481768104675 + }, + { + "x": 6.909332453031493, + "y": 7.465256288242815, + "heading": -4.862460012160711e-7, + "angularVelocity": 0.000011177496474955052, + "velocityX": 1.1458851560607914, + "velocityY": 0.12255458075551605, + "timestamp": 6.497668873362834 + }, + { + "x": 6.970706462860108, + "y": 7.47182035446167, + "heading": 1.452985890125102e-17, + "angularVelocity": 0.000004539234871288793, + "velocityX": 0.5729425946984215, + "velocityY": 0.061277292158443734, + "timestamp": 6.6047895699152 + }, + { + "x": 6.970706462860107, + "y": 7.47182035446167, + "heading": 6.946622712118294e-18, + "angularVelocity": -5.942954968372966e-18, + "velocityX": 1.343946406102527e-16, + "velocityY": 1.4373642665254536e-17, + "timestamp": 6.711910266467566 } ], "constraints": [ @@ -918,10 +1008,10 @@ { "scope": [ "first", - 2 + 1 ], "type": "MaxVelocity", - "velocity": 1.7 + "velocity": 1.4 }, { "scope": [ @@ -931,8 +1021,8 @@ }, { "scope": [ - 2, - 3 + 1, + 2 ], "type": "MaxVelocity", "velocity": 1 @@ -943,7 +1033,21 @@ "last" ], "type": "MaxVelocity", - "velocity": 1.7 + "velocity": 2.2 + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2, + 3 + ], + "type": "MaxVelocity", + "velocity": 1.4 } ], "usesControlIntervalGuessing": true, @@ -987,11 +1091,20 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 22 + "controlIntervalCount": 21 + }, + { + "x": 2.707773208618164, + "y": 2.199841260910034, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 }, { - "x": 2.8538730144500732, - "y": 2.2070720195770264, + "x": 7.06566047668457, + "y": 0.7616762518882751, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -1004,586 +1117,748 @@ "x": 0.6313003897666931, "y": 4.255156517028809, "heading": -1.2897, - "angularVelocity": -2.3969037863354903e-26, - "velocityX": -5.149474371067618e-26, - "velocityY": 3.9840780609445665e-27, + "angularVelocity": -5.873954682063349e-26, + "velocityX": 4.9142740531775466e-26, + "velocityY": -2.6291295229347494e-26, "timestamp": 0 }, { - "x": 0.6442648163276663, - "y": 4.253481865948087, - "heading": -1.289699890378596, - "angularVelocity": 0.0000022236977895922636, - "velocityX": 0.2629866575194569, - "velocityY": -0.03397071888670311, - "timestamp": 0.049296898493848725 - }, - { - "x": 0.6701936687758512, - "y": 4.250132563317103, - "heading": -1.2896996605083761, - "angularVelocity": 0.000004662975296506846, - "velocityX": 0.5259733013714917, - "velocityY": -0.06794144729820102, - "timestamp": 0.09859379698769745 - }, - { - "x": 0.7090869461006044, - "y": 4.245108608431542, - "heading": -1.2896992944482415, - "angularVelocity": 0.0000074256220104019626, - "velocityX": 0.7889599247223699, - "velocityY": -0.10191218999686634, - "timestamp": 0.14789069548154618 - }, - { - "x": 0.760944646617517, - "y": 4.238410000117552, - "heading": -1.2896987656297503, - "angularVelocity": 0.000010727216264499804, - "velocityX": 1.0519465139045912, - "velocityY": -0.13588295650740967, - "timestamp": 0.1971875939753949 - }, - { - "x": 0.82576676695776, - "y": 4.230036736027443, - "heading": -1.2896980209162199, - "angularVelocity": 0.000015106701495517666, - "velocityX": 1.3149330347492647, - "velocityY": -0.16985377064146243, - "timestamp": 0.24648449246924362 - }, - { - "x": 0.9035532970147546, - "y": 4.21998880911824, - "heading": -1.2896969008990993, - "angularVelocity": 0.000022719829335288094, - "velocityX": 1.5779193505794464, - "velocityY": -0.2038247276440018, - "timestamp": 0.29578139096309236 - }, - { - "x": 0.9915565704265453, - "y": 4.208621313799639, - "heading": -1.27289000896861, - "angularVelocity": 0.3409320351580815, - "velocityX": 1.7851685623340352, - "velocityY": -0.2305925051252473, - "timestamp": 0.3450782894569411 - }, - { - "x": 1.0795598293475788, - "y": 4.197253706298393, - "heading": -1.2169726689327969, - "angularVelocity": 1.134297323852746, - "velocityX": 1.7851682683853676, - "velocityY": -0.23059478077843756, - "timestamp": 0.3943751879507898 - }, - { - "x": 1.1675630883225618, - "y": 4.185886099214807, - "heading": -1.123282876673827, - "angularVelocity": 1.9005210291406174, - "velocityX": 1.7851682694797597, - "velocityY": -0.23059477230609737, - "timestamp": 0.4436720864446385 - }, - { - "x": 1.255566347295848, - "y": 4.174518492118083, - "heading": -0.9939341877349891, - "angularVelocity": 2.6238707279927147, - "velocityX": 1.785168269445332, - "velocityY": -0.23059477257260114, - "timestamp": 0.49296898493848723 - }, - { - "x": 1.3435696062497653, - "y": 4.163150884871416, - "heading": -0.8327019973298604, - "angularVelocity": 3.2706355842091646, - "velocityX": 1.7851682690524295, - "velocityY": -0.23059477561424194, - "timestamp": 0.542265883432336 - }, - { - "x": 1.4315728650960726, - "y": 4.15178327679168, - "heading": -0.6477412989500829, - "angularVelocity": 3.7519743438394353, - "velocityX": 1.7851682668695343, - "velocityY": -0.23059479251323747, - "timestamp": 0.5915627819261847 - }, - { - "x": 1.519576123922667, - "y": 4.140415668559335, - "heading": -0.46074819376328596, - "angularVelocity": 3.7932022277249358, - "velocityX": 1.7851682664696522, - "velocityY": -0.23059479560896612, - "timestamp": 0.6408596804200335 - }, - { - "x": 1.6075793828656728, - "y": 4.129048061228197, - "heading": -0.29527670366521236, - "angularVelocity": 3.356630846030226, - "velocityX": 1.7851682688310881, - "velocityY": -0.23059477732777772, - "timestamp": 0.6901565789138823 - }, - { - "x": 1.6955826418903344, - "y": 4.117680454529199, - "heading": -0.16085599473456663, - "angularVelocity": 2.7267579307736534, - "velocityX": 1.7851682704874938, - "velocityY": -0.23059476450461613, - "timestamp": 0.739453477407731 - }, - { - "x": 1.7835859004541794, - "y": 4.106312844262756, - "heading": -0.061669148748961934, - "angularVelocity": 2.0120301482654375, - "velocityX": 1.7851682611397215, - "velocityY": -0.2305948368711734, - "timestamp": 0.7887503759015798 + "x": 0.6559720854839329, + "y": 4.251969633538223, + "heading": -1.2896999488381438, + "angularVelocity": 7.523222320248662e-7, + "velocityX": 0.36279108159264123, + "velocityY": -0.04686232035731875, + "timestamp": 0.06800524315242767 + }, + { + "x": 0.7053154746686687, + "y": 4.245595866614296, + "heading": -1.2896998311579277, + "angularVelocity": 0.0000017304579846578334, + "velocityX": 0.725582130103366, + "velocityY": -0.09372463987285283, + "timestamp": 0.13601048630485535 + }, + { + "x": 0.7793305505716808, + "y": 4.236035216428766, + "heading": -1.2896996008899766, + "angularVelocity": 0.0000033860323161189202, + "velocityX": 1.08837307936851, + "velocityY": -0.1405869568630212, + "timestamp": 0.20401572945728302 + }, + { + "x": 0.8737534102892339, + "y": 4.223838467574396, + "heading": -1.2688277674096835, + "angularVelocity": 0.3069150629093525, + "velocityX": 1.3884644086326217, + "velocityY": -0.17935012491658225, + "timestamp": 0.2720209726097107 + }, + { + "x": 0.9681762693258235, + "y": 4.211641713448134, + "heading": -1.1855973267492907, + "angularVelocity": 1.2238827008358628, + "velocityX": 1.388464398619224, + "velocityY": -0.17935020243841734, + "timestamp": 0.3400262157621384 + }, + { + "x": 1.0625991283627494, + "y": 4.1994449593244765, + "heading": -1.0462610924914664, + "angularVelocity": 2.0489042873578804, + "velocityX": 1.3884643986241696, + "velocityY": -0.17935020240012747, + "timestamp": 0.40803145891456605 + }, + { + "x": 1.157021987398968, + "y": 4.187248205195344, + "heading": -0.8608605661850172, + "angularVelocity": 2.726268118634478, + "velocityX": 1.3884643986137708, + "velocityY": -0.17935020248062902, + "timestamp": 0.4760367020669937 + }, + { + "x": 1.2514448464316574, + "y": 4.175051451038889, + "heading": -0.6467912799124919, + "angularVelocity": 3.1478350249069513, + "velocityX": 1.3884643985618728, + "velocityY": -0.17935020288240236, + "timestamp": 0.5440419452194214 + }, + { + "x": 1.3458677054640462, + "y": 4.162854696880105, + "heading": -0.43243447427425874, + "angularVelocity": 3.1520629248802394, + "velocityX": 1.3884643985574503, + "velocityY": -0.17935020291663928, + "timestamp": 0.612047188371849 + }, + { + "x": 1.4402905644997468, + "y": 4.150657942746961, + "heading": -0.24636637690677357, + "angularVelocity": 2.7360845832200056, + "velocityX": 1.388464398606151, + "velocityY": -0.1793502025396201, + "timestamp": 0.6800524315242766 + }, + { + "x": 1.534713423536453, + "y": 4.138461188621602, + "heading": -0.10616220410834029, + "angularVelocity": 2.0616671053462476, + "velocityX": 1.3884643986209388, + "velocityY": -0.17935020242513838, + "timestamp": 0.7480576746767043 + }, + { + "x": 1.629136282573076, + "y": 4.1262644344955985, + "heading": -0.02194266261213042, + "angularVelocity": 1.2384271799078685, + "velocityX": 1.3884643986197136, + "velocityY": -0.17935020243462574, + "timestamp": 0.8160629178291319 + }, + { + "x": 1.723559141925496, + "y": 4.114067682814502, + "heading": -3.99732500715661e-7, + "angularVelocity": 0.3226554580570808, + "velocityX": 1.3884644032634323, + "velocityY": -0.17935016648287658, + "timestamp": 0.8840681609815595 + }, + { + "x": 1.7975742180033953, + "y": 4.104507033982872, + "heading": -1.6923886778574664e-7, + "angularVelocity": 0.0000033893509124477877, + "velocityX": 1.0883730819401845, + "velocityY": -0.14058693695427216, + "timestamp": 0.9520734041339871 + }, + { + "x": 1.846917607267625, + "y": 4.0981332676743545, + "heading": -5.130838068287636e-8, + "angularVelocity": 0.0000017341381581202444, + "velocityX": 0.7255821312723041, + "velocityY": -0.0937246308234074, + "timestamp": 1.0200786472864147 }, { "x": 1.8715893030166624, "y": 4.094946384429932, - "heading": -2.0036368727453063e-25, - "angularVelocity": 1.2509742120319602, - "velocityX": 1.785171182188348, - "velocityY": -0.23057150003548213, - "timestamp": 0.8380472743954286 - }, - { - "x": 2.0015014856340083, - "y": 4.096925192128385, - "heading": 0.03734833063779682, - "angularVelocity": 0.5174202657607754, - "velocityX": 1.7997911796197699, - "velocityY": 0.02741421605031293, - "timestamp": 0.9102290830899192 - }, - { - "x": 2.1314136526758922, - "y": 4.098905022118577, - "heading": 0.0000024861301875270727, - "angularVelocity": -0.5173858231465974, - "velocityX": 1.799790963838784, - "velocityY": 0.027428378784090407, - "timestamp": 0.9824108917844099 - }, - { - "x": 2.243505108940701, - "y": 4.100613269261141, - "heading": 0.0000014496891722730681, - "angularVelocity": -0.000014358756506653064, - "velocityX": 1.5529045100440175, - "velocityY": 0.02366589551382172, - "timestamp": 1.0545927004789006 - }, - { - "x": 2.3275737089377446, - "y": 4.101894454670087, - "heading": 7.139853498794832e-7, - "angularVelocity": -0.000010192371675077432, - "velocityX": 1.1646784905718206, - "velocityY": 0.017749422356118257, - "timestamp": 1.1267745091733914 - }, - { - "x": 2.3836194445295247, - "y": 4.102748578291145, - "heading": 2.3558195309312242e-7, - "angularVelocity": -0.000006627755738446538, - "velocityX": 0.7764523583634968, - "velocityY": 0.011832948446505252, - "timestamp": 1.1989563178678821 + "heading": 2.5370456810536765e-25, + "angularVelocity": 7.544768359679741e-7, + "velocityX": 0.36279108206021565, + "velocityY": -0.04686231673753878, + "timestamp": 1.0880838904388423 + }, + { + "x": 1.8715893030166624, + "y": 4.094946384429932, + "heading": 1.2477380749356057e-25, + "angularVelocity": -6.112732953488003e-26, + "velocityX": 1.2210785237870183e-25, + "velocityY": 1.786690711606501e-26, + "timestamp": 1.15608913359127 + }, + { + "x": 1.93115886834108, + "y": 4.095854097514598, + "heading": 1.7422071194171223e-22, + "angularVelocity": 1.654264546193207e-21, + "velocityX": 0.5660317007877702, + "velocityY": 0.008625115498882304, + "timestamp": 1.261329821067806 + }, + { + "x": 2.03638733817559, + "y": 4.097457554891332, + "heading": -5.527071650385784e-16, + "angularVelocity": -5.251840828030702e-15, + "velocityX": 0.9998839076186332, + "velocityY": 0.015236097513057894, + "timestamp": 1.3665705085443418 + }, + { + "x": 2.1416158080101013, + "y": 4.099061012268066, + "heading": 4.742146642802244e-17, + "angularVelocity": 5.7024392927916176e-15, + "velocityX": 0.9998839076186449, + "velocityY": 0.01523609751305807, + "timestamp": 1.4718111960208777 + }, + { + "x": 2.2468442778446125, + "y": 4.1006644696448005, + "heading": 4.447647793878566e-16, + "angularVelocity": 3.7755674396219104e-15, + "velocityX": 0.9998839076186449, + "velocityY": 0.01523609751305807, + "timestamp": 1.5770518834974137 + }, + { + "x": 2.3520727476791223, + "y": 4.1022679270215345, + "heading": -4.754598494729311e-22, + "angularVelocity": -4.226172077665966e-15, + "velocityX": 0.9998839076186332, + "velocityY": 0.015236097513057894, + "timestamp": 1.6822925709739496 }, { "x": 2.41164231300354, "y": 4.103175640106201, - "heading": -7.741029200580404e-24, - "angularVelocity": -0.000003263730257719399, - "velocityX": 0.3882261885764356, - "velocityY": 0.005916474285980588, - "timestamp": 1.271138126562373 + "heading": 1.0888859372640401e-27, + "angularVelocity": 4.5178433335499566e-21, + "velocityX": 0.5660317007877702, + "velocityY": 0.008625115498882303, + "timestamp": 1.7875332584504855 }, { "x": 2.41164231300354, "y": 4.103175640106201, - "heading": -4.8316826521123155e-24, - "angularVelocity": 4.329873823266725e-23, - "velocityX": 4.0284965680453105e-22, - "velocityY": -2.2471582465129843e-22, - "timestamp": 1.3433199352568637 - }, - { - "x": 2.344735769891283, - "y": 4.110806937625484, - "heading": -6.978848485329857e-9, - "angularVelocity": -6.237346131654942e-8, - "velocityX": -0.5979772576247802, - "velocityY": 0.068204724836005, - "timestamp": 1.4552080418012758 - }, - { - "x": 2.2335684425317694, - "y": 4.123486578658459, - "heading": -0.015246855809319428, - "angularVelocity": -0.13626871793043496, - "velocityX": -0.9935580357273067, - "velocityY": 0.11332429714450055, - "timestamp": 1.567096148345688 - }, - { - "x": 2.122401115172327, - "y": 4.136166219692066, - "heading": -0.060800247349853634, - "angularVelocity": -0.40713345633794035, - "velocityX": -0.9935580357266695, - "velocityY": 0.1133242971501526, - "timestamp": 1.6789842548901002 - }, - { - "x": 2.0112337878128743, - "y": 4.148845860725579, - "heading": -0.12944768713850766, - "angularVelocity": -0.6135365224131806, - "velocityX": -0.9935580357267644, - "velocityY": 0.11332429714932027, - "timestamp": 1.7908723614345123 - }, - { - "x": 1.9000664604534496, - "y": 4.16152550175934, - "heading": -0.21530108218408722, - "angularVelocity": -0.7673147548662962, - "velocityX": -0.9935580357265124, - "velocityY": 0.11332429715152995, - "timestamp": 1.9027604679789245 - }, - { - "x": 1.788899133093997, - "y": 4.174205142792856, - "heading": -0.31351675402178747, - "angularVelocity": -0.8778026089726992, - "velocityX": -0.9935580357267627, - "velocityY": 0.1133242971493348, - "timestamp": 2.0146485745233367 - }, - { - "x": 1.6777318057328703, - "y": 4.1868847838116965, - "heading": -0.4200302675690305, - "angularVelocity": -0.9519645727936626, - "velocityX": -0.9935580357417213, - "velocityY": 0.11332429701818696, - "timestamp": 2.1265366810677486 - }, - { - "x": 1.5665644783751262, - "y": 4.199564424860192, - "heading": -0.5313117322584114, - "angularVelocity": -0.994578138161715, - "velocityX": -0.9935580357114923, - "velocityY": 0.11332429728321564, - "timestamp": 2.2384247876121606 - }, - { - "x": 1.455397151015652, - "y": 4.212244065893518, - "heading": -0.6441464282968985, - "angularVelocity": -1.008460144007346, - "velocityX": -0.9935580357269562, - "velocityY": 0.1133242971476382, - "timestamp": 2.3503128941565725 - }, - { - "x": 1.344229823656203, - "y": 4.2249237069270675, - "heading": -0.7554226234189, - "angularVelocity": -0.994531041401011, - "velocityX": -0.9935580357267284, - "velocityY": 0.11332429714963566, - "timestamp": 2.4622010007009845 - }, - { - "x": 1.2330624962967764, - "y": 4.237603347960814, - "heading": -0.8619240929821974, - "angularVelocity": -0.951856929682008, - "velocityX": -0.9935580357265276, - "velocityY": 0.11332429715139572, - "timestamp": 2.5740891072453964 - }, - { - "x": 1.121895168937324, - "y": 4.250282988994331, - "heading": -0.9601218669400209, - "angularVelocity": -0.8776426466636605, - "velocityX": -0.9935580357267606, - "velocityY": 0.11332429714935303, - "timestamp": 2.6859772137898084 - }, - { - "x": 1.0107278415778729, - "y": 4.262962630027863, - "heading": -1.0459506988902485, - "angularVelocity": -0.7670952221911026, - "velocityX": -0.9935580357267462, - "velocityY": 0.1133242971494796, - "timestamp": 2.7978653203342203 - }, - { - "x": 0.8995605142184344, - "y": 4.275642271061504, - "heading": -1.114565563469038, - "angularVelocity": -0.6132453814611101, - "velocityX": -0.9935580357266346, - "velocityY": 0.11332429715045739, - "timestamp": 2.9097534268786323 - }, - { - "x": 0.7883931868589816, - "y": 4.28832191209502, - "heading": -1.1600763958246794, - "angularVelocity": -0.40675308360479684, - "velocityX": -0.9935580357267626, - "velocityY": 0.11332429714933458, - "timestamp": 3.021641533423044 - }, - { - "x": 0.6772258594993766, - "y": 4.301001553127193, - "heading": -1.1752681604416075, - "angularVelocity": -0.13577640274838362, - "velocityX": -0.9935580357281231, - "velocityY": 0.1133242971373439, - "timestamp": 3.133529639967456 + "heading": 1.1713747854415584e-27, + "angularVelocity": 7.835758562721149e-28, + "velocityX": 1.927931679024526e-27, + "velocityY": -6.962214190137859e-27, + "timestamp": 1.8927739459270214 + }, + { + "x": 2.3717259825826824, + "y": 4.107728459294388, + "heading": -1.1285196891796717e-8, + "angularVelocity": -1.3058226942034976e-7, + "velocityX": -0.46187630249296374, + "velocityY": 0.0526811776129561, + "timestamp": 1.9791960684854004 + }, + { + "x": 2.291893324189882, + "y": 4.116834097391441, + "heading": -3.6937331478847774e-8, + "angularVelocity": -2.968237047142972e-7, + "velocityX": -0.9237525766492569, + "velocityY": 0.10536235199385893, + "timestamp": 2.065618191043779 + }, + { + "x": 2.172144345171922, + "y": 4.1304925535593915, + "heading": -8.620167407001187e-8, + "angularVelocity": -5.70043191867749e-7, + "velocityX": -1.385628765795106, + "velocityY": 0.1580435166785527, + "timestamp": 2.152040313602158 + }, + { + "x": 2.051932794176137, + "y": 4.144203770253025, + "heading": -0.04631321106269493, + "angularVelocity": -0.5358943230043436, + "velocityX": -1.390981237640637, + "velocityY": 0.1586540145941372, + "timestamp": 2.2384624361605368 + }, + { + "x": 1.9317212431803543, + "y": 4.1579149869466745, + "heading": -0.12967736913883382, + "angularVelocity": -0.9646159525858174, + "velocityX": -1.390981237640616, + "velocityY": 0.15865401459432438, + "timestamp": 2.3248845587189155 + }, + { + "x": 1.8115096921846017, + "y": 4.17162620364059, + "heading": -0.24110956900589844, + "angularVelocity": -1.2893943884772234, + "velocityX": -1.3909812376402653, + "velocityY": 0.1586540145973992, + "timestamp": 2.4113066812772943 + }, + { + "x": 1.6912981411889283, + "y": 4.1853374203352, + "heading": -0.37208012204709173, + "angularVelocity": -1.5154748479212776, + "velocityX": -1.3909812376393478, + "velocityY": 0.15865401460544476, + "timestamp": 2.497728803835673 + }, + { + "x": 1.5710865901933302, + "y": 4.19904863703047, + "heading": -0.5145334190025777, + "angularVelocity": -1.648342955928414, + "velocityX": -1.390981237638478, + "velocityY": 0.15865401461306952, + "timestamp": 2.584150926394052 + }, + { + "x": 1.4508750391977163, + "y": 4.212759853725601, + "heading": -0.6607676403003127, + "angularVelocity": -1.6920924523573535, + "velocityX": -1.3909812376386603, + "velocityY": 0.1586540146114714, + "timestamp": 2.6705730489524306 + }, + { + "x": 1.3306634882020019, + "y": 4.226471070419849, + "heading": -0.8032128974555454, + "angularVelocity": -1.6482499265047517, + "velocityX": -1.3909812376398256, + "velocityY": 0.15865401460125392, + "timestamp": 2.7569951715108094 + }, + { + "x": 1.2104519372061997, + "y": 4.240182287113329, + "heading": -0.9341724094209435, + "angularVelocity": -1.5153470904042419, + "velocityX": -1.39098123764084, + "velocityY": 0.15865401459236247, + "timestamp": 2.843417294069188 + }, + { + "x": 1.0902403862103798, + "y": 4.253893503806654, + "heading": -1.0455959838838398, + "angularVelocity": -1.289294582965476, + "velocityX": -1.3909812376410438, + "velocityY": 0.15865401459057368, + "timestamp": 2.929839416627567 + }, + { + "x": 0.9700288352145804, + "y": 4.267604720500159, + "heading": -1.1289559376744989, + "angularVelocity": -0.9645673043305384, + "velocityX": -1.3909812376408073, + "velocityY": 0.1586540145926479, + "timestamp": 3.0162615391859458 + }, + { + "x": 0.8498172842187934, + "y": 4.281315937193773, + "heading": -1.1752680812283343, + "angularVelocity": -0.5358829681896692, + "velocityX": -1.390981237640663, + "velocityY": 0.15865401459391124, + "timestamp": 3.1026836617443245 + }, + { + "x": 0.7300683052008342, + "y": 4.29497439336173, + "heading": -1.175268130494836, + "angularVelocity": -5.700681762741162e-7, + "velocityX": -1.3856287657950983, + "velocityY": 0.1580435166786215, + "timestamp": 3.1891057843027033 + }, + { + "x": 0.6502356468080341, + "y": 4.304080031458785, + "heading": -1.1752681561486036, + "angularVelocity": -2.9684260090089324e-7, + "velocityX": -0.9237525766492534, + "velocityY": 0.10536235199389017, + "timestamp": 3.275527906861082 }, { "x": 0.6103193163871765, "y": 4.308632850646973, "heading": -1.1752681674346586, - "angularVelocity": -6.250039740114642e-8, - "velocityX": -0.5979772576242739, - "velocityY": 0.06820472484044302, - "timestamp": 3.245417746511868 + "angularVelocity": -1.3059219754587254e-7, + "velocityX": -0.4618763024929623, + "velocityY": 0.05268117761296858, + "timestamp": 3.361950029419461 }, { "x": 0.6103193163871765, "y": 4.308632850646973, "heading": -1.1752681674346586, - "angularVelocity": -8.29300949382696e-24, - "velocityX": -1.1501418857045935e-22, - "velocityY": 1.733872473382952e-23, - "timestamp": 3.35730585305628 - }, - { - "x": 0.644363601367681, - "y": 4.2767432034820265, - "heading": -1.1752681587825877, - "angularVelocity": 9.290967586148177e-8, - "velocityX": 0.36558224475309986, - "velocityY": -0.34244481273791916, - "timestamp": 3.450429316980237 - }, - { - "x": 0.712452169167196, - "y": 4.212963911179069, - "heading": -1.1752681390823587, - "angularVelocity": 2.1154957206444014e-7, - "velocityX": 0.7311644662951432, - "velocityY": -0.6848896037097378, - "timestamp": 3.543552780904194 - }, - { - "x": 0.8145850133012432, - "y": 4.117294979818902, - "heading": -1.175268101145713, - "angularVelocity": 4.07380097343134e-7, - "velocityX": 1.0967466182040557, - "velocityY": -1.027334329383292, - "timestamp": 3.6366762448281507 - }, - { - "x": 0.9369198335620701, - "y": 4.002702640501709, - "heading": -1.1572816768173708, - "angularVelocity": 0.19314599747953293, - "velocityX": 1.3136841683716076, - "velocityY": -1.2305420619959617, - "timestamp": 3.7297997087521075 - }, - { - "x": 1.0592546538469616, - "y": 3.888110301210203, - "heading": -1.110256724093758, - "angularVelocity": 0.5049742647246526, - "velocityX": 1.313684168630027, - "velocityY": -1.2305420617201315, - "timestamp": 3.8229231726760644 - }, - { - "x": 1.1815894741316775, - "y": 3.7735179619185093, - "heading": -1.0410814207725312, - "angularVelocity": 0.7428343019726364, - "velocityX": 1.3136841686281384, - "velocityY": -1.2305420617221479, - "timestamp": 3.9160466366000213 - }, - { - "x": 1.3039242944162523, - "y": 3.658925622626665, - "heading": -0.9554475532957203, - "angularVelocity": 0.9195734766346116, - "velocityX": 1.3136841686266243, - "velocityY": -1.2305420617237641, - "timestamp": 4.009170100523979 - }, - { - "x": 1.426259114700816, - "y": 3.5443332833348076, - "heading": -0.8580438926758113, - "angularVelocity": 1.0459626018577572, - "velocityX": 1.3136841686264993, - "velocityY": -1.2305420617238971, - "timestamp": 4.102293564447936 - }, - { - "x": 1.548593934985741, - "y": 3.429740944043337, - "heading": -0.752782430466053, - "angularVelocity": 1.1303430711696154, - "velocityX": 1.3136841686303846, - "velocityY": -1.2305420617197496, - "timestamp": 4.195417028371893 - }, - { - "x": 1.6709287552712397, - "y": 3.3151486047524794, - "heading": -0.6430288452135179, - "angularVelocity": 1.1785814297260035, - "velocityX": 1.313684168636549, - "velocityY": -1.2305420617131688, - "timestamp": 4.2885404922958505 - }, - { - "x": 1.7932635755573523, - "y": 3.2005562654622763, - "heading": -0.5318250037177289, - "angularVelocity": 1.1941549080111136, - "velocityX": 1.3136841686431349, - "velocityY": -1.2305420617061376, - "timestamp": 4.381663956219808 - }, - { - "x": 1.915598395843716, - "y": 3.085963926172342, - "heading": -0.4220971673663666, - "angularVelocity": 1.1783049268975236, - "velocityX": 1.3136841686458358, - "velocityY": -1.2305420617032545, - "timestamp": 4.474787420143765 - }, - { - "x": 2.0379332161298342, - "y": 2.9713715868821446, - "heading": -0.3168810175382735, - "angularVelocity": 1.1298564872330217, - "velocityX": 1.3136841686431986, - "velocityY": -1.23054206170607, - "timestamp": 4.5679108840677225 - }, - { - "x": 2.1602680364155047, - "y": 2.8567792475914704, - "heading": -0.2195335566330217, - "angularVelocity": 1.0453591050344102, - "velocityX": 1.3136841686383904, - "velocityY": -1.2305420617112026, - "timestamp": 4.66103434799168 - }, - { - "x": 2.2826028567007413, - "y": 2.742186908300333, - "heading": -0.1339605112634315, - "angularVelocity": 0.9189203425622967, - "velocityX": 1.3136841686337362, - "velocityY": -1.2305420617161718, - "timestamp": 4.754157811915637 - }, - { - "x": 2.404937676985743, - "y": 2.6275945690089433, - "heading": -0.06484983243968821, - "angularVelocity": 0.7421403361904352, - "velocityX": 1.3136841686312044, - "velocityY": -1.2305420617188745, - "timestamp": 4.8472812758395944 - }, - { - "x": 2.527272497270682, - "y": 2.5130022297174874, - "heading": -0.017897793209595524, - "angularVelocity": 0.5041912881208213, - "velocityX": 1.3136841686305336, - "velocityY": -1.230542061719591, - "timestamp": 4.940404739763552 - }, - { - "x": 2.6496073175587562, - "y": 2.3984098904293836, - "heading": -6.631716442759581e-8, - "angularVelocity": 0.1921935260810968, - "velocityX": 1.3136841686642036, - "velocityY": -1.2305420616835967, - "timestamp": 5.033528203687509 - }, - { - "x": 2.751740161678901, - "y": 2.302740959054374, - "heading": -2.8363565851208167e-8, - "angularVelocity": 4.0756214360088557e-7, - "velocityX": 1.0967466180547656, - "velocityY": -1.0273343295426693, - "timestamp": 5.126651667611466 - }, - { - "x": 2.8198287294720963, - "y": 2.238961666744671, - "heading": -8.655352303676696e-9, - "angularVelocity": 2.1163531420636224e-7, - "velocityX": 0.731164466227284, - "velocityY": -0.6848896037821823, - "timestamp": 5.219775131535424 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 5.90328227721677e-29, - "angularVelocity": 9.294491354772317e-8, - "velocityX": 0.36558224472595613, - "velocityY": -0.34244481276689687, - "timestamp": 5.312898595459381 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 3.223926333927225e-29, - "angularVelocity": 1.2163249082376476e-28, - "velocityX": -5.169137847741255e-28, - "velocityY": -3.368633280520026e-27, - "timestamp": 5.406022059383338 + "angularVelocity": 2.1702475171162593e-20, + "velocityX": 1.1792878924166797e-18, + "velocityY": 4.495937551493145e-19, + "timestamp": 3.4483721519778396 + }, + { + "x": 0.6300678197041616, + "y": 4.288384760931574, + "heading": -1.175268157994953, + "angularVelocity": 1.3017909604726324e-7, + "velocityX": 0.2723434865673803, + "velocityY": -0.27923307710503237, + "timestamp": 3.5208853725651776 + }, + { + "x": 0.6695648255579587, + "y": 4.2478885823022825, + "heading": -1.1752681380888745, + "angularVelocity": 2.7451654280492334e-7, + "velocityX": 0.5446869623757147, + "velocityY": -0.5584661431568354, + "timestamp": 3.5933985931525156 + }, + { + "x": 0.7288103326482797, + "y": 4.1871443160949395, + "heading": -1.1752681060053147, + "angularVelocity": 4.4245117776336073e-7, + "velocityX": 0.8170304202523102, + "velocityY": -0.8376991907865939, + "timestamp": 3.6659118137398536 + }, + { + "x": 0.8078043383745492, + "y": 4.106151964981228, + "heading": -1.1752680583220747, + "angularVelocity": 6.575799524254357e-7, + "velocityX": 1.0893738422654429, + "velocityY": -1.1169322015722793, + "timestamp": 3.7384250343271916 + }, + { + "x": 0.9065468349350514, + "y": 4.004911536976184, + "heading": -1.1752679847726952, + "angularVelocity": 0.0000010142892403280003, + "velocityX": 1.361717156688315, + "velocityY": -1.3961651018258798, + "timestamp": 3.8109382549145296 + }, + { + "x": 1.0179332358263944, + "y": 3.8907073460690462, + "heading": -1.1631247283774966, + "angularVelocity": 0.16746265435243096, + "velocityX": 1.5360840408017031, + "velocityY": -1.5749430239356925, + "timestamp": 3.8834514755018676 + }, + { + "x": 1.1293196368164982, + "y": 3.7765031552582196, + "heading": -1.1282612851800045, + "angularVelocity": 0.48078740559455874, + "velocityX": 1.5360840421636726, + "velocityY": -1.574943022607494, + "timestamp": 3.9559646960892056 + }, + { + "x": 1.2407060378057415, + "y": 3.662298964446554, + "heading": -1.075461795921885, + "angularVelocity": 0.7281360396139813, + "velocityX": 1.536084042151809, + "velocityY": -1.5749430226190646, + "timestamp": 4.028477916676543 + }, + { + "x": 1.3520924387937403, + "y": 3.5480947736336743, + "heading": -1.0088030071879124, + "angularVelocity": 0.9192639382729556, + "velocityX": 1.5360840421346433, + "velocityY": -1.5749430226358063, + "timestamp": 4.100991137263881 + }, + { + "x": 1.4634788397811216, + "y": 3.433890582820193, + "heading": -0.9316910575553985, + "angularVelocity": 1.0634191807773266, + "velocityX": 1.5360840421261268, + "velocityY": -1.5749430226441121, + "timestamp": 4.173504357851218 + }, + { + "x": 1.5748652407697343, + "y": 3.3196863920079127, + "heading": -0.8469508363078505, + "angularVelocity": 1.1686175370666496, + "velocityX": 1.5360840421431137, + "velocityY": -1.5749430226275445, + "timestamp": 4.246017578438556 + }, + { + "x": 1.6862516417617748, + "y": 3.2054822011989756, + "heading": -0.7569419256362268, + "angularVelocity": 1.2412758658706868, + "velocityX": 1.5360840421903812, + "velocityY": -1.574943022581443, + "timestamp": 4.318530799025893 + }, + { + "x": 1.7976380427587992, + "y": 3.091278010394899, + "heading": -0.6636838179227306, + "angularVelocity": 1.2860842058610031, + "velocityX": 1.5360840422591127, + "velocityY": -1.5749430225144072, + "timestamp": 4.391044019613231 + }, + { + "x": 1.9090244437601376, + "y": 2.97707381959503, + "heading": -0.5689879854749277, + "angularVelocity": 1.3059112763270342, + "velocityX": 1.5360840423186024, + "velocityY": -1.5749430224563852, + "timestamp": 4.4635572402005685 + }, + { + "x": 2.020410844763615, + "y": 2.862869628797248, + "heading": -0.474307979727344, + "angularVelocity": 1.3056930168139573, + "velocityX": 1.536084042348108, + "velocityY": -1.5749430224276075, + "timestamp": 4.536070460787906 + }, + { + "x": 2.1317972457669723, + "y": 2.7486654379993483, + "heading": -0.38126625391951774, + "angularVelocity": 1.2831001719991362, + "velocityX": 1.5360840423464452, + "velocityY": -1.5749430224292291, + "timestamp": 4.608583681375244 + }, + { + "x": 2.2431836467675113, + "y": 2.6344612471986997, + "heading": -0.2916725863861523, + "angularVelocity": 1.235549418543011, + "velocityX": 1.53608404230758, + "velocityY": -1.574943022467136, + "timestamp": 4.681096901962581 + }, + { + "x": 2.3545700477891547, + "y": 2.5202570564186346, + "heading": -0.20754002234173088, + "angularVelocity": 1.1602375865114323, + "velocityX": 1.5360840425986217, + "velocityY": -1.5749430221832754, + "timestamp": 4.753610122549919 + }, + { + "x": 2.4659564457927856, + "y": 2.4060528626950206, + "heading": -0.13123738107052257, + "angularVelocity": 1.052258342039986, + "velocityX": 1.5360840009784527, + "velocityY": -1.5749430627765482, + "timestamp": 4.826123343137256 + }, + { + "x": 2.5776911376512928, + "y": 2.2921894086816477, + "heading": -0.06561694187646017, + "angularVelocity": 0.9049444868474842, + "velocityX": 1.5408871782755584, + "velocityY": -1.570244061581048, + "timestamp": 4.898636563724594 + }, + { + "x": 2.707773208618164, + "y": 2.199841260910034, + "heading": -3.166218110489124e-20, + "angularVelocity": 0.9048962567790874, + "velocityX": 1.7939083371727866, + "velocityY": -1.273535322574458, + "timestamp": 4.971149784311931 + }, + { + "x": 2.9414445375132345, + "y": 2.1169413339978127, + "heading": 0.10198203037760128, + "angularVelocity": 0.9048949439058986, + "velocityX": 2.0733849215401037, + "velocityY": -0.7355778702905278, + "timestamp": 5.083850193419517 + }, + { + "x": 3.182916067581859, + "y": 2.0606724433524106, + "heading": 0.16861688305400238, + "angularVelocity": 0.5912565287391953, + "velocityX": 2.1425967481458996, + "velocityY": -0.4992784950025, + "timestamp": 5.196550602527102 + }, + { + "x": 3.424387608838958, + "y": 2.0044036007210995, + "heading": 0.20259327348567144, + "angularVelocity": 0.30147530697311203, + "velocityX": 2.1425968474221575, + "velocityY": -0.49927806896952603, + "timestamp": 5.309251011634688 + }, + { + "x": 3.665859150097934, + "y": 1.948134758097842, + "heading": 0.21381125643897023, + "angularVelocity": 0.09953808546180999, + "velocityX": 2.1425968474388095, + "velocityY": -0.49927806889806664, + "timestamp": 5.421951420742273 + }, + { + "x": 3.9073306913569463, + "y": 1.8918659154747415, + "heading": 0.20936580431528803, + "angularVelocity": -0.03944486234659494, + "velocityX": 2.142596847439134, + "velocityY": -0.49927806889667364, + "timestamp": 5.534651829849858 + }, + { + "x": 4.148802232616018, + "y": 1.8355970728518949, + "heading": 0.194339459989948, + "angularVelocity": -0.1333299891660092, + "velocityX": 2.1425968474396595, + "velocityY": -0.499278068894418, + "timestamp": 5.647352238957444 + }, + { + "x": 4.390273773874961, + "y": 1.7793282302284978, + "heading": 0.17242386429566534, + "angularVelocity": -0.19445888322689361, + "velocityX": 2.1425968474385213, + "velocityY": -0.4992780688993026, + "timestamp": 5.760052648065029 + }, + { + "x": 4.63174531513399, + "y": 1.7230593876054667, + "heading": 0.1463342525403458, + "angularVelocity": -0.2314952710635637, + "velocityX": 2.1425968474392776, + "velocityY": -0.49927806889605797, + "timestamp": 5.8727530571726145 + }, + { + "x": 4.873216856425177, + "y": 1.666790545120442, + "heading": 0.11799314125849616, + "angularVelocity": -0.25147301155613905, + "velocityX": 2.142596847724626, + "velocityY": -0.4992780676715144, + "timestamp": 5.9854534662802 + }, + { + "x": 5.114688397651964, + "y": 1.6105217023590483, + "heading": 0.09119359799766093, + "angularVelocity": -0.23779455170625943, + "velocityX": 2.142596847153193, + "velocityY": -0.4992780701237589, + "timestamp": 6.098153875387785 + }, + { + "x": 5.356159938910963, + "y": 1.5542528597358918, + "heading": 0.0654533975586384, + "angularVelocity": -0.22839491571376452, + "velocityX": 2.1425968474390182, + "velocityY": -0.4992780688971698, + "timestamp": 6.210854284495371 + }, + { + "x": 5.597631480169973, + "y": 1.4979840171127818, + "heading": 0.0412375602535577, + "angularVelocity": -0.2148691162423773, + "velocityX": 2.1425968474391146, + "velocityY": -0.4992780688967575, + "timestamp": 6.323554693602956 + }, + { + "x": 5.83910302142829, + "y": 1.4417151744866985, + "heading": 0.02039281976589829, + "angularVelocity": -0.18495709689728446, + "velocityX": 2.1425968474329675, + "velocityY": -0.4992780689231372, + "timestamp": 6.4362551027105415 + }, + { + "x": 6.080574562688075, + "y": 1.3854463318669135, + "heading": 0.0054820082027082755, + "angularVelocity": -0.1323048574647852, + "velocityX": 2.142596847445989, + "velocityY": -0.4992780688672553, + "timestamp": 6.548955511818127 + }, + { + "x": 6.322046103947842, + "y": 1.3291774892470614, + "heading": 1.7208419499617324e-8, + "angularVelocity": -0.048642157004622995, + "velocityX": 2.1425968474458243, + "velocityY": -0.49927806886784937, + "timestamp": 6.661655920925712 + }, + { + "x": 6.521663094047702, + "y": 1.2826617924406252, + "heading": 7.562434879527147e-9, + "angularVelocity": -8.558961478397665e-8, + "velocityX": 1.77121797232612, + "velocityY": -0.4127376038363034, + "timestamp": 6.774356330033298 + }, + { + "x": 6.654741095505695, + "y": 1.2516513260250568, + "heading": 2.3471800542886645e-9, + "angularVelocity": -4.6275384952626535e-8, + "velocityX": 1.1808120530508086, + "velocityY": -0.2751584192206885, + "timestamp": 6.887056739140883 + }, + { + "x": 6.721280097961426, + "y": 1.236146092414856, + "heading": 2.1151810600521028e-20, + "angularVelocity": -2.082672164849484e-8, + "velocityX": 0.5904060418468521, + "velocityY": -0.13757921318102126, + "timestamp": 6.999757148248468 + }, + { + "x": 6.721280097961426, + "y": 1.236146092414856, + "heading": 1.0022136862139773e-20, + "angularVelocity": -9.827265373654583e-21, + "velocityX": -9.566371602911776e-20, + "velocityY": -2.1047841440605612e-18, + "timestamp": 7.112457557356054 } ], "constraints": [ @@ -1617,19 +1892,13 @@ ], "type": "StopPoint" }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - }, { "scope": [ "first", - 2 + 1 ], "type": "MaxVelocity", - "velocity": 1.8 + "velocity": 1.4 }, { "scope": [ @@ -1639,8 +1908,8 @@ }, { "scope": [ - 2, - 3 + 1, + 2 ], "type": "MaxVelocity", "velocity": 1 @@ -1651,7 +1920,21 @@ "last" ], "type": "MaxVelocity", - "velocity": 1.8 + "velocity": 2.2 + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2, + 3 + ], + "type": "MaxVelocity", + "velocity": 1.4 } ], "usesControlIntervalGuessing": true, @@ -3324,369 +3607,6 @@ "usesDefaultFieldObstacles": true, "circleObstacles": [] }, - "Minor Trolling": { - "waypoints": [ - { - "x": 1.3449708223342896, - "y": 5.55503511428833, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "x": 2.9089035987854004, - "y": 6.2927398681640625, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 21 - }, - { - "x": 7.748241424560547, - "y": 7.487820148468018, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.3449708223342236, - "y": 5.555035114288299, - "heading": -2.807134781824586e-27, - "angularVelocity": -1.1824423749701342e-27, - "velocityX": 1.0706002805992919e-14, - "velocityY": 5.221920773935383e-15, - "timestamp": 0 - }, - { - "x": 1.365504399542175, - "y": 5.564891275244084, - "heading": 4.85604013060267e-16, - "angularVelocity": 7.462630925494432e-15, - "velocityX": 0.3155544519289174, - "velocityY": 0.15146681150722127, - "timestamp": 0.0650714229588586 - }, - { - "x": 1.4065715429019814, - "y": 5.584603619832141, - "heading": 1.3883704475037994e-15, - "angularVelocity": 1.3873470009876698e-14, - "velocityX": 0.6311087339497233, - "velocityY": 0.3029339715000781, - "timestamp": 0.1301428459177172 - }, - { - "x": 1.468172239047218, - "y": 5.61417217545247, - "heading": 2.6242758618538536e-15, - "angularVelocity": 1.89930596063391e-14, - "velocityX": 0.9466628105589432, - "velocityY": 0.45440155256870507, - "timestamp": 0.19521426887657578 - }, - { - "x": 1.5503064714818364, - "y": 5.653596975897173, - "heading": 4.078952886027264e-15, - "angularVelocity": 2.235508244039901e-14, - "velocityX": 1.2622166336613463, - "velocityY": 0.6058696529452037, - "timestamp": 0.2602856918354344 - }, - { - "x": 1.6529742192756731, - "y": 5.702878064004003, - "heading": 5.604758992776914e-15, - "angularVelocity": 2.3448174910470438e-14, - "velocityX": 1.5777701351146785, - "velocityY": 0.7573384116406815, - "timestamp": 0.32535711479429297 - }, - { - "x": 1.7761754548178867, - "y": 5.762015496221117, - "heading": 6.9858603583809815e-15, - "angularVelocity": 2.122439164645601e-14, - "velocityX": 1.8933232122520756, - "velocityY": 0.9088080378161675, - "timestamp": 0.39042853775315156 - }, - { - "x": 1.9199101395052205, - "y": 5.83100935135826, - "heading": 7.90901440496065e-15, - "angularVelocity": 1.418678130210118e-14, - "velocityX": 2.2088756961450784, - "velocityY": 1.0602788751184204, - "timestamp": 0.45549996071201015 - }, - { - "x": 2.08417821405006, - "y": 5.90985975023635, - "heading": 7.855703271430976e-15, - "angularVelocity": -8.192710650167832e-16, - "velocityX": 2.5244272689220897, - "velocityY": 1.211751569163871, - "timestamp": 0.5205713836708687 - }, - { - "x": 2.268979570346587, - "y": 5.998566912663145, - "heading": 5.863255908612344e-15, - "angularVelocity": -3.061939132638555e-14, - "velocityX": 2.8399771803581713, - "velocityY": 1.3632276411553363, - "timestamp": 0.5856428066297273 - }, - { - "x": 2.4743139162079135, - "y": 6.097131433441971, - "heading": -1.2204518276452846e-17, - "angularVelocity": -9.029248417989759e-14, - "velocityX": 3.1555226015453033, - "velocityY": 1.5147128539232129, - "timestamp": 0.6507142295885859 - }, - { - "x": 2.689210027815467, - "y": 6.200264987635721, - "heading": -1.4438324489222705e-17, - "angularVelocity": -3.4328528690565883e-17, - "velocityX": 3.30246522722405, - "velocityY": 1.584928521679745, - "timestamp": 0.7157856525474445 - }, - { - "x": 2.908903598785465, - "y": 6.292739868164094, - "heading": 1.5911631049954432e-27, - "angularVelocity": 2.2188425943885138e-16, - "velocityX": 3.376191283060841, - "velocityY": 1.4211289122549224, - "timestamp": 0.7808570755063031 - }, - { - "x": 3.20151198928919, - "y": 6.37574601842352, - "heading": 1.0232489607381783e-17, - "angularVelocity": 1.232355609804863e-16, - "velocityX": 3.524045521992114, - "velocityY": 0.9996892147076375, - "timestamp": 0.8638890308762548 - }, - { - "x": 3.4969572289513846, - "y": 6.448008052427147, - "heading": 3.0679029856294505e-17, - "angularVelocity": 2.4624905145380905e-16, - "velocityX": 3.5582112735491953, - "velocityY": 0.8702918494652072, - "timestamp": 0.9469209862462065 - }, - { - "x": 3.7924024756754835, - "y": 6.520270057557982, - "heading": 7.697011069326025e-17, - "angularVelocity": 5.575092219866701e-16, - "velocityX": 3.558211358599641, - "velocityY": 0.8702915017341162, - "timestamp": 1.0299529416161581 - }, - { - "x": 4.08784754370095, - "y": 6.592532793299271, - "heading": 7.158177464139197e-17, - "angularVelocity": -6.489472669278147e-17, - "velocityX": 3.558209206432672, - "velocityY": 0.8703003008819995, - "timestamp": 1.1129848969861098 - }, - { - "x": 4.383292663763094, - "y": 6.664795316289082, - "heading": 1.0134319525693488e-16, - "angularVelocity": 3.5843333430253186e-16, - "velocityX": 3.558209833139292, - "velocityY": 0.8702977385976698, - "timestamp": 1.1960168523560615 - }, - { - "x": 4.6787378051682404, - "y": 6.737057752018029, - "heading": 1.1308261583790461e-16, - "angularVelocity": 1.413843685032946e-16, - "velocityX": 3.5582100901849465, - "velocityY": 0.8702966876665797, - "timestamp": 1.2790488077260131 - }, - { - "x": 4.974182963018651, - "y": 6.809320120510445, - "heading": 1.2605550724777224e-16, - "angularVelocity": 1.5623974352141762e-16, - "velocityX": 3.55821028824441, - "velocityY": 0.8702958778996337, - "timestamp": 1.3620807630959648 - }, - { - "x": 5.269628134219715, - "y": 6.8815824344186165, - "heading": 1.5997529005985398e-16, - "angularVelocity": 4.0851480206826883e-16, - "velocityX": 3.558210449033731, - "velocityY": 0.8702952205112429, - "timestamp": 1.4451127184659165 - }, - { - "x": 5.5650733160418735, - "y": 6.953844704902323, - "heading": 1.4460998185548282e-16, - "angularVelocity": -1.8505294900854803e-16, - "velocityX": 3.5582105769494934, - "velocityY": 0.8702946975262731, - "timestamp": 1.5281446738358682 - }, - { - "x": 5.860518506022417, - "y": 7.026106942030369, - "heading": 1.5264609117120294e-16, - "angularVelocity": 9.678333216567522e-17, - "velocityX": 3.558210675205449, - "velocityY": 0.870294295805505, - "timestamp": 1.6111766292058198 - }, - { - "x": 6.155963701926025, - "y": 7.098369154941873, - "heading": 1.7041919564100958e-16, - "angularVelocity": 2.1405137807285622e-16, - "velocityX": 3.55821074654022, - "velocityY": 0.8702940041522215, - "timestamp": 1.6942085845757715 - }, - { - "x": 6.451408867259249, - "y": 7.170631492835253, - "heading": 1.605762513625872e-16, - "angularVelocity": -1.1854405001861668e-16, - "velocityX": 3.5582103783640893, - "velocityY": 0.87029550937845, - "timestamp": 1.7772405399457232 - }, - { - "x": 6.739593860452669, - "y": 7.241117888394939, - "heading": 1.1916521054214916e-16, - "angularVelocity": -4.987361868954257e-16, - "velocityX": 3.470772089003605, - "velocityY": 0.8489068485215399, - "timestamp": 1.8602724953156748 - }, - { - "x": 6.9917557416698095, - "y": 7.302793471475189, - "heading": 9.153953371814369e-17, - "angularVelocity": -3.3271138440380534e-16, - "velocityX": 3.036925724483133, - "velocityY": 0.7427933354749008, - "timestamp": 1.9433044506856265 - }, - { - "x": 7.207894502741401, - "y": 7.355658247620443, - "heading": 6.912972849883134e-17, - "angularVelocity": -2.6989374293489497e-16, - "velocityX": 2.6030792615756186, - "velocityY": 0.6366798892030741, - "timestamp": 2.026336406055578 - }, - { - "x": 7.388010140347625, - "y": 7.399712221155376, - "heading": 4.649348498129856e-17, - "angularVelocity": -2.726208652687308e-16, - "velocityX": 2.169232758685661, - "velocityY": 0.5305664950156702, - "timestamp": 2.10936836142553 - }, - { - "x": 7.532102652510152, - "y": 7.434955395544809, - "heading": 2.777377355142468e-17, - "angularVelocity": -2.2545189195214148e-16, - "velocityX": 1.7353862319695992, - "velocityY": 0.4244531425570638, - "timestamp": 2.1924003167954815 - }, - { - "x": 7.640172037880081, - "y": 7.461387773529582, - "heading": 1.6169989490516543e-17, - "angularVelocity": -1.3975082250070357e-16, - "velocityX": 1.3015396890079767, - "velocityY": 0.3183398231079052, - "timestamp": 2.275432272165433 - }, - { - "x": 7.712218295476737, - "y": 7.479009357269755, - "heading": 5.705869423341737e-18, - "angularVelocity": -1.2602521546942366e-16, - "velocityX": 0.8676931342355209, - "velocityY": 0.21222652967353675, - "timestamp": 2.358464227535385 - }, - { - "x": 7.7482414245605495, - "y": 7.4878201484680185, - "heading": 2.4801573105040754e-27, - "angularVelocity": -6.871895759912153e-17, - "velocityX": 0.43384657055603204, - "velocityY": 0.10611325674564437, - "timestamp": 2.4414961829053365 - }, - { - "x": 7.748241424560549, - "y": 7.487820148468018, - "heading": 1.21542731991438e-27, - "angularVelocity": -5.937794751127806e-28, - "velocityX": 8.50988429287625e-16, - "velocityY": 2.197763699220458e-16, - "timestamp": 2.524528138275288 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "WptZeroVelocity" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, "amp exit": { "waypoints": [ { @@ -4100,6 +4020,451 @@ "last" ], "type": "StopPoint" + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 2.3 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "source exit": { + "waypoints": [ + { + "x": 0.6313003897666931, + "y": 4.255156517028809, + "heading": -1.2897, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "x": 3.0872981548309326, + "y": 2.0589683055877686, + "heading": -0.4081492403189991, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.6313003897666931, + "y": 4.255156517028809, + "heading": -1.2897, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 4.938118129828658e-32, + "timestamp": 0 + }, + { + "x": 0.6547225060554301, + "y": 4.233901437352739, + "heading": -1.2896998806389426, + "angularVelocity": 0.0000015565956410580199, + "velocityX": 0.3054494072906231, + "velocityY": -0.2771889358303716, + "timestamp": 0.0766808372505756 + }, + { + "x": 0.7015667377724408, + "y": 4.191391278797934, + "heading": -1.289699622292801, + "angularVelocity": 0.000003369109545120088, + "velocityX": 0.6108988033598843, + "velocityY": -0.5543778612626656, + "timestamp": 0.1533616745011512 + }, + { + "x": 0.7718330834830761, + "y": 4.127626042692661, + "heading": -1.289699192032996, + "angularVelocity": 0.000005611047281688477, + "velocityX": 0.9163481807197896, + "velocityY": -0.8315667693729352, + "timestamp": 0.2300425117517268 + }, + { + "x": 0.8655215403161615, + "y": 4.042605731691311, + "heading": -1.2896985232175402, + "angularVelocity": 0.000008722067696213578, + "velocityX": 1.2217975206365133, + "velocityY": -1.1087556428671073, + "timestamp": 0.3067233490023024 + }, + { + "x": 0.9826320996478911, + "y": 3.936330353745298, + "heading": -1.2896974106735322, + "angularVelocity": 0.000014508761822433673, + "velocityX": 1.5272467480896021, + "velocityY": -1.3859444126663505, + "timestamp": 0.38340418625287803 + }, + { + "x": 1.113237043415476, + "y": 3.8178091039826962, + "heading": -1.2790397450792215, + "angularVelocity": 0.1389873399462232, + "velocityX": 1.7032279308687956, + "velocityY": -1.5456436576885717, + "timestamp": 0.46008502350345365 + }, + { + "x": 1.2438419873099618, + "y": 3.6992878543599033, + "heading": -1.2518722162171183, + "angularVelocity": 0.3542935867187001, + "velocityX": 1.7032279325237185, + "velocityY": -1.5456436558653144, + "timestamp": 0.5367658607540292 + }, + { + "x": 1.3744469312210372, + "y": 3.5807666047553917, + "heading": -1.212078387332647, + "angularVelocity": 0.5189540217777049, + "velocityX": 1.7032279327400672, + "velocityY": -1.5456436556269055, + "timestamp": 0.6134466980046048 + }, + { + "x": 1.5050518752146576, + "y": 3.4622453552418415, + "heading": -1.162751180302459, + "angularVelocity": 0.6432794528441913, + "velocityX": 1.70322793381654, + "velocityY": -1.5456436544406769, + "timestamp": 0.6901275352551804 + }, + { + "x": 1.6356568192666512, + "y": 3.343724105792617, + "heading": -1.1063318116829663, + "angularVelocity": 0.7357688132058888, + "velocityX": 1.7032279345777896, + "velocityY": -1.5456436536018043, + "timestamp": 0.7668083725057561 + }, + { + "x": 1.7662617633452586, + "y": 3.22520285637272, + "heading": -1.044747551464474, + "angularVelocity": 0.8031245148934248, + "velocityX": 1.703227934924863, + "velocityY": -1.54564365321933, + "timestamp": 0.8434892097563317 + }, + { + "x": 1.8968667073747523, + "y": 3.1066816068987038, + "heading": -0.9795374269556493, + "angularVelocity": 0.8504096570533181, + "velocityX": 1.7032279342843641, + "velocityY": -1.5456436539251106, + "timestamp": 0.9201700470069073 + }, + { + "x": 2.0274716511557966, + "y": 2.988160357150909, + "heading": -0.911962353611442, + "angularVelocity": 0.8812511152347953, + "velocityX": 1.7032279310443175, + "velocityY": -1.5456436574954706, + "timestamp": 0.9968508842574829 + }, + { + "x": 2.158076594153494, + "y": 2.869639106539904, + "heading": -0.8431013334736036, + "angularVelocity": 0.8980212345989256, + "velocityX": 1.7032279208286418, + "velocityY": -1.5456436687526536, + "timestamp": 1.0735317215080584 + }, + { + "x": 2.288681534488559, + "y": 2.751117852994802, + "heading": -0.7739408419480853, + "angularVelocity": 0.9019266612794504, + "velocityX": 1.7032278861050756, + "velocityY": -1.5456437070164089, + "timestamp": 1.150212558758634 + }, + { + "x": 2.419286472128144, + "y": 2.632596596479403, + "heading": -0.705434667355344, + "angularVelocity": 0.8933936697754613, + "velocityX": 1.703227850953138, + "velocityY": -1.5456437457522652, + "timestamp": 1.2268933960092097 + }, + { + "x": 2.5498914265636636, + "y": 2.5140753584723274, + "heading": -0.6385807654336066, + "angularVelocity": 0.8718462697959073, + "velocityX": 1.7032280699900582, + "velocityY": -1.5456435043839587, + "timestamp": 1.3035742332597853 + }, + { + "x": 2.680496509530869, + "y": 2.3955542621014185, + "heading": -0.5745105008427962, + "angularVelocity": 0.8355446665435626, + "velocityX": 1.7032297461804866, + "velocityY": -1.5456416572971043, + "timestamp": 1.380255070510361 + }, + { + "x": 2.8111011984005865, + "y": 2.277032731452427, + "heading": -0.5145723016278465, + "angularVelocity": 0.7816581216906595, + "velocityX": 1.7032246067284333, + "velocityY": -1.5456473207470263, + "timestamp": 1.4569359077609365 + }, + { + "x": 2.9417192752167938, + "y": 2.158525955314311, + "heading": -0.46033957015895777, + "angularVelocity": 0.7072527298009322, + "velocityX": 1.7033991998467222, + "velocityY": -1.5454549061698755, + "timestamp": 1.5336167450115121 + }, + { + "x": 3.0872981548309326, + "y": 2.058968305587769, + "heading": -0.4081492403189991, + "angularVelocity": 0.6806176316178785, + "velocityX": 1.8985040439558556, + "velocityY": -1.2983380632792187, + "timestamp": 1.6102975822620877 + }, + { + "x": 3.288618968752435, + "y": 1.9787858722638514, + "heading": -0.3440273039506474, + "angularVelocity": 0.6805713802306704, + "velocityX": 2.1367599289661108, + "velocityY": -0.8510327729965853, + "timestamp": 1.70451537958802 + }, + { + "x": 3.4973190456299355, + "y": 1.9204457429666257, + "heading": -0.2885241078330584, + "angularVelocity": 0.5890946051900753, + "velocityX": 2.215081256416269, + "velocityY": -0.6192049798766697, + "timestamp": 1.798733176913952 + }, + { + "x": 3.7060194347295887, + "y": 1.8621067305954644, + "heading": -0.24314566154797057, + "angularVelocity": 0.48163348722870364, + "velocityX": 2.215084570250411, + "velocityY": -0.6191931251517853, + "timestamp": 1.8929509742398842 + }, + { + "x": 3.914719841235457, + "y": 1.8037677804928323, + "heading": -0.20514297433349235, + "angularVelocity": 0.40334934898785263, + "velocityX": 2.2150847549948707, + "velocityY": -0.6191924642519187, + "timestamp": 1.9871687715658164 + }, + { + "x": 4.123420253984318, + "y": 1.7454288527237616, + "heading": -0.17255263081013517, + "angularVelocity": 0.34590432432439966, + "velocityX": 2.215084821256147, + "velocityY": -0.6191922272100663, + "timestamp": 2.0813865688917486 + }, + { + "x": 4.332120665934124, + "y": 1.68708992209617, + "heading": -0.14399111470382664, + "angularVelocity": 0.3031435346286237, + "velocityX": 2.215084812775212, + "velocityY": -0.6191922575495665, + "timestamp": 2.175604366217681 + }, + { + "x": 4.540821077336117, + "y": 1.6287509895088441, + "heading": -0.11850572029946865, + "angularVelocity": 0.27049448328946984, + "velocityX": 2.2150848069609004, + "velocityY": -0.6191922783496103, + "timestamp": 2.2698221635436133 + }, + { + "x": 4.749521488592162, + "y": 1.5704120563993975, + "heading": -0.09547134573973612, + "angularVelocity": 0.2444800792788083, + "velocityX": 2.2150848054118457, + "velocityY": -0.6191922838912438, + "timestamp": 2.3640399608695457 + }, + { + "x": 4.958221899850797, + "y": 1.5120731232992075, + "heading": -0.07452259619207273, + "angularVelocity": 0.22234386859179237, + "velocityX": 2.21508480543933, + "velocityY": -0.6191922837929981, + "timestamp": 2.458257758195478 + }, + { + "x": 5.166922311144001, + "y": 1.4537341903226784, + "heading": -0.055514581735497524, + "angularVelocity": 0.2017454769272586, + "velocityX": 2.2150848058062342, + "velocityY": -0.619192282480497, + "timestamp": 2.5524755555214105 + }, + { + "x": 5.3756227224674955, + "y": 1.3953952574545083, + "heading": -0.038508342854423575, + "angularVelocity": 0.18049921950761413, + "velocityX": 2.215084806127732, + "velocityY": -0.619192281330405, + "timestamp": 2.646693352847343 + }, + { + "x": 5.5843231338114325, + "y": 1.3370563246594693, + "heading": -0.023778548081694613, + "angularVelocity": 0.15633771103522998, + "velocityX": 2.2150848063447075, + "velocityY": -0.6191922805542137, + "timestamp": 2.7409111501732752 + }, + { + "x": 5.793023545170744, + "y": 1.278717391919428, + "heading": -0.01184315101724952, + "angularVelocity": 0.12667879533577564, + "velocityX": 2.2150848065078788, + "velocityY": -0.619192279970488, + "timestamp": 2.8351289474992076 + }, + { + "x": 6.001723956538716, + "y": 1.2203784592103721, + "heading": -0.0035175510847978773, + "angularVelocity": 0.08836546988728466, + "velocityX": 2.2150848065998088, + "velocityY": -0.6191922796416123, + "timestamp": 2.92934674482514 + }, + { + "x": 6.210424367778128, + "y": 1.1620395260406786, + "heading": 9.95199444319593e-7, + "angularVelocity": 0.03734481577905508, + "velocityX": 2.2150848052352985, + "velocityY": -0.619192284530687, + "timestamp": 3.0235645421510724 + }, + { + "x": 6.394372705631795, + "y": 1.1106196478377304, + "heading": 5.066604047984432e-7, + "angularVelocity": -0.00000518520965030981, + "velocityX": 1.952373575634825, + "velocityY": -0.545755469373464, + "timestamp": 3.117782339477005 + }, + { + "x": 6.532333969364687, + "y": 1.0720547363320128, + "heading": 2.3088395723940584e-7, + "angularVelocity": -0.000002927010090872124, + "velocityX": 1.4642802914998674, + "velocityY": -0.4093166323163768, + "timestamp": 3.212000136802937 + }, + { + "x": 6.624308148185072, + "y": 1.0463447945031064, + "heading": 7.204386582174162e-8, + "angularVelocity": -0.00000168588200609885, + "velocityX": 0.9761868928246619, + "velocityY": -0.27287776363489813, + "timestamp": 3.3062179341288696 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": -7.646523997427724e-7, + "velocityX": 0.4880934559605067, + "velocityY": -0.1364388844420419, + "timestamp": 3.400435731454802 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": 0, + "velocityX": -8.306900544678907e-28, + "velocityY": -1.1351138938300595e-27, + "timestamp": 3.4946535287807343 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + "first", + "last" + ], + "type": "MaxVelocity", + "velocity": 2.3 } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/2 piece left.1.traj b/src/main/deploy/choreo/2 piece left.1.traj index 4017f3c..1f477c4 100644 --- a/src/main/deploy/choreo/2 piece left.1.traj +++ b/src/main/deploy/choreo/2 piece left.1.traj @@ -4,217 +4,154 @@ "x": 0.6235248446464539, "y": 6.866034507751465, "heading": 1.3037, - "angularVelocity": -1.8232741591844444e-18, - "velocityX": 7.12403473371085e-18, - "velocityY": -2.6478556818989226e-17, + "angularVelocity": -4.322191282064769e-19, + "velocityX": -7.217119807256306e-20, + "velocityY": -5.737284120005055e-18, "timestamp": 0 }, { - "x": 0.6371647021269348, - "y": 6.867787221776129, - "heading": 1.30369672515582, - "angularVelocity": -0.00006476812380618724, - "velocityX": 0.26976183581085494, - "velocityY": 0.03466424437509273, - "timestamp": 0.05056259140393951 - }, - { - "x": 0.6644444154536387, - "y": 6.871292657837969, - "heading": 1.3036900282942052, - "angularVelocity": -0.00013244696185552165, - "velocityX": 0.5395236393002303, - "velocityY": 0.06932864721736587, - "timestamp": 0.10112518280787902 - }, - { - "x": 0.7053639821708213, - "y": 6.876550827985717, - "heading": 1.303679688712071, - "angularVelocity": -0.0002044907479422861, - "velocityX": 0.8092853942211979, - "velocityY": 0.10399328835306255, - "timestamp": 0.15168777421181853 - }, - { - "x": 0.7599233981740878, - "y": 6.8835617523794905, - "heading": 1.303665338742152, - "angularVelocity": -0.00028380606137315504, - "velocityX": 1.0790470679676354, - "velocityY": 0.1386583282048117, - "timestamp": 0.20225036561575804 - }, - { - "x": 0.8281226552122382, - "y": 6.892325471630372, - "heading": 1.303646243766396, - "angularVelocity": -0.0003776502593285092, - "velocityX": 1.3488085785262331, - "velocityY": 0.17332417124092056, - "timestamp": 0.2528129570196975 - }, - { - "x": 0.9099617281962122, - "y": 6.902842109861635, - "heading": 1.3036202050692922, - "angularVelocity": -0.0005149794814836402, - "velocityX": 1.618569592886762, - "velocityY": 0.20799246912103908, - "timestamp": 0.303375548423637 - }, - { - "x": 0.9952174677554063, - "y": 6.913794817693266, - "heading": 1.2650372111423984, - "angularVelocity": -0.7630739021791462, - "velocityX": 1.686142604481928, - "velocityY": 0.21661682139925917, - "timestamp": 0.3539381398275765 - }, - { - "x": 1.0804727987833038, - "y": 6.9247507050181865, - "heading": 1.186135227119023, - "angularVelocity": -1.5604814119006516, - "velocityX": 1.6861345247675534, - "velocityY": 0.21667970372394155, - "timestamp": 0.404500731231516 - }, - { - "x": 1.1657282386661474, - "y": 6.935705745230898, - "heading": 1.0686962835514935, - "angularVelocity": -2.322644870578755, - "velocityX": 1.6861366776426912, - "velocityY": 0.21666294998988928, - "timestamp": 0.45506332263545546 - }, - { - "x": 1.2509836774698653, - "y": 6.946660793838387, - "heading": 0.9157151714041298, - "angularVelocity": -3.0255789487768316, - "velocityX": 1.6861366563003188, - "velocityY": 0.21666311601734475, - "timestamp": 0.5056259140393949 - }, - { - "x": 1.336239105659407, - "y": 6.957615925046211, - "heading": 0.7330874583136245, - "angularVelocity": -3.61191363060312, - "velocityX": 1.6861364463787976, - "velocityY": 0.21666474964277663, - "timestamp": 0.5561885054433344 - }, - { - "x": 1.4214945304345679, - "y": 6.96857108283117, - "heading": 0.5355420616291058, - "angularVelocity": -3.9069476306376054, - "velocityX": 1.6861363788509953, - "velocityY": 0.21666527527119953, - "timestamp": 0.6067510968472739 - }, - { - "x": 1.5067500117706687, - "y": 6.979525800477682, - "heading": 0.35218632067901057, - "angularVelocity": -3.6263121778170837, - "velocityX": 1.68613749748313, - "velocityY": 0.21665657044742084, - "timestamp": 0.6573136882512134 - }, - { - "x": 1.5920056795979993, - "y": 6.990479066662584, - "heading": 0.19830964964916573, - "angularVelocity": -3.043290835324073, - "velocityX": 1.6861411858073423, - "velocityY": 0.21662786421283695, - "timestamp": 0.7078762796551529 - }, - { - "x": 1.677260793789504, - "y": 7.001436641396165, - "heading": 0.07990683643812377, - "angularVelocity": -2.341707770971106, - "velocityX": 1.686130236292877, - "velocityY": 0.21671307639360837, - "timestamp": 0.7584388710590924 + "x": 0.6479605438901133, + "y": 6.869173153614982, + "heading": 1.3036976853929116, + "angularVelocity": -0.00003420125047541537, + "velocityX": 0.3610683967062306, + "velocityY": 0.04637746677385236, + "timestamp": 0.06767609535082215 + }, + { + "x": 0.6968319343921952, + "y": 6.875450489923095, + "heading": 1.3036923603900246, + "angularVelocity": -0.00007868366015476742, + "velocityX": 0.7221366754204779, + "velocityY": 0.0927555922895904, + "timestamp": 0.1353521907016443 + }, + { + "x": 0.7701389910336393, + "y": 6.884866659045138, + "heading": 1.3036818951032874, + "angularVelocity": -0.0001546378626426958, + "velocityX": 1.0832045829687984, + "velocityY": 0.13913582149253725, + "timestamp": 0.20302828605246648 + }, + { + "x": 0.8641137016555179, + "y": 6.896935578985996, + "heading": 1.278165357348036, + "angularVelocity": -0.377039154268251, + "velocityX": 1.3885953398275834, + "velocityY": 0.17833357374260927, + "timestamp": 0.2707043814032886 + }, + { + "x": 0.9580882030461527, + "y": 6.909006127999201, + "heading": 1.179816306963164, + "angularVelocity": -1.4532317485966955, + "velocityX": 1.3885922481710244, + "velocityY": 0.17835764534927084, + "timestamp": 0.33838047675411076 + }, + { + "x": 1.052062741771001, + "y": 6.921076386345661, + "heading": 1.012746961397015, + "angularVelocity": -2.4686611232531686, + "velocityX": 1.3885927998313032, + "velocityY": 0.17835335038004493, + "timestamp": 0.4060565721049329 + }, + { + "x": 1.1460372822563507, + "y": 6.933146630985501, + "heading": 0.7854621166033714, + "angularVelocity": -3.358421368955095, + "velocityX": 1.3885928258449378, + "velocityY": 0.17835314784740675, + "timestamp": 0.47373266745575504 + }, + { + "x": 1.2400118226682206, + "y": 6.945216876197425, + "heading": 0.5235178410871962, + "angularVelocity": -3.870558343508111, + "velocityX": 1.388592824759185, + "velocityY": 0.17835315630066892, + "timestamp": 0.5414087628065772 + }, + { + "x": 1.3339863634408795, + "y": 6.9572871186003775, + "heading": 0.29518267414695665, + "angularVelocity": -3.3739412085846014, + "velocityX": 1.3885928300902979, + "velocityY": 0.17835311479456636, + "timestamp": 0.6090848581573994 + }, + { + "x": 1.4279609034991196, + "y": 6.9693573665655375, + "heading": 0.1268000189106551, + "angularVelocity": -2.4880669365369346, + "velocityX": 1.388592819533854, + "velocityY": 0.1783531969832136, + "timestamp": 0.6767609535082216 + }, + { + "x": 1.5219354277832027, + "y": 6.981427737341889, + "heading": 0.027024429838266126, + "angularVelocity": -1.4743106639821368, + "velocityX": 1.38859258645071, + "velocityY": 0.1783550116740703, + "timestamp": 0.7444370488590438 + }, + { + "x": 1.615910044720068, + "y": 6.993497386738595, + "heading": 0.000018019004787293556, + "angularVelocity": -0.3990539154701806, + "velocityX": 1.3885939555128846, + "velocityY": 0.17834435237639756, + "timestamp": 0.812113144209866 + }, + { + "x": 1.6892171360383907, + "y": 7.002913285881713, + "heading": 0.000007615738994617179, + "angularVelocity": -0.00015372142465942866, + "velocityX": 1.083205095363587, + "velocityY": 0.13913183221207334, + "timestamp": 0.8797892395606882 + }, + { + "x": 1.7380885418555654, + "y": 7.0091905029584325, + "heading": 0.0000023106612364746317, + "angularVelocity": -0.00007838924114270227, + "velocityX": 0.7221369017203642, + "velocityY": 0.09275383049478103, + "timestamp": 0.9474653349115104 }, { "x": 1.7625242471694946, "y": 7.0123291015625, - "heading": -3.350712590761302e-17, - "angularVelocity": -1.5803548477125327, - "velocityX": 1.6862951643207778, - "velocityY": 0.21542527516670237, - "timestamp": 0.8090014624630318 - }, - { - "x": 1.894716655716008, - "y": 7.013428377195941, - "heading": -0.044649055027065385, - "angularVelocity": -0.5741688805746129, - "velocityX": 1.6999411788133891, - "velocityY": 0.01413624229031357, - "timestamp": 0.886764393436201 - }, - { - "x": 2.02690952861163, - "y": 7.014470317880742, - "heading": -0.00008383158388879456, - "angularVelocity": 0.5730908401401845, - "velocityX": 1.6999471501560728, - "velocityY": 0.013398937922744884, - "timestamp": 0.9645273244093701 - }, - { - "x": 2.1570160896928328, - "y": 7.015495222736415, - "heading": -0.0000539532537599614, - "angularVelocity": 0.00038422330222239603, - "velocityX": 1.673118019768238, - "velocityY": 0.013179864015495759, - "timestamp": 1.042290255382539 - }, - { - "x": 2.2545960183213234, - "y": 7.01626391842549, - "heading": -0.00002792078971867657, - "angularVelocity": 0.00033476701193603026, - "velocityX": 1.254838615357224, - "velocityY": 0.009885117233306755, - "timestamp": 1.1200531863557082 - }, - { - "x": 2.319649306326726, - "y": 7.016776387311738, - "heading": -0.000009516691596222152, - "angularVelocity": 0.00023666929592428788, - "velocityX": 0.8365591058784542, - "velocityY": 0.006590143656300914, - "timestamp": 1.1978161173288773 - }, - { - "x": 2.3521759510040283, - "y": 7.017032623291016, - "heading": 5.0938422434116983e-17, - "angularVelocity": 0.000122380824348686, - "velocityX": 0.4182795616143265, - "velocityY": 0.0032950915824648324, - "timestamp": 1.2755790483020464 - }, - { - "x": 2.3521759510040283, - "y": 7.017032623291016, - "heading": 2.5675724430541464e-17, - "angularVelocity": -4.4489652123602634e-18, - "velocityX": -3.43042618417509e-18, - "velocityY": 7.376106260350096e-18, - "timestamp": 1.3533419792752155 + "heading": 9.925554069905119e-19, + "angularVelocity": -0.00003414294551807602, + "velocityX": 0.36106848640215494, + "velocityY": 0.04637676845564921, + "timestamp": 1.0151414302623325 + }, + { + "x": 1.7625242471694946, + "y": 7.0123291015625, + "heading": 4.930172708842328e-19, + "angularVelocity": -9.633639100834258e-20, + "velocityX": -4.2792851430038895e-18, + "velocityY": -3.4270302349667407e-18, + "timestamp": 1.0828175256131547 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.2.traj b/src/main/deploy/choreo/2 piece left.2.traj index afd3d19..83f1e50 100644 --- a/src/main/deploy/choreo/2 piece left.2.traj +++ b/src/main/deploy/choreo/2 piece left.2.traj @@ -1,175 +1,76 @@ { "samples": [ { - "x": 2.3521759510040283, - "y": 7.017032623291016, - "heading": 2.5675724430541464e-17, - "angularVelocity": -4.4489652123602634e-18, - "velocityX": -3.43042618417509e-18, - "velocityY": 7.376106260350096e-18, + "x": 1.7625242471694946, + "y": 7.0123291015625, + "heading": 4.930172708842328e-19, + "angularVelocity": -9.633639100834258e-20, + "velocityX": -4.2792851430038895e-18, + "velocityY": -3.4270302349667407e-18, "timestamp": 0 }, { - "x": 2.289226350907918, - "y": 7.0088843634816955, - "heading": 3.162897756010949e-9, - "angularVelocity": 2.911644384649021e-8, - "velocityX": -0.5794902817179733, - "velocityY": -0.07500980729353342, - "timestamp": 0.10862925933716783 - }, - { - "x": 2.181495855279594, - "y": 6.9949396193307365, - "heading": 0.012994469205035452, - "angularVelocity": 0.1196221544860667, - "velocityX": -0.9917263201983659, - "velocityY": -0.12837005642905872, - "timestamp": 0.21725851867433565 - }, - { - "x": 2.0737653596512815, - "y": 6.980994875179695, - "heading": 0.05695780222793572, - "angularVelocity": 0.40470986630264244, - "velocityX": -0.9917263201982718, - "velocityY": -0.1283700564298165, - "timestamp": 0.3258877780115035 - }, - { - "x": 1.9660348640229757, - "y": 6.967050131028595, - "heading": 0.1246841048489742, - "angularVelocity": 0.6234628039838408, - "velocityX": -0.9917263201982012, - "velocityY": -0.12837005643036203, - "timestamp": 0.4345170373486713 - }, - { - "x": 1.858304368394671, - "y": 6.953105386877488, - "heading": 0.21021952796955365, - "angularVelocity": 0.7874068519153866, - "velocityX": -0.9917263201981937, - "velocityY": -0.12837005643041954, - "timestamp": 0.5431462966858391 - }, - { - "x": 1.7505738727663673, - "y": 6.939160642726373, - "heading": 0.30861254552236733, - "angularVelocity": 0.905769017971645, - "velocityX": -0.9917263201981846, - "velocityY": -0.1283700564304895, - "timestamp": 0.651775556023007 - }, - { - "x": 1.642843377138065, - "y": 6.9252158985752486, - "heading": 0.4156678969757047, - "angularVelocity": 0.9855111975039318, - "velocityX": -0.9917263201981728, - "velocityY": -0.12837005643058202, - "timestamp": 0.7604048153601748 + "x": 1.83126410757397, + "y": 7.012877424284074, + "heading": -8.433317163645367e-17, + "angularVelocity": -7.503648365992863e-16, + "velocityX": 0.6080666216218581, + "velocityY": 0.00485041347050172, + "timestamp": 0.11304659384382854 }, { - "x": 1.5351128815097632, - "y": 6.911271154424119, - "heading": 0.5277146421232866, - "angularVelocity": 1.0314600857196923, - "velocityX": -0.9917263201981663, - "velocityY": -0.12837005643063132, - "timestamp": 0.8690340746973426 + "x": 1.9443071033303654, + "y": 7.013779143355416, + "heading": -9.171422334116161e-15, + "angularVelocity": -8.038357329937531e-14, + "velocityX": 0.9999681716422337, + "velocityY": 0.007976525790660745, + "timestamp": 0.22609318768765707 }, { - "x": 1.4273823858814612, - "y": 6.897326410272993, - "heading": 0.6413892672091689, - "angularVelocity": 1.0464457345976608, - "velocityX": -0.9917263201981706, - "velocityY": -0.12837005643059782, - "timestamp": 0.9776633340345104 + "x": 2.0573500990867615, + "y": 7.014680862426758, + "heading": 4.721221727458621e-16, + "angularVelocity": 8.530592721947881e-14, + "velocityX": 0.9999681716422396, + "velocityY": 0.007976525790660303, + "timestamp": 0.3391397815314856 }, { - "x": 1.3196518902531575, - "y": 6.883381666121877, - "heading": 0.7534304016146443, - "angularVelocity": 1.0314084353435349, - "velocityX": -0.9917263201981834, - "velocityY": -0.12837005643049992, - "timestamp": 1.0862925933716783 + "x": 2.1703930948431576, + "y": 7.0155825814981, + "heading": 9.192667005316373e-15, + "angularVelocity": 7.714115512942701e-14, + "velocityX": 0.9999681716422396, + "velocityY": 0.00797652579066031, + "timestamp": 0.45218637537531414 }, { - "x": 1.2119213946248526, - "y": 6.869436921970773, - "heading": 0.8604745712214651, - "angularVelocity": 0.9854082616413032, - "velocityX": -0.9917263201981971, - "velocityY": -0.12837005643039434, - "timestamp": 1.194921852708846 + "x": 2.283436090599553, + "y": 7.0164843005694415, + "heading": 8.651637279707332e-17, + "angularVelocity": -8.055218934866491e-14, + "velocityX": 0.9999681716422337, + "velocityY": 0.007976525790660681, + "timestamp": 0.5652329692191427 }, { - "x": 1.1041908989965465, - "y": 6.855492177819676, - "heading": 0.9588506837381976, - "angularVelocity": 0.9056133965839597, - "velocityX": -0.9917263201982054, - "velocityY": -0.12837005643033003, - "timestamp": 1.303551112046014 - }, - { - "x": 0.9964604033682406, - "y": 6.8415474336685795, - "heading": 1.0443629237764005, - "angularVelocity": 0.7871934372008065, - "velocityX": -0.9917263201982054, - "velocityY": -0.12837005643032937, - "timestamp": 1.4121803713831818 - }, - { - "x": 0.8887299077399343, - "y": 6.827602689517485, - "heading": 1.1120586417031153, - "angularVelocity": 0.6231812528206429, - "velocityX": -0.991726320198208, - "velocityY": -0.12837005643030963, - "timestamp": 1.5208096307203496 - }, - { - "x": 0.7809994121116139, - "y": 6.813657945366499, - "heading": 1.1559822740636867, - "angularVelocity": 0.4043443969754001, - "velocityX": -0.9917263201983385, - "velocityY": -0.12837005642930135, - "timestamp": 1.6294388900575174 - }, - { - "x": 0.6732689164832739, - "y": 6.799713201215669, - "heading": 1.1689257207199144, - "angularVelocity": 0.11915248925755068, - "velocityX": -0.9917263201985188, - "velocityY": -0.12837005642787808, - "timestamp": 1.7380681493946852 - }, - { - "x": 0.6103193163871765, - "y": 6.79156494140625, - "heading": 1.1689257238838953, - "angularVelocity": 2.912641665287057e-8, - "velocityX": -0.5794902817178565, - "velocityY": -0.07500980729443667, - "timestamp": 1.846697408731853 + "x": 2.3521759510040283, + "y": 7.017032623291016, + "heading": -6.215056253707531e-20, + "angularVelocity": -7.658658295322792e-16, + "velocityX": 0.6080666216218581, + "velocityY": 0.0048504134705017595, + "timestamp": 0.6782795630629712 }, { - "x": 0.6103193163871765, - "y": 6.79156494140625, - "heading": 1.1689257238838953, - "angularVelocity": 4.3622376722558945e-19, - "velocityX": -5.6270263381275445e-18, - "velocityY": -6.414998799232099e-19, - "timestamp": 1.955326668069021 + "x": 2.3521759510040283, + "y": 7.017032623291016, + "heading": -6.764077311276182e-20, + "angularVelocity": -4.856600085384507e-20, + "velocityX": -1.0267091726602369e-19, + "velocityY": 4.493167408339835e-21, + "timestamp": 0.7913261569067997 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.3.traj b/src/main/deploy/choreo/2 piece left.3.traj index a634a1b..674fd2d 100644 --- a/src/main/deploy/choreo/2 piece left.3.traj +++ b/src/main/deploy/choreo/2 piece left.3.traj @@ -1,202 +1,175 @@ { "samples": [ { - "x": 0.6103193163871765, + "x": 2.3521759510040283, + "y": 7.017032623291016, + "heading": -6.764077311276182e-20, + "angularVelocity": -4.856600085384507e-20, + "velocityX": -1.0267091726602369e-19, + "velocityY": 4.493167408339835e-21, + "timestamp": 0 + }, + { + "x": 2.3142846244738164, + "y": 7.01212793182799, + "heading": 6.683917732209663e-8, + "angularVelocity": 7.930688018171609e-7, + "velocityX": -0.4495930401075561, + "velocityY": -0.058195775856320804, + "timestamp": 0.08427916615690312 + }, + { + "x": 2.2385019738243845, + "y": 7.002318549276381, + "heading": 2.2158259200811548e-7, + "angularVelocity": 0.0000018360814628605592, + "velocityX": -0.8991860516078994, + "velocityY": -0.11639154726979174, + "timestamp": 0.16855833231380624 + }, + { + "x": 2.124828006289812, + "y": 6.987604476750434, + "heading": 5.265338458246272e-7, + "angularVelocity": 0.0000036183468313963867, + "velocityX": -1.348778977273516, + "velocityY": -0.17458730546233459, + "timestamp": 0.2528374984707096 + }, + { + "x": 2.0078133931582736, + "y": 6.972457986338443, + "heading": 0.04540585981844075, + "angularVelocity": 0.5387491992987151, + "velocityX": -1.3884168349944475, + "velocityY": -0.17971808576977197, + "timestamp": 0.3371166646276129 + }, + { + "x": 1.8907987799711805, + "y": 6.957311496355622, + "heading": 0.12790574879755856, + "angularVelocity": 0.9788882916274608, + "velocityX": -1.388416835653617, + "velocityY": -0.17971808067752473, + "timestamp": 0.42139583078451626 + }, + { + "x": 1.773784166758088, + "y": 6.942165006573663, + "heading": 0.23868110863736844, + "angularVelocity": 1.314386044512811, + "velocityX": -1.388416835962111, + "velocityY": -0.17971807829424596, + "timestamp": 0.5056749969414196 + }, + { + "x": 1.6567695535340663, + "y": 6.927018516876137, + "heading": 0.36923521647113, + "angularVelocity": 1.549067388620196, + "velocityX": -1.3884168360917915, + "velocityY": -0.1797180772923964, + "timestamp": 0.5899541630983229 + }, + { + "x": 1.5397549403057158, + "y": 6.911872027212054, + "heading": 0.5114515305873049, + "angularVelocity": 1.6874432982809748, + "velocityX": -1.388416836143153, + "velocityY": -0.17971807689559988, + "timestamp": 0.6742333292552263 + }, + { + "x": 1.4227403270757022, + "y": 6.896725537560817, + "heading": 0.6575137656671957, + "angularVelocity": 1.733076414258367, + "velocityX": -1.3884168361628832, + "velocityY": -0.17971807674317355, + "timestamp": 0.7585124954121296 + }, + { + "x": 1.305725713846615, + "y": 6.8815790479024255, + "heading": 0.7997212587593243, + "angularVelocity": 1.6873386339321397, + "velocityX": -1.388416836151895, + "velocityY": -0.1797180768280647, + "timestamp": 0.842791661569033 + }, + { + "x": 1.1887111006243336, + "y": 6.866432558191455, + "heading": 0.9302631260100628, + "angularVelocity": 1.5489221500804566, + "velocityX": -1.3884168360711413, + "velocityY": -0.17971807745192941, + "timestamp": 0.9270708277259363 + }, + { + "x": 1.0716964874224564, + "y": 6.851286068322852, + "heading": 1.0410285148792768, + "angularVelocity": 1.3142677356703014, + "velocityX": -1.3884168358290385, + "velocityY": -0.17971807932230188, + "timestamp": 1.0113499938828396 + }, + { + "x": 0.9546818742710608, + "y": 6.836139578064252, + "heading": 1.1235226807914271, + "angularVelocity": 0.9788203855573323, + "velocityX": -1.3884168352300577, + "velocityY": -0.17971808394974448, + "timestamp": 1.095629160039743 + }, + { + "x": 0.8376672612285322, + "y": 6.820993086964611, + "heading": 1.1689251973993973, + "angularVelocity": 0.538715778505021, + "velocityX": -1.3884168339383145, + "velocityY": -0.1797180939289554, + "timestamp": 1.1799083261966463 + }, + { + "x": 0.7239932936157987, + "y": 6.8062790150425, + "heading": 1.168925502319645, + "angularVelocity": 0.000003617978933894318, + "velocityX": -1.3487789782009216, + "velocityY": -0.1745872982976388, + "timestamp": 1.2641874923535497 + }, + { + "x": 0.6482106429312192, + "y": 6.796469632762423, + "heading": 1.1689256570497522, + "angularVelocity": 0.0000018359235639903555, + "velocityX": -0.899186052024937, + "velocityY": -0.11639154404795703, + "timestamp": 1.348466658510453 + }, + { + "x": 0.6103193163871763, "y": 6.79156494140625, "heading": 1.1689257238838953, - "angularVelocity": 4.3622376722558945e-19, - "velocityX": -5.6270263381275445e-18, - "velocityY": -6.414998799232099e-19, - "timestamp": 0 + "angularVelocity": 7.930090686887444e-7, + "velocityX": -0.4495930402716653, + "velocityY": -0.05819577458848967, + "timestamp": 1.4327458246673563 }, { - "x": 0.6557794938984264, - "y": 6.805365933173805, - "heading": 1.1689257265280788, - "angularVelocity": 2.813571130767411e-8, - "velocityX": 0.4837237542621103, - "velocityY": 0.14685089051159525, - "timestamp": 0.09397962599665277 - }, - { - "x": 0.7466998461856557, - "y": 6.832967915878576, - "heading": 1.1689257334168288, - "angularVelocity": 7.330046251495819e-8, - "velocityX": 0.9674474794192922, - "velocityY": 0.2937017721878802, - "timestamp": 0.18795925199330554 - }, - { - "x": 0.8830803650430104, - "y": 6.874370887029686, - "heading": 1.1689257494213645, - "angularVelocity": 1.7029792885174175e-7, - "velocityX": 1.4511711172612278, - "velocityY": 0.4405526273597229, - "timestamp": 0.2819388779899583 - }, - { - "x": 1.0359561926852463, - "y": 6.920781573342344, - "heading": 1.1411189880762886, - "angularVelocity": -0.295880740641233, - "velocityX": 1.6266911686548022, - "velocityY": 0.4938377421752201, - "timestamp": 0.3759185039866111 - }, - { - "x": 1.188832020320688, - "y": 6.96719225967738, - "heading": 1.0851032009960204, - "angularVelocity": -0.5960418174282094, - "velocityX": 1.6266911685825045, - "velocityY": 0.4938377424133325, - "timestamp": 0.46989812998326386 - }, - { - "x": 1.3417078479593374, - "y": 7.013602946001849, - "heading": 1.0076693097111147, - "angularVelocity": -0.8239433862788942, - "velocityX": 1.6266911686166408, - "velocityY": 0.4938377423008883, - "timestamp": 0.5638777559799166 - }, - { - "x": 1.4945836755981692, - "y": 7.0600136323257185, - "heading": 0.9144858910263077, - "angularVelocity": -0.9915278731597208, - "velocityX": 1.6266911686185797, - "velocityY": 0.4938377422945011, - "timestamp": 0.6578573819765694 - }, - { - "x": 1.6474595032368191, - "y": 7.106424318650186, - "heading": 0.810295986033273, - "angularVelocity": -1.1086435372358867, - "velocityX": 1.6266911686166439, - "velocityY": 0.4938377423008778, - "timestamp": 0.7518370079732217 - }, - { - "x": 1.8003353308752952, - "y": 7.152835004975228, - "heading": 0.6991463049745296, - "angularVelocity": -1.1826997594425672, - "velocityX": 1.6266911686147918, - "velocityY": 0.4938377423069784, - "timestamp": 0.8458166339698745 - }, - { - "x": 1.9532111585137473, - "y": 7.199245691300347, - "heading": 0.5846243537681264, - "angularVelocity": -1.2185827512282525, - "velocityX": 1.6266911686145398, - "velocityY": 0.49383774230780825, - "timestamp": 0.9397962599665273 - }, - { - "x": 2.1060869861523015, - "y": 7.24565637762513, - "heading": 0.47009178756221137, - "angularVelocity": -1.2186957012362925, - "velocityX": 1.6266911686156262, - "velocityY": 0.49383774230423, - "timestamp": 1.03377588596318 - }, - { - "x": 2.258962813791129, - "y": 7.2920670639490135, - "heading": 0.3589127090659498, - "angularVelocity": -1.1830125659387238, - "velocityX": 1.626691168618534, - "velocityY": 0.49383774229465205, - "timestamp": 1.1277555119598328 - }, - { - "x": 2.411838641430174, - "y": 7.33847775027218, - "heading": 0.25468045110617066, - "angularVelocity": -1.1090941983903169, - "velocityX": 1.6266911686208465, - "velocityY": 0.4938377422870341, - "timestamp": 1.2217351379564856 - }, - { - "x": 2.56471446906933, - "y": 7.38488843659498, - "heading": 0.1614473718452329, - "angularVelocity": -0.9920562916930376, - "velocityX": 1.6266911686220302, - "velocityY": 0.4938377422831351, - "timestamp": 1.3157147639531384 - }, - { - "x": 2.7175902967080585, - "y": 7.43129912291919, - "heading": 0.083958784465868, - "angularVelocity": -0.8245253857695168, - "velocityX": 1.6266911686174785, - "velocityY": 0.4938377422981283, - "timestamp": 1.4096943899497911 - }, - { - "x": 2.87046612433849, - "y": 7.477709809270727, - "heading": 0.027881129348107082, - "angularVelocity": -0.5967001307257632, - "velocityX": 1.6266911685292, - "velocityY": 0.493837742588916, - "timestamp": 1.503674015946444 - }, - { - "x": 3.02334195198337, - "y": 7.524120495574675, - "heading": -2.5550742755505775e-8, - "angularVelocity": -0.29667233299952617, - "velocityX": 1.6266911686829375, - "velocityY": 0.4938377420825434, - "timestamp": 1.5976536419430967 - }, - { - "x": 3.159722470841706, - "y": 7.565523466722555, - "heading": -9.539534540603276e-9, - "angularVelocity": 1.7036892885524613e-7, - "velocityX": 1.4511711172716637, - "velocityY": 0.44055262732534617, - "timestamp": 1.6916332679397494 - }, - { - "x": 3.2506428231294207, - "y": 7.5931254494257265, - "heading": -2.646376503470658e-9, - "angularVelocity": 7.334736613811233e-8, - "velocityX": 0.9674474794244572, - "velocityY": 0.2937017721708666, - "timestamp": 1.7856128939364022 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 7.567002470257346e-19, - "angularVelocity": 2.8159044862445686e-8, - "velocityX": 0.48372375426422565, - "velocityY": 0.1468508905046274, - "timestamp": 1.879592519933055 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 3.6938220334538545e-19, - "angularVelocity": -1.9084817747204875e-19, - "velocityX": 4.2439574212322616e-19, - "velocityY": 1.26437464030373e-19, - "timestamp": 1.9735721459297078 + "x": 0.6103193163871763, + "y": 6.79156494140625, + "heading": 1.1689257238838953, + "angularVelocity": -1.1197773464222718e-17, + "velocityX": 1.4630555201274963e-16, + "velocityY": 1.5632470182389935e-17, + "timestamp": 1.5170249908242597 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.4.traj b/src/main/deploy/choreo/2 piece left.4.traj new file mode 100644 index 0000000..732eb34 --- /dev/null +++ b/src/main/deploy/choreo/2 piece left.4.traj @@ -0,0 +1,292 @@ +{ + "samples": [ + { + "x": 0.6103193163871763, + "y": 6.79156494140625, + "heading": 1.1689257238838953, + "angularVelocity": -1.1197773464222718e-17, + "velocityX": 1.4630555201274963e-16, + "velocityY": 1.5632470182389935e-17, + "timestamp": 0 + }, + { + "x": 0.6716933262156661, + "y": 6.798129007626276, + "heading": 1.1689262102304376, + "angularVelocity": 0.000004540173449016303, + "velocityX": 0.5729425946972534, + "velocityY": 0.06127729216936612, + "timestamp": 0.10712069655236656 + }, + { + "x": 0.7944413423015934, + "y": 6.811257139685496, + "heading": 1.1689274078355216, + "angularVelocity": 0.00001117995982505779, + "velocityX": 1.1458851560577887, + "velocityY": 0.12255458078358349, + "timestamp": 0.2142413931047331 + }, + { + "x": 0.9785633539293457, + "y": 6.830949336441548, + "heading": 1.1689299826958874, + "angularVelocity": 0.000024037001706306202, + "velocityX": 1.7188276173852506, + "velocityY": 0.1838318587335218, + "timestamp": 0.32136208965709967 + }, + { + "x": 1.2128924819066704, + "y": 6.856011279048505, + "heading": 1.1639992699084272, + "angularVelocity": -0.046029506399353645, + "velocityX": 2.1875243115392906, + "velocityY": 0.23395985475790373, + "timestamp": 0.4284827862094662 + }, + { + "x": 1.4472216098815815, + "y": 6.88107322166846, + "heading": 1.1410466957992857, + "angularVelocity": -0.2142683426066124, + "velocityX": 2.1875243115167615, + "velocityY": 0.23395985487924773, + "timestamp": 0.5356034827618328 + }, + { + "x": 1.6815507378566321, + "y": 6.906135164287114, + "heading": 1.1053748993971433, + "angularVelocity": -0.3330056427023327, + "velocityX": 2.1875243115180623, + "velocityY": 0.23395985486708806, + "timestamp": 0.6427241793141989 + }, + { + "x": 1.9158798658322573, + "y": 6.931197106900394, + "heading": 1.060764506700477, + "angularVelocity": -0.4164498003880933, + "velocityX": 2.1875243115234255, + "velocityY": 0.23395985481693854, + "timestamp": 0.749844875866565 + }, + { + "x": 2.15020899380867, + "y": 6.956259049506311, + "heading": 1.0098936328394574, + "angularVelocity": -0.47489304586580033, + "velocityX": 2.187524311530778, + "velocityY": 0.23395985474819475, + "timestamp": 0.8569655724189311 + }, + { + "x": 2.3845381217856128, + "y": 6.981320992107275, + "heading": 0.9546528217127299, + "angularVelocity": -0.5156875646315714, + "velocityX": 2.1875243115357237, + "velocityY": 0.23395985470195227, + "timestamp": 0.9640862689712972 + }, + { + "x": 2.618867249763438, + "y": 7.006382934699983, + "heading": 0.896375656527792, + "angularVelocity": -0.5440327318675406, + "velocityX": 2.187524311543967, + "velocityY": 0.23395985462487984, + "timestamp": 1.0712069655236633 + }, + { + "x": 2.853196377741319, + "y": 7.0314448772921745, + "heading": 0.8360053257441781, + "angularVelocity": -0.5635729856751034, + "velocityX": 2.1875243115444807, + "velocityY": 0.2339598546200722, + "timestamp": 1.1783276620760295 + }, + { + "x": 3.0875255057190256, + "y": 7.056506819885997, + "heading": 0.7742143737208824, + "angularVelocity": -0.5768348602278626, + "velocityX": 2.187524311542854, + "velocityY": 0.23395985463528263, + "timestamp": 1.2854483586283956 + }, + { + "x": 3.321854633695538, + "y": 7.08156876249098, + "heading": 0.7114910094502581, + "angularVelocity": -0.585539174868618, + "velocityX": 2.1875243115317105, + "velocityY": 0.2339598547394751, + "timestamp": 1.3925690551807617 + }, + { + "x": 3.5561837616716114, + "y": 7.106630705100066, + "heading": 0.6482025434058349, + "angularVelocity": -0.5908145492079041, + "velocityX": 2.187524311527614, + "velocityY": 0.23395985477777761, + "timestamp": 1.4996897517331278 + }, + { + "x": 3.7905128896459725, + "y": 7.131692647725167, + "heading": 0.5846437426209585, + "angularVelocity": -0.5933381954233768, + "velocityX": 2.1875243115116247, + "velocityY": 0.2339598549272787, + "timestamp": 1.606810448285494 + }, + { + "x": 4.024842017618935, + "y": 7.156754590363345, + "heading": 0.5210758768415071, + "angularVelocity": -0.5934228195424006, + "velocityX": 2.1875243114985685, + "velocityY": 0.23395985504935324, + "timestamp": 1.71393114483786 + }, + { + "x": 4.259171145591179, + "y": 7.181816533008237, + "heading": 0.45776123122730383, + "angularVelocity": -0.5910589424075633, + "velocityX": 2.187524311491864, + "velocityY": 0.23395985511203934, + "timestamp": 1.8210518413902261 + }, + { + "x": 4.493500273561688, + "y": 7.206878475669362, + "heading": 0.39499750155382707, + "angularVelocity": -0.585915996567426, + "velocityX": 2.187524311475657, + "velocityY": 0.23395985526357388, + "timestamp": 1.9281725379425922 + }, + { + "x": 4.727829401532455, + "y": 7.231940418328061, + "heading": 0.3331565162700674, + "angularVelocity": -0.5773019339313977, + "velocityX": 2.18752431147808, + "velocityY": 0.23395985524092341, + "timestamp": 2.0352932344949584 + }, + { + "x": 4.962158529503271, + "y": 7.257002360986307, + "heading": 0.27273200291903427, + "angularVelocity": -0.5640787942551723, + "velocityX": 2.1875243114785317, + "velocityY": 0.23395985523669777, + "timestamp": 2.1424139310473245 + }, + { + "x": 5.1964876574745045, + "y": 7.282064303640645, + "heading": 0.21440226305549417, + "angularVelocity": -0.544523530380754, + "velocityX": 2.1875243114824334, + "velocityY": 0.23395985520021775, + "timestamp": 2.2495346275996906 + }, + { + "x": 5.430816785446193, + "y": 7.307126246290732, + "heading": 0.1591156180940034, + "angularVelocity": -0.5161154355868443, + "velocityX": 2.187524311486677, + "velocityY": 0.2339598551605356, + "timestamp": 2.3566553241520567 + }, + { + "x": 5.665145913418822, + "y": 7.332188188932028, + "heading": 0.10820921967960505, + "angularVelocity": -0.4752246769559841, + "velocityX": 2.1875243114954563, + "velocityY": 0.2339598550784526, + "timestamp": 2.463776020704423 + }, + { + "x": 5.899475041392132, + "y": 7.357250131566959, + "heading": 0.06357495743964915, + "angularVelocity": -0.4166726288802297, + "velocityX": 2.1875243115018086, + "velocityY": 0.23395985501905772, + "timestamp": 2.570896717256789 + }, + { + "x": 6.133804169366042, + "y": 7.382312074196284, + "heading": 0.02788975594931021, + "angularVelocity": -0.3331307827418212, + "velocityX": 2.1875243115074072, + "velocityY": 0.23395985496671218, + "timestamp": 2.678017413809155 + }, + { + "x": 6.368133297340411, + "y": 7.40737401682131, + "heading": 0.004930772411512917, + "angularVelocity": -0.21432817631627044, + "velocityX": 2.1875243115117, + "velocityY": 0.23395985492657667, + "timestamp": 2.785138110361521 + }, + { + "x": 6.602462425316754, + "y": 7.432435959437442, + "heading": -0.000004257837407987995, + "angularVelocity": -0.046069811042615996, + "velocityX": 2.1875243115301304, + "velocityY": 0.23395985484355303, + "timestamp": 2.8922588069138873 + }, + { + "x": 6.786584436945245, + "y": 7.452128156186601, + "heading": -0.0000016835872093411556, + "angularVelocity": 0.000024031305634360455, + "velocityX": 1.7188276173921362, + "velocityY": 0.1838318586691832, + "timestamp": 2.9993795034662534 + }, + { + "x": 6.909332453031493, + "y": 7.465256288242815, + "heading": -4.862460012160711e-7, + "angularVelocity": 0.000011177496474955052, + "velocityX": 1.1458851560607914, + "velocityY": 0.12255458075551605, + "timestamp": 3.1065002000186195 + }, + { + "x": 6.970706462860108, + "y": 7.47182035446167, + "heading": 1.452985890125102e-17, + "angularVelocity": 0.000004539234871288793, + "velocityX": 0.5729425946984215, + "velocityY": 0.061277292158443734, + "timestamp": 3.2136208965709856 + }, + { + "x": 6.970706462860107, + "y": 7.47182035446167, + "heading": 6.946622712118294e-18, + "angularVelocity": -5.942954968372966e-18, + "velocityX": 1.343946406102527e-16, + "velocityY": 1.4373642665254536e-17, + "timestamp": 3.3207415931233517 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece left.traj b/src/main/deploy/choreo/2 piece left.traj index bc91ed1..093e229 100644 --- a/src/main/deploy/choreo/2 piece left.traj +++ b/src/main/deploy/choreo/2 piece left.traj @@ -4,568 +4,658 @@ "x": 0.6235248446464539, "y": 6.866034507751465, "heading": 1.3037, - "angularVelocity": -1.8232741591844444e-18, - "velocityX": 7.12403473371085e-18, - "velocityY": -2.6478556818989226e-17, + "angularVelocity": -4.322191282064769e-19, + "velocityX": -7.217119807256306e-20, + "velocityY": -5.737284120005055e-18, "timestamp": 0 }, { - "x": 0.6371647021269348, - "y": 6.867787221776129, - "heading": 1.30369672515582, - "angularVelocity": -0.00006476812380618724, - "velocityX": 0.26976183581085494, - "velocityY": 0.03466424437509273, - "timestamp": 0.05056259140393951 - }, - { - "x": 0.6644444154536387, - "y": 6.871292657837969, - "heading": 1.3036900282942052, - "angularVelocity": -0.00013244696185552165, - "velocityX": 0.5395236393002303, - "velocityY": 0.06932864721736587, - "timestamp": 0.10112518280787902 - }, - { - "x": 0.7053639821708213, - "y": 6.876550827985717, - "heading": 1.303679688712071, - "angularVelocity": -0.0002044907479422861, - "velocityX": 0.8092853942211979, - "velocityY": 0.10399328835306255, - "timestamp": 0.15168777421181853 - }, - { - "x": 0.7599233981740878, - "y": 6.8835617523794905, - "heading": 1.303665338742152, - "angularVelocity": -0.00028380606137315504, - "velocityX": 1.0790470679676354, - "velocityY": 0.1386583282048117, - "timestamp": 0.20225036561575804 - }, - { - "x": 0.8281226552122382, - "y": 6.892325471630372, - "heading": 1.303646243766396, - "angularVelocity": -0.0003776502593285092, - "velocityX": 1.3488085785262331, - "velocityY": 0.17332417124092056, - "timestamp": 0.2528129570196975 - }, - { - "x": 0.9099617281962122, - "y": 6.902842109861635, - "heading": 1.3036202050692922, - "angularVelocity": -0.0005149794814836402, - "velocityX": 1.618569592886762, - "velocityY": 0.20799246912103908, - "timestamp": 0.303375548423637 - }, - { - "x": 0.9952174677554063, - "y": 6.913794817693266, - "heading": 1.2650372111423984, - "angularVelocity": -0.7630739021791462, - "velocityX": 1.686142604481928, - "velocityY": 0.21661682139925917, - "timestamp": 0.3539381398275765 - }, - { - "x": 1.0804727987833038, - "y": 6.9247507050181865, - "heading": 1.186135227119023, - "angularVelocity": -1.5604814119006516, - "velocityX": 1.6861345247675534, - "velocityY": 0.21667970372394155, - "timestamp": 0.404500731231516 - }, - { - "x": 1.1657282386661474, - "y": 6.935705745230898, - "heading": 1.0686962835514935, - "angularVelocity": -2.322644870578755, - "velocityX": 1.6861366776426912, - "velocityY": 0.21666294998988928, - "timestamp": 0.45506332263545546 - }, - { - "x": 1.2509836774698653, - "y": 6.946660793838387, - "heading": 0.9157151714041298, - "angularVelocity": -3.0255789487768316, - "velocityX": 1.6861366563003188, - "velocityY": 0.21666311601734475, - "timestamp": 0.5056259140393949 - }, - { - "x": 1.336239105659407, - "y": 6.957615925046211, - "heading": 0.7330874583136245, - "angularVelocity": -3.61191363060312, - "velocityX": 1.6861364463787976, - "velocityY": 0.21666474964277663, - "timestamp": 0.5561885054433344 - }, - { - "x": 1.4214945304345679, - "y": 6.96857108283117, - "heading": 0.5355420616291058, - "angularVelocity": -3.9069476306376054, - "velocityX": 1.6861363788509953, - "velocityY": 0.21666527527119953, - "timestamp": 0.6067510968472739 - }, - { - "x": 1.5067500117706687, - "y": 6.979525800477682, - "heading": 0.35218632067901057, - "angularVelocity": -3.6263121778170837, - "velocityX": 1.68613749748313, - "velocityY": 0.21665657044742084, - "timestamp": 0.6573136882512134 - }, - { - "x": 1.5920056795979993, - "y": 6.990479066662584, - "heading": 0.19830964964916573, - "angularVelocity": -3.043290835324073, - "velocityX": 1.6861411858073423, - "velocityY": 0.21662786421283695, - "timestamp": 0.7078762796551529 - }, - { - "x": 1.677260793789504, - "y": 7.001436641396165, - "heading": 0.07990683643812377, - "angularVelocity": -2.341707770971106, - "velocityX": 1.686130236292877, - "velocityY": 0.21671307639360837, - "timestamp": 0.7584388710590924 + "x": 0.6479605438901133, + "y": 6.869173153614982, + "heading": 1.3036976853929116, + "angularVelocity": -0.00003420125047541537, + "velocityX": 0.3610683967062306, + "velocityY": 0.04637746677385236, + "timestamp": 0.06767609535082215 + }, + { + "x": 0.6968319343921952, + "y": 6.875450489923095, + "heading": 1.3036923603900246, + "angularVelocity": -0.00007868366015476742, + "velocityX": 0.7221366754204779, + "velocityY": 0.0927555922895904, + "timestamp": 0.1353521907016443 + }, + { + "x": 0.7701389910336393, + "y": 6.884866659045138, + "heading": 1.3036818951032874, + "angularVelocity": -0.0001546378626426958, + "velocityX": 1.0832045829687984, + "velocityY": 0.13913582149253725, + "timestamp": 0.20302828605246648 + }, + { + "x": 0.8641137016555179, + "y": 6.896935578985996, + "heading": 1.278165357348036, + "angularVelocity": -0.377039154268251, + "velocityX": 1.3885953398275834, + "velocityY": 0.17833357374260927, + "timestamp": 0.2707043814032886 + }, + { + "x": 0.9580882030461527, + "y": 6.909006127999201, + "heading": 1.179816306963164, + "angularVelocity": -1.4532317485966955, + "velocityX": 1.3885922481710244, + "velocityY": 0.17835764534927084, + "timestamp": 0.33838047675411076 + }, + { + "x": 1.052062741771001, + "y": 6.921076386345661, + "heading": 1.012746961397015, + "angularVelocity": -2.4686611232531686, + "velocityX": 1.3885927998313032, + "velocityY": 0.17835335038004493, + "timestamp": 0.4060565721049329 + }, + { + "x": 1.1460372822563507, + "y": 6.933146630985501, + "heading": 0.7854621166033714, + "angularVelocity": -3.358421368955095, + "velocityX": 1.3885928258449378, + "velocityY": 0.17835314784740675, + "timestamp": 0.47373266745575504 + }, + { + "x": 1.2400118226682206, + "y": 6.945216876197425, + "heading": 0.5235178410871962, + "angularVelocity": -3.870558343508111, + "velocityX": 1.388592824759185, + "velocityY": 0.17835315630066892, + "timestamp": 0.5414087628065772 + }, + { + "x": 1.3339863634408795, + "y": 6.9572871186003775, + "heading": 0.29518267414695665, + "angularVelocity": -3.3739412085846014, + "velocityX": 1.3885928300902979, + "velocityY": 0.17835311479456636, + "timestamp": 0.6090848581573994 + }, + { + "x": 1.4279609034991196, + "y": 6.9693573665655375, + "heading": 0.1268000189106551, + "angularVelocity": -2.4880669365369346, + "velocityX": 1.388592819533854, + "velocityY": 0.1783531969832136, + "timestamp": 0.6767609535082216 + }, + { + "x": 1.5219354277832027, + "y": 6.981427737341889, + "heading": 0.027024429838266126, + "angularVelocity": -1.4743106639821368, + "velocityX": 1.38859258645071, + "velocityY": 0.1783550116740703, + "timestamp": 0.7444370488590438 + }, + { + "x": 1.615910044720068, + "y": 6.993497386738595, + "heading": 0.000018019004787293556, + "angularVelocity": -0.3990539154701806, + "velocityX": 1.3885939555128846, + "velocityY": 0.17834435237639756, + "timestamp": 0.812113144209866 + }, + { + "x": 1.6892171360383907, + "y": 7.002913285881713, + "heading": 0.000007615738994617179, + "angularVelocity": -0.00015372142465942866, + "velocityX": 1.083205095363587, + "velocityY": 0.13913183221207334, + "timestamp": 0.8797892395606882 + }, + { + "x": 1.7380885418555654, + "y": 7.0091905029584325, + "heading": 0.0000023106612364746317, + "angularVelocity": -0.00007838924114270227, + "velocityX": 0.7221369017203642, + "velocityY": 0.09275383049478103, + "timestamp": 0.9474653349115104 }, { "x": 1.7625242471694946, "y": 7.0123291015625, - "heading": -3.350712590761302e-17, - "angularVelocity": -1.5803548477125327, - "velocityX": 1.6862951643207778, - "velocityY": 0.21542527516670237, - "timestamp": 0.8090014624630318 - }, - { - "x": 1.894716655716008, - "y": 7.013428377195941, - "heading": -0.044649055027065385, - "angularVelocity": -0.5741688805746129, - "velocityX": 1.6999411788133891, - "velocityY": 0.01413624229031357, - "timestamp": 0.886764393436201 - }, - { - "x": 2.02690952861163, - "y": 7.014470317880742, - "heading": -0.00008383158388879456, - "angularVelocity": 0.5730908401401845, - "velocityX": 1.6999471501560728, - "velocityY": 0.013398937922744884, - "timestamp": 0.9645273244093701 - }, - { - "x": 2.1570160896928328, - "y": 7.015495222736415, - "heading": -0.0000539532537599614, - "angularVelocity": 0.00038422330222239603, - "velocityX": 1.673118019768238, - "velocityY": 0.013179864015495759, - "timestamp": 1.042290255382539 - }, - { - "x": 2.2545960183213234, - "y": 7.01626391842549, - "heading": -0.00002792078971867657, - "angularVelocity": 0.00033476701193603026, - "velocityX": 1.254838615357224, - "velocityY": 0.009885117233306755, - "timestamp": 1.1200531863557082 - }, - { - "x": 2.319649306326726, - "y": 7.016776387311738, - "heading": -0.000009516691596222152, - "angularVelocity": 0.00023666929592428788, - "velocityX": 0.8365591058784542, - "velocityY": 0.006590143656300914, - "timestamp": 1.1978161173288773 + "heading": 9.925554069905119e-19, + "angularVelocity": -0.00003414294551807602, + "velocityX": 0.36106848640215494, + "velocityY": 0.04637676845564921, + "timestamp": 1.0151414302623325 + }, + { + "x": 1.7625242471694946, + "y": 7.0123291015625, + "heading": 4.930172708842328e-19, + "angularVelocity": -9.633639100834258e-20, + "velocityX": -4.2792851430038895e-18, + "velocityY": -3.4270302349667407e-18, + "timestamp": 1.0828175256131547 + }, + { + "x": 1.83126410757397, + "y": 7.012877424284074, + "heading": -8.433317163645367e-17, + "angularVelocity": -7.503648365992863e-16, + "velocityX": 0.6080666216218581, + "velocityY": 0.00485041347050172, + "timestamp": 1.1958641194569832 + }, + { + "x": 1.9443071033303654, + "y": 7.013779143355416, + "heading": -9.171422334116161e-15, + "angularVelocity": -8.038357329937531e-14, + "velocityX": 0.9999681716422337, + "velocityY": 0.007976525790660745, + "timestamp": 1.3089107133008118 + }, + { + "x": 2.0573500990867615, + "y": 7.014680862426758, + "heading": 4.721221727458621e-16, + "angularVelocity": 8.530592721947881e-14, + "velocityX": 0.9999681716422396, + "velocityY": 0.007976525790660303, + "timestamp": 1.4219573071446403 + }, + { + "x": 2.1703930948431576, + "y": 7.0155825814981, + "heading": 9.192667005316373e-15, + "angularVelocity": 7.714115512942701e-14, + "velocityX": 0.9999681716422396, + "velocityY": 0.00797652579066031, + "timestamp": 1.5350039009884688 + }, + { + "x": 2.283436090599553, + "y": 7.0164843005694415, + "heading": 8.651637279707332e-17, + "angularVelocity": -8.055218934866491e-14, + "velocityX": 0.9999681716422337, + "velocityY": 0.007976525790660681, + "timestamp": 1.6480504948322974 }, { "x": 2.3521759510040283, "y": 7.017032623291016, - "heading": 5.0938422434116983e-17, - "angularVelocity": 0.000122380824348686, - "velocityX": 0.4182795616143265, - "velocityY": 0.0032950915824648324, - "timestamp": 1.2755790483020464 + "heading": -6.215056253707531e-20, + "angularVelocity": -7.658658295322792e-16, + "velocityX": 0.6080666216218581, + "velocityY": 0.0048504134705017595, + "timestamp": 1.761097088676126 }, { "x": 2.3521759510040283, "y": 7.017032623291016, - "heading": 2.5675724430541464e-17, - "angularVelocity": -4.4489652123602634e-18, - "velocityX": -3.43042618417509e-18, - "velocityY": 7.376106260350096e-18, - "timestamp": 1.3533419792752155 - }, - { - "x": 2.289226350907918, - "y": 7.0088843634816955, - "heading": 3.162897756010949e-9, - "angularVelocity": 2.911644384649021e-8, - "velocityX": -0.5794902817179733, - "velocityY": -0.07500980729353342, - "timestamp": 1.4619712386123833 - }, - { - "x": 2.181495855279594, - "y": 6.9949396193307365, - "heading": 0.012994469205035452, - "angularVelocity": 0.1196221544860667, - "velocityX": -0.9917263201983659, - "velocityY": -0.12837005642905872, - "timestamp": 1.5706004979495511 - }, - { - "x": 2.0737653596512815, - "y": 6.980994875179695, - "heading": 0.05695780222793572, - "angularVelocity": 0.40470986630264244, - "velocityX": -0.9917263201982718, - "velocityY": -0.1283700564298165, - "timestamp": 1.679229757286719 - }, - { - "x": 1.9660348640229757, - "y": 6.967050131028595, - "heading": 0.1246841048489742, - "angularVelocity": 0.6234628039838408, - "velocityX": -0.9917263201982012, - "velocityY": -0.12837005643036203, - "timestamp": 1.7878590166238868 - }, - { - "x": 1.858304368394671, - "y": 6.953105386877488, - "heading": 0.21021952796955365, - "angularVelocity": 0.7874068519153866, - "velocityX": -0.9917263201981937, - "velocityY": -0.12837005643041954, - "timestamp": 1.8964882759610546 - }, - { - "x": 1.7505738727663673, - "y": 6.939160642726373, - "heading": 0.30861254552236733, - "angularVelocity": 0.905769017971645, - "velocityX": -0.9917263201981846, - "velocityY": -0.1283700564304895, - "timestamp": 2.0051175352982225 - }, - { - "x": 1.642843377138065, - "y": 6.9252158985752486, - "heading": 0.4156678969757047, - "angularVelocity": 0.9855111975039318, - "velocityX": -0.9917263201981728, - "velocityY": -0.12837005643058202, - "timestamp": 2.1137467946353903 - }, - { - "x": 1.5351128815097632, - "y": 6.911271154424119, - "heading": 0.5277146421232866, - "angularVelocity": 1.0314600857196923, - "velocityX": -0.9917263201981663, - "velocityY": -0.12837005643063132, - "timestamp": 2.222376053972558 - }, - { - "x": 1.4273823858814612, - "y": 6.897326410272993, - "heading": 0.6413892672091689, - "angularVelocity": 1.0464457345976608, - "velocityX": -0.9917263201981706, - "velocityY": -0.12837005643059782, - "timestamp": 2.331005313309726 - }, - { - "x": 1.3196518902531575, - "y": 6.883381666121877, - "heading": 0.7534304016146443, - "angularVelocity": 1.0314084353435349, - "velocityX": -0.9917263201981834, - "velocityY": -0.12837005643049992, - "timestamp": 2.4396345726468938 - }, - { - "x": 1.2119213946248526, - "y": 6.869436921970773, - "heading": 0.8604745712214651, - "angularVelocity": 0.9854082616413032, - "velocityX": -0.9917263201981971, - "velocityY": -0.12837005643039434, - "timestamp": 2.5482638319840616 - }, - { - "x": 1.1041908989965465, - "y": 6.855492177819676, - "heading": 0.9588506837381976, - "angularVelocity": 0.9056133965839597, - "velocityX": -0.9917263201982054, - "velocityY": -0.12837005643033003, - "timestamp": 2.6568930913212294 - }, - { - "x": 0.9964604033682406, - "y": 6.8415474336685795, - "heading": 1.0443629237764005, - "angularVelocity": 0.7871934372008065, - "velocityX": -0.9917263201982054, - "velocityY": -0.12837005643032937, - "timestamp": 2.7655223506583972 - }, - { - "x": 0.8887299077399343, - "y": 6.827602689517485, - "heading": 1.1120586417031153, - "angularVelocity": 0.6231812528206429, - "velocityX": -0.991726320198208, - "velocityY": -0.12837005643030963, - "timestamp": 2.874151609995565 - }, - { - "x": 0.7809994121116139, - "y": 6.813657945366499, - "heading": 1.1559822740636867, - "angularVelocity": 0.4043443969754001, - "velocityX": -0.9917263201983385, - "velocityY": -0.12837005642930135, - "timestamp": 2.982780869332733 - }, - { - "x": 0.6732689164832739, - "y": 6.799713201215669, - "heading": 1.1689257207199144, - "angularVelocity": 0.11915248925755068, - "velocityX": -0.9917263201985188, - "velocityY": -0.12837005642787808, - "timestamp": 3.0914101286699007 - }, - { - "x": 0.6103193163871765, + "heading": -6.764077311276182e-20, + "angularVelocity": -4.856600085384507e-20, + "velocityX": -1.0267091726602369e-19, + "velocityY": 4.493167408339835e-21, + "timestamp": 1.8741436825199544 + }, + { + "x": 2.3142846244738164, + "y": 7.01212793182799, + "heading": 6.683917732209663e-8, + "angularVelocity": 7.930688018171609e-7, + "velocityX": -0.4495930401075561, + "velocityY": -0.058195775856320804, + "timestamp": 1.9584228486768576 + }, + { + "x": 2.2385019738243845, + "y": 7.002318549276381, + "heading": 2.2158259200811548e-7, + "angularVelocity": 0.0000018360814628605592, + "velocityX": -0.8991860516078994, + "velocityY": -0.11639154726979174, + "timestamp": 2.0427020148337607 + }, + { + "x": 2.124828006289812, + "y": 6.987604476750434, + "heading": 5.265338458246272e-7, + "angularVelocity": 0.0000036183468313963867, + "velocityX": -1.348778977273516, + "velocityY": -0.17458730546233459, + "timestamp": 2.126981180990664 + }, + { + "x": 2.0078133931582736, + "y": 6.972457986338443, + "heading": 0.04540585981844075, + "angularVelocity": 0.5387491992987151, + "velocityX": -1.3884168349944475, + "velocityY": -0.17971808576977197, + "timestamp": 2.2112603471475674 + }, + { + "x": 1.8907987799711805, + "y": 6.957311496355622, + "heading": 0.12790574879755856, + "angularVelocity": 0.9788882916274608, + "velocityX": -1.388416835653617, + "velocityY": -0.17971808067752473, + "timestamp": 2.2955395133044707 + }, + { + "x": 1.773784166758088, + "y": 6.942165006573663, + "heading": 0.23868110863736844, + "angularVelocity": 1.314386044512811, + "velocityX": -1.388416835962111, + "velocityY": -0.17971807829424596, + "timestamp": 2.379818679461374 + }, + { + "x": 1.6567695535340663, + "y": 6.927018516876137, + "heading": 0.36923521647113, + "angularVelocity": 1.549067388620196, + "velocityX": -1.3884168360917915, + "velocityY": -0.1797180772923964, + "timestamp": 2.4640978456182774 + }, + { + "x": 1.5397549403057158, + "y": 6.911872027212054, + "heading": 0.5114515305873049, + "angularVelocity": 1.6874432982809748, + "velocityX": -1.388416836143153, + "velocityY": -0.17971807689559988, + "timestamp": 2.5483770117751807 + }, + { + "x": 1.4227403270757022, + "y": 6.896725537560817, + "heading": 0.6575137656671957, + "angularVelocity": 1.733076414258367, + "velocityX": -1.3884168361628832, + "velocityY": -0.17971807674317355, + "timestamp": 2.632656177932084 + }, + { + "x": 1.305725713846615, + "y": 6.8815790479024255, + "heading": 0.7997212587593243, + "angularVelocity": 1.6873386339321397, + "velocityX": -1.388416836151895, + "velocityY": -0.1797180768280647, + "timestamp": 2.7169353440889874 + }, + { + "x": 1.1887111006243336, + "y": 6.866432558191455, + "heading": 0.9302631260100628, + "angularVelocity": 1.5489221500804566, + "velocityX": -1.3884168360711413, + "velocityY": -0.17971807745192941, + "timestamp": 2.8012145102458907 + }, + { + "x": 1.0716964874224564, + "y": 6.851286068322852, + "heading": 1.0410285148792768, + "angularVelocity": 1.3142677356703014, + "velocityX": -1.3884168358290385, + "velocityY": -0.17971807932230188, + "timestamp": 2.885493676402794 + }, + { + "x": 0.9546818742710608, + "y": 6.836139578064252, + "heading": 1.1235226807914271, + "angularVelocity": 0.9788203855573323, + "velocityX": -1.3884168352300577, + "velocityY": -0.17971808394974448, + "timestamp": 2.9697728425596974 + }, + { + "x": 0.8376672612285322, + "y": 6.820993086964611, + "heading": 1.1689251973993973, + "angularVelocity": 0.538715778505021, + "velocityX": -1.3884168339383145, + "velocityY": -0.1797180939289554, + "timestamp": 3.0540520087166008 + }, + { + "x": 0.7239932936157987, + "y": 6.8062790150425, + "heading": 1.168925502319645, + "angularVelocity": 0.000003617978933894318, + "velocityX": -1.3487789782009216, + "velocityY": -0.1745872982976388, + "timestamp": 3.138331174873504 + }, + { + "x": 0.6482106429312192, + "y": 6.796469632762423, + "heading": 1.1689256570497522, + "angularVelocity": 0.0000018359235639903555, + "velocityX": -0.899186052024937, + "velocityY": -0.11639154404795703, + "timestamp": 3.2226103410304074 + }, + { + "x": 0.6103193163871763, "y": 6.79156494140625, "heading": 1.1689257238838953, - "angularVelocity": 2.912641665287057e-8, - "velocityX": -0.5794902817178565, - "velocityY": -0.07500980729443667, - "timestamp": 3.2000393880070686 + "angularVelocity": 7.930090686887444e-7, + "velocityX": -0.4495930402716653, + "velocityY": -0.05819577458848967, + "timestamp": 3.306889507187311 }, { - "x": 0.6103193163871765, + "x": 0.6103193163871763, "y": 6.79156494140625, "heading": 1.1689257238838953, - "angularVelocity": 4.3622376722558945e-19, - "velocityX": -5.6270263381275445e-18, - "velocityY": -6.414998799232099e-19, - "timestamp": 3.3086686473442364 - }, - { - "x": 0.6557794938984264, - "y": 6.805365933173805, - "heading": 1.1689257265280788, - "angularVelocity": 2.813571130767411e-8, - "velocityX": 0.4837237542621103, - "velocityY": 0.14685089051159525, - "timestamp": 3.402648273340889 - }, - { - "x": 0.7466998461856557, - "y": 6.832967915878576, - "heading": 1.1689257334168288, - "angularVelocity": 7.330046251495819e-8, - "velocityX": 0.9674474794192922, - "velocityY": 0.2937017721878802, - "timestamp": 3.496627899337542 - }, - { - "x": 0.8830803650430104, - "y": 6.874370887029686, - "heading": 1.1689257494213645, - "angularVelocity": 1.7029792885174175e-7, - "velocityX": 1.4511711172612278, - "velocityY": 0.4405526273597229, - "timestamp": 3.5906075253341947 - }, - { - "x": 1.0359561926852463, - "y": 6.920781573342344, - "heading": 1.1411189880762886, - "angularVelocity": -0.295880740641233, - "velocityX": 1.6266911686548022, - "velocityY": 0.4938377421752201, - "timestamp": 3.6845871513308475 - }, - { - "x": 1.188832020320688, - "y": 6.96719225967738, - "heading": 1.0851032009960204, - "angularVelocity": -0.5960418174282094, - "velocityX": 1.6266911685825045, - "velocityY": 0.4938377424133325, - "timestamp": 3.7785667773275002 - }, - { - "x": 1.3417078479593374, - "y": 7.013602946001849, - "heading": 1.0076693097111147, - "angularVelocity": -0.8239433862788942, - "velocityX": 1.6266911686166408, - "velocityY": 0.4938377423008883, - "timestamp": 3.872546403324153 - }, - { - "x": 1.4945836755981692, - "y": 7.0600136323257185, - "heading": 0.9144858910263077, - "angularVelocity": -0.9915278731597208, - "velocityX": 1.6266911686185797, - "velocityY": 0.4938377422945011, - "timestamp": 3.966526029320806 - }, - { - "x": 1.6474595032368191, - "y": 7.106424318650186, - "heading": 0.810295986033273, - "angularVelocity": -1.1086435372358867, - "velocityX": 1.6266911686166439, - "velocityY": 0.4938377423008778, - "timestamp": 4.060505655317458 - }, - { - "x": 1.8003353308752952, - "y": 7.152835004975228, - "heading": 0.6991463049745296, - "angularVelocity": -1.1826997594425672, - "velocityX": 1.6266911686147918, - "velocityY": 0.4938377423069784, - "timestamp": 4.154485281314111 - }, - { - "x": 1.9532111585137473, - "y": 7.199245691300347, - "heading": 0.5846243537681264, - "angularVelocity": -1.2185827512282525, - "velocityX": 1.6266911686145398, - "velocityY": 0.49383774230780825, - "timestamp": 4.248464907310764 - }, - { - "x": 2.1060869861523015, - "y": 7.24565637762513, - "heading": 0.47009178756221137, - "angularVelocity": -1.2186957012362925, - "velocityX": 1.6266911686156262, - "velocityY": 0.49383774230423, - "timestamp": 4.342444533307416 - }, - { - "x": 2.258962813791129, - "y": 7.2920670639490135, - "heading": 0.3589127090659498, - "angularVelocity": -1.1830125659387238, - "velocityX": 1.626691168618534, - "velocityY": 0.49383774229465205, - "timestamp": 4.436424159304069 - }, - { - "x": 2.411838641430174, - "y": 7.33847775027218, - "heading": 0.25468045110617066, - "angularVelocity": -1.1090941983903169, - "velocityX": 1.6266911686208465, - "velocityY": 0.4938377422870341, - "timestamp": 4.530403785300722 - }, - { - "x": 2.56471446906933, - "y": 7.38488843659498, - "heading": 0.1614473718452329, - "angularVelocity": -0.9920562916930376, - "velocityX": 1.6266911686220302, - "velocityY": 0.4938377422831351, - "timestamp": 4.624383411297375 - }, - { - "x": 2.7175902967080585, - "y": 7.43129912291919, - "heading": 0.083958784465868, - "angularVelocity": -0.8245253857695168, - "velocityX": 1.6266911686174785, - "velocityY": 0.4938377422981283, - "timestamp": 4.7183630372940275 - }, - { - "x": 2.87046612433849, - "y": 7.477709809270727, - "heading": 0.027881129348107082, - "angularVelocity": -0.5967001307257632, - "velocityX": 1.6266911685292, - "velocityY": 0.493837742588916, - "timestamp": 4.81234266329068 - }, - { - "x": 3.02334195198337, - "y": 7.524120495574675, - "heading": -2.5550742755505775e-8, - "angularVelocity": -0.29667233299952617, - "velocityX": 1.6266911686829375, - "velocityY": 0.4938377420825434, - "timestamp": 4.906322289287333 - }, - { - "x": 3.159722470841706, - "y": 7.565523466722555, - "heading": -9.539534540603276e-9, - "angularVelocity": 1.7036892885524613e-7, - "velocityX": 1.4511711172716637, - "velocityY": 0.44055262732534617, - "timestamp": 5.000301915283986 - }, - { - "x": 3.2506428231294207, - "y": 7.5931254494257265, - "heading": -2.646376503470658e-9, - "angularVelocity": 7.334736613811233e-8, - "velocityX": 0.9674474794244572, - "velocityY": 0.2937017721708666, - "timestamp": 5.094281541280639 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 7.567002470257346e-19, - "angularVelocity": 2.8159044862445686e-8, - "velocityX": 0.48372375426422565, - "velocityY": 0.1468508905046274, - "timestamp": 5.188261167277291 - }, - { - "x": 3.296103000640869, - "y": 7.606926441192627, - "heading": 3.6938220334538545e-19, - "angularVelocity": -1.9084817747204875e-19, - "velocityX": 4.2439574212322616e-19, - "velocityY": 1.26437464030373e-19, - "timestamp": 5.282240793273944 + "angularVelocity": -1.1197773464222718e-17, + "velocityX": 1.4630555201274963e-16, + "velocityY": 1.5632470182389935e-17, + "timestamp": 3.391168673344214 + }, + { + "x": 0.6716933262156661, + "y": 6.798129007626276, + "heading": 1.1689262102304376, + "angularVelocity": 0.000004540173449016303, + "velocityX": 0.5729425946972534, + "velocityY": 0.06127729216936612, + "timestamp": 3.4982893698965807 + }, + { + "x": 0.7944413423015934, + "y": 6.811257139685496, + "heading": 1.1689274078355216, + "angularVelocity": 0.00001117995982505779, + "velocityX": 1.1458851560577887, + "velocityY": 0.12255458078358349, + "timestamp": 3.6054100664489472 + }, + { + "x": 0.9785633539293457, + "y": 6.830949336441548, + "heading": 1.1689299826958874, + "angularVelocity": 0.000024037001706306202, + "velocityX": 1.7188276173852506, + "velocityY": 0.1838318587335218, + "timestamp": 3.712530763001314 + }, + { + "x": 1.2128924819066704, + "y": 6.856011279048505, + "heading": 1.1639992699084272, + "angularVelocity": -0.046029506399353645, + "velocityX": 2.1875243115392906, + "velocityY": 0.23395985475790373, + "timestamp": 3.8196514595536804 + }, + { + "x": 1.4472216098815815, + "y": 6.88107322166846, + "heading": 1.1410466957992857, + "angularVelocity": -0.2142683426066124, + "velocityX": 2.1875243115167615, + "velocityY": 0.23395985487924773, + "timestamp": 3.926772156106047 + }, + { + "x": 1.6815507378566321, + "y": 6.906135164287114, + "heading": 1.1053748993971433, + "angularVelocity": -0.3330056427023327, + "velocityX": 2.1875243115180623, + "velocityY": 0.23395985486708806, + "timestamp": 4.033892852658413 + }, + { + "x": 1.9158798658322573, + "y": 6.931197106900394, + "heading": 1.060764506700477, + "angularVelocity": -0.4164498003880933, + "velocityX": 2.1875243115234255, + "velocityY": 0.23395985481693854, + "timestamp": 4.141013549210779 + }, + { + "x": 2.15020899380867, + "y": 6.956259049506311, + "heading": 1.0098936328394574, + "angularVelocity": -0.47489304586580033, + "velocityX": 2.187524311530778, + "velocityY": 0.23395985474819475, + "timestamp": 4.248134245763145 + }, + { + "x": 2.3845381217856128, + "y": 6.981320992107275, + "heading": 0.9546528217127299, + "angularVelocity": -0.5156875646315714, + "velocityX": 2.1875243115357237, + "velocityY": 0.23395985470195227, + "timestamp": 4.355254942315511 + }, + { + "x": 2.618867249763438, + "y": 7.006382934699983, + "heading": 0.896375656527792, + "angularVelocity": -0.5440327318675406, + "velocityX": 2.187524311543967, + "velocityY": 0.23395985462487984, + "timestamp": 4.4623756388678775 + }, + { + "x": 2.853196377741319, + "y": 7.0314448772921745, + "heading": 0.8360053257441781, + "angularVelocity": -0.5635729856751034, + "velocityX": 2.1875243115444807, + "velocityY": 0.2339598546200722, + "timestamp": 4.569496335420244 + }, + { + "x": 3.0875255057190256, + "y": 7.056506819885997, + "heading": 0.7742143737208824, + "angularVelocity": -0.5768348602278626, + "velocityX": 2.187524311542854, + "velocityY": 0.23395985463528263, + "timestamp": 4.67661703197261 + }, + { + "x": 3.321854633695538, + "y": 7.08156876249098, + "heading": 0.7114910094502581, + "angularVelocity": -0.585539174868618, + "velocityX": 2.1875243115317105, + "velocityY": 0.2339598547394751, + "timestamp": 4.783737728524976 + }, + { + "x": 3.5561837616716114, + "y": 7.106630705100066, + "heading": 0.6482025434058349, + "angularVelocity": -0.5908145492079041, + "velocityX": 2.187524311527614, + "velocityY": 0.23395985477777761, + "timestamp": 4.890858425077342 + }, + { + "x": 3.7905128896459725, + "y": 7.131692647725167, + "heading": 0.5846437426209585, + "angularVelocity": -0.5933381954233768, + "velocityX": 2.1875243115116247, + "velocityY": 0.2339598549272787, + "timestamp": 4.997979121629708 + }, + { + "x": 4.024842017618935, + "y": 7.156754590363345, + "heading": 0.5210758768415071, + "angularVelocity": -0.5934228195424006, + "velocityX": 2.1875243114985685, + "velocityY": 0.23395985504935324, + "timestamp": 5.105099818182074 + }, + { + "x": 4.259171145591179, + "y": 7.181816533008237, + "heading": 0.45776123122730383, + "angularVelocity": -0.5910589424075633, + "velocityX": 2.187524311491864, + "velocityY": 0.23395985511203934, + "timestamp": 5.21222051473444 + }, + { + "x": 4.493500273561688, + "y": 7.206878475669362, + "heading": 0.39499750155382707, + "angularVelocity": -0.585915996567426, + "velocityX": 2.187524311475657, + "velocityY": 0.23395985526357388, + "timestamp": 5.319341211286806 + }, + { + "x": 4.727829401532455, + "y": 7.231940418328061, + "heading": 0.3331565162700674, + "angularVelocity": -0.5773019339313977, + "velocityX": 2.18752431147808, + "velocityY": 0.23395985524092341, + "timestamp": 5.4264619078391725 + }, + { + "x": 4.962158529503271, + "y": 7.257002360986307, + "heading": 0.27273200291903427, + "angularVelocity": -0.5640787942551723, + "velocityX": 2.1875243114785317, + "velocityY": 0.23395985523669777, + "timestamp": 5.533582604391539 + }, + { + "x": 5.1964876574745045, + "y": 7.282064303640645, + "heading": 0.21440226305549417, + "angularVelocity": -0.544523530380754, + "velocityX": 2.1875243114824334, + "velocityY": 0.23395985520021775, + "timestamp": 5.640703300943905 + }, + { + "x": 5.430816785446193, + "y": 7.307126246290732, + "heading": 0.1591156180940034, + "angularVelocity": -0.5161154355868443, + "velocityX": 2.187524311486677, + "velocityY": 0.2339598551605356, + "timestamp": 5.747823997496271 + }, + { + "x": 5.665145913418822, + "y": 7.332188188932028, + "heading": 0.10820921967960505, + "angularVelocity": -0.4752246769559841, + "velocityX": 2.1875243114954563, + "velocityY": 0.2339598550784526, + "timestamp": 5.854944694048637 + }, + { + "x": 5.899475041392132, + "y": 7.357250131566959, + "heading": 0.06357495743964915, + "angularVelocity": -0.4166726288802297, + "velocityX": 2.1875243115018086, + "velocityY": 0.23395985501905772, + "timestamp": 5.962065390601003 + }, + { + "x": 6.133804169366042, + "y": 7.382312074196284, + "heading": 0.02788975594931021, + "angularVelocity": -0.3331307827418212, + "velocityX": 2.1875243115074072, + "velocityY": 0.23395985496671218, + "timestamp": 6.069186087153369 + }, + { + "x": 6.368133297340411, + "y": 7.40737401682131, + "heading": 0.004930772411512917, + "angularVelocity": -0.21432817631627044, + "velocityX": 2.1875243115117, + "velocityY": 0.23395985492657667, + "timestamp": 6.176306783705735 + }, + { + "x": 6.602462425316754, + "y": 7.432435959437442, + "heading": -0.000004257837407987995, + "angularVelocity": -0.046069811042615996, + "velocityX": 2.1875243115301304, + "velocityY": 0.23395985484355303, + "timestamp": 6.283427480258101 + }, + { + "x": 6.786584436945245, + "y": 7.452128156186601, + "heading": -0.0000016835872093411556, + "angularVelocity": 0.000024031305634360455, + "velocityX": 1.7188276173921362, + "velocityY": 0.1838318586691832, + "timestamp": 6.3905481768104675 + }, + { + "x": 6.909332453031493, + "y": 7.465256288242815, + "heading": -4.862460012160711e-7, + "angularVelocity": 0.000011177496474955052, + "velocityX": 1.1458851560607914, + "velocityY": 0.12255458075551605, + "timestamp": 6.497668873362834 + }, + { + "x": 6.970706462860108, + "y": 7.47182035446167, + "heading": 1.452985890125102e-17, + "angularVelocity": 0.000004539234871288793, + "velocityX": 0.5729425946984215, + "velocityY": 0.061277292158443734, + "timestamp": 6.6047895699152 + }, + { + "x": 6.970706462860107, + "y": 7.47182035446167, + "heading": 6.946622712118294e-18, + "angularVelocity": -5.942954968372966e-18, + "velocityX": 1.343946406102527e-16, + "velocityY": 1.4373642665254536e-17, + "timestamp": 6.711910266467566 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.1.traj b/src/main/deploy/choreo/2 piece right.1.traj index e8e5fa6..5a7e503 100644 --- a/src/main/deploy/choreo/2 piece right.1.traj +++ b/src/main/deploy/choreo/2 piece right.1.traj @@ -4,226 +4,163 @@ "x": 0.6313003897666931, "y": 4.255156517028809, "heading": -1.2897, - "angularVelocity": -2.3969037863354903e-26, - "velocityX": -5.149474371067618e-26, - "velocityY": 3.9840780609445665e-27, + "angularVelocity": -3.635906927924615e-19, + "velocityX": -6.136781108423474e-19, + "velocityY": 9.990271880480628e-19, "timestamp": 0 }, { - "x": 0.6442648163276663, - "y": 4.253481865948087, - "heading": -1.289699890378596, - "angularVelocity": 0.0000022236977895922636, - "velocityX": 0.2629866575194569, - "velocityY": -0.03397071888670311, - "timestamp": 0.049296898493848725 - }, - { - "x": 0.6701936687758512, - "y": 4.250132563317103, - "heading": -1.2896996605083761, - "angularVelocity": 0.000004662975296506846, - "velocityX": 0.5259733013714917, - "velocityY": -0.06794144729820102, - "timestamp": 0.09859379698769745 - }, - { - "x": 0.7090869461006044, - "y": 4.245108608431542, - "heading": -1.2896992944482415, - "angularVelocity": 0.0000074256220104019626, - "velocityX": 0.7889599247223699, - "velocityY": -0.10191218999686634, - "timestamp": 0.14789069548154618 - }, - { - "x": 0.760944646617517, - "y": 4.238410000117552, - "heading": -1.2896987656297503, - "angularVelocity": 0.000010727216264499804, - "velocityX": 1.0519465139045912, - "velocityY": -0.13588295650740967, - "timestamp": 0.1971875939753949 - }, - { - "x": 0.82576676695776, - "y": 4.230036736027443, - "heading": -1.2896980209162199, - "angularVelocity": 0.000015106701495517666, - "velocityX": 1.3149330347492647, - "velocityY": -0.16985377064146243, - "timestamp": 0.24648449246924362 - }, - { - "x": 0.9035532970147546, - "y": 4.21998880911824, - "heading": -1.2896969008990993, - "angularVelocity": 0.000022719829335288094, - "velocityX": 1.5779193505794464, - "velocityY": -0.2038247276440018, - "timestamp": 0.29578139096309236 - }, - { - "x": 0.9915565704265453, - "y": 4.208621313799639, - "heading": -1.27289000896861, - "angularVelocity": 0.3409320351580815, - "velocityX": 1.7851685623340352, - "velocityY": -0.2305925051252473, - "timestamp": 0.3450782894569411 - }, - { - "x": 1.0795598293475788, - "y": 4.197253706298393, - "heading": -1.2169726689327969, - "angularVelocity": 1.134297323852746, - "velocityX": 1.7851682683853676, - "velocityY": -0.23059478077843756, - "timestamp": 0.3943751879507898 - }, - { - "x": 1.1675630883225618, - "y": 4.185886099214807, - "heading": -1.123282876673827, - "angularVelocity": 1.9005210291406174, - "velocityX": 1.7851682694797597, - "velocityY": -0.23059477230609737, - "timestamp": 0.4436720864446385 - }, - { - "x": 1.255566347295848, - "y": 4.174518492118083, - "heading": -0.9939341877349891, - "angularVelocity": 2.6238707279927147, - "velocityX": 1.785168269445332, - "velocityY": -0.23059477257260114, - "timestamp": 0.49296898493848723 - }, - { - "x": 1.3435696062497653, - "y": 4.163150884871416, - "heading": -0.8327019973298604, - "angularVelocity": 3.2706355842091646, - "velocityX": 1.7851682690524295, - "velocityY": -0.23059477561424194, - "timestamp": 0.542265883432336 - }, - { - "x": 1.4315728650960726, - "y": 4.15178327679168, - "heading": -0.6477412989500829, - "angularVelocity": 3.7519743438394353, - "velocityX": 1.7851682668695343, - "velocityY": -0.23059479251323747, - "timestamp": 0.5915627819261847 - }, - { - "x": 1.519576123922667, - "y": 4.140415668559335, - "heading": -0.46074819376328596, - "angularVelocity": 3.7932022277249358, - "velocityX": 1.7851682664696522, - "velocityY": -0.23059479560896612, - "timestamp": 0.6408596804200335 - }, - { - "x": 1.6075793828656728, - "y": 4.129048061228197, - "heading": -0.29527670366521236, - "angularVelocity": 3.356630846030226, - "velocityX": 1.7851682688310881, - "velocityY": -0.23059477732777772, - "timestamp": 0.6901565789138823 - }, - { - "x": 1.6955826418903344, - "y": 4.117680454529199, - "heading": -0.16085599473456663, - "angularVelocity": 2.7267579307736534, - "velocityX": 1.7851682704874938, - "velocityY": -0.23059476450461613, - "timestamp": 0.739453477407731 - }, - { - "x": 1.7835859004541794, - "y": 4.106312844262756, - "heading": -0.061669148748961934, - "angularVelocity": 2.0120301482654375, - "velocityX": 1.7851682611397215, - "velocityY": -0.2305948368711734, - "timestamp": 0.7887503759015798 + "x": 0.655972085024139, + "y": 4.251969629979563, + "heading": -1.2896996398384766, + "angularVelocity": 0.000005296084636152658, + "velocityX": 0.3627910748292595, + "velocityY": -0.04686237268621969, + "timestamp": 0.06800524315284516 + }, + { + "x": 0.7053154730729085, + "y": 4.245595854265352, + "heading": -1.2896988073147642, + "angularVelocity": 0.000012242051844172605, + "velocityX": 0.7255821133948134, + "velocityY": -0.09372476913119913, + "timestamp": 0.13601048630569032 + }, + { + "x": 0.7793305465080104, + "y": 4.2360351849906746, + "heading": -1.2896971657533272, + "angularVelocity": 0.000024138748145691714, + "velocityX": 1.08837304307183, + "velocityY": -0.1405872375632783, + "timestamp": 0.20401572945853547 + }, + { + "x": 0.8737534122985577, + "y": 4.223838483147547, + "heading": -1.2688252083137923, + "angularVelocity": 0.3069168856969461, + "velocityX": 1.388464497925952, + "velocityY": -0.17934943362697756, + "timestamp": 0.27202097261138064 + }, + { + "x": 0.9681762695915698, + "y": 4.211641715518689, + "heading": -1.1855949428153905, + "angularVelocity": 1.2238801251153222, + "velocityX": 1.3884643729718302, + "velocityY": -0.1793504009896027, + "timestamp": 0.3400262157642258 + }, + { + "x": 1.0625991288182361, + "y": 4.199444962859495, + "heading": -1.0462591608699763, + "angularVelocity": 2.048897636205047, + "velocityX": 1.3884644014057288, + "velocityY": -0.17935018086444265, + "timestamp": 0.408031458917071 + }, + { + "x": 1.1570219877844825, + "y": 4.187248208184227, + "heading": -0.8608593609825309, + "angularVelocity": 2.7262574368089574, + "velocityX": 1.388464397576322, + "velocityY": -0.17935021051031644, + "timestamp": 0.47603670206991616 + }, + { + "x": 1.2514448467064523, + "y": 4.175051453166187, + "heading": -0.6467911549093175, + "angularVelocity": 3.1478191408283704, + "velocityX": 1.3884643969252477, + "velocityY": -0.17935021555069555, + "timestamp": 0.5440419452227613 + }, + { + "x": 1.3458677056257224, + "y": 4.162854698127245, + "heading": -0.4324354912476929, + "angularVelocity": 3.152046132382022, + "velocityX": 1.3884643968855472, + "velocityY": -0.17935021585804314, + "timestamp": 0.6120471883756065 + }, + { + "x": 1.4402905645694315, + "y": 4.1506579432775, + "heading": -0.24636817246823917, + "angularVelocity": 2.7360731342619897, + "velocityX": 1.3884643972449147, + "velocityY": -0.17935021307595345, + "timestamp": 0.6800524315284516 + }, + { + "x": 1.5347134236322062, + "y": 4.138461189349519, + "heading": -0.10616450182646274, + "angularVelocity": 2.061659721246224, + "velocityX": 1.388464398995746, + "velocityY": -0.17935019952165523, + "timestamp": 0.7480576746812968 + }, + { + "x": 1.6291362817773116, + "y": 4.126264428317271, + "heading": -0.02194518417619484, + "angularVelocity": 1.238423888301977, + "velocityX": 1.3884643855016514, + "velocityY": -0.17935030398809765, + "timestamp": 0.816062917834142 + }, + { + "x": 1.7235591439282192, + "y": 4.114067698297376, + "heading": -0.0000028452682314893883, + "angularVelocity": 0.3226565760326282, + "velocityX": 1.3884644444059648, + "velocityY": -0.1793498479592596, + "timestamp": 0.8840681609869872 + }, + { + "x": 1.7975742187998267, + "y": 4.104507040143629, + "heading": -0.0000011981495890210145, + "angularVelocity": 0.000024220465453921752, + "velocityX": 1.0883730641952847, + "velocityY": -0.14058707403279755, + "timestamp": 0.9520734041398323 + }, + { + "x": 1.846917607499234, + "y": 4.098133269466462, + "heading": -3.6197485474192144e-7, + "angularVelocity": 0.000012295739203525973, + "velocityX": 0.7255821229622761, + "velocityY": -0.0937246950627145, + "timestamp": 1.0200786472926775 }, { "x": 1.8715893030166624, "y": 4.094946384429932, - "heading": -2.0036368727453063e-25, - "angularVelocity": 1.2509742120319602, - "velocityX": 1.785171182188348, - "velocityY": -0.23057150003548213, - "timestamp": 0.8380472743954286 - }, - { - "x": 2.0015014856340083, - "y": 4.096925192128385, - "heading": 0.03734833063779682, - "angularVelocity": 0.5174202657607754, - "velocityX": 1.7997911796197699, - "velocityY": 0.02741421605031293, - "timestamp": 0.9102290830899192 - }, - { - "x": 2.1314136526758922, - "y": 4.098905022118577, - "heading": 0.0000024861301875270727, - "angularVelocity": -0.5173858231465974, - "velocityX": 1.799790963838784, - "velocityY": 0.027428378784090407, - "timestamp": 0.9824108917844099 - }, - { - "x": 2.243505108940701, - "y": 4.100613269261141, - "heading": 0.0000014496891722730681, - "angularVelocity": -0.000014358756506653064, - "velocityX": 1.5529045100440175, - "velocityY": 0.02366589551382172, - "timestamp": 1.0545927004789006 - }, - { - "x": 2.3275737089377446, - "y": 4.101894454670087, - "heading": 7.139853498794832e-7, - "angularVelocity": -0.000010192371675077432, - "velocityX": 1.1646784905718206, - "velocityY": 0.017749422356118257, - "timestamp": 1.1267745091733914 - }, - { - "x": 2.3836194445295247, - "y": 4.102748578291145, - "heading": 2.3558195309312242e-7, - "angularVelocity": -0.000006627755738446538, - "velocityX": 0.7764523583634968, - "velocityY": 0.011832948446505252, - "timestamp": 1.1989563178678821 - }, - { - "x": 2.41164231300354, - "y": 4.103175640106201, - "heading": -7.741029200580404e-24, - "angularVelocity": -0.000003263730257719399, - "velocityX": 0.3882261885764356, - "velocityY": 0.005916474285980588, - "timestamp": 1.271138126562373 - }, - { - "x": 2.41164231300354, - "y": 4.103175640106201, - "heading": -4.8316826521123155e-24, - "angularVelocity": 4.329873823266725e-23, - "velocityX": 4.0284965680453105e-22, - "velocityY": -2.2471582465129843e-22, - "timestamp": 1.3433199352568637 + "heading": 1.3449524044377914e-18, + "angularVelocity": 0.000005322749217003313, + "velocityX": 0.3627910786522388, + "velocityY": -0.04686234308975064, + "timestamp": 1.0880838904455226 + }, + { + "x": 1.8715893030166624, + "y": 4.094946384429932, + "heading": 6.590717682190205e-19, + "angularVelocity": -3.942176622173566e-19, + "velocityX": -4.110872129098873e-19, + "velocityY": 5.381793683550551e-19, + "timestamp": 1.1560891335983676 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.2.traj b/src/main/deploy/choreo/2 piece right.2.traj index 3a3de28..a518e5f 100644 --- a/src/main/deploy/choreo/2 piece right.2.traj +++ b/src/main/deploy/choreo/2 piece right.2.traj @@ -1,175 +1,76 @@ { "samples": [ { - "x": 2.41164231300354, - "y": 4.103175640106201, - "heading": -4.8316826521123155e-24, - "angularVelocity": 4.329873823266725e-23, - "velocityX": 4.0284965680453105e-22, - "velocityY": -2.2471582465129843e-22, + "x": 1.8715893030166624, + "y": 4.094946384429932, + "heading": 6.590717682190205e-19, + "angularVelocity": -3.942176622173566e-19, + "velocityX": -4.110872129098873e-19, + "velocityY": 5.381793683550551e-19, "timestamp": 0 }, { - "x": 2.344735769891283, - "y": 4.110806937625484, - "heading": -6.978848485329857e-9, - "angularVelocity": -6.237346131654942e-8, - "velocityX": -0.5979772576247802, - "velocityY": 0.068204724836005, - "timestamp": 0.11188810654441217 - }, - { - "x": 2.2335684425317694, - "y": 4.123486578658459, - "heading": -0.015246855809319428, - "angularVelocity": -0.13626871793043496, - "velocityX": -0.9935580357273067, - "velocityY": 0.11332429714450055, - "timestamp": 0.22377621308882434 - }, - { - "x": 2.122401115172327, - "y": 4.136166219692066, - "heading": -0.060800247349853634, - "angularVelocity": -0.40713345633794035, - "velocityX": -0.9935580357266695, - "velocityY": 0.1133242971501526, - "timestamp": 0.3356643196332365 - }, - { - "x": 2.0112337878128743, - "y": 4.148845860725579, - "heading": -0.12944768713850766, - "angularVelocity": -0.6135365224131806, - "velocityX": -0.9935580357267644, - "velocityY": 0.11332429714932027, - "timestamp": 0.4475524261776487 - }, - { - "x": 1.9000664604534496, - "y": 4.16152550175934, - "heading": -0.21530108218408722, - "angularVelocity": -0.7673147548662962, - "velocityX": -0.9935580357265124, - "velocityY": 0.11332429715152995, - "timestamp": 0.5594405327220608 - }, - { - "x": 1.788899133093997, - "y": 4.174205142792856, - "heading": -0.31351675402178747, - "angularVelocity": -0.8778026089726992, - "velocityX": -0.9935580357267627, - "velocityY": 0.1133242971493348, - "timestamp": 0.671328639266473 - }, - { - "x": 1.6777318057328703, - "y": 4.1868847838116965, - "heading": -0.4200302675690305, - "angularVelocity": -0.9519645727936626, - "velocityX": -0.9935580357417213, - "velocityY": 0.11332429701818696, - "timestamp": 0.783216745810885 + "x": 1.93115886834108, + "y": 4.095854097514598, + "heading": -1.5234931737044087e-18, + "angularVelocity": -2.073879403640416e-17, + "velocityX": 0.5660317007877699, + "velocityY": 0.00862511549888298, + "timestamp": 0.10524068747653592 }, { - "x": 1.5665644783751262, - "y": 4.199564424860192, - "heading": -0.5313117322584114, - "angularVelocity": -0.994578138161715, - "velocityX": -0.9935580357114923, - "velocityY": 0.11332429728321564, - "timestamp": 0.8951048523552969 + "x": 2.03638733817559, + "y": 4.097457554891332, + "heading": -7.082480253674674e-16, + "angularVelocity": -6.715316567570261e-15, + "velocityX": 0.9998839076186332, + "velocityY": 0.015236097513059094, + "timestamp": 0.21048137495307184 }, { - "x": 1.455397151015652, - "y": 4.212244065893518, - "heading": -0.6441464282968985, - "angularVelocity": -1.008460144007346, - "velocityX": -0.9935580357269562, - "velocityY": 0.1133242971476382, - "timestamp": 1.0069929588997089 + "x": 2.1416158080101013, + "y": 4.099061012268066, + "heading": 3.9820301050635646e-17, + "angularVelocity": 7.108166473969379e-15, + "velocityX": 0.9998839076186449, + "velocityY": 0.015236097513059273, + "timestamp": 0.31572206242960776 }, { - "x": 1.344229823656203, - "y": 4.2249237069270675, - "heading": -0.7554226234189, - "angularVelocity": -0.994531041401011, - "velocityX": -0.9935580357267284, - "velocityY": 0.11332429714963566, - "timestamp": 1.1188810654441208 + "x": 2.2468442778446125, + "y": 4.1006644696448005, + "heading": 5.47356752522011e-16, + "angularVelocity": 4.822625769948835e-15, + "velocityX": 0.9998839076186449, + "velocityY": 0.015236097513059273, + "timestamp": 0.4209627499061437 }, { - "x": 1.2330624962967764, - "y": 4.237603347960814, - "heading": -0.8619240929821974, - "angularVelocity": -0.951856929682008, - "velocityX": -0.9935580357265276, - "velocityY": 0.11332429715139572, - "timestamp": 1.2307691719885328 + "x": 2.3520727476791223, + "y": 4.1022679270215345, + "heading": 2.3383723081945662e-18, + "angularVelocity": -5.178780120903659e-15, + "velocityX": 0.9998839076186332, + "velocityY": 0.015236097513059094, + "timestamp": 0.5262034373826796 }, { - "x": 1.121895168937324, - "y": 4.250282988994331, - "heading": -0.9601218669400209, - "angularVelocity": -0.8776426466636605, - "velocityX": -0.9935580357267606, - "velocityY": 0.11332429714935303, - "timestamp": 1.3426572785329447 - }, - { - "x": 1.0107278415778729, - "y": 4.262962630027863, - "heading": -1.0459506988902485, - "angularVelocity": -0.7670952221911026, - "velocityX": -0.9935580357267462, - "velocityY": 0.1133242971494796, - "timestamp": 1.4545453850773566 - }, - { - "x": 0.8995605142184344, - "y": 4.275642271061504, - "heading": -1.114565563469038, - "angularVelocity": -0.6132453814611101, - "velocityX": -0.9935580357266346, - "velocityY": 0.11332429715045739, - "timestamp": 1.5664334916217686 - }, - { - "x": 0.7883931868589816, - "y": 4.28832191209502, - "heading": -1.1600763958246794, - "angularVelocity": -0.40675308360479684, - "velocityX": -0.9935580357267626, - "velocityY": 0.11332429714933458, - "timestamp": 1.6783215981661805 - }, - { - "x": 0.6772258594993766, - "y": 4.301001553127193, - "heading": -1.1752681604416075, - "angularVelocity": -0.13577640274838362, - "velocityX": -0.9935580357281231, - "velocityY": 0.1133242971373439, - "timestamp": 1.7902097047105925 - }, - { - "x": 0.6103193163871765, - "y": 4.308632850646973, - "heading": -1.1752681674346586, - "angularVelocity": -6.250039740114642e-8, - "velocityX": -0.5979772576242739, - "velocityY": 0.06820472484044302, - "timestamp": 1.9020978112550044 + "x": 2.41164231300354, + "y": 4.103175640106201, + "heading": 1.4822311094594617e-19, + "angularVelocity": -2.0810859845807786e-17, + "velocityX": 0.5660317007877698, + "velocityY": 0.00862511549888298, + "timestamp": 0.6314441248592155 }, { - "x": 0.6103193163871765, - "y": 4.308632850646973, - "heading": -1.1752681674346586, - "angularVelocity": -8.29300949382696e-24, - "velocityX": -1.1501418857045935e-22, - "velocityY": 1.733872473382952e-23, - "timestamp": 2.0139859177994164 + "x": 2.41164231300354, + "y": 4.103175640106201, + "heading": 1.6064214107253007e-19, + "angularVelocity": 1.180059777481661e-19, + "velocityX": -1.7693428317330896e-20, + "velocityY": 3.049321257143075e-21, + "timestamp": 0.7366848123357514 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.3.traj b/src/main/deploy/choreo/2 piece right.3.traj index b55a9f3..216bbbd 100644 --- a/src/main/deploy/choreo/2 piece right.3.traj +++ b/src/main/deploy/choreo/2 piece right.3.traj @@ -1,211 +1,175 @@ { "samples": [ + { + "x": 2.41164231300354, + "y": 4.103175640106201, + "heading": 1.6064214107253007e-19, + "angularVelocity": 1.180059777481661e-19, + "velocityX": -1.7693428317330896e-20, + "velocityY": 3.049321257143075e-21, + "timestamp": 0 + }, + { + "x": 2.3717259825825794, + "y": 4.107728459293482, + "heading": -2.4661471568252517e-8, + "angularVelocity": -2.853606326541506e-7, + "velocityX": -0.4618763024941506, + "velocityY": 0.05268117760248238, + "timestamp": 0.08642212255837989 + }, + { + "x": 2.2918933241895187, + "y": 4.116834097388246, + "heading": -8.139982855367353e-8, + "angularVelocity": -6.565258443780271e-7, + "velocityX": -0.9237525766522542, + "velocityY": 0.10536235196736803, + "timestamp": 0.17284424511675978 + }, + { + "x": 2.1721443451709694, + "y": 4.1304925535509796, + "heading": -1.9247747670303723e-7, + "angularVelocity": -0.0000012852918310874542, + "velocityX": -1.3856287658019122, + "velocityY": 0.15804351661818175, + "timestamp": 0.25926636767513944 + }, + { + "x": 2.051932794176593, + "y": 4.1442037702569685, + "heading": -0.046313366659817376, + "angularVelocity": -0.5358948937068193, + "velocityX": -1.390981237624328, + "velocityY": 0.15865401473711088, + "timestamp": 0.3456884902335191 + }, + { + "x": 1.9317212431804152, + "y": 4.157914986947169, + "heading": -0.12967755850503584, + "angularVelocity": -0.964616343331585, + "velocityX": -1.3909812376451667, + "velocityY": 0.15865401455441974, + "timestamp": 0.4321106127918988 + }, + { + "x": 1.8115096921845533, + "y": 4.171626203640135, + "heading": -0.2411097709793792, + "angularVelocity": -1.2893945343574351, + "velocityX": -1.3909812376415176, + "velocityY": 0.158654014586412, + "timestamp": 0.5185327353502784 + }, + { + "x": 1.6912981411888635, + "y": 4.185337420334612, + "heading": -0.3720803056041949, + "angularVelocity": -1.5154746348233066, + "velocityX": -1.3909812376395225, + "velocityY": 0.1586540146039027, + "timestamp": 0.6049548579086581 + }, + { + "x": 1.571086590193276, + "y": 4.1990486370299855, + "heading": -0.5145335305215332, + "angularVelocity": -1.6483421223669708, + "velocityX": -1.3909812376383404, + "velocityY": 0.15865401461426823, + "timestamp": 0.6913769804670378 + }, + { + "x": 1.4508750391976706, + "y": 4.212759853725203, + "heading": -0.6607675274594663, + "angularVelocity": -1.6920898562650917, + "velocityX": -1.3909812376385464, + "velocityY": 0.15865401461246012, + "timestamp": 0.7777991030254174 + }, + { + "x": 1.3306634882019563, + "y": 4.226471070419467, + "heading": -0.8032127126381129, + "angularVelocity": -1.648249093655645, + "velocityX": -1.390981237639804, + "velocityY": 0.15865401460143336, + "timestamp": 0.8642212255837971 + }, + { + "x": 1.2104519372061566, + "y": 4.240182287112979, + "heading": -0.9341722064333875, + "angularVelocity": -1.5153468801557017, + "velocityX": -1.390981237640797, + "velocityY": 0.1586540145927282, + "timestamp": 0.9506433481421768 + }, + { + "x": 1.0902403862103853, + "y": 4.253893503806742, + "heading": -1.045595793963093, + "angularVelocity": -1.2892947341629668, + "velocityX": -1.3909812376404647, + "velocityY": 0.15865401459564232, + "timestamp": 1.0370654707005564 + }, + { + "x": 0.9700288352147577, + "y": 4.267604720501764, + "heading": -1.1289557818648916, + "angularVelocity": -0.9645676990343222, + "velocityX": -1.3909812376388042, + "velocityY": 0.15865401461020098, + "timestamp": 1.123487593258936 + }, + { + "x": 0.8498172842183673, + "y": 4.281315937190096, + "heading": -1.175267974928737, + "angularVelocity": -0.5358835410754968, + "velocityX": -1.3909812376476314, + "velocityY": 0.15865401453280073, + "timestamp": 1.2099097158173158 + }, + { + "x": 0.7300683052006729, + "y": 4.294974393360328, + "heading": -1.1752680860230837, + "angularVelocity": -0.0000012854850518362776, + "velocityX": -1.3856287657920174, + "velocityY": 0.15804351670493202, + "timestamp": 1.2963318383756954 + }, + { + "x": 0.6502356468079885, + "y": 4.304080031458386, + "heading": -1.1752681427696843, + "angularVelocity": -6.566212326774967e-7, + "velocityX": -0.9237525766479047, + "velocityY": 0.10536235200550112, + "timestamp": 1.382753960934075 + }, { "x": 0.6103193163871765, "y": 4.308632850646973, "heading": -1.1752681674346586, - "angularVelocity": -8.29300949382696e-24, - "velocityX": -1.1501418857045935e-22, - "velocityY": 1.733872473382952e-23, - "timestamp": 0 + "angularVelocity": -2.8540116177195145e-7, + "velocityX": -0.4618763024924286, + "velocityY": 0.05268117761757928, + "timestamp": 1.4691760834924548 }, { - "x": 0.644363601367681, - "y": 4.2767432034820265, - "heading": -1.1752681587825877, - "angularVelocity": 9.290967586148177e-8, - "velocityX": 0.36558224475309986, - "velocityY": -0.34244481273791916, - "timestamp": 0.09312346392395687 - }, - { - "x": 0.712452169167196, - "y": 4.212963911179069, - "heading": -1.1752681390823587, - "angularVelocity": 2.1154957206444014e-7, - "velocityX": 0.7311644662951432, - "velocityY": -0.6848896037097378, - "timestamp": 0.18624692784791375 - }, - { - "x": 0.8145850133012432, - "y": 4.117294979818902, - "heading": -1.175268101145713, - "angularVelocity": 4.07380097343134e-7, - "velocityX": 1.0967466182040557, - "velocityY": -1.027334329383292, - "timestamp": 0.2793703917718706 - }, - { - "x": 0.9369198335620701, - "y": 4.002702640501709, - "heading": -1.1572816768173708, - "angularVelocity": 0.19314599747953293, - "velocityX": 1.3136841683716076, - "velocityY": -1.2305420619959617, - "timestamp": 0.3724938556958275 - }, - { - "x": 1.0592546538469616, - "y": 3.888110301210203, - "heading": -1.110256724093758, - "angularVelocity": 0.5049742647246526, - "velocityX": 1.313684168630027, - "velocityY": -1.2305420617201315, - "timestamp": 0.46561731961978436 - }, - { - "x": 1.1815894741316775, - "y": 3.7735179619185093, - "heading": -1.0410814207725312, - "angularVelocity": 0.7428343019726364, - "velocityX": 1.3136841686281384, - "velocityY": -1.2305420617221479, - "timestamp": 0.5587407835437412 - }, - { - "x": 1.3039242944162523, - "y": 3.658925622626665, - "heading": -0.9554475532957203, - "angularVelocity": 0.9195734766346116, - "velocityX": 1.3136841686266243, - "velocityY": -1.2305420617237641, - "timestamp": 0.6518642474676986 - }, - { - "x": 1.426259114700816, - "y": 3.5443332833348076, - "heading": -0.8580438926758113, - "angularVelocity": 1.0459626018577572, - "velocityX": 1.3136841686264993, - "velocityY": -1.2305420617238971, - "timestamp": 0.7449877113916559 - }, - { - "x": 1.548593934985741, - "y": 3.429740944043337, - "heading": -0.752782430466053, - "angularVelocity": 1.1303430711696154, - "velocityX": 1.3136841686303846, - "velocityY": -1.2305420617197496, - "timestamp": 0.8381111753156132 - }, - { - "x": 1.6709287552712397, - "y": 3.3151486047524794, - "heading": -0.6430288452135179, - "angularVelocity": 1.1785814297260035, - "velocityX": 1.313684168636549, - "velocityY": -1.2305420617131688, - "timestamp": 0.9312346392395705 - }, - { - "x": 1.7932635755573523, - "y": 3.2005562654622763, - "heading": -0.5318250037177289, - "angularVelocity": 1.1941549080111136, - "velocityX": 1.3136841686431349, - "velocityY": -1.2305420617061376, - "timestamp": 1.0243581031635278 - }, - { - "x": 1.915598395843716, - "y": 3.085963926172342, - "heading": -0.4220971673663666, - "angularVelocity": 1.1783049268975236, - "velocityX": 1.3136841686458358, - "velocityY": -1.2305420617032545, - "timestamp": 1.1174815670874851 - }, - { - "x": 2.0379332161298342, - "y": 2.9713715868821446, - "heading": -0.3168810175382735, - "angularVelocity": 1.1298564872330217, - "velocityX": 1.3136841686431986, - "velocityY": -1.23054206170607, - "timestamp": 1.2106050310114425 - }, - { - "x": 2.1602680364155047, - "y": 2.8567792475914704, - "heading": -0.2195335566330217, - "angularVelocity": 1.0453591050344102, - "velocityX": 1.3136841686383904, - "velocityY": -1.2305420617112026, - "timestamp": 1.3037284949353998 - }, - { - "x": 2.2826028567007413, - "y": 2.742186908300333, - "heading": -0.1339605112634315, - "angularVelocity": 0.9189203425622967, - "velocityX": 1.3136841686337362, - "velocityY": -1.2305420617161718, - "timestamp": 1.396851958859357 - }, - { - "x": 2.404937676985743, - "y": 2.6275945690089433, - "heading": -0.06484983243968821, - "angularVelocity": 0.7421403361904352, - "velocityX": 1.3136841686312044, - "velocityY": -1.2305420617188745, - "timestamp": 1.4899754227833144 - }, - { - "x": 2.527272497270682, - "y": 2.5130022297174874, - "heading": -0.017897793209595524, - "angularVelocity": 0.5041912881208213, - "velocityX": 1.3136841686305336, - "velocityY": -1.230542061719591, - "timestamp": 1.5830988867072717 - }, - { - "x": 2.6496073175587562, - "y": 2.3984098904293836, - "heading": -6.631716442759581e-8, - "angularVelocity": 0.1921935260810968, - "velocityX": 1.3136841686642036, - "velocityY": -1.2305420616835967, - "timestamp": 1.676222350631229 - }, - { - "x": 2.751740161678901, - "y": 2.302740959054374, - "heading": -2.8363565851208167e-8, - "angularVelocity": 4.0756214360088557e-7, - "velocityX": 1.0967466180547656, - "velocityY": -1.0273343295426693, - "timestamp": 1.7693458145551864 - }, - { - "x": 2.8198287294720963, - "y": 2.238961666744671, - "heading": -8.655352303676696e-9, - "angularVelocity": 2.1163531420636224e-7, - "velocityX": 0.731164466227284, - "velocityY": -0.6848896037821823, - "timestamp": 1.8624692784791437 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 5.90328227721677e-29, - "angularVelocity": 9.294491354772317e-8, - "velocityX": 0.36558224472595613, - "velocityY": -0.34244481276689687, - "timestamp": 1.955592742403101 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 3.223926333927225e-29, - "angularVelocity": 1.2163249082376476e-28, - "velocityX": -5.169137847741255e-28, - "velocityY": -3.368633280520026e-27, - "timestamp": 2.0487162063270583 + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, + "angularVelocity": 2.0861005593011723e-18, + "velocityX": 4.162620858676712e-18, + "velocityY": -5.846759932567415e-18, + "timestamp": 1.5555982060508344 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.4.traj b/src/main/deploy/choreo/2 piece right.4.traj index 6b228f6..b44a8ec 100644 --- a/src/main/deploy/choreo/2 piece right.4.traj +++ b/src/main/deploy/choreo/2 piece right.4.traj @@ -1,211 +1,382 @@ { "samples": [ { - "x": 0.6313003897666931, - "y": 4.255156517028809, - "heading": -1.2897, - "angularVelocity": 1.3869170673526514e-18, - "velocityX": 3.369685785773421e-18, - "velocityY": -4.0149346678321615e-18, + "x": 0.6103193163871765, + "y": 4.308632850646973, + "heading": -1.1752681674346586, + "angularVelocity": 2.0861005593011723e-18, + "velocityX": 4.162620858676712e-18, + "velocityY": -5.846759932567415e-18, "timestamp": 0 }, { - "x": 0.6646314230620342, - "y": 4.224442212407106, - "heading": -1.2897000396998657, - "angularVelocity": -4.3248936361314274e-7, - "velocityX": 0.3631074604740814, - "velocityY": -0.33460088238466335, - "timestamp": 0.09179385422657882 - }, - { - "x": 0.7312934874764815, - "y": 4.163013605155588, - "heading": -1.2897001341367849, - "angularVelocity": -0.0000010287934852966955, - "velocityX": 0.7262148972403135, - "velocityY": -0.6692017430697588, - "timestamp": 0.18358770845315764 - }, - { - "x": 0.8312865764814927, - "y": 4.070870701250135, - "heading": -1.2897003287062443, - "angularVelocity": -0.000002119634924884961, - "velocityX": 1.089322262884764, - "velocityY": -1.0038025386537657, - "timestamp": 0.27538156267973646 - }, - { - "x": 0.9527932600072707, - "y": 3.95890317697528, - "heading": -1.2722797546854878, - "angularVelocity": 0.18977930676879948, - "velocityX": 1.3236908347464913, - "velocityY": -1.2197714674718914, - "timestamp": 0.3671754169063153 - }, - { - "x": 1.0742999434312064, - "y": 3.8469356525899365, - "heading": -1.2220809193558444, - "angularVelocity": 0.5468648827593129, - "velocityX": 1.3236908336370237, - "velocityY": -1.2197714686755494, - "timestamp": 0.4589692711328941 - }, - { - "x": 1.1958066268271696, - "y": 3.7349681281742373, - "heading": -1.1467025590367632, - "angularVelocity": 0.8211700113715802, - "velocityX": 1.3236908333322916, - "velocityY": -1.2197714690062433, - "timestamp": 0.5507631253594729 - }, - { - "x": 1.317313310224962, - "y": 3.623000603760523, - "heading": -1.052531421525605, - "angularVelocity": 1.0258980658847963, - "velocityX": 1.3236908333522177, - "velocityY": -1.2197714689846195, - "timestamp": 0.6425569795860517 - }, - { - "x": 1.4388199936228405, - "y": 3.5110330793469027, - "heading": -0.944886293720955, - "angularVelocity": 1.1726833861768642, - "velocityX": 1.3236908333531598, - "velocityY": -1.2197714689835972, - "timestamp": 0.7343508338126306 - }, - { - "x": 1.5603266770211324, - "y": 3.3990655549337307, - "heading": -0.8282324643064151, - "angularVelocity": 1.2708239609004537, - "velocityX": 1.3236908333576596, - "velocityY": -1.2197714689787138, - "timestamp": 0.8261446880392094 - }, - { - "x": 1.6818333604204667, - "y": 3.2870980305216904, - "heading": -0.7064237333235655, - "angularVelocity": 1.3269813323469737, - "velocityX": 1.3236908333690174, - "velocityY": -1.2197714689663888, - "timestamp": 0.9179385422657882 - }, - { - "x": 1.8033400438208684, - "y": 3.1751305061108077, - "heading": -0.5829476815179916, - "angularVelocity": 1.3451450845586277, - "velocityX": 1.3236908333806428, - "velocityY": -1.2197714689537729, - "timestamp": 1.009732396492367 - }, - { - "x": 1.924846727221886, - "y": 3.063162981700594, - "heading": -0.46116555861480535, - "angularVelocity": 1.3266914645787278, - "velocityX": 1.3236908333873554, - "velocityY": -1.2197714689464882, - "timestamp": 1.1015262507189463 - }, - { - "x": 2.046353410622716, - "y": 2.9511954572901766, - "heading": -0.34455649847407266, - "angularVelocity": 1.2703362455279548, - "velocityX": 1.3236908333853132, - "velocityY": -1.2197714689487043, - "timestamp": 1.1933201049455255 - }, - { - "x": 2.1678600940228114, - "y": 2.8392279328789614, - "heading": -0.23696281685541168, - "angularVelocity": 1.1721229326865672, - "velocityX": 1.323690833377306, - "velocityY": -1.2197714689573935, - "timestamp": 1.2851139591721048 - }, - { - "x": 2.289366777422274, - "y": 2.72726040846706, - "heading": -0.14284186452712305, - "angularVelocity": 1.0253513497316018, - "velocityX": 1.3236908333704145, - "velocityY": -1.2197714689648724, - "timestamp": 1.376907813398684 - }, - { - "x": 2.4108734608217963, - "y": 2.615292884055223, - "heading": -0.06751124359649471, - "angularVelocity": 0.8206499396428691, - "velocityX": 1.3236908333710633, - "velocityY": -1.2197714689641683, - "timestamp": 1.4687016676252633 - }, - { - "x": 2.532380144216022, - "y": 2.5033253596376386, - "heading": -0.01736204764919088, - "angularVelocity": 0.5463241125437254, - "velocityX": 1.323690833313363, - "velocityY": -1.2197714690267845, - "timestamp": 1.5604955218518426 - }, - { - "x": 2.653886827590858, - "y": 2.3913578351989813, - "heading": 3.290091654253242e-7, - "angularVelocity": 0.18914530612801114, - "velocityX": 1.323690833102131, - "velocityY": -1.2197714692563444, - "timestamp": 1.6522893760784219 - }, - { - "x": 2.753879916683977, - "y": 2.2992149313891423, - "heading": 1.3424729952041108e-7, - "angularVelocity": -0.000002121730997647816, - "velocityX": 1.0893222638446092, - "velocityY": -1.0038025376121436, - "timestamp": 1.7440832303050011 - }, - { - "x": 2.820541981138631, - "y": 2.2377863241812563, - "heading": 3.972989913786345e-8, - "angularVelocity": -0.0000010296702451404156, - "velocityX": 0.7262148976783225, - "velocityY": -0.6692017425944325, - "timestamp": 1.8358770845315804 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": -2.973359997375955e-18, - "angularVelocity": -4.328165483079531e-7, - "velocityX": 0.363107460649484, - "velocityY": -0.33460088219431694, - "timestamp": 1.9276709387581596 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": -1.4515098757453194e-18, - "angularVelocity": 7.662849167209691e-19, - "velocityX": 3.0637745601974264e-18, - "velocityY": -2.8228110009733016e-18, - "timestamp": 2.019464792984739 + "x": 0.6301200123670696, + "y": 4.288467558517564, + "heading": -1.1752680974581398, + "angularVelocity": 9.654043757607686e-7, + "velocityX": 0.27317275726109946, + "velocityY": -0.27820276911277114, + "timestamp": 0.07248415317259349 + }, + { + "x": 0.669721403553962, + "y": 4.248136975067876, + "heading": -1.1752679470653256, + "angularVelocity": 0.000002074837150226295, + "velocityX": 0.5463455038592624, + "velocityY": -0.5564055270626612, + "timestamp": 0.14496830634518698 + }, + { + "x": 0.7291234886592751, + "y": 4.187641101646045, + "heading": -1.1752676989048652, + "angularVelocity": 0.0000034236512224661995, + "velocityX": 0.8195182326800431, + "velocityY": -0.8346082664135338, + "timestamp": 0.21745245951778047 + }, + { + "x": 0.8083262651042996, + "y": 4.106979940946794, + "heading": -1.175267318473291, + "angularVelocity": 0.000005248479232969504, + "velocityX": 1.0926909259246422, + "velocityY": -1.1128109685876706, + "timestamp": 0.28993661269037396 + }, + { + "x": 0.9073297251428862, + "y": 4.0061535010442855, + "heading": -1.1752667040574094, + "angularVelocity": 0.000008476554555828568, + "velocityX": 1.3658635123024454, + "velocityY": -1.3910135593697104, + "timestamp": 0.36242076586296745 + }, + { + "x": 1.0190552899168186, + "y": 3.892370696728415, + "heading": -1.1634451754658162, + "angularVelocity": 0.16309121475754929, + "velocityX": 1.5413791826732202, + "velocityY": -1.5697611041263104, + "timestamp": 0.43490491903556094 + }, + { + "x": 1.13078085649812, + "y": 3.778587894187107, + "heading": -1.1292999776950812, + "angularVelocity": 0.47107121041240924, + "velocityX": 1.5413792076079027, + "velocityY": -1.56976107964423, + "timestamp": 0.5073890722081544 + }, + { + "x": 1.2425064227992928, + "y": 3.6648050913707353, + "heading": -1.0775130899678211, + "angularVelocity": 0.7144580637363105, + "velocityX": 1.541379203743213, + "velocityY": -1.569761083439046, + "timestamp": 0.5798732253807479 + }, + { + "x": 1.3542319891398829, + "y": 3.551022288593068, + "heading": -1.012058895995312, + "angularVelocity": 0.9030138465803919, + "velocityX": 1.541379204287016, + "velocityY": -1.569761082905074, + "timestamp": 0.652357378553341 + }, + { + "x": 1.4659575555155513, + "y": 3.4372394858498447, + "heading": -0.9362447812989615, + "angularVelocity": 1.0459405453192618, + "velocityX": 1.5413792047709636, + "velocityY": -1.5697610824298727, + "timestamp": 0.724841531725934 + }, + { + "x": 1.5776831219356193, + "y": 3.3234566831502192, + "heading": -0.8527996233329523, + "angularVelocity": 1.151219326068592, + "velocityX": 1.541379205383503, + "velocityY": -1.5697610818284016, + "timestamp": 0.7973256848985271 + }, + { + "x": 1.689408688396406, + "y": 3.209673880490576, + "heading": -0.7639848157453195, + "angularVelocity": 1.2252996510306196, + "velocityX": 1.5413792059452611, + "velocityY": -1.5697610812767913, + "timestamp": 0.8698098380711201 + }, + { + "x": 1.801134254849195, + "y": 3.0958910778230817, + "heading": -0.67170962738432, + "angularVelocity": 1.2730394758324133, + "velocityX": 1.541379205834932, + "velocityY": -1.5697610813851162, + "timestamp": 0.9422939912437132 + }, + { + "x": 1.9128598211418812, + "y": 2.982108274998379, + "heading": -0.5776425629397484, + "angularVelocity": 1.2977604114458139, + "velocityX": 1.541379203626127, + "velocityY": -1.569761083553978, + "timestamp": 1.0147781444163062 + }, + { + "x": 2.0245853866391266, + "y": 2.868325471392617, + "heading": -0.4833222706746966, + "angularVelocity": 1.3012539725816417, + "velocityX": 1.541379192652128, + "velocityY": -1.5697610943295663, + "timestamp": 1.0872622975888993 + }, + { + "x": 2.136310952683287, + "y": 2.754542668323881, + "heading": -0.39024489642260013, + "angularVelocity": 1.2841065278154646, + "velocityX": 1.5413792001974338, + "velocityY": -1.5697610869206908, + "timestamp": 1.1597464507614923 + }, + { + "x": 2.248036532216041, + "y": 2.640759878499861, + "heading": -0.2999850951622067, + "angularVelocity": 1.2452349556389624, + "velocityX": 1.5413793862876775, + "velocityY": -1.5697609041950171, + "timestamp": 1.2322306039340853 + }, + { + "x": 2.3597621971135103, + "y": 2.5269771724971815, + "heading": -0.2143083418093875, + "angularVelocity": 1.1820066814993688, + "velocityX": 1.5413805639894413, + "velocityY": -1.5697597477858372, + "timestamp": 1.3047147571066784 + }, + { + "x": 2.4714874560252893, + "y": 2.4131940678483024, + "heading": -0.13529003999281422, + "angularVelocity": 1.090145892005274, + "velocityX": 1.5413749629625242, + "velocityY": -1.569765247556264, + "timestamp": 1.3771989102792714 + }, + { + "x": 2.5832186089625115, + "y": 2.299416750928513, + "heading": -0.06541193776693101, + "angularVelocity": 0.9640466111193448, + "velocityX": 1.541456277638765, + "velocityY": -1.569685399357166, + "timestamp": 1.4496830634518645 + }, + { + "x": 2.707773208618164, + "y": 2.199841260910034, + "heading": -4.9936521691807046e-20, + "angularVelocity": 0.902430874941302, + "velocityX": 1.7183700740639751, + "velocityY": -1.3737553059546213, + "timestamp": 1.5221672166244575 + }, + { + "x": 2.9476724815434237, + "y": 2.102446510883036, + "heading": 0.10620154481535027, + "angularVelocity": 0.9023913668819601, + "velocityX": 2.0384169852270295, + "velocityY": -0.8275602935604923, + "timestamp": 1.6398562246458166 + }, + { + "x": 3.193867360329526, + "y": 2.0222874179592125, + "heading": 0.17246350493624535, + "angularVelocity": 0.5630259039051577, + "velocityX": 2.09191056093718, + "velocityY": -0.6811094279023517, + "timestamp": 1.7575452326671757 + }, + { + "x": 3.4400624973149907, + "y": 1.9421291180546993, + "heading": 0.20745441768725084, + "angularVelocity": 0.297316744692814, + "velocityX": 2.091912754849484, + "velocityY": -0.681102689640865, + "timestamp": 1.8752342406885347 + }, + { + "x": 3.686257637089198, + "y": 1.8619708267154145, + "heading": 0.22097348970875463, + "angularVelocity": 0.11487115278497866, + "velocityX": 2.0919127785453435, + "velocityY": -0.6811026168623736, + "timestamp": 1.9929232487098938 + }, + { + "x": 3.9324527753419964, + "y": 1.7818125307033403, + "heading": 0.21984741537240557, + "angularVelocity": -0.00956822013587358, + "velocityX": 2.0919127656179786, + "velocityY": -0.681102656566928, + "timestamp": 2.110612256731253 + }, + { + "x": 4.178647913249799, + "y": 1.70165423363166, + "heading": 0.2088072590170498, + "angularVelocity": -0.09380788011509143, + "velocityX": 2.091912762686554, + "velocityY": -0.6811026655703678, + "timestamp": 2.228301264752612 + }, + { + "x": 4.424843051210236, + "y": 1.6214959367216402, + "heading": 0.19113828458329582, + "angularVelocity": -0.15013275012506105, + "velocityX": 2.091912763133796, + "velocityY": -0.6811026641967436, + "timestamp": 2.345990272773971 + }, + { + "x": 4.671038189250915, + "y": 1.5413376400580665, + "heading": 0.16914865389762265, + "angularVelocity": -0.18684523776151937, + "velocityX": 2.0919127638155985, + "velocityY": -0.6811026621026981, + "timestamp": 2.46367928079533 + }, + { + "x": 4.917233327344427, + "y": 1.4611793435567624, + "heading": 0.1445059099238244, + "angularVelocity": -0.20938866244279256, + "velocityX": 2.091912764264522, + "velocityY": -0.6811026607238972, + "timestamp": 2.581368288816689 + }, + { + "x": 5.1634284654633715, + "y": 1.381021047133573, + "heading": 0.11848538680518875, + "angularVelocity": -0.2210956108487915, + "velocityX": 2.091912764480629, + "velocityY": -0.6811026600601565, + "timestamp": 2.699057296838048 + }, + { + "x": 5.409623603591471, + "y": 1.3008627507385027, + "heading": 0.09216479440710998, + "angularVelocity": -0.22364529058949856, + "velocityX": 2.0919127645584195, + "velocityY": -0.6811026598212322, + "timestamp": 2.8167463048594072 + }, + { + "x": 5.6558187417204895, + "y": 1.2207044543462509, + "heading": 0.06658302708804537, + "angularVelocity": -0.2173675158725422, + "velocityX": 2.0919127645662163, + "velocityY": -0.6811026597972839, + "timestamp": 2.9344353128807663 + }, + { + "x": 5.902013879846113, + "y": 1.140546157943573, + "heading": 0.04288417914505449, + "angularVelocity": -0.20136840594933111, + "velocityX": 2.0919127645373723, + "velocityY": -0.6811026598858735, + "timestamp": 3.0521243209021254 + }, + { + "x": 6.148209017964777, + "y": 1.0603878615195235, + "heading": 0.022487251104018585, + "angularVelocity": -0.17331209077189777, + "velocityX": 2.091912764478247, + "velocityY": -0.6811026600674663, + "timestamp": 3.1698133289234844 + }, + { + "x": 6.394404156072785, + "y": 0.9802295650627441, + "heading": 0.0073000213594665506, + "angularVelocity": -0.12904543933112522, + "velocityX": 2.0919127643876987, + "velocityY": -0.681102660345571, + "timestamp": 3.2875023369448435 + }, + { + "x": 6.640599294205446, + "y": 0.9000712686819135, + "heading": 5.724943832354313e-7, + "angularVelocity": -0.062023199853654426, + "velocityX": 2.091912764597172, + "velocityY": -0.6811026597002373, + "timestamp": 3.4051913449662026 + }, + { + "x": 6.85312987863822, + "y": 0.8308737624937756, + "heading": 2.400111394619599e-7, + "angularVelocity": -0.000002825100224376875, + "velocityX": 1.805866053303826, + "velocityY": -0.5879691515080071, + "timestamp": 3.5228803529875616 + }, + { + "x": 6.994816942867847, + "y": 0.7847420891244803, + "heading": 7.233437732941088e-8, + "angularVelocity": -0.0000014247444565862307, + "velocityX": 1.2039107696779348, + "velocityY": -0.39197945623709224, + "timestamp": 3.6405693610089207 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -1.572940436060772e-18, + "angularVelocity": -6.146230510677986e-7, + "velocityX": 0.6019553993000372, + "velocityY": -0.19598973280511345, + "timestamp": 3.7582583690302798 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -7.302182224417507e-19, + "angularVelocity": 9.559430564762068e-19, + "velocityX": -2.228194904091084e-17, + "velocityY": 8.974683860818082e-18, + "timestamp": 3.875947377051639 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.5.traj b/src/main/deploy/choreo/2 piece right.5.traj new file mode 100644 index 0000000..93ac904 --- /dev/null +++ b/src/main/deploy/choreo/2 piece right.5.traj @@ -0,0 +1,184 @@ +{ + "samples": [ + { + "x": 2.7077732086181636, + "y": 2.1998412609100346, + "heading": 2.735444848762468e-18, + "angularVelocity": -2.5364805221997074e-18, + "velocityX": -1.3365971515795454e-16, + "velocityY": 1.2755059167320944e-16, + "timestamp": 0 + }, + { + "x": 2.783994140884582, + "y": 2.1815396244139365, + "heading": -6.913548175875285e-18, + "angularVelocity": -7.993053002780074e-17, + "velocityX": 0.6314005512964709, + "velocityY": -0.15160747880743441, + "timestamp": 0.1207172406009338 + }, + { + "x": 2.9364360018279116, + "y": 2.144936352283628, + "heading": -1.1825996831406165e-17, + "angularVelocity": -4.069384481589834e-17, + "velocityX": 1.2628010728581114, + "velocityY": -0.30321495047514957, + "timestamp": 0.2414344812018676 + }, + { + "x": 3.165098780679637, + "y": 2.0900314471047694, + "heading": -1.8877982605670813e-17, + "angularVelocity": -5.841738710433222e-17, + "velocityX": 1.8942015052152972, + "velocityY": -0.45482240072371566, + "timestamp": 0.3621517218028014 + }, + { + "x": 3.4233367594479955, + "y": 2.028025152031049, + "heading": 1.3234683466552848e-14, + "angularVelocity": 1.0979012925727819e-13, + "velocityX": 2.139197164239702, + "velocityY": -0.513649042713796, + "timestamp": 0.4828689624037352 + }, + { + "x": 3.681574738216356, + "y": 1.9660188569573283, + "heading": 8.966413209720006e-15, + "angularVelocity": -3.5357586336345784e-14, + "velocityX": 2.139197164239714, + "velocityY": -0.5136490427137981, + "timestamp": 0.603586203004669 + }, + { + "x": 3.939812716984716, + "y": 1.9040125618836077, + "heading": 5.837294795235967e-14, + "angularVelocity": 4.092748848192075e-13, + "velocityX": 2.1391971642397136, + "velocityY": -0.513649042713798, + "timestamp": 0.7243034436056028 + }, + { + "x": 4.198050695753076, + "y": 1.8420062668098869, + "heading": 1.0674696317407068e-13, + "angularVelocity": 4.007216780378996e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 0.8450206842065366 + }, + { + "x": 4.456288674521436, + "y": 1.7799999717361663, + "heading": 1.4572897454366185e-13, + "angularVelocity": 3.229200002877606e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 0.9657379248074704 + }, + { + "x": 4.714526653289795, + "y": 1.7179936766624455, + "heading": 1.5144680025336213e-13, + "angularVelocity": 4.736544408434699e-14, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 1.0864551654084043 + }, + { + "x": 4.972764632058155, + "y": 1.655987381588725, + "heading": 1.2356361201484233e-13, + "angularVelocity": -2.309793373317404e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.5136490427137981, + "timestamp": 1.207172406009338 + }, + { + "x": 5.231002610826515, + "y": 1.5939810865150041, + "heading": 8.481737461837235e-14, + "angularVelocity": -3.2096689092287344e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.5136490427137981, + "timestamp": 1.3278896466102719 + }, + { + "x": 5.489240589594875, + "y": 1.5319747914412833, + "heading": 3.644351765856618e-14, + "angularVelocity": -4.0072036702462715e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 1.4486068872112057 + }, + { + "x": 5.747478568363234, + "y": 1.4699684963675628, + "heading": 8.953330830931353e-15, + "angularVelocity": -2.27723784032737e-13, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 1.5693241278121395 + }, + { + "x": 6.005716547131594, + "y": 1.407962201293842, + "heading": -3.230680878301987e-16, + "angularVelocity": -7.684402718769493e-14, + "velocityX": 2.139197164239714, + "velocityY": -0.513649042713798, + "timestamp": 1.6900413684130733 + }, + { + "x": 6.263954525899953, + "y": 1.3459559062201216, + "heading": 2.5738532927961268e-17, + "angularVelocity": 2.88945157312828e-15, + "velocityX": 2.139197164239702, + "velocityY": -0.5136490427137953, + "timestamp": 1.810758609014007 + }, + { + "x": 6.492617304751678, + "y": 1.2910510010412626, + "heading": 1.5719519359178488e-17, + "angularVelocity": -8.299571394273679e-17, + "velocityX": 1.894201505215297, + "velocityY": -0.4548224007237162, + "timestamp": 1.931475849614941 + }, + { + "x": 6.645059165695007, + "y": 1.254447728910954, + "heading": 9.463618751509253e-18, + "angularVelocity": -5.18227601676043e-17, + "velocityX": 1.2628010728581116, + "velocityY": -0.3032149504751499, + "timestamp": 2.0521930902158747 + }, + { + "x": 6.721280097961426, + "y": 1.236146092414856, + "heading": -1.8630663507818504e-28, + "angularVelocity": -7.839492275233649e-17, + "velocityX": 0.6314005512964709, + "velocityY": -0.15160747880743458, + "timestamp": 2.1729103308168085 + }, + { + "x": 6.721280097961426, + "y": 1.236146092414856, + "heading": -8.648149721186173e-29, + "angularVelocity": 1.1053632926040662e-28, + "velocityX": -7.56285135271065e-18, + "velocityY": 1.8159388918179766e-18, + "timestamp": 2.2936275714177423 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/2 piece right.traj b/src/main/deploy/choreo/2 piece right.traj index 86b9a75..73113ca 100644 --- a/src/main/deploy/choreo/2 piece right.traj +++ b/src/main/deploy/choreo/2 piece right.traj @@ -4,586 +4,757 @@ "x": 0.6313003897666931, "y": 4.255156517028809, "heading": -1.2897, - "angularVelocity": -2.3969037863354903e-26, - "velocityX": -5.149474371067618e-26, - "velocityY": 3.9840780609445665e-27, + "angularVelocity": -3.635906927924615e-19, + "velocityX": -6.136781108423474e-19, + "velocityY": 9.990271880480628e-19, "timestamp": 0 }, { - "x": 0.6442648163276663, - "y": 4.253481865948087, - "heading": -1.289699890378596, - "angularVelocity": 0.0000022236977895922636, - "velocityX": 0.2629866575194569, - "velocityY": -0.03397071888670311, - "timestamp": 0.049296898493848725 - }, - { - "x": 0.6701936687758512, - "y": 4.250132563317103, - "heading": -1.2896996605083761, - "angularVelocity": 0.000004662975296506846, - "velocityX": 0.5259733013714917, - "velocityY": -0.06794144729820102, - "timestamp": 0.09859379698769745 - }, - { - "x": 0.7090869461006044, - "y": 4.245108608431542, - "heading": -1.2896992944482415, - "angularVelocity": 0.0000074256220104019626, - "velocityX": 0.7889599247223699, - "velocityY": -0.10191218999686634, - "timestamp": 0.14789069548154618 - }, - { - "x": 0.760944646617517, - "y": 4.238410000117552, - "heading": -1.2896987656297503, - "angularVelocity": 0.000010727216264499804, - "velocityX": 1.0519465139045912, - "velocityY": -0.13588295650740967, - "timestamp": 0.1971875939753949 - }, - { - "x": 0.82576676695776, - "y": 4.230036736027443, - "heading": -1.2896980209162199, - "angularVelocity": 0.000015106701495517666, - "velocityX": 1.3149330347492647, - "velocityY": -0.16985377064146243, - "timestamp": 0.24648449246924362 - }, - { - "x": 0.9035532970147546, - "y": 4.21998880911824, - "heading": -1.2896969008990993, - "angularVelocity": 0.000022719829335288094, - "velocityX": 1.5779193505794464, - "velocityY": -0.2038247276440018, - "timestamp": 0.29578139096309236 - }, - { - "x": 0.9915565704265453, - "y": 4.208621313799639, - "heading": -1.27289000896861, - "angularVelocity": 0.3409320351580815, - "velocityX": 1.7851685623340352, - "velocityY": -0.2305925051252473, - "timestamp": 0.3450782894569411 - }, - { - "x": 1.0795598293475788, - "y": 4.197253706298393, - "heading": -1.2169726689327969, - "angularVelocity": 1.134297323852746, - "velocityX": 1.7851682683853676, - "velocityY": -0.23059478077843756, - "timestamp": 0.3943751879507898 - }, - { - "x": 1.1675630883225618, - "y": 4.185886099214807, - "heading": -1.123282876673827, - "angularVelocity": 1.9005210291406174, - "velocityX": 1.7851682694797597, - "velocityY": -0.23059477230609737, - "timestamp": 0.4436720864446385 - }, - { - "x": 1.255566347295848, - "y": 4.174518492118083, - "heading": -0.9939341877349891, - "angularVelocity": 2.6238707279927147, - "velocityX": 1.785168269445332, - "velocityY": -0.23059477257260114, - "timestamp": 0.49296898493848723 - }, - { - "x": 1.3435696062497653, - "y": 4.163150884871416, - "heading": -0.8327019973298604, - "angularVelocity": 3.2706355842091646, - "velocityX": 1.7851682690524295, - "velocityY": -0.23059477561424194, - "timestamp": 0.542265883432336 - }, - { - "x": 1.4315728650960726, - "y": 4.15178327679168, - "heading": -0.6477412989500829, - "angularVelocity": 3.7519743438394353, - "velocityX": 1.7851682668695343, - "velocityY": -0.23059479251323747, - "timestamp": 0.5915627819261847 - }, - { - "x": 1.519576123922667, - "y": 4.140415668559335, - "heading": -0.46074819376328596, - "angularVelocity": 3.7932022277249358, - "velocityX": 1.7851682664696522, - "velocityY": -0.23059479560896612, - "timestamp": 0.6408596804200335 - }, - { - "x": 1.6075793828656728, - "y": 4.129048061228197, - "heading": -0.29527670366521236, - "angularVelocity": 3.356630846030226, - "velocityX": 1.7851682688310881, - "velocityY": -0.23059477732777772, - "timestamp": 0.6901565789138823 - }, - { - "x": 1.6955826418903344, - "y": 4.117680454529199, - "heading": -0.16085599473456663, - "angularVelocity": 2.7267579307736534, - "velocityX": 1.7851682704874938, - "velocityY": -0.23059476450461613, - "timestamp": 0.739453477407731 - }, - { - "x": 1.7835859004541794, - "y": 4.106312844262756, - "heading": -0.061669148748961934, - "angularVelocity": 2.0120301482654375, - "velocityX": 1.7851682611397215, - "velocityY": -0.2305948368711734, - "timestamp": 0.7887503759015798 + "x": 0.655972085024139, + "y": 4.251969629979563, + "heading": -1.2896996398384766, + "angularVelocity": 0.000005296084636152658, + "velocityX": 0.3627910748292595, + "velocityY": -0.04686237268621969, + "timestamp": 0.06800524315284516 + }, + { + "x": 0.7053154730729085, + "y": 4.245595854265352, + "heading": -1.2896988073147642, + "angularVelocity": 0.000012242051844172605, + "velocityX": 0.7255821133948134, + "velocityY": -0.09372476913119913, + "timestamp": 0.13601048630569032 + }, + { + "x": 0.7793305465080104, + "y": 4.2360351849906746, + "heading": -1.2896971657533272, + "angularVelocity": 0.000024138748145691714, + "velocityX": 1.08837304307183, + "velocityY": -0.1405872375632783, + "timestamp": 0.20401572945853547 + }, + { + "x": 0.8737534122985577, + "y": 4.223838483147547, + "heading": -1.2688252083137923, + "angularVelocity": 0.3069168856969461, + "velocityX": 1.388464497925952, + "velocityY": -0.17934943362697756, + "timestamp": 0.27202097261138064 + }, + { + "x": 0.9681762695915698, + "y": 4.211641715518689, + "heading": -1.1855949428153905, + "angularVelocity": 1.2238801251153222, + "velocityX": 1.3884643729718302, + "velocityY": -0.1793504009896027, + "timestamp": 0.3400262157642258 + }, + { + "x": 1.0625991288182361, + "y": 4.199444962859495, + "heading": -1.0462591608699763, + "angularVelocity": 2.048897636205047, + "velocityX": 1.3884644014057288, + "velocityY": -0.17935018086444265, + "timestamp": 0.408031458917071 + }, + { + "x": 1.1570219877844825, + "y": 4.187248208184227, + "heading": -0.8608593609825309, + "angularVelocity": 2.7262574368089574, + "velocityX": 1.388464397576322, + "velocityY": -0.17935021051031644, + "timestamp": 0.47603670206991616 + }, + { + "x": 1.2514448467064523, + "y": 4.175051453166187, + "heading": -0.6467911549093175, + "angularVelocity": 3.1478191408283704, + "velocityX": 1.3884643969252477, + "velocityY": -0.17935021555069555, + "timestamp": 0.5440419452227613 + }, + { + "x": 1.3458677056257224, + "y": 4.162854698127245, + "heading": -0.4324354912476929, + "angularVelocity": 3.152046132382022, + "velocityX": 1.3884643968855472, + "velocityY": -0.17935021585804314, + "timestamp": 0.6120471883756065 + }, + { + "x": 1.4402905645694315, + "y": 4.1506579432775, + "heading": -0.24636817246823917, + "angularVelocity": 2.7360731342619897, + "velocityX": 1.3884643972449147, + "velocityY": -0.17935021307595345, + "timestamp": 0.6800524315284516 + }, + { + "x": 1.5347134236322062, + "y": 4.138461189349519, + "heading": -0.10616450182646274, + "angularVelocity": 2.061659721246224, + "velocityX": 1.388464398995746, + "velocityY": -0.17935019952165523, + "timestamp": 0.7480576746812968 + }, + { + "x": 1.6291362817773116, + "y": 4.126264428317271, + "heading": -0.02194518417619484, + "angularVelocity": 1.238423888301977, + "velocityX": 1.3884643855016514, + "velocityY": -0.17935030398809765, + "timestamp": 0.816062917834142 + }, + { + "x": 1.7235591439282192, + "y": 4.114067698297376, + "heading": -0.0000028452682314893883, + "angularVelocity": 0.3226565760326282, + "velocityX": 1.3884644444059648, + "velocityY": -0.1793498479592596, + "timestamp": 0.8840681609869872 + }, + { + "x": 1.7975742187998267, + "y": 4.104507040143629, + "heading": -0.0000011981495890210145, + "angularVelocity": 0.000024220465453921752, + "velocityX": 1.0883730641952847, + "velocityY": -0.14058707403279755, + "timestamp": 0.9520734041398323 + }, + { + "x": 1.846917607499234, + "y": 4.098133269466462, + "heading": -3.6197485474192144e-7, + "angularVelocity": 0.000012295739203525973, + "velocityX": 0.7255821229622761, + "velocityY": -0.0937246950627145, + "timestamp": 1.0200786472926775 }, { "x": 1.8715893030166624, "y": 4.094946384429932, - "heading": -2.0036368727453063e-25, - "angularVelocity": 1.2509742120319602, - "velocityX": 1.785171182188348, - "velocityY": -0.23057150003548213, - "timestamp": 0.8380472743954286 - }, - { - "x": 2.0015014856340083, - "y": 4.096925192128385, - "heading": 0.03734833063779682, - "angularVelocity": 0.5174202657607754, - "velocityX": 1.7997911796197699, - "velocityY": 0.02741421605031293, - "timestamp": 0.9102290830899192 - }, - { - "x": 2.1314136526758922, - "y": 4.098905022118577, - "heading": 0.0000024861301875270727, - "angularVelocity": -0.5173858231465974, - "velocityX": 1.799790963838784, - "velocityY": 0.027428378784090407, - "timestamp": 0.9824108917844099 - }, - { - "x": 2.243505108940701, - "y": 4.100613269261141, - "heading": 0.0000014496891722730681, - "angularVelocity": -0.000014358756506653064, - "velocityX": 1.5529045100440175, - "velocityY": 0.02366589551382172, - "timestamp": 1.0545927004789006 - }, - { - "x": 2.3275737089377446, - "y": 4.101894454670087, - "heading": 7.139853498794832e-7, - "angularVelocity": -0.000010192371675077432, - "velocityX": 1.1646784905718206, - "velocityY": 0.017749422356118257, - "timestamp": 1.1267745091733914 - }, - { - "x": 2.3836194445295247, - "y": 4.102748578291145, - "heading": 2.3558195309312242e-7, - "angularVelocity": -0.000006627755738446538, - "velocityX": 0.7764523583634968, - "velocityY": 0.011832948446505252, - "timestamp": 1.1989563178678821 + "heading": 1.3449524044377914e-18, + "angularVelocity": 0.000005322749217003313, + "velocityX": 0.3627910786522388, + "velocityY": -0.04686234308975064, + "timestamp": 1.0880838904455226 + }, + { + "x": 1.8715893030166624, + "y": 4.094946384429932, + "heading": 6.590717682190205e-19, + "angularVelocity": -3.942176622173566e-19, + "velocityX": -4.110872129098873e-19, + "velocityY": 5.381793683550551e-19, + "timestamp": 1.1560891335983676 + }, + { + "x": 1.93115886834108, + "y": 4.095854097514598, + "heading": -1.5234931737044087e-18, + "angularVelocity": -2.073879403640416e-17, + "velocityX": 0.5660317007877699, + "velocityY": 0.00862511549888298, + "timestamp": 1.2613298210749035 + }, + { + "x": 2.03638733817559, + "y": 4.097457554891332, + "heading": -7.082480253674674e-16, + "angularVelocity": -6.715316567570261e-15, + "velocityX": 0.9998839076186332, + "velocityY": 0.015236097513059094, + "timestamp": 1.3665705085514395 + }, + { + "x": 2.1416158080101013, + "y": 4.099061012268066, + "heading": 3.9820301050635646e-17, + "angularVelocity": 7.108166473969379e-15, + "velocityX": 0.9998839076186449, + "velocityY": 0.015236097513059273, + "timestamp": 1.4718111960279754 + }, + { + "x": 2.2468442778446125, + "y": 4.1006644696448005, + "heading": 5.47356752522011e-16, + "angularVelocity": 4.822625769948835e-15, + "velocityX": 0.9998839076186449, + "velocityY": 0.015236097513059273, + "timestamp": 1.5770518835045113 + }, + { + "x": 2.3520727476791223, + "y": 4.1022679270215345, + "heading": 2.3383723081945662e-18, + "angularVelocity": -5.178780120903659e-15, + "velocityX": 0.9998839076186332, + "velocityY": 0.015236097513059094, + "timestamp": 1.6822925709810472 }, { "x": 2.41164231300354, "y": 4.103175640106201, - "heading": -7.741029200580404e-24, - "angularVelocity": -0.000003263730257719399, - "velocityX": 0.3882261885764356, - "velocityY": 0.005916474285980588, - "timestamp": 1.271138126562373 + "heading": 1.4822311094594617e-19, + "angularVelocity": -2.0810859845807786e-17, + "velocityX": 0.5660317007877698, + "velocityY": 0.00862511549888298, + "timestamp": 1.7875332584575832 }, { "x": 2.41164231300354, "y": 4.103175640106201, - "heading": -4.8316826521123155e-24, - "angularVelocity": 4.329873823266725e-23, - "velocityX": 4.0284965680453105e-22, - "velocityY": -2.2471582465129843e-22, - "timestamp": 1.3433199352568637 - }, - { - "x": 2.344735769891283, - "y": 4.110806937625484, - "heading": -6.978848485329857e-9, - "angularVelocity": -6.237346131654942e-8, - "velocityX": -0.5979772576247802, - "velocityY": 0.068204724836005, - "timestamp": 1.4552080418012758 - }, - { - "x": 2.2335684425317694, - "y": 4.123486578658459, - "heading": -0.015246855809319428, - "angularVelocity": -0.13626871793043496, - "velocityX": -0.9935580357273067, - "velocityY": 0.11332429714450055, - "timestamp": 1.567096148345688 - }, - { - "x": 2.122401115172327, - "y": 4.136166219692066, - "heading": -0.060800247349853634, - "angularVelocity": -0.40713345633794035, - "velocityX": -0.9935580357266695, - "velocityY": 0.1133242971501526, - "timestamp": 1.6789842548901002 - }, - { - "x": 2.0112337878128743, - "y": 4.148845860725579, - "heading": -0.12944768713850766, - "angularVelocity": -0.6135365224131806, - "velocityX": -0.9935580357267644, - "velocityY": 0.11332429714932027, - "timestamp": 1.7908723614345123 - }, - { - "x": 1.9000664604534496, - "y": 4.16152550175934, - "heading": -0.21530108218408722, - "angularVelocity": -0.7673147548662962, - "velocityX": -0.9935580357265124, - "velocityY": 0.11332429715152995, - "timestamp": 1.9027604679789245 - }, - { - "x": 1.788899133093997, - "y": 4.174205142792856, - "heading": -0.31351675402178747, - "angularVelocity": -0.8778026089726992, - "velocityX": -0.9935580357267627, - "velocityY": 0.1133242971493348, - "timestamp": 2.0146485745233367 - }, - { - "x": 1.6777318057328703, - "y": 4.1868847838116965, - "heading": -0.4200302675690305, - "angularVelocity": -0.9519645727936626, - "velocityX": -0.9935580357417213, - "velocityY": 0.11332429701818696, - "timestamp": 2.1265366810677486 - }, - { - "x": 1.5665644783751262, - "y": 4.199564424860192, - "heading": -0.5313117322584114, - "angularVelocity": -0.994578138161715, - "velocityX": -0.9935580357114923, - "velocityY": 0.11332429728321564, - "timestamp": 2.2384247876121606 - }, - { - "x": 1.455397151015652, - "y": 4.212244065893518, - "heading": -0.6441464282968985, - "angularVelocity": -1.008460144007346, - "velocityX": -0.9935580357269562, - "velocityY": 0.1133242971476382, - "timestamp": 2.3503128941565725 - }, - { - "x": 1.344229823656203, - "y": 4.2249237069270675, - "heading": -0.7554226234189, - "angularVelocity": -0.994531041401011, - "velocityX": -0.9935580357267284, - "velocityY": 0.11332429714963566, - "timestamp": 2.4622010007009845 - }, - { - "x": 1.2330624962967764, - "y": 4.237603347960814, - "heading": -0.8619240929821974, - "angularVelocity": -0.951856929682008, - "velocityX": -0.9935580357265276, - "velocityY": 0.11332429715139572, - "timestamp": 2.5740891072453964 - }, - { - "x": 1.121895168937324, - "y": 4.250282988994331, - "heading": -0.9601218669400209, - "angularVelocity": -0.8776426466636605, - "velocityX": -0.9935580357267606, - "velocityY": 0.11332429714935303, - "timestamp": 2.6859772137898084 - }, - { - "x": 1.0107278415778729, - "y": 4.262962630027863, - "heading": -1.0459506988902485, - "angularVelocity": -0.7670952221911026, - "velocityX": -0.9935580357267462, - "velocityY": 0.1133242971494796, - "timestamp": 2.7978653203342203 - }, - { - "x": 0.8995605142184344, - "y": 4.275642271061504, - "heading": -1.114565563469038, - "angularVelocity": -0.6132453814611101, - "velocityX": -0.9935580357266346, - "velocityY": 0.11332429715045739, - "timestamp": 2.9097534268786323 - }, - { - "x": 0.7883931868589816, - "y": 4.28832191209502, - "heading": -1.1600763958246794, - "angularVelocity": -0.40675308360479684, - "velocityX": -0.9935580357267626, - "velocityY": 0.11332429714933458, - "timestamp": 3.021641533423044 - }, - { - "x": 0.6772258594993766, - "y": 4.301001553127193, - "heading": -1.1752681604416075, - "angularVelocity": -0.13577640274838362, - "velocityX": -0.9935580357281231, - "velocityY": 0.1133242971373439, - "timestamp": 3.133529639967456 + "heading": 1.6064214107253007e-19, + "angularVelocity": 1.180059777481661e-19, + "velocityX": -1.7693428317330896e-20, + "velocityY": 3.049321257143075e-21, + "timestamp": 1.892773945934119 + }, + { + "x": 2.3717259825825794, + "y": 4.107728459293482, + "heading": -2.4661471568252517e-8, + "angularVelocity": -2.853606326541506e-7, + "velocityX": -0.4618763024941506, + "velocityY": 0.05268117760248238, + "timestamp": 1.979196068492499 + }, + { + "x": 2.2918933241895187, + "y": 4.116834097388246, + "heading": -8.139982855367353e-8, + "angularVelocity": -6.565258443780271e-7, + "velocityX": -0.9237525766522542, + "velocityY": 0.10536235196736803, + "timestamp": 2.065618191050879 + }, + { + "x": 2.1721443451709694, + "y": 4.1304925535509796, + "heading": -1.9247747670303723e-7, + "angularVelocity": -0.0000012852918310874542, + "velocityX": -1.3856287658019122, + "velocityY": 0.15804351661818175, + "timestamp": 2.1520403136092585 + }, + { + "x": 2.051932794176593, + "y": 4.1442037702569685, + "heading": -0.046313366659817376, + "angularVelocity": -0.5358948937068193, + "velocityX": -1.390981237624328, + "velocityY": 0.15865401473711088, + "timestamp": 2.238462436167638 + }, + { + "x": 1.9317212431804152, + "y": 4.157914986947169, + "heading": -0.12967755850503584, + "angularVelocity": -0.964616343331585, + "velocityX": -1.3909812376451667, + "velocityY": 0.15865401455441974, + "timestamp": 2.324884558726018 + }, + { + "x": 1.8115096921845533, + "y": 4.171626203640135, + "heading": -0.2411097709793792, + "angularVelocity": -1.2893945343574351, + "velocityX": -1.3909812376415176, + "velocityY": 0.158654014586412, + "timestamp": 2.4113066812843975 + }, + { + "x": 1.6912981411888635, + "y": 4.185337420334612, + "heading": -0.3720803056041949, + "angularVelocity": -1.5154746348233066, + "velocityX": -1.3909812376395225, + "velocityY": 0.1586540146039027, + "timestamp": 2.497728803842777 + }, + { + "x": 1.571086590193276, + "y": 4.1990486370299855, + "heading": -0.5145335305215332, + "angularVelocity": -1.6483421223669708, + "velocityX": -1.3909812376383404, + "velocityY": 0.15865401461426823, + "timestamp": 2.584150926401157 + }, + { + "x": 1.4508750391976706, + "y": 4.212759853725203, + "heading": -0.6607675274594663, + "angularVelocity": -1.6920898562650917, + "velocityX": -1.3909812376385464, + "velocityY": 0.15865401461246012, + "timestamp": 2.6705730489595365 + }, + { + "x": 1.3306634882019563, + "y": 4.226471070419467, + "heading": -0.8032127126381129, + "angularVelocity": -1.648249093655645, + "velocityX": -1.390981237639804, + "velocityY": 0.15865401460143336, + "timestamp": 2.756995171517916 + }, + { + "x": 1.2104519372061566, + "y": 4.240182287112979, + "heading": -0.9341722064333875, + "angularVelocity": -1.5153468801557017, + "velocityX": -1.390981237640797, + "velocityY": 0.1586540145927282, + "timestamp": 2.843417294076296 + }, + { + "x": 1.0902403862103853, + "y": 4.253893503806742, + "heading": -1.045595793963093, + "angularVelocity": -1.2892947341629668, + "velocityX": -1.3909812376404647, + "velocityY": 0.15865401459564232, + "timestamp": 2.9298394166346755 + }, + { + "x": 0.9700288352147577, + "y": 4.267604720501764, + "heading": -1.1289557818648916, + "angularVelocity": -0.9645676990343222, + "velocityX": -1.3909812376388042, + "velocityY": 0.15865401461020098, + "timestamp": 3.016261539193055 + }, + { + "x": 0.8498172842183673, + "y": 4.281315937190096, + "heading": -1.175267974928737, + "angularVelocity": -0.5358835410754968, + "velocityX": -1.3909812376476314, + "velocityY": 0.15865401453280073, + "timestamp": 3.102683661751435 + }, + { + "x": 0.7300683052006729, + "y": 4.294974393360328, + "heading": -1.1752680860230837, + "angularVelocity": -0.0000012854850518362776, + "velocityX": -1.3856287657920174, + "velocityY": 0.15804351670493202, + "timestamp": 3.1891057843098145 + }, + { + "x": 0.6502356468079885, + "y": 4.304080031458386, + "heading": -1.1752681427696843, + "angularVelocity": -6.566212326774967e-7, + "velocityX": -0.9237525766479047, + "velocityY": 0.10536235200550112, + "timestamp": 3.275527906868194 }, { "x": 0.6103193163871765, "y": 4.308632850646973, "heading": -1.1752681674346586, - "angularVelocity": -6.250039740114642e-8, - "velocityX": -0.5979772576242739, - "velocityY": 0.06820472484044302, - "timestamp": 3.245417746511868 + "angularVelocity": -2.8540116177195145e-7, + "velocityX": -0.4618763024924286, + "velocityY": 0.05268117761757928, + "timestamp": 3.361950029426574 }, { "x": 0.6103193163871765, "y": 4.308632850646973, "heading": -1.1752681674346586, - "angularVelocity": -8.29300949382696e-24, - "velocityX": -1.1501418857045935e-22, - "velocityY": 1.733872473382952e-23, - "timestamp": 3.35730585305628 - }, - { - "x": 0.644363601367681, - "y": 4.2767432034820265, - "heading": -1.1752681587825877, - "angularVelocity": 9.290967586148177e-8, - "velocityX": 0.36558224475309986, - "velocityY": -0.34244481273791916, - "timestamp": 3.450429316980237 - }, - { - "x": 0.712452169167196, - "y": 4.212963911179069, - "heading": -1.1752681390823587, - "angularVelocity": 2.1154957206444014e-7, - "velocityX": 0.7311644662951432, - "velocityY": -0.6848896037097378, - "timestamp": 3.543552780904194 - }, - { - "x": 0.8145850133012432, - "y": 4.117294979818902, - "heading": -1.175268101145713, - "angularVelocity": 4.07380097343134e-7, - "velocityX": 1.0967466182040557, - "velocityY": -1.027334329383292, - "timestamp": 3.6366762448281507 - }, - { - "x": 0.9369198335620701, - "y": 4.002702640501709, - "heading": -1.1572816768173708, - "angularVelocity": 0.19314599747953293, - "velocityX": 1.3136841683716076, - "velocityY": -1.2305420619959617, - "timestamp": 3.7297997087521075 - }, - { - "x": 1.0592546538469616, - "y": 3.888110301210203, - "heading": -1.110256724093758, - "angularVelocity": 0.5049742647246526, - "velocityX": 1.313684168630027, - "velocityY": -1.2305420617201315, - "timestamp": 3.8229231726760644 - }, - { - "x": 1.1815894741316775, - "y": 3.7735179619185093, - "heading": -1.0410814207725312, - "angularVelocity": 0.7428343019726364, - "velocityX": 1.3136841686281384, - "velocityY": -1.2305420617221479, - "timestamp": 3.9160466366000213 - }, - { - "x": 1.3039242944162523, - "y": 3.658925622626665, - "heading": -0.9554475532957203, - "angularVelocity": 0.9195734766346116, - "velocityX": 1.3136841686266243, - "velocityY": -1.2305420617237641, - "timestamp": 4.009170100523979 - }, - { - "x": 1.426259114700816, - "y": 3.5443332833348076, - "heading": -0.8580438926758113, - "angularVelocity": 1.0459626018577572, - "velocityX": 1.3136841686264993, - "velocityY": -1.2305420617238971, - "timestamp": 4.102293564447936 - }, - { - "x": 1.548593934985741, - "y": 3.429740944043337, - "heading": -0.752782430466053, - "angularVelocity": 1.1303430711696154, - "velocityX": 1.3136841686303846, - "velocityY": -1.2305420617197496, - "timestamp": 4.195417028371893 - }, - { - "x": 1.6709287552712397, - "y": 3.3151486047524794, - "heading": -0.6430288452135179, - "angularVelocity": 1.1785814297260035, - "velocityX": 1.313684168636549, - "velocityY": -1.2305420617131688, - "timestamp": 4.2885404922958505 - }, - { - "x": 1.7932635755573523, - "y": 3.2005562654622763, - "heading": -0.5318250037177289, - "angularVelocity": 1.1941549080111136, - "velocityX": 1.3136841686431349, - "velocityY": -1.2305420617061376, - "timestamp": 4.381663956219808 - }, - { - "x": 1.915598395843716, - "y": 3.085963926172342, - "heading": -0.4220971673663666, - "angularVelocity": 1.1783049268975236, - "velocityX": 1.3136841686458358, - "velocityY": -1.2305420617032545, - "timestamp": 4.474787420143765 - }, - { - "x": 2.0379332161298342, - "y": 2.9713715868821446, - "heading": -0.3168810175382735, - "angularVelocity": 1.1298564872330217, - "velocityX": 1.3136841686431986, - "velocityY": -1.23054206170607, - "timestamp": 4.5679108840677225 - }, - { - "x": 2.1602680364155047, - "y": 2.8567792475914704, - "heading": -0.2195335566330217, - "angularVelocity": 1.0453591050344102, - "velocityX": 1.3136841686383904, - "velocityY": -1.2305420617112026, - "timestamp": 4.66103434799168 - }, - { - "x": 2.2826028567007413, - "y": 2.742186908300333, - "heading": -0.1339605112634315, - "angularVelocity": 0.9189203425622967, - "velocityX": 1.3136841686337362, - "velocityY": -1.2305420617161718, - "timestamp": 4.754157811915637 - }, - { - "x": 2.404937676985743, - "y": 2.6275945690089433, - "heading": -0.06484983243968821, - "angularVelocity": 0.7421403361904352, - "velocityX": 1.3136841686312044, - "velocityY": -1.2305420617188745, - "timestamp": 4.8472812758395944 - }, - { - "x": 2.527272497270682, - "y": 2.5130022297174874, - "heading": -0.017897793209595524, - "angularVelocity": 0.5041912881208213, - "velocityX": 1.3136841686305336, - "velocityY": -1.230542061719591, - "timestamp": 4.940404739763552 - }, - { - "x": 2.6496073175587562, - "y": 2.3984098904293836, - "heading": -6.631716442759581e-8, - "angularVelocity": 0.1921935260810968, - "velocityX": 1.3136841686642036, - "velocityY": -1.2305420616835967, - "timestamp": 5.033528203687509 - }, - { - "x": 2.751740161678901, - "y": 2.302740959054374, - "heading": -2.8363565851208167e-8, - "angularVelocity": 4.0756214360088557e-7, - "velocityX": 1.0967466180547656, - "velocityY": -1.0273343295426693, - "timestamp": 5.126651667611466 - }, - { - "x": 2.8198287294720963, - "y": 2.238961666744671, - "heading": -8.655352303676696e-9, - "angularVelocity": 2.1163531420636224e-7, - "velocityX": 0.731164466227284, - "velocityY": -0.6848896037821823, - "timestamp": 5.219775131535424 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 5.90328227721677e-29, - "angularVelocity": 9.294491354772317e-8, - "velocityX": 0.36558224472595613, - "velocityY": -0.34244481276689687, - "timestamp": 5.312898595459381 - }, - { - "x": 2.8538730144500732, - "y": 2.2070720195770264, - "heading": 3.223926333927225e-29, - "angularVelocity": 1.2163249082376476e-28, - "velocityX": -5.169137847741255e-28, - "velocityY": -3.368633280520026e-27, - "timestamp": 5.406022059383338 + "angularVelocity": 2.0861005593011723e-18, + "velocityX": 4.162620858676712e-18, + "velocityY": -5.846759932567415e-18, + "timestamp": 3.4483721519849535 + }, + { + "x": 0.6301200123670696, + "y": 4.288467558517564, + "heading": -1.1752680974581398, + "angularVelocity": 9.654043757607686e-7, + "velocityX": 0.27317275726109946, + "velocityY": -0.27820276911277114, + "timestamp": 3.520856305157547 + }, + { + "x": 0.669721403553962, + "y": 4.248136975067876, + "heading": -1.1752679470653256, + "angularVelocity": 0.000002074837150226295, + "velocityX": 0.5463455038592624, + "velocityY": -0.5564055270626612, + "timestamp": 3.5933404583301405 + }, + { + "x": 0.7291234886592751, + "y": 4.187641101646045, + "heading": -1.1752676989048652, + "angularVelocity": 0.0000034236512224661995, + "velocityX": 0.8195182326800431, + "velocityY": -0.8346082664135338, + "timestamp": 3.665824611502734 + }, + { + "x": 0.8083262651042996, + "y": 4.106979940946794, + "heading": -1.175267318473291, + "angularVelocity": 0.000005248479232969504, + "velocityX": 1.0926909259246422, + "velocityY": -1.1128109685876706, + "timestamp": 3.7383087646753275 + }, + { + "x": 0.9073297251428862, + "y": 4.0061535010442855, + "heading": -1.1752667040574094, + "angularVelocity": 0.000008476554555828568, + "velocityX": 1.3658635123024454, + "velocityY": -1.3910135593697104, + "timestamp": 3.810792917847921 + }, + { + "x": 1.0190552899168186, + "y": 3.892370696728415, + "heading": -1.1634451754658162, + "angularVelocity": 0.16309121475754929, + "velocityX": 1.5413791826732202, + "velocityY": -1.5697611041263104, + "timestamp": 3.8832770710205144 + }, + { + "x": 1.13078085649812, + "y": 3.778587894187107, + "heading": -1.1292999776950812, + "angularVelocity": 0.47107121041240924, + "velocityX": 1.5413792076079027, + "velocityY": -1.56976107964423, + "timestamp": 3.955761224193108 + }, + { + "x": 1.2425064227992928, + "y": 3.6648050913707353, + "heading": -1.0775130899678211, + "angularVelocity": 0.7144580637363105, + "velocityX": 1.541379203743213, + "velocityY": -1.569761083439046, + "timestamp": 4.028245377365701 + }, + { + "x": 1.3542319891398829, + "y": 3.551022288593068, + "heading": -1.012058895995312, + "angularVelocity": 0.9030138465803919, + "velocityX": 1.541379204287016, + "velocityY": -1.569761082905074, + "timestamp": 4.1007295305382945 + }, + { + "x": 1.4659575555155513, + "y": 3.4372394858498447, + "heading": -0.9362447812989615, + "angularVelocity": 1.0459405453192618, + "velocityX": 1.5413792047709636, + "velocityY": -1.5697610824298727, + "timestamp": 4.1732136837108875 + }, + { + "x": 1.5776831219356193, + "y": 3.3234566831502192, + "heading": -0.8527996233329523, + "angularVelocity": 1.151219326068592, + "velocityX": 1.541379205383503, + "velocityY": -1.5697610818284016, + "timestamp": 4.245697836883481 + }, + { + "x": 1.689408688396406, + "y": 3.209673880490576, + "heading": -0.7639848157453195, + "angularVelocity": 1.2252996510306196, + "velocityX": 1.5413792059452611, + "velocityY": -1.5697610812767913, + "timestamp": 4.318181990056074 + }, + { + "x": 1.801134254849195, + "y": 3.0958910778230817, + "heading": -0.67170962738432, + "angularVelocity": 1.2730394758324133, + "velocityX": 1.541379205834932, + "velocityY": -1.5697610813851162, + "timestamp": 4.390666143228667 + }, + { + "x": 1.9128598211418812, + "y": 2.982108274998379, + "heading": -0.5776425629397484, + "angularVelocity": 1.2977604114458139, + "velocityX": 1.541379203626127, + "velocityY": -1.569761083553978, + "timestamp": 4.46315029640126 + }, + { + "x": 2.0245853866391266, + "y": 2.868325471392617, + "heading": -0.4833222706746966, + "angularVelocity": 1.3012539725816417, + "velocityX": 1.541379192652128, + "velocityY": -1.5697610943295663, + "timestamp": 4.535634449573853 + }, + { + "x": 2.136310952683287, + "y": 2.754542668323881, + "heading": -0.39024489642260013, + "angularVelocity": 1.2841065278154646, + "velocityX": 1.5413792001974338, + "velocityY": -1.5697610869206908, + "timestamp": 4.608118602746446 + }, + { + "x": 2.248036532216041, + "y": 2.640759878499861, + "heading": -0.2999850951622067, + "angularVelocity": 1.2452349556389624, + "velocityX": 1.5413793862876775, + "velocityY": -1.5697609041950171, + "timestamp": 4.680602755919039 + }, + { + "x": 2.3597621971135103, + "y": 2.5269771724971815, + "heading": -0.2143083418093875, + "angularVelocity": 1.1820066814993688, + "velocityX": 1.5413805639894413, + "velocityY": -1.5697597477858372, + "timestamp": 4.753086909091632 + }, + { + "x": 2.4714874560252893, + "y": 2.4131940678483024, + "heading": -0.13529003999281422, + "angularVelocity": 1.090145892005274, + "velocityX": 1.5413749629625242, + "velocityY": -1.569765247556264, + "timestamp": 4.825571062264225 + }, + { + "x": 2.5832186089625115, + "y": 2.299416750928513, + "heading": -0.06541193776693101, + "angularVelocity": 0.9640466111193448, + "velocityX": 1.541456277638765, + "velocityY": -1.569685399357166, + "timestamp": 4.898055215436818 + }, + { + "x": 2.707773208618164, + "y": 2.199841260910034, + "heading": -4.9936521691807046e-20, + "angularVelocity": 0.902430874941302, + "velocityX": 1.7183700740639751, + "velocityY": -1.3737553059546213, + "timestamp": 4.970539368609411 + }, + { + "x": 2.9476724815434237, + "y": 2.102446510883036, + "heading": 0.10620154481535027, + "angularVelocity": 0.9023913668819601, + "velocityX": 2.0384169852270295, + "velocityY": -0.8275602935604923, + "timestamp": 5.08822837663077 + }, + { + "x": 3.193867360329526, + "y": 2.0222874179592125, + "heading": 0.17246350493624535, + "angularVelocity": 0.5630259039051577, + "velocityX": 2.09191056093718, + "velocityY": -0.6811094279023517, + "timestamp": 5.205917384652129 + }, + { + "x": 3.4400624973149907, + "y": 1.9421291180546993, + "heading": 0.20745441768725084, + "angularVelocity": 0.297316744692814, + "velocityX": 2.091912754849484, + "velocityY": -0.681102689640865, + "timestamp": 5.323606392673488 + }, + { + "x": 3.686257637089198, + "y": 1.8619708267154145, + "heading": 0.22097348970875463, + "angularVelocity": 0.11487115278497866, + "velocityX": 2.0919127785453435, + "velocityY": -0.6811026168623736, + "timestamp": 5.441295400694847 + }, + { + "x": 3.9324527753419964, + "y": 1.7818125307033403, + "heading": 0.21984741537240557, + "angularVelocity": -0.00956822013587358, + "velocityX": 2.0919127656179786, + "velocityY": -0.681102656566928, + "timestamp": 5.558984408716206 + }, + { + "x": 4.178647913249799, + "y": 1.70165423363166, + "heading": 0.2088072590170498, + "angularVelocity": -0.09380788011509143, + "velocityX": 2.091912762686554, + "velocityY": -0.6811026655703678, + "timestamp": 5.676673416737565 + }, + { + "x": 4.424843051210236, + "y": 1.6214959367216402, + "heading": 0.19113828458329582, + "angularVelocity": -0.15013275012506105, + "velocityX": 2.091912763133796, + "velocityY": -0.6811026641967436, + "timestamp": 5.7943624247589245 + }, + { + "x": 4.671038189250915, + "y": 1.5413376400580665, + "heading": 0.16914865389762265, + "angularVelocity": -0.18684523776151937, + "velocityX": 2.0919127638155985, + "velocityY": -0.6811026621026981, + "timestamp": 5.9120514327802836 + }, + { + "x": 4.917233327344427, + "y": 1.4611793435567624, + "heading": 0.1445059099238244, + "angularVelocity": -0.20938866244279256, + "velocityX": 2.091912764264522, + "velocityY": -0.6811026607238972, + "timestamp": 6.029740440801643 + }, + { + "x": 5.1634284654633715, + "y": 1.381021047133573, + "heading": 0.11848538680518875, + "angularVelocity": -0.2210956108487915, + "velocityX": 2.091912764480629, + "velocityY": -0.6811026600601565, + "timestamp": 6.147429448823002 + }, + { + "x": 5.409623603591471, + "y": 1.3008627507385027, + "heading": 0.09216479440710998, + "angularVelocity": -0.22364529058949856, + "velocityX": 2.0919127645584195, + "velocityY": -0.6811026598212322, + "timestamp": 6.265118456844361 + }, + { + "x": 5.6558187417204895, + "y": 1.2207044543462509, + "heading": 0.06658302708804537, + "angularVelocity": -0.2173675158725422, + "velocityX": 2.0919127645662163, + "velocityY": -0.6811026597972839, + "timestamp": 6.38280746486572 + }, + { + "x": 5.902013879846113, + "y": 1.140546157943573, + "heading": 0.04288417914505449, + "angularVelocity": -0.20136840594933111, + "velocityX": 2.0919127645373723, + "velocityY": -0.6811026598858735, + "timestamp": 6.500496472887079 + }, + { + "x": 6.148209017964777, + "y": 1.0603878615195235, + "heading": 0.022487251104018585, + "angularVelocity": -0.17331209077189777, + "velocityX": 2.091912764478247, + "velocityY": -0.6811026600674663, + "timestamp": 6.618185480908438 + }, + { + "x": 6.394404156072785, + "y": 0.9802295650627441, + "heading": 0.0073000213594665506, + "angularVelocity": -0.12904543933112522, + "velocityX": 2.0919127643876987, + "velocityY": -0.681102660345571, + "timestamp": 6.735874488929797 + }, + { + "x": 6.640599294205446, + "y": 0.9000712686819135, + "heading": 5.724943832354313e-7, + "angularVelocity": -0.062023199853654426, + "velocityX": 2.091912764597172, + "velocityY": -0.6811026597002373, + "timestamp": 6.853563496951156 + }, + { + "x": 6.85312987863822, + "y": 0.8308737624937756, + "heading": 2.400111394619599e-7, + "angularVelocity": -0.000002825100224376875, + "velocityX": 1.805866053303826, + "velocityY": -0.5879691515080071, + "timestamp": 6.971252504972515 + }, + { + "x": 6.994816942867847, + "y": 0.7847420891244803, + "heading": 7.233437732941088e-8, + "angularVelocity": -0.0000014247444565862307, + "velocityX": 1.2039107696779348, + "velocityY": -0.39197945623709224, + "timestamp": 7.088941512993874 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -1.572940436060772e-18, + "angularVelocity": -6.146230510677986e-7, + "velocityX": 0.6019553993000372, + "velocityY": -0.19598973280511345, + "timestamp": 7.206630521015233 + }, + { + "x": 7.06566047668457, + "y": 0.7616762518882751, + "heading": -7.302182224417507e-19, + "angularVelocity": 9.559430564762068e-19, + "velocityX": -2.228194904091084e-17, + "velocityY": 8.974683860818082e-18, + "timestamp": 7.324319529036592 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Minor Trolling.traj b/src/main/deploy/choreo/Minor Trolling.traj deleted file mode 100644 index 6e8ab91..0000000 --- a/src/main/deploy/choreo/Minor Trolling.traj +++ /dev/null @@ -1,310 +0,0 @@ -{ - "samples": [ - { - "x": 1.3449708223342236, - "y": 5.555035114288299, - "heading": -2.807134781824586e-27, - "angularVelocity": -1.1824423749701342e-27, - "velocityX": 1.0706002805992919e-14, - "velocityY": 5.221920773935383e-15, - "timestamp": 0 - }, - { - "x": 1.365504399542175, - "y": 5.564891275244084, - "heading": 4.85604013060267e-16, - "angularVelocity": 7.462630925494432e-15, - "velocityX": 0.3155544519289174, - "velocityY": 0.15146681150722127, - "timestamp": 0.0650714229588586 - }, - { - "x": 1.4065715429019814, - "y": 5.584603619832141, - "heading": 1.3883704475037994e-15, - "angularVelocity": 1.3873470009876698e-14, - "velocityX": 0.6311087339497233, - "velocityY": 0.3029339715000781, - "timestamp": 0.1301428459177172 - }, - { - "x": 1.468172239047218, - "y": 5.61417217545247, - "heading": 2.6242758618538536e-15, - "angularVelocity": 1.89930596063391e-14, - "velocityX": 0.9466628105589432, - "velocityY": 0.45440155256870507, - "timestamp": 0.19521426887657578 - }, - { - "x": 1.5503064714818364, - "y": 5.653596975897173, - "heading": 4.078952886027264e-15, - "angularVelocity": 2.235508244039901e-14, - "velocityX": 1.2622166336613463, - "velocityY": 0.6058696529452037, - "timestamp": 0.2602856918354344 - }, - { - "x": 1.6529742192756731, - "y": 5.702878064004003, - "heading": 5.604758992776914e-15, - "angularVelocity": 2.3448174910470438e-14, - "velocityX": 1.5777701351146785, - "velocityY": 0.7573384116406815, - "timestamp": 0.32535711479429297 - }, - { - "x": 1.7761754548178867, - "y": 5.762015496221117, - "heading": 6.9858603583809815e-15, - "angularVelocity": 2.122439164645601e-14, - "velocityX": 1.8933232122520756, - "velocityY": 0.9088080378161675, - "timestamp": 0.39042853775315156 - }, - { - "x": 1.9199101395052205, - "y": 5.83100935135826, - "heading": 7.90901440496065e-15, - "angularVelocity": 1.418678130210118e-14, - "velocityX": 2.2088756961450784, - "velocityY": 1.0602788751184204, - "timestamp": 0.45549996071201015 - }, - { - "x": 2.08417821405006, - "y": 5.90985975023635, - "heading": 7.855703271430976e-15, - "angularVelocity": -8.192710650167832e-16, - "velocityX": 2.5244272689220897, - "velocityY": 1.211751569163871, - "timestamp": 0.5205713836708687 - }, - { - "x": 2.268979570346587, - "y": 5.998566912663145, - "heading": 5.863255908612344e-15, - "angularVelocity": -3.061939132638555e-14, - "velocityX": 2.8399771803581713, - "velocityY": 1.3632276411553363, - "timestamp": 0.5856428066297273 - }, - { - "x": 2.4743139162079135, - "y": 6.097131433441971, - "heading": -1.2204518276452846e-17, - "angularVelocity": -9.029248417989759e-14, - "velocityX": 3.1555226015453033, - "velocityY": 1.5147128539232129, - "timestamp": 0.6507142295885859 - }, - { - "x": 2.689210027815467, - "y": 6.200264987635721, - "heading": -1.4438324489222705e-17, - "angularVelocity": -3.4328528690565883e-17, - "velocityX": 3.30246522722405, - "velocityY": 1.584928521679745, - "timestamp": 0.7157856525474445 - }, - { - "x": 2.908903598785465, - "y": 6.292739868164094, - "heading": 1.5911631049954432e-27, - "angularVelocity": 2.2188425943885138e-16, - "velocityX": 3.376191283060841, - "velocityY": 1.4211289122549224, - "timestamp": 0.7808570755063031 - }, - { - "x": 3.20151198928919, - "y": 6.37574601842352, - "heading": 1.0232489607381783e-17, - "angularVelocity": 1.232355609804863e-16, - "velocityX": 3.524045521992114, - "velocityY": 0.9996892147076375, - "timestamp": 0.8638890308762548 - }, - { - "x": 3.4969572289513846, - "y": 6.448008052427147, - "heading": 3.0679029856294505e-17, - "angularVelocity": 2.4624905145380905e-16, - "velocityX": 3.5582112735491953, - "velocityY": 0.8702918494652072, - "timestamp": 0.9469209862462065 - }, - { - "x": 3.7924024756754835, - "y": 6.520270057557982, - "heading": 7.697011069326025e-17, - "angularVelocity": 5.575092219866701e-16, - "velocityX": 3.558211358599641, - "velocityY": 0.8702915017341162, - "timestamp": 1.0299529416161581 - }, - { - "x": 4.08784754370095, - "y": 6.592532793299271, - "heading": 7.158177464139197e-17, - "angularVelocity": -6.489472669278147e-17, - "velocityX": 3.558209206432672, - "velocityY": 0.8703003008819995, - "timestamp": 1.1129848969861098 - }, - { - "x": 4.383292663763094, - "y": 6.664795316289082, - "heading": 1.0134319525693488e-16, - "angularVelocity": 3.5843333430253186e-16, - "velocityX": 3.558209833139292, - "velocityY": 0.8702977385976698, - "timestamp": 1.1960168523560615 - }, - { - "x": 4.6787378051682404, - "y": 6.737057752018029, - "heading": 1.1308261583790461e-16, - "angularVelocity": 1.413843685032946e-16, - "velocityX": 3.5582100901849465, - "velocityY": 0.8702966876665797, - "timestamp": 1.2790488077260131 - }, - { - "x": 4.974182963018651, - "y": 6.809320120510445, - "heading": 1.2605550724777224e-16, - "angularVelocity": 1.5623974352141762e-16, - "velocityX": 3.55821028824441, - "velocityY": 0.8702958778996337, - "timestamp": 1.3620807630959648 - }, - { - "x": 5.269628134219715, - "y": 6.8815824344186165, - "heading": 1.5997529005985398e-16, - "angularVelocity": 4.0851480206826883e-16, - "velocityX": 3.558210449033731, - "velocityY": 0.8702952205112429, - "timestamp": 1.4451127184659165 - }, - { - "x": 5.5650733160418735, - "y": 6.953844704902323, - "heading": 1.4460998185548282e-16, - "angularVelocity": -1.8505294900854803e-16, - "velocityX": 3.5582105769494934, - "velocityY": 0.8702946975262731, - "timestamp": 1.5281446738358682 - }, - { - "x": 5.860518506022417, - "y": 7.026106942030369, - "heading": 1.5264609117120294e-16, - "angularVelocity": 9.678333216567522e-17, - "velocityX": 3.558210675205449, - "velocityY": 0.870294295805505, - "timestamp": 1.6111766292058198 - }, - { - "x": 6.155963701926025, - "y": 7.098369154941873, - "heading": 1.7041919564100958e-16, - "angularVelocity": 2.1405137807285622e-16, - "velocityX": 3.55821074654022, - "velocityY": 0.8702940041522215, - "timestamp": 1.6942085845757715 - }, - { - "x": 6.451408867259249, - "y": 7.170631492835253, - "heading": 1.605762513625872e-16, - "angularVelocity": -1.1854405001861668e-16, - "velocityX": 3.5582103783640893, - "velocityY": 0.87029550937845, - "timestamp": 1.7772405399457232 - }, - { - "x": 6.739593860452669, - "y": 7.241117888394939, - "heading": 1.1916521054214916e-16, - "angularVelocity": -4.987361868954257e-16, - "velocityX": 3.470772089003605, - "velocityY": 0.8489068485215399, - "timestamp": 1.8602724953156748 - }, - { - "x": 6.9917557416698095, - "y": 7.302793471475189, - "heading": 9.153953371814369e-17, - "angularVelocity": -3.3271138440380534e-16, - "velocityX": 3.036925724483133, - "velocityY": 0.7427933354749008, - "timestamp": 1.9433044506856265 - }, - { - "x": 7.207894502741401, - "y": 7.355658247620443, - "heading": 6.912972849883134e-17, - "angularVelocity": -2.6989374293489497e-16, - "velocityX": 2.6030792615756186, - "velocityY": 0.6366798892030741, - "timestamp": 2.026336406055578 - }, - { - "x": 7.388010140347625, - "y": 7.399712221155376, - "heading": 4.649348498129856e-17, - "angularVelocity": -2.726208652687308e-16, - "velocityX": 2.169232758685661, - "velocityY": 0.5305664950156702, - "timestamp": 2.10936836142553 - }, - { - "x": 7.532102652510152, - "y": 7.434955395544809, - "heading": 2.777377355142468e-17, - "angularVelocity": -2.2545189195214148e-16, - "velocityX": 1.7353862319695992, - "velocityY": 0.4244531425570638, - "timestamp": 2.1924003167954815 - }, - { - "x": 7.640172037880081, - "y": 7.461387773529582, - "heading": 1.6169989490516543e-17, - "angularVelocity": -1.3975082250070357e-16, - "velocityX": 1.3015396890079767, - "velocityY": 0.3183398231079052, - "timestamp": 2.275432272165433 - }, - { - "x": 7.712218295476737, - "y": 7.479009357269755, - "heading": 5.705869423341737e-18, - "angularVelocity": -1.2602521546942366e-16, - "velocityX": 0.8676931342355209, - "velocityY": 0.21222652967353675, - "timestamp": 2.358464227535385 - }, - { - "x": 7.7482414245605495, - "y": 7.4878201484680185, - "heading": 2.4801573105040754e-27, - "angularVelocity": -6.871895759912153e-17, - "velocityX": 0.43384657055603204, - "velocityY": 0.10611325674564437, - "timestamp": 2.4414961829053365 - }, - { - "x": 7.748241424560549, - "y": 7.487820148468018, - "heading": 1.21542731991438e-27, - "angularVelocity": -5.937794751127806e-28, - "velocityX": 8.50988429287625e-16, - "velocityY": 2.197763699220458e-16, - "timestamp": 2.524528138275288 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/source exit.1.traj b/src/main/deploy/choreo/source exit.1.traj new file mode 100644 index 0000000..4729d36 --- /dev/null +++ b/src/main/deploy/choreo/source exit.1.traj @@ -0,0 +1,382 @@ +{ + "samples": [ + { + "x": 0.6313003897666931, + "y": 4.255156517028809, + "heading": -1.2897, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 4.938118129828658e-32, + "timestamp": 0 + }, + { + "x": 0.6547225060554301, + "y": 4.233901437352739, + "heading": -1.2896998806389426, + "angularVelocity": 0.0000015565956410580199, + "velocityX": 0.3054494072906231, + "velocityY": -0.2771889358303716, + "timestamp": 0.0766808372505756 + }, + { + "x": 0.7015667377724408, + "y": 4.191391278797934, + "heading": -1.289699622292801, + "angularVelocity": 0.000003369109545120088, + "velocityX": 0.6108988033598843, + "velocityY": -0.5543778612626656, + "timestamp": 0.1533616745011512 + }, + { + "x": 0.7718330834830761, + "y": 4.127626042692661, + "heading": -1.289699192032996, + "angularVelocity": 0.000005611047281688477, + "velocityX": 0.9163481807197896, + "velocityY": -0.8315667693729352, + "timestamp": 0.2300425117517268 + }, + { + "x": 0.8655215403161615, + "y": 4.042605731691311, + "heading": -1.2896985232175402, + "angularVelocity": 0.000008722067696213578, + "velocityX": 1.2217975206365133, + "velocityY": -1.1087556428671073, + "timestamp": 0.3067233490023024 + }, + { + "x": 0.9826320996478911, + "y": 3.936330353745298, + "heading": -1.2896974106735322, + "angularVelocity": 0.000014508761822433673, + "velocityX": 1.5272467480896021, + "velocityY": -1.3859444126663505, + "timestamp": 0.38340418625287803 + }, + { + "x": 1.113237043415476, + "y": 3.8178091039826962, + "heading": -1.2790397450792215, + "angularVelocity": 0.1389873399462232, + "velocityX": 1.7032279308687956, + "velocityY": -1.5456436576885717, + "timestamp": 0.46008502350345365 + }, + { + "x": 1.2438419873099618, + "y": 3.6992878543599033, + "heading": -1.2518722162171183, + "angularVelocity": 0.3542935867187001, + "velocityX": 1.7032279325237185, + "velocityY": -1.5456436558653144, + "timestamp": 0.5367658607540292 + }, + { + "x": 1.3744469312210372, + "y": 3.5807666047553917, + "heading": -1.212078387332647, + "angularVelocity": 0.5189540217777049, + "velocityX": 1.7032279327400672, + "velocityY": -1.5456436556269055, + "timestamp": 0.6134466980046048 + }, + { + "x": 1.5050518752146576, + "y": 3.4622453552418415, + "heading": -1.162751180302459, + "angularVelocity": 0.6432794528441913, + "velocityX": 1.70322793381654, + "velocityY": -1.5456436544406769, + "timestamp": 0.6901275352551804 + }, + { + "x": 1.6356568192666512, + "y": 3.343724105792617, + "heading": -1.1063318116829663, + "angularVelocity": 0.7357688132058888, + "velocityX": 1.7032279345777896, + "velocityY": -1.5456436536018043, + "timestamp": 0.7668083725057561 + }, + { + "x": 1.7662617633452586, + "y": 3.22520285637272, + "heading": -1.044747551464474, + "angularVelocity": 0.8031245148934248, + "velocityX": 1.703227934924863, + "velocityY": -1.54564365321933, + "timestamp": 0.8434892097563317 + }, + { + "x": 1.8968667073747523, + "y": 3.1066816068987038, + "heading": -0.9795374269556493, + "angularVelocity": 0.8504096570533181, + "velocityX": 1.7032279342843641, + "velocityY": -1.5456436539251106, + "timestamp": 0.9201700470069073 + }, + { + "x": 2.0274716511557966, + "y": 2.988160357150909, + "heading": -0.911962353611442, + "angularVelocity": 0.8812511152347953, + "velocityX": 1.7032279310443175, + "velocityY": -1.5456436574954706, + "timestamp": 0.9968508842574829 + }, + { + "x": 2.158076594153494, + "y": 2.869639106539904, + "heading": -0.8431013334736036, + "angularVelocity": 0.8980212345989256, + "velocityX": 1.7032279208286418, + "velocityY": -1.5456436687526536, + "timestamp": 1.0735317215080584 + }, + { + "x": 2.288681534488559, + "y": 2.751117852994802, + "heading": -0.7739408419480853, + "angularVelocity": 0.9019266612794504, + "velocityX": 1.7032278861050756, + "velocityY": -1.5456437070164089, + "timestamp": 1.150212558758634 + }, + { + "x": 2.419286472128144, + "y": 2.632596596479403, + "heading": -0.705434667355344, + "angularVelocity": 0.8933936697754613, + "velocityX": 1.703227850953138, + "velocityY": -1.5456437457522652, + "timestamp": 1.2268933960092097 + }, + { + "x": 2.5498914265636636, + "y": 2.5140753584723274, + "heading": -0.6385807654336066, + "angularVelocity": 0.8718462697959073, + "velocityX": 1.7032280699900582, + "velocityY": -1.5456435043839587, + "timestamp": 1.3035742332597853 + }, + { + "x": 2.680496509530869, + "y": 2.3955542621014185, + "heading": -0.5745105008427962, + "angularVelocity": 0.8355446665435626, + "velocityX": 1.7032297461804866, + "velocityY": -1.5456416572971043, + "timestamp": 1.380255070510361 + }, + { + "x": 2.8111011984005865, + "y": 2.277032731452427, + "heading": -0.5145723016278465, + "angularVelocity": 0.7816581216906595, + "velocityX": 1.7032246067284333, + "velocityY": -1.5456473207470263, + "timestamp": 1.4569359077609365 + }, + { + "x": 2.9417192752167938, + "y": 2.158525955314311, + "heading": -0.46033957015895777, + "angularVelocity": 0.7072527298009322, + "velocityX": 1.7033991998467222, + "velocityY": -1.5454549061698755, + "timestamp": 1.5336167450115121 + }, + { + "x": 3.0872981548309326, + "y": 2.058968305587769, + "heading": -0.4081492403189991, + "angularVelocity": 0.6806176316178785, + "velocityX": 1.8985040439558556, + "velocityY": -1.2983380632792187, + "timestamp": 1.6102975822620877 + }, + { + "x": 3.288618968752435, + "y": 1.9787858722638514, + "heading": -0.3440273039506474, + "angularVelocity": 0.6805713802306704, + "velocityX": 2.1367599289661108, + "velocityY": -0.8510327729965853, + "timestamp": 1.70451537958802 + }, + { + "x": 3.4973190456299355, + "y": 1.9204457429666257, + "heading": -0.2885241078330584, + "angularVelocity": 0.5890946051900753, + "velocityX": 2.215081256416269, + "velocityY": -0.6192049798766697, + "timestamp": 1.798733176913952 + }, + { + "x": 3.7060194347295887, + "y": 1.8621067305954644, + "heading": -0.24314566154797057, + "angularVelocity": 0.48163348722870364, + "velocityX": 2.215084570250411, + "velocityY": -0.6191931251517853, + "timestamp": 1.8929509742398842 + }, + { + "x": 3.914719841235457, + "y": 1.8037677804928323, + "heading": -0.20514297433349235, + "angularVelocity": 0.40334934898785263, + "velocityX": 2.2150847549948707, + "velocityY": -0.6191924642519187, + "timestamp": 1.9871687715658164 + }, + { + "x": 4.123420253984318, + "y": 1.7454288527237616, + "heading": -0.17255263081013517, + "angularVelocity": 0.34590432432439966, + "velocityX": 2.215084821256147, + "velocityY": -0.6191922272100663, + "timestamp": 2.0813865688917486 + }, + { + "x": 4.332120665934124, + "y": 1.68708992209617, + "heading": -0.14399111470382664, + "angularVelocity": 0.3031435346286237, + "velocityX": 2.215084812775212, + "velocityY": -0.6191922575495665, + "timestamp": 2.175604366217681 + }, + { + "x": 4.540821077336117, + "y": 1.6287509895088441, + "heading": -0.11850572029946865, + "angularVelocity": 0.27049448328946984, + "velocityX": 2.2150848069609004, + "velocityY": -0.6191922783496103, + "timestamp": 2.2698221635436133 + }, + { + "x": 4.749521488592162, + "y": 1.5704120563993975, + "heading": -0.09547134573973612, + "angularVelocity": 0.2444800792788083, + "velocityX": 2.2150848054118457, + "velocityY": -0.6191922838912438, + "timestamp": 2.3640399608695457 + }, + { + "x": 4.958221899850797, + "y": 1.5120731232992075, + "heading": -0.07452259619207273, + "angularVelocity": 0.22234386859179237, + "velocityX": 2.21508480543933, + "velocityY": -0.6191922837929981, + "timestamp": 2.458257758195478 + }, + { + "x": 5.166922311144001, + "y": 1.4537341903226784, + "heading": -0.055514581735497524, + "angularVelocity": 0.2017454769272586, + "velocityX": 2.2150848058062342, + "velocityY": -0.619192282480497, + "timestamp": 2.5524755555214105 + }, + { + "x": 5.3756227224674955, + "y": 1.3953952574545083, + "heading": -0.038508342854423575, + "angularVelocity": 0.18049921950761413, + "velocityX": 2.215084806127732, + "velocityY": -0.619192281330405, + "timestamp": 2.646693352847343 + }, + { + "x": 5.5843231338114325, + "y": 1.3370563246594693, + "heading": -0.023778548081694613, + "angularVelocity": 0.15633771103522998, + "velocityX": 2.2150848063447075, + "velocityY": -0.6191922805542137, + "timestamp": 2.7409111501732752 + }, + { + "x": 5.793023545170744, + "y": 1.278717391919428, + "heading": -0.01184315101724952, + "angularVelocity": 0.12667879533577564, + "velocityX": 2.2150848065078788, + "velocityY": -0.619192279970488, + "timestamp": 2.8351289474992076 + }, + { + "x": 6.001723956538716, + "y": 1.2203784592103721, + "heading": -0.0035175510847978773, + "angularVelocity": 0.08836546988728466, + "velocityX": 2.2150848065998088, + "velocityY": -0.6191922796416123, + "timestamp": 2.92934674482514 + }, + { + "x": 6.210424367778128, + "y": 1.1620395260406786, + "heading": 9.95199444319593e-7, + "angularVelocity": 0.03734481577905508, + "velocityX": 2.2150848052352985, + "velocityY": -0.619192284530687, + "timestamp": 3.0235645421510724 + }, + { + "x": 6.394372705631795, + "y": 1.1106196478377304, + "heading": 5.066604047984432e-7, + "angularVelocity": -0.00000518520965030981, + "velocityX": 1.952373575634825, + "velocityY": -0.545755469373464, + "timestamp": 3.117782339477005 + }, + { + "x": 6.532333969364687, + "y": 1.0720547363320128, + "heading": 2.3088395723940584e-7, + "angularVelocity": -0.000002927010090872124, + "velocityX": 1.4642802914998674, + "velocityY": -0.4093166323163768, + "timestamp": 3.212000136802937 + }, + { + "x": 6.624308148185072, + "y": 1.0463447945031064, + "heading": 7.204386582174162e-8, + "angularVelocity": -0.00000168588200609885, + "velocityX": 0.9761868928246619, + "velocityY": -0.27287776363489813, + "timestamp": 3.3062179341288696 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": -7.646523997427724e-7, + "velocityX": 0.4880934559605067, + "velocityY": -0.1364388844420419, + "timestamp": 3.400435731454802 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": 0, + "velocityX": -8.306900544678907e-28, + "velocityY": -1.1351138938300595e-27, + "timestamp": 3.4946535287807343 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/source exit.traj b/src/main/deploy/choreo/source exit.traj new file mode 100644 index 0000000..4729d36 --- /dev/null +++ b/src/main/deploy/choreo/source exit.traj @@ -0,0 +1,382 @@ +{ + "samples": [ + { + "x": 0.6313003897666931, + "y": 4.255156517028809, + "heading": -1.2897, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 4.938118129828658e-32, + "timestamp": 0 + }, + { + "x": 0.6547225060554301, + "y": 4.233901437352739, + "heading": -1.2896998806389426, + "angularVelocity": 0.0000015565956410580199, + "velocityX": 0.3054494072906231, + "velocityY": -0.2771889358303716, + "timestamp": 0.0766808372505756 + }, + { + "x": 0.7015667377724408, + "y": 4.191391278797934, + "heading": -1.289699622292801, + "angularVelocity": 0.000003369109545120088, + "velocityX": 0.6108988033598843, + "velocityY": -0.5543778612626656, + "timestamp": 0.1533616745011512 + }, + { + "x": 0.7718330834830761, + "y": 4.127626042692661, + "heading": -1.289699192032996, + "angularVelocity": 0.000005611047281688477, + "velocityX": 0.9163481807197896, + "velocityY": -0.8315667693729352, + "timestamp": 0.2300425117517268 + }, + { + "x": 0.8655215403161615, + "y": 4.042605731691311, + "heading": -1.2896985232175402, + "angularVelocity": 0.000008722067696213578, + "velocityX": 1.2217975206365133, + "velocityY": -1.1087556428671073, + "timestamp": 0.3067233490023024 + }, + { + "x": 0.9826320996478911, + "y": 3.936330353745298, + "heading": -1.2896974106735322, + "angularVelocity": 0.000014508761822433673, + "velocityX": 1.5272467480896021, + "velocityY": -1.3859444126663505, + "timestamp": 0.38340418625287803 + }, + { + "x": 1.113237043415476, + "y": 3.8178091039826962, + "heading": -1.2790397450792215, + "angularVelocity": 0.1389873399462232, + "velocityX": 1.7032279308687956, + "velocityY": -1.5456436576885717, + "timestamp": 0.46008502350345365 + }, + { + "x": 1.2438419873099618, + "y": 3.6992878543599033, + "heading": -1.2518722162171183, + "angularVelocity": 0.3542935867187001, + "velocityX": 1.7032279325237185, + "velocityY": -1.5456436558653144, + "timestamp": 0.5367658607540292 + }, + { + "x": 1.3744469312210372, + "y": 3.5807666047553917, + "heading": -1.212078387332647, + "angularVelocity": 0.5189540217777049, + "velocityX": 1.7032279327400672, + "velocityY": -1.5456436556269055, + "timestamp": 0.6134466980046048 + }, + { + "x": 1.5050518752146576, + "y": 3.4622453552418415, + "heading": -1.162751180302459, + "angularVelocity": 0.6432794528441913, + "velocityX": 1.70322793381654, + "velocityY": -1.5456436544406769, + "timestamp": 0.6901275352551804 + }, + { + "x": 1.6356568192666512, + "y": 3.343724105792617, + "heading": -1.1063318116829663, + "angularVelocity": 0.7357688132058888, + "velocityX": 1.7032279345777896, + "velocityY": -1.5456436536018043, + "timestamp": 0.7668083725057561 + }, + { + "x": 1.7662617633452586, + "y": 3.22520285637272, + "heading": -1.044747551464474, + "angularVelocity": 0.8031245148934248, + "velocityX": 1.703227934924863, + "velocityY": -1.54564365321933, + "timestamp": 0.8434892097563317 + }, + { + "x": 1.8968667073747523, + "y": 3.1066816068987038, + "heading": -0.9795374269556493, + "angularVelocity": 0.8504096570533181, + "velocityX": 1.7032279342843641, + "velocityY": -1.5456436539251106, + "timestamp": 0.9201700470069073 + }, + { + "x": 2.0274716511557966, + "y": 2.988160357150909, + "heading": -0.911962353611442, + "angularVelocity": 0.8812511152347953, + "velocityX": 1.7032279310443175, + "velocityY": -1.5456436574954706, + "timestamp": 0.9968508842574829 + }, + { + "x": 2.158076594153494, + "y": 2.869639106539904, + "heading": -0.8431013334736036, + "angularVelocity": 0.8980212345989256, + "velocityX": 1.7032279208286418, + "velocityY": -1.5456436687526536, + "timestamp": 1.0735317215080584 + }, + { + "x": 2.288681534488559, + "y": 2.751117852994802, + "heading": -0.7739408419480853, + "angularVelocity": 0.9019266612794504, + "velocityX": 1.7032278861050756, + "velocityY": -1.5456437070164089, + "timestamp": 1.150212558758634 + }, + { + "x": 2.419286472128144, + "y": 2.632596596479403, + "heading": -0.705434667355344, + "angularVelocity": 0.8933936697754613, + "velocityX": 1.703227850953138, + "velocityY": -1.5456437457522652, + "timestamp": 1.2268933960092097 + }, + { + "x": 2.5498914265636636, + "y": 2.5140753584723274, + "heading": -0.6385807654336066, + "angularVelocity": 0.8718462697959073, + "velocityX": 1.7032280699900582, + "velocityY": -1.5456435043839587, + "timestamp": 1.3035742332597853 + }, + { + "x": 2.680496509530869, + "y": 2.3955542621014185, + "heading": -0.5745105008427962, + "angularVelocity": 0.8355446665435626, + "velocityX": 1.7032297461804866, + "velocityY": -1.5456416572971043, + "timestamp": 1.380255070510361 + }, + { + "x": 2.8111011984005865, + "y": 2.277032731452427, + "heading": -0.5145723016278465, + "angularVelocity": 0.7816581216906595, + "velocityX": 1.7032246067284333, + "velocityY": -1.5456473207470263, + "timestamp": 1.4569359077609365 + }, + { + "x": 2.9417192752167938, + "y": 2.158525955314311, + "heading": -0.46033957015895777, + "angularVelocity": 0.7072527298009322, + "velocityX": 1.7033991998467222, + "velocityY": -1.5454549061698755, + "timestamp": 1.5336167450115121 + }, + { + "x": 3.0872981548309326, + "y": 2.058968305587769, + "heading": -0.4081492403189991, + "angularVelocity": 0.6806176316178785, + "velocityX": 1.8985040439558556, + "velocityY": -1.2983380632792187, + "timestamp": 1.6102975822620877 + }, + { + "x": 3.288618968752435, + "y": 1.9787858722638514, + "heading": -0.3440273039506474, + "angularVelocity": 0.6805713802306704, + "velocityX": 2.1367599289661108, + "velocityY": -0.8510327729965853, + "timestamp": 1.70451537958802 + }, + { + "x": 3.4973190456299355, + "y": 1.9204457429666257, + "heading": -0.2885241078330584, + "angularVelocity": 0.5890946051900753, + "velocityX": 2.215081256416269, + "velocityY": -0.6192049798766697, + "timestamp": 1.798733176913952 + }, + { + "x": 3.7060194347295887, + "y": 1.8621067305954644, + "heading": -0.24314566154797057, + "angularVelocity": 0.48163348722870364, + "velocityX": 2.215084570250411, + "velocityY": -0.6191931251517853, + "timestamp": 1.8929509742398842 + }, + { + "x": 3.914719841235457, + "y": 1.8037677804928323, + "heading": -0.20514297433349235, + "angularVelocity": 0.40334934898785263, + "velocityX": 2.2150847549948707, + "velocityY": -0.6191924642519187, + "timestamp": 1.9871687715658164 + }, + { + "x": 4.123420253984318, + "y": 1.7454288527237616, + "heading": -0.17255263081013517, + "angularVelocity": 0.34590432432439966, + "velocityX": 2.215084821256147, + "velocityY": -0.6191922272100663, + "timestamp": 2.0813865688917486 + }, + { + "x": 4.332120665934124, + "y": 1.68708992209617, + "heading": -0.14399111470382664, + "angularVelocity": 0.3031435346286237, + "velocityX": 2.215084812775212, + "velocityY": -0.6191922575495665, + "timestamp": 2.175604366217681 + }, + { + "x": 4.540821077336117, + "y": 1.6287509895088441, + "heading": -0.11850572029946865, + "angularVelocity": 0.27049448328946984, + "velocityX": 2.2150848069609004, + "velocityY": -0.6191922783496103, + "timestamp": 2.2698221635436133 + }, + { + "x": 4.749521488592162, + "y": 1.5704120563993975, + "heading": -0.09547134573973612, + "angularVelocity": 0.2444800792788083, + "velocityX": 2.2150848054118457, + "velocityY": -0.6191922838912438, + "timestamp": 2.3640399608695457 + }, + { + "x": 4.958221899850797, + "y": 1.5120731232992075, + "heading": -0.07452259619207273, + "angularVelocity": 0.22234386859179237, + "velocityX": 2.21508480543933, + "velocityY": -0.6191922837929981, + "timestamp": 2.458257758195478 + }, + { + "x": 5.166922311144001, + "y": 1.4537341903226784, + "heading": -0.055514581735497524, + "angularVelocity": 0.2017454769272586, + "velocityX": 2.2150848058062342, + "velocityY": -0.619192282480497, + "timestamp": 2.5524755555214105 + }, + { + "x": 5.3756227224674955, + "y": 1.3953952574545083, + "heading": -0.038508342854423575, + "angularVelocity": 0.18049921950761413, + "velocityX": 2.215084806127732, + "velocityY": -0.619192281330405, + "timestamp": 2.646693352847343 + }, + { + "x": 5.5843231338114325, + "y": 1.3370563246594693, + "heading": -0.023778548081694613, + "angularVelocity": 0.15633771103522998, + "velocityX": 2.2150848063447075, + "velocityY": -0.6191922805542137, + "timestamp": 2.7409111501732752 + }, + { + "x": 5.793023545170744, + "y": 1.278717391919428, + "heading": -0.01184315101724952, + "angularVelocity": 0.12667879533577564, + "velocityX": 2.2150848065078788, + "velocityY": -0.619192279970488, + "timestamp": 2.8351289474992076 + }, + { + "x": 6.001723956538716, + "y": 1.2203784592103721, + "heading": -0.0035175510847978773, + "angularVelocity": 0.08836546988728466, + "velocityX": 2.2150848065998088, + "velocityY": -0.6191922796416123, + "timestamp": 2.92934674482514 + }, + { + "x": 6.210424367778128, + "y": 1.1620395260406786, + "heading": 9.95199444319593e-7, + "angularVelocity": 0.03734481577905508, + "velocityX": 2.2150848052352985, + "velocityY": -0.619192284530687, + "timestamp": 3.0235645421510724 + }, + { + "x": 6.394372705631795, + "y": 1.1106196478377304, + "heading": 5.066604047984432e-7, + "angularVelocity": -0.00000518520965030981, + "velocityX": 1.952373575634825, + "velocityY": -0.545755469373464, + "timestamp": 3.117782339477005 + }, + { + "x": 6.532333969364687, + "y": 1.0720547363320128, + "heading": 2.3088395723940584e-7, + "angularVelocity": -0.000002927010090872124, + "velocityX": 1.4642802914998674, + "velocityY": -0.4093166323163768, + "timestamp": 3.212000136802937 + }, + { + "x": 6.624308148185072, + "y": 1.0463447945031064, + "heading": 7.204386582174162e-8, + "angularVelocity": -0.00000168588200609885, + "velocityX": 0.9761868928246619, + "velocityY": -0.27287776363489813, + "timestamp": 3.3062179341288696 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": -7.646523997427724e-7, + "velocityX": 0.4880934559605067, + "velocityY": -0.1364388844420419, + "timestamp": 3.400435731454802 + }, + { + "x": 6.670295238494873, + "y": 1.0334898233413696, + "heading": 0, + "angularVelocity": 0, + "velocityX": -8.306900544678907e-28, + "velocityY": -1.1351138938300595e-27, + "timestamp": 3.4946535287807343 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/1 piece source side.auto b/src/main/deploy/pathplanner/autos/1 piece source side.auto index 6efc70a..f2977fc 100644 --- a/src/main/deploy/pathplanner/autos/1 piece source side.auto +++ b/src/main/deploy/pathplanner/autos/1 piece source side.auto @@ -44,7 +44,7 @@ { "type": "path", "data": { - "pathName": "2 piece right.4" + "pathName": "source exit.1" } } ] diff --git a/src/main/deploy/pathplanner/autos/2 piece amp side.auto b/src/main/deploy/pathplanner/autos/2 piece amp side.auto index 560c508..3617285 100644 --- a/src/main/deploy/pathplanner/autos/2 piece amp side.auto +++ b/src/main/deploy/pathplanner/autos/2 piece amp side.auto @@ -72,6 +72,18 @@ "data": { "pathName": "2 piece left.1" } + }, + { + "type": "wait", + "data": { + "waitTime": 1.0 + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece left.2" + } } ] } @@ -82,19 +94,19 @@ { "type": "path", "data": { - "pathName": "2 piece left.2" + "pathName": "2 piece left.3" } }, { - "type": "named", + "type": "wait", "data": { - "name": "SpeakerSetting" + "waitTime": 0.2 } }, { - "type": "wait", + "type": "named", "data": { - "waitTime": 0.2 + "name": "SpeakerSetting" } }, { @@ -108,6 +120,12 @@ "data": { "name": "ResetScoring" } + }, + { + "type": "path", + "data": { + "pathName": "2 piece left.4" + } } ] } diff --git a/src/main/deploy/pathplanner/autos/2 piece source side.auto b/src/main/deploy/pathplanner/autos/2 piece source side.auto index ee78595..4703595 100644 --- a/src/main/deploy/pathplanner/autos/2 piece source side.auto +++ b/src/main/deploy/pathplanner/autos/2 piece source side.auto @@ -72,6 +72,18 @@ "data": { "pathName": "2 piece right.1" } + }, + { + "type": "wait", + "data": { + "waitTime": 0.3 + } + }, + { + "type": "path", + "data": { + "pathName": "2 piece right.2" + } } ] } @@ -82,7 +94,7 @@ { "type": "path", "data": { - "pathName": "2 piece right.2" + "pathName": "2 piece right.3" } }, { @@ -94,7 +106,7 @@ { "type": "wait", "data": { - "waitTime": 0.2 + "waitTime": 1.0 } }, { @@ -112,7 +124,7 @@ { "type": "path", "data": { - "pathName": "2 piece right.3" + "pathName": "2 piece right.4" } } ] diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8b71ba0..6ef78cb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,7 +7,6 @@ import frc.robot.constants.Constants.ShuffleboardConstants; import frc.robot.constants.IntakeConstants; import frc.robot.constants.ScoringConstants; -import frc.robot.constants.ScoringConstants.ScoringPosition; import frc.robot.io.GyroIO; import frc.robot.io.GyroIOPigeon2; import frc.robot.io.GyroIOSim; @@ -148,7 +147,7 @@ public RobotContainer() { NamedCommands.registerCommand("ResetScoring", new SetScoringPosition(arm, shooter, ScoringConstants.lowCarryPosition)); NamedCommands.registerCommand("StopShooter", new StopShooter(shooter)); - NamedCommands.registerCommand("RaceTimeout", Commands.idle().withTimeout(4)); + NamedCommands.registerCommand("RaceTimeout", Commands.idle().withTimeout(5)); NamedCommands.registerCommand("Print", Commands.print("hello!")); // create paths final List autoNames = AutoBuilder.getAllAutoNames(); @@ -215,7 +214,7 @@ private void configureBindings() { // *OPERATOR CONTROLS* // operatorController.a() - .onTrue(new SetScoringPosition(arm, shooter, new ScoringPosition(84, 1750))); + .onTrue(new SetScoringPosition(arm, shooter, ScoringConstants.ampPosition)); operatorController.b() .onTrue(new SetScoringPosition(arm, shooter, ScoringConstants.subwooferPosition)); operatorController.x().onTrue( diff --git a/src/main/java/frc/robot/commands/arm/SetArmPosition.java b/src/main/java/frc/robot/commands/arm/SetArmPosition.java index 5ef2811..dc50558 100644 --- a/src/main/java/frc/robot/commands/arm/SetArmPosition.java +++ b/src/main/java/frc/robot/commands/arm/SetArmPosition.java @@ -11,7 +11,7 @@ public class SetArmPosition extends Command { private Arm arm; - protected double targetPosition_rad; // did I do this right? + protected double targetPosition_deg; // did I do this right? /** * Command to set the arm position, cancels the command and moves on @@ -21,17 +21,19 @@ public class SetArmPosition extends Command { */ public SetArmPosition(Arm arm, double targetPosition_deg) { this.arm = arm; - this.targetPosition_rad = targetPosition_deg; + this.targetPosition_deg = targetPosition_deg; addRequirements(arm); } @Override public void initialize() { - arm.setPosition(targetPosition_rad); + } @Override - public void execute() {} + public void execute() { + arm.setPosition(targetPosition_deg, arm.getBestPIDSlot(targetPosition_deg)); + } @Override public void end(boolean interrupted) { diff --git a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java index 2b70162..5e5d33d 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeWithArm.java +++ b/src/main/java/frc/robot/commands/intake/IntakeWithArm.java @@ -1,6 +1,5 @@ package frc.robot.commands.intake; -import frc.robot.constants.IntakeConstants; import frc.robot.constants.LightsConstants; import frc.robot.subsystems.Arm; import frc.robot.subsystems.Intake; @@ -25,7 +24,6 @@ private IntakeWithArm(Intake intake, Arm arm) { public void initialize() { if (!intake.isHoldingNote()) { arm.setPosition(-2); - intake.setMotorSpeed(IntakeConstants.intakeSpeed); Lights.setStatus(LightsConstants.Mode.Intaking); } } @@ -42,6 +40,9 @@ public void end(boolean interrupted) { else arm.setPosition(40); // Commands.waitSeconds(.015).andThen(intake::stopMotor, intake).schedule(); + if (interrupted) { + intake.stopMotor(); + } } // Returns true when the command should end. diff --git a/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java b/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java index a052794..64f7b75 100644 --- a/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java +++ b/src/main/java/frc/robot/commands/scoring/SetScoringPosition.java @@ -13,6 +13,7 @@ public class SetScoringPosition extends Command { private Shooter shooter; private Supplier posSupplier; + private ScoringPosition targetPosition; /** * Create a set scoring position command that adjusts dynamically according to a method that provides a {@link ScoringPosition} object @@ -40,14 +41,15 @@ public SetScoringPosition(Arm arm, Shooter shooter, ScoringPosition position) { // Called when the command is initially scheduled. @Override public void initialize() { - var targetPosition = posSupplier.get(); - arm.setPosition(targetPosition.armAngle_deg()); + targetPosition = posSupplier.get(); shooter.setVelocity(targetPosition.shooterSpeed_rpm()); } // Called every time the scheduler runs while the command is scheduled. @Override - public void execute() {} + public void execute() { + arm.setPosition(targetPosition.armAngle_deg(), arm.getBestPIDSlot(targetPosition.armAngle_deg())); + } // Called once the command ends or is interrupted. @Override diff --git a/src/main/java/frc/robot/constants/ArmConstants.java b/src/main/java/frc/robot/constants/ArmConstants.java index 8eb77dc..8da5914 100644 --- a/src/main/java/frc/robot/constants/ArmConstants.java +++ b/src/main/java/frc/robot/constants/ArmConstants.java @@ -4,12 +4,15 @@ public final class ArmConstants { public static final int leadMotorId = 29; public static final int followerMotorId = 30; - public static final double shaftEncoderOffset_deg = 125.28; // needs to be tuned + public static final double shaftEncoderOffset_deg = 124.52; // needs to be tuned // pid - public static final double kP = .000089; + public static final double kP = .000095; public static final double kI = 0; - public static final double kD = .00000185; + public static final double kD = .00000165; + + public static final double kPHigh = 0.000067; + public static final double kDHigh = 0.00000225; public static final double maxOutput = .5; public static final double minOutput = -maxOutput; @@ -28,9 +31,12 @@ public final class ArmConstants { public static double maxAccel_rpmps = 7200; public static double maxVelocity_rpm = 3800; + public static double maxVelocityHigh_rpm = 1900; + // Conversion (not needed) // public static final int gearRatio = 1; // 1 IS PLACEHOLDER // closeEnough public static final double accuracyTolerance_deg = .65; + public static final double accuracyToleranceHigh_deg = 1; } \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/ScoringConstants.java b/src/main/java/frc/robot/constants/ScoringConstants.java index abd39c3..996a50d 100644 --- a/src/main/java/frc/robot/constants/ScoringConstants.java +++ b/src/main/java/frc/robot/constants/ScoringConstants.java @@ -16,7 +16,7 @@ public record ScoringPosition(double armAngle_deg, double shooterSpeed_rpm) {}; scoringMap.put(3.227, new ScoringPosition(19, 3450)); } - public static final ScoringPosition ampPosition = new ScoringPosition(84, 1750); + public static final ScoringPosition ampPosition = new ScoringPosition(87, 1750); public static final ScoringPosition subwooferPosition = new ScoringPosition(-1.5, 3250); public static final ScoringPosition carryPosition = new ScoringPosition(40, 0); public static final ScoringPosition lowCarryPosition = new ScoringPosition(4, 0); diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 26df863..3205726 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -80,13 +80,22 @@ public Arm() { pidController.setSmartMotionAllowedClosedLoopError(accuracyTolerance_deg, 0); pidController.setOutputRange(minOutput, maxOutput); // since we are using smartmotion, the PID numbers are for velocity control, not position. - pidController.setP(kP); - pidController.setD(kD); + pidController.setP(kP, 0); + pidController.setD(kD, 0); // Treats 0 and 360 degrees as the same number, so going from one side of 0 to the other doesnt make it do a 360 pidController.setPositionPIDWrappingEnabled(true); pidController.setPositionPIDWrappingMinInput(0); pidController.setPositionPIDWrappingMaxInput(360); + // high angles PID loop + pidController.setSmartMotionAccelStrategy(AccelStrategy.kTrapezoidal, 1); + pidController.setSmartMotionMaxAccel(maxAccel_rpmps, 1); + pidController.setSmartMotionMaxVelocity(maxVelocityHigh_rpm, 1); + pidController.setSmartMotionAllowedClosedLoopError(accuracyToleranceHigh_deg, 1); + + pidController.setP(kPHigh, 1); + pidController.setD(kDHigh, 1); + // if (tuningMode) { // new AutoSetterTunableNumber("Arm/kP", kP, (value) -> pidController.setP(value)); // new AutoSetterTunableNumber("Arm/kD", kD, (value) -> pidController.setD(value)); @@ -141,12 +150,16 @@ private void updateInputs(ArmIOInputs inputs) { inputs.motorVelocity_rpm = Math102.truncate(motorEncoder.getVelocity(), 2); } - public void setPosition(double position_deg) { + public void setPosition(double position_deg, int pidSlot) { targetPosition_deg = position_deg; - pidController.setReference(targetPosition_deg + shaftEncoderOffset_deg, ControlType.kSmartMotion, 0, + pidController.setReference(targetPosition_deg + shaftEncoderOffset_deg, ControlType.kSmartMotion, pidSlot, feedforwardController.calculate(Units.degreesToRadians(targetPosition_deg), 0)); } + public void setPosition(double position_deg) { + setPosition(position_deg, 0); + } + public void setMotorVoltage(double voltage_V) { Logger.recordOutput("Arm/targetVoltage_V", voltage_V); pidController.setReference(voltage_V, ControlType.kVoltage, 0, @@ -167,4 +180,9 @@ public void stop() { public boolean closeEnough() { return MathUtil.isNear(targetPosition_deg, inputs.shaftPosition_deg, 5); } + + public int getBestPIDSlot(double targetPosition_deg) { + boolean chooseTop = (inputs.shaftPosition_deg > 65) && (targetPosition_deg > 65); + return chooseTop ? 1 : 0; + } }