From fedc2cd34cb5c80f9a2bdc42ee779b718814bea8 Mon Sep 17 00:00:00 2001 From: Eddieman855 <69999780+Eddieman855@users.noreply.github.com> Date: Mon, 29 Jan 2024 22:07:01 -0800 Subject: [PATCH] 4 piece auto --- choreo.chor | 1922 ++++++++--------- src/main/deploy/choreo/n1 to n2.traj | 714 +++--- src/main/deploy/choreo/n2 to n3.traj | 714 +++--- src/main/deploy/choreo/s1 to n1.traj | 358 +-- .../team3647/frc2023/auto/AutoCommands.java | 67 +- .../frc2023/commands/DrivetrainCommands.java | 1 + .../frc2023/commands/ShooterCommands.java | 2 +- .../frc2023/constants/FieldConstants.java | 2 +- .../frc2023/constants/ShooterConstants.java | 6 +- .../constants/SwerveDriveConstants.java | 4 +- .../frc2023/constants/TunerConstants.java | 2 +- .../frc2023/constants/VisionConstants.java | 2 +- .../frc2023/robot/RobotContainer.java | 21 +- .../frc2023/subsystems/Superstructure.java | 17 +- .../frc2023/subsystems/SwerveDrive.java | 9 +- .../team3647/frc2023/util/AprilTagCamera.java | 2 +- .../frc2023/util/AprilTagLimelight.java | 198 +- .../frc2023/util/AprilTagPhotonVision.java | 4 +- .../java/team3647/frc2023/util/AutoDrive.java | 8 +- .../team3647/frc2023/util/TargetingUtil.java | 42 +- .../frc2023/util/VisionController.java | 16 +- 21 files changed, 2078 insertions(+), 2033 deletions(-) diff --git a/choreo.chor b/choreo.chor index c97af1c..3158b98 100644 --- a/choreo.chor +++ b/choreo.chor @@ -1,10 +1,10 @@ { "version": "v0.2", "robotConfiguration": { - "mass": 31, + "mass": 50, "rotationalInertia": 4, - "motorMaxTorque": 1.0949416342412452, - "motorMaxVelocity": 5104, + "motorMaxTorque": 0.5, + "motorMaxVelocity": 2500, "gearing": 6.12, "wheelbase": 0.5, "trackWidth": 0.5, @@ -25,8 +25,8 @@ "controlIntervalCount": 20 }, { - "x": 2.5, - "y": 6.957088470458984, + "x": 2.9100513458251953, + "y": 6.66, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -39,190 +39,190 @@ "x": 0.7777742147445679, "y": 6.587954998016357, "heading": 1.04, - "angularVelocity": 3.456261362722889e-34, - "velocityX": -1.1558052906437545e-31, + "angularVelocity": 0, + "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 0.7963313164649566, - "y": 6.590858044372771, - "heading": 1.0212479427537267, - "angularVelocity": -0.5403793813658812, - "velocityX": 0.5347613339651155, - "velocityY": 0.08365729549312781, - "timestamp": 0.034701652011361266 - }, - { - "x": 0.8334366047936193, - "y": 6.597354793156111, - "heading": 0.9842134904961697, - "angularVelocity": -1.0672244723516933, - "velocityX": 1.0692657605036897, - "velocityY": 0.1872172766072533, - "timestamp": 0.06940330402272253 - }, - { - "x": 0.8888948620431416, - "y": 6.60825583696797, - "heading": 0.9287093637187599, - "angularVelocity": -1.5994664103956173, - "velocityX": 1.5981445849138671, - "velocityY": 0.3141361629783576, - "timestamp": 0.10410495603408379 - }, - { - "x": 0.9624427298967876, - "y": 6.624092354477085, - "heading": 0.8539129665807309, - "angularVelocity": -2.1554131519024216, - "velocityX": 2.1194341937832384, - "velocityY": 0.4563620632219429, - "timestamp": 0.13880660804544506 - }, - { - "x": 1.0542492622786634, - "y": 6.644882073171641, - "heading": 0.7600424721123056, - "angularVelocity": -2.705072785517315, - "velocityX": 2.645595441733396, - "velocityY": 0.5990988177666401, - "timestamp": 0.17350826005680633 - }, - { - "x": 1.1664507744572048, - "y": 6.671052140823148, - "heading": 0.6611723277837077, - "angularVelocity": -2.8491480548599815, - "velocityX": 3.233319040309866, - "velocityY": 0.7541447203418256, - "timestamp": 0.2082099120681676 - }, - { - "x": 1.2913236884116448, - "y": 6.70181970935028, - "heading": 0.6001895893740606, - "angularVelocity": -1.7573439555466015, - "velocityX": 3.598471735972595, - "velocityY": 0.8866312334945337, - "timestamp": 0.24291156407952888 - }, - { - "x": 1.4286273378821592, - "y": 6.734134961997154, - "heading": 0.577040600299819, - "angularVelocity": -0.6670860818574766, - "velocityX": 3.9566891347294235, - "velocityY": 0.9312309580044206, - "timestamp": 0.2776132160908901 - }, - { - "x": 1.5744661161236646, - "y": 6.762134785686582, - "heading": 0.577039742085532, - "angularVelocity": -0.00002473122278590041, - "velocityX": 4.202646553938067, - "velocityY": 0.8068729315901393, - "timestamp": 0.3123148681022514 - }, - { - "x": 1.7203049375629715, - "y": 6.790134384466256, - "heading": 0.577038883923436, - "angularVelocity": -0.000024729718795994622, - "velocityX": 4.202647798772216, - "velocityY": 0.8068664503495967, - "timestamp": 0.34701652011361267 - }, - { - "x": 1.8657539829456187, - "y": 6.820090502206212, - "heading": 0.5770365067477514, - "angularVelocity": -0.0000685032425500324, - "velocityX": 4.191415594134462, - "velocityY": 0.8632475978419899, - "timestamp": 0.38171817212497394 - }, - { - "x": 1.9989519675183611, - "y": 6.8523001934695875, - "heading": 0.5408190014813394, - "angularVelocity": -1.0436824521943329, - "velocityX": 3.838375894298458, - "velocityY": 0.9281889880294489, - "timestamp": 0.4164198241363352 - }, - { - "x": 2.1201153456577337, - "y": 6.881905888069826, - "heading": 0.4637582440967774, - "angularVelocity": -2.2206653838650827, - "velocityX": 3.491573775787503, - "velocityY": 0.8531494290400246, - "timestamp": 0.4511214761476965 - }, - { - "x": 2.230128544512573, - "y": 6.907090743568618, - "heading": 0.3438736710895332, - "angularVelocity": -3.4547223563879386, - "velocityX": 3.1702582579878573, - "velocityY": 0.7257537909303905, - "timestamp": 0.48582312815905776 - }, - { - "x": 2.320682619145, - "y": 6.924998011921803, - "heading": 0.23280303009322398, - "angularVelocity": -3.2007306441763914, - "velocityX": 2.6095032767540833, - "velocityY": 0.5160350391192005, - "timestamp": 0.520524780170419 - }, - { - "x": 2.3926583087168334, - "y": 6.938338116341775, - "heading": 0.14104839688844084, - "angularVelocity": -2.644099859417151, - "velocityX": 2.0741286192446693, - "velocityY": 0.38442274781631036, - "timestamp": 0.5552264321817802 - }, - { - "x": 2.4464370134331435, - "y": 6.947890317145862, - "heading": 0.07108140123042413, - "angularVelocity": -2.016243942366481, - "velocityX": 1.5497447988557767, - "velocityY": 0.27526645708275965, - "timestamp": 0.5899280841931415 - }, - { - "x": 2.4821822846008574, - "y": 6.954062116580002, - "heading": 0.02387349227336643, - "angularVelocity": -1.3603937052219282, - "velocityX": 1.0300740482329511, - "velocityY": 0.17785318785745177, - "timestamp": 0.6246297362045027 - }, - { - "x": 2.5, - "y": 6.957088470458984, - "heading": 2.0093661788817544e-33, - "angularVelocity": -0.6879641426163315, - "velocityX": 0.5134543852064684, - "velocityY": 0.08721065723302289, - "timestamp": 0.6593313882158639 - }, - { - "x": 2.5, - "y": 6.957088470458984, - "heading": 0, - "angularVelocity": -8.025462815650884e-32, - "velocityX": -3.4298034808089893e-33, + "x": 0.8022597960341636, + "y": 6.587072803769289, + "heading": 0.9986739281683281, + "angularVelocity": -0.5541098437511959, + "velocityX": 0.3283085234375779, + "velocityY": -0.011828671217327015, + "timestamp": 0.07458101006095233 + }, + { + "x": 0.8519509596093587, + "y": 6.58624052081795, + "heading": 0.92130341357734, + "angularVelocity": -1.0374023431403834, + "velocityX": 0.6662709922349006, + "velocityY": -0.011159448640586789, + "timestamp": 0.14916202012190466 + }, + { + "x": 0.9274840945472737, + "y": 6.586388414118182, + "heading": 0.8127223882915726, + "angularVelocity": -1.455880326601429, + "velocityX": 1.0127663178090307, + "velocityY": 0.001982988700662192, + "timestamp": 0.223743030182857 + }, + { + "x": 1.0299326460915046, + "y": 6.587708067588331, + "heading": 0.6829443155359307, + "angularVelocity": -1.7400954029669882, + "velocityX": 1.3736546536510383, + "velocityY": 0.017694228987703408, + "timestamp": 0.2983240402438093 + }, + { + "x": 1.1563896366352349, + "y": 6.592337580333048, + "heading": 0.5969389995681239, + "angularVelocity": -1.1531798228204053, + "velocityX": 1.6955655392755504, + "velocityY": 0.062073612852970986, + "timestamp": 0.3729050503047616 + }, + { + "x": 1.303789514545485, + "y": 6.601009577302425, + "heading": 0.5715164500982508, + "angularVelocity": -0.3408716166382244, + "velocityX": 1.9763727762570436, + "velocityY": 0.1162762070688939, + "timestamp": 0.4474860603657139 + }, + { + "x": 1.4600489951884137, + "y": 6.6056939320175365, + "heading": 0.5715163560688652, + "angularVelocity": -0.0000012607685095724167, + "velocityX": 2.095164446220536, + "velocityY": 0.06280894709532578, + "timestamp": 0.5220670704266662 + }, + { + "x": 1.6163084771514713, + "y": 6.610378242604074, + "heading": 0.5715162620402611, + "angularVelocity": -0.000001260758262137434, + "velocityX": 2.095164463921114, + "velocityY": 0.0628083554069323, + "timestamp": 0.5966480804876185 + }, + { + "x": 1.7725679591142018, + "y": 6.6150625531994836, + "heading": 0.5715161680117671, + "angularVelocity": -0.0000012607567828315742, + "velocityX": 2.095164463916719, + "velocityY": 0.06280835552621343, + "timestamp": 0.6712290905485708 + }, + { + "x": 1.9288274410769612, + "y": 6.619746863793874, + "heading": 0.5715160739832708, + "angularVelocity": -0.0000012607568122906995, + "velocityX": 2.0951644639171065, + "velocityY": 0.06280835551253054, + "timestamp": 0.7458101006095231 + }, + { + "x": 2.085086923039661, + "y": 6.624431174390149, + "heading": 0.5715159799547684, + "angularVelocity": -0.0000012607568901512097, + "velocityX": 2.09516446391631, + "velocityY": 0.06280835553779388, + "timestamp": 0.8203911106704754 + }, + { + "x": 2.241346405003104, + "y": 6.629115484970691, + "heading": 0.5715158859260843, + "angularVelocity": -0.0000012607593264970524, + "velocityX": 2.0951644639262916, + "velocityY": 0.06280835532631046, + "timestamp": 0.8949721207314277 + }, + { + "x": 2.397605884569563, + "y": 6.633799875776661, + "heading": 0.5715157918938158, + "angularVelocity": -0.0000012608069432354713, + "velocityX": 2.095164431787018, + "velocityY": 0.06280943101139673, + "timestamp": 0.96955313079238 + }, + { + "x": 2.541564258519034, + "y": 6.643991369670441, + "heading": 0.5358742044161485, + "angularVelocity": -0.4778909195321637, + "velocityX": 1.9302282689900117, + "velocityY": 0.1366499848331686, + "timestamp": 1.0441341408533324 + }, + { + "x": 2.6647430298386063, + "y": 6.651108311020372, + "heading": 0.4363244672087119, + "angularVelocity": -1.3347866585073835, + "velocityX": 1.6516103927652543, + "velocityY": 0.09542564982805292, + "timestamp": 1.1187151509142848 + }, + { + "x": 2.765146548502248, + "y": 6.655141686978524, + "heading": 0.28217231220283606, + "angularVelocity": -2.0669089206452895, + "velocityX": 1.3462343642383414, + "velocityY": 0.054080468404030795, + "timestamp": 1.193296160975237 + }, + { + "x": 2.838497759398012, + "y": 6.6577675266394865, + "heading": 0.14782655645686552, + "angularVelocity": -1.8013399877003722, + "velocityX": 0.9835105589992653, + "velocityY": 0.03520788547662426, + "timestamp": 1.2678771710361894 + }, + { + "x": 2.88643730414052, + "y": 6.659290340099078, + "heading": 0.050922701669744434, + "angularVelocity": -1.2993100349254527, + "velocityX": 0.6427848684717067, + "velocityY": 0.020418246660172383, + "timestamp": 1.3424581810971417 + }, + { + "x": 2.9100513458251953, + "y": 6.66, + "heading": -9.944840194397303e-31, + "angularVelocity": -0.6827837492158759, + "velocityX": 0.3166227122075712, + "velocityY": 0.00951528948644474, + "timestamp": 1.417039191158094 + }, + { + "x": 2.9100513458251953, + "y": 6.66, + "heading": -9.944840194397303e-31, + "angularVelocity": 0, + "velocityX": -3.154930727150579e-32, "velocityY": 0, - "timestamp": 0.6940330402272251 + "timestamp": 1.4916202012190463 } ], "constraints": [ @@ -231,14 +231,14 @@ "first" ], "type": "StopPoint", - "uuid": "ecacd200-fbc8-479b-9419-adbfcf928d7e" + "uuid": "8a938451-bc8a-4878-accc-f317a81eff2c" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "e15fd1d7-0b02-4323-8191-cf274496326b" + "uuid": "87a75af8-fc1f-4486-bf19-15d942fa1b60" } ], "usesControlIntervalGuessing": false, @@ -842,14 +842,14 @@ "first" ], "type": "StopPoint", - "uuid": "21ee00cc-d73a-4b8a-9f66-82b5d82985fe" + "uuid": "f89139aa-e584-4d70-bd19-00d3fc80fe4a" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "b3667f27-30e2-43c3-9227-8d59f6e41e1e" + "uuid": "b573a51f-d717-45a2-b7a4-0634d6faf415" }, { "scope": [ @@ -857,7 +857,7 @@ 2 ], "type": "MaxVelocity", - "uuid": "dc3f259a-1a9a-4047-8c48-ee10a4c154dd", + "uuid": "5b089b9e-e3cd-480b-a3f0-3199985f0deb", "velocity": 2 } ], @@ -958,14 +958,14 @@ "first" ], "type": "StopPoint", - "uuid": "60076cec-345f-4756-b246-814f2042662a" + "uuid": "52322f3c-332d-4cf1-80ae-c7a6c8c4b968" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "40b325f5-f60f-4955-b80b-8bfdcc2f98ae" + "uuid": "fd884c7b-8f72-4f8b-8a32-ed059aee0ed1" }, { "scope": [ @@ -973,7 +973,7 @@ 1 ], "type": "MaxVelocity", - "uuid": "e33eecc7-1a71-4100-b0f1-976abd58d3a8", + "uuid": "ae458b46-06e4-4342-ab4d-fc3a603d2549", "velocity": 2 } ], @@ -1569,14 +1569,14 @@ "first" ], "type": "StopPoint", - "uuid": "34e0b06e-861c-485e-9bfc-0a09d98d177a" + "uuid": "de325963-c4e2-41b4-8df2-9751661729fc" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "3db9fa3d-2832-41d3-b056-428d71c1f9b9" + "uuid": "63652bc7-2a77-47e2-9928-640d974f7860" }, { "scope": [ @@ -1584,7 +1584,7 @@ "last" ], "type": "MaxVelocity", - "uuid": "59d18add-44d2-4dbb-8eea-c9e49f01fd49", + "uuid": "4605ab78-6ee7-4c53-8d08-e584753fc453", "velocity": 2 } ], @@ -1596,8 +1596,8 @@ "n2 to n3": { "waypoints": [ { - "x": 2.5, - "y": 5.509, + "x": 2.7, + "y": 5.27, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -1614,8 +1614,8 @@ "controlIntervalCount": 20 }, { - "x": 2.5, - "y": 4.060801029205322, + "x": 2.7, + "y": 4, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -1625,373 +1625,373 @@ ], "trajectory": [ { - "x": 2.5, - "y": 5.509, + "x": 2.7, + "y": 5.27, "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": 1.5949944891681758e-33, + "velocityY": -5.00840856616175e-28, "timestamp": 0 }, { - "x": 2.4943444648641293, - "y": 5.5064791255479575, - "heading": -1.6665950731162323e-20, - "angularVelocity": -8.896918377207487e-19, - "velocityX": -0.3019139532075592, - "velocityY": -0.13457385606700714, - "timestamp": 0.01873227479480579 - }, - { - "x": 2.483082181980465, - "y": 5.501330655009401, - "heading": -6.635323653535645e-20, - "angularVelocity": -2.6524961112440185e-18, - "velocityX": -0.6012234502766525, - "velocityY": -0.2748449184605893, - "timestamp": 0.03746454958961158 - }, - { - "x": 2.46627509012907, - "y": 5.49342628716381, - "heading": 3.593397333313667e-19, - "angularVelocity": 2.2725103255105993e-17, - "velocityX": -0.8972264199355992, - "velocityY": -0.42196518747333667, - "timestamp": 0.05619682438441737 - }, - { - "x": 2.4440039523453803, - "y": 5.482609176230366, - "heading": 1.0617530792760623e-18, - "angularVelocity": 3.749748835023413e-17, - "velocityX": -1.1889179519117046, - "velocityY": -0.5774584801866853, - "timestamp": 0.07492909917922316 - }, - { - "x": 2.416377557137626, - "y": 5.468683837447711, - "heading": 1.4162827940624112e-18, - "angularVelocity": 1.8926140859171294e-17, - "velocityX": -1.474801939987141, - "velocityY": -0.7433874921864966, - "timestamp": 0.09366137397402896 - }, - { - "x": 2.3835484355934935, - "y": 5.451401194530706, - "heading": 2.0446425305173568e-18, - "angularVelocity": 3.3544221150580245e-17, - "velocityX": -1.7525432390748785, - "velocityY": -0.9226131426280576, - "timestamp": 0.11239364876883476 - }, - { - "x": 2.3457413715260427, - "y": 5.430436170960037, - "heading": 2.625153686840878e-18, - "angularVelocity": 3.098988185000503e-17, - "velocityX": -2.0182847241773043, - "velocityY": -1.1191926127672032, - "timestamp": 0.13112592356364056 - }, - { - "x": 2.303308675591435, - "y": 5.405354917239582, - "heading": 3.474873201601955e-18, - "angularVelocity": 4.536124688156702e-17, - "velocityX": -2.2652185278838264, - "velocityY": -1.3389326173781921, - "timestamp": 0.14985819835844635 - }, - { - "x": 2.256844133996144, - "y": 5.375574300168143, - "heading": 4.0955382647030466e-18, - "angularVelocity": 3.31334537599372e-17, - "velocityX": -2.4804537678559324, - "velocityY": -1.5898024878269215, - "timestamp": 0.16859047315325215 - }, - { - "x": 2.2074197303799292, - "y": 5.3403550272659315, - "heading": 4.355462667028749e-18, - "angularVelocity": 1.3875749071945138e-17, - "velocityX": -2.6384624482403214, - "velocityY": -1.8801385997164786, - "timestamp": 0.18732274794805795 - }, - { - "x": 2.156967572169077, - "y": 5.2990297375325435, - "heading": 4.29698949681871e-18, - "angularVelocity": -3.1215197468412938e-18, - "velocityX": -2.6933278933555838, - "velocityY": -2.2061009773807925, - "timestamp": 0.20605502274286375 - }, - { - "x": 2.108241591757582, - "y": 5.251758019086516, - "heading": 4.0630092494696724e-18, - "angularVelocity": -1.2490750274876772e-17, - "velocityX": -2.6011779639814714, - "velocityY": -2.5235439349379956, - "timestamp": 0.22478729753766954 - }, - { - "x": 2.0635590541012263, - "y": 5.199796918965488, - "heading": 3.723409843166543e-18, - "angularVelocity": -1.8129101367482953e-17, - "velocityX": -2.3853236270452767, - "velocityY": -2.773880945598276, - "timestamp": 0.24351957233247534 - }, - { - "x": 2.024123123965078, - "y": 5.144547524590311, - "heading": 3.368607724278227e-18, - "angularVelocity": -1.894068001303013e-17, - "velocityX": -2.105239783642413, - "velocityY": -2.949422586438396, - "timestamp": 0.2622518471272811 - }, - { - "x": 1.9904575467629446, - "y": 5.087052505755269, - "heading": 3.0083094680873597e-18, - "angularVelocity": -1.9234087711961826e-17, - "velocityX": -1.7971964201308785, - "velocityY": -3.0693025521374286, - "timestamp": 0.2809841219220869 - }, - { - "x": 1.9627923272283192, - "y": 5.028029241661778, - "heading": 2.6490143792152282e-18, - "angularVelocity": -1.918053490889217e-17, - "velocityX": -1.4768745300648793, - "velocityY": -3.150886090452353, - "timestamp": 0.29971639671689265 - }, - { - "x": 1.941233279386563, - "y": 4.967979084271092, - "heading": 2.295124883149197e-18, - "angularVelocity": -1.889196390427566e-17, - "velocityX": -1.1509038852950093, - "velocityY": -3.205705556237633, - "timestamp": 0.3184486715116984 - }, - { - "x": 1.9258304578861423, - "y": 4.907265050190802, - "heading": 1.8766425205004917e-18, - "angularVelocity": -2.2340176069469045e-17, - "velocityX": -0.822261133212249, - "velocityY": -3.241145816263607, - "timestamp": 0.3371809463065042 - }, - { - "x": 1.9166071629104302, - "y": 4.846159459726292, - "heading": 1.4687210226502946e-18, - "angularVelocity": -2.1776397057375608e-17, - "velocityX": -0.49237452881426474, - "velocityY": -3.262048583733586, - "timestamp": 0.35591322110130996 + "x": 2.6930041825612396, + "y": 5.26928432609697, + "heading": -1.1816834704103417e-20, + "angularVelocity": -3.1498607863339557e-19, + "velocityX": -0.18646463918650766, + "velocityY": -0.01907538000761375, + "timestamp": 0.03751819899629495 + }, + { + "x": 2.679023019715013, + "y": 5.267757112086426, + "heading": -1.1982484405963989e-18, + "angularVelocity": -3.162288568366952e-17, + "velocityX": -0.37265015966331894, + "velocityY": -0.040705952083294986, + "timestamp": 0.0750363979925899 + }, + { + "x": 2.658071304417103, + "y": 5.265299821378877, + "heading": -2.3024750226762525e-18, + "angularVelocity": -2.9431811326509344e-17, + "velocityX": -0.5584413926695573, + "velocityY": -0.06549596658054355, + "timestamp": 0.11255459698888486 + }, + { + "x": 2.6301707261184073, + "y": 5.26176227954264, + "heading": -3.3161342568916012e-18, + "angularVelocity": -2.7017849555311248e-17, + "velocityX": -0.7436545208766203, + "velocityY": -0.0942886900483679, + "timestamp": 0.1500727959851798 + }, + { + "x": 2.595354617234827, + "y": 5.25694838737037, + "heading": -4.228322057875787e-18, + "angularVelocity": -2.43132528127072e-17, + "velocityX": -0.927979215822151, + "velocityY": -0.1283081891234354, + "timestamp": 0.18759099498147475 + }, + { + "x": 2.5536774231155643, + "y": 5.250592050828935, + "heading": -4.902812238816008e-18, + "angularVelocity": -1.79777158616401e-17, + "velocityX": -1.1108527390521832, + "velocityY": -0.16942008709418127, + "timestamp": 0.22510919397776968 + }, + { + "x": 2.505235624376603, + "y": 5.24231380432782, + "heading": -5.418282495230493e-18, + "angularVelocity": -1.3739235384430708e-17, + "velocityX": -1.2911546938550877, + "velocityY": -0.22064615900354675, + "timestamp": 0.2626273929740646 + }, + { + "x": 2.4502205541283706, + "y": 5.2315364184801245, + "heading": -5.7648703440253685e-18, + "angularVelocity": -9.237882353094148e-18, + "velocityX": -1.4663569073180298, + "velocityY": -0.28725754796798325, + "timestamp": 0.30014559197035956 + }, + { + "x": 2.3890767277206044, + "y": 5.217310567080376, + "heading": -5.890943886177935e-18, + "angularVelocity": -3.360353857609014e-18, + "velocityX": -1.6297111280302612, + "velocityY": -0.37917202265783617, + "timestamp": 0.3376637909666545 + }, + { + "x": 2.3230664799507315, + "y": 5.198008155784784, + "heading": -6.1785882537098945e-18, + "angularVelocity": -7.666822033208596e-18, + "velocityX": -1.7594194160703869, + "velocityY": -0.5144812867392538, + "timestamp": 0.37518198996294944 + }, + { + "x": 2.255862321531191, + "y": 5.17177551793778, + "heading": -6.695668627212835e-18, + "angularVelocity": -1.3782112845000978e-17, + "velocityX": -1.7912415898848488, + "velocityY": -0.6991976840308759, + "timestamp": 0.4127001889592444 + }, + { + "x": 2.192202860236617, + "y": 5.139469280223623, + "heading": -6.883874082813852e-18, + "angularVelocity": -5.01635811715478e-18, + "velocityX": -1.696762184690978, + "velocityY": -0.8610817837371232, + "timestamp": 0.4502183879555393 + }, + { + "x": 2.134134507416613, + "y": 5.102897710542656, + "heading": -6.85446046853456e-18, + "angularVelocity": 7.840090786572423e-19, + "velocityX": -1.5477382809785758, + "velocityY": -0.974768796461879, + "timestamp": 0.48773658695183425 + }, + { + "x": 2.0823628902154088, + "y": 5.063194893980567, + "heading": -6.674766174603623e-18, + "angularVelocity": 4.7895544482881795e-18, + "velocityX": -1.3799067808711225, + "velocityY": -1.058228209887372, + "timestamp": 0.5252547859481292 + }, + { + "x": 2.0371815070623285, + "y": 5.0210380148882745, + "heading": -6.384013416846825e-18, + "angularVelocity": 7.749679994101778e-18, + "velocityX": -1.2042524524521976, + "velocityY": -1.1236381334912477, + "timestamp": 0.5627729849444242 + }, + { + "x": 1.9987362683691992, + "y": 4.976861735113259, + "heading": -6.007530542513076e-18, + "angularVelocity": 1.0034709668421525e-17, + "velocityX": -1.0247090671030967, + "velocityY": -1.1774626969472783, + "timestamp": 0.6002911839407191 + }, + { + "x": 1.9671096330518483, + "y": 4.930964909309413, + "heading": -5.562569285242097e-18, + "angularVelocity": 1.1859879305883021e-17, + "velocityX": -0.8429678439572205, + "velocityY": -1.2233216687202904, + "timestamp": 0.6378093829370141 + }, + { + "x": 1.9423526866284888, + "y": 4.883564417353199, + "heading": -5.061777213472376e-18, + "angularVelocity": 1.3347986496029381e-17, + "velocityX": -0.6598650011288002, + "velocityY": -1.2633999825124016, + "timestamp": 0.675327581933309 + }, + { + "x": 1.9244993041283396, + "y": 4.8348243554396655, + "heading": -4.514546757319931e-18, + "angularVelocity": 1.4585743490895e-17, + "velocityX": -0.47585926236927334, + "velocityY": -1.2991045203921598, + "timestamp": 0.7128457809296039 }, { "x": 1.913573145866394, "y": 4.784873008728027, - "heading": 1.0730296908708793e-18, - "angularVelocity": -2.1123504400879342e-17, - "velocityX": -0.16196735726282194, - "velocityY": -3.2717036061874047, - "timestamp": 0.37464549589611573 - }, - { - "x": 1.9172081226819078, - "y": 4.720543496953144, - "heading": 6.726891716838813e-19, - "angularVelocity": -2.0365443785309124e-17, - "velocityX": 0.18491238625516085, - "velocityY": -3.272462008053034, - "timestamp": 0.3943033289333137 - }, - { - "x": 1.9276595873253093, - "y": 4.656396088408969, - "heading": 3.1960685238797674e-19, - "angularVelocity": -1.7961405046966803e-17, - "velocityX": 0.5316692141816879, - "velocityY": -3.26319836081565, - "timestamp": 0.4139611619705117 - }, - { - "x": 1.9449161546870903, - "y": 4.592682541241059, - "heading": -6.838836479966531e-20, - "angularVelocity": -1.9737434946765602e-17, - "velocityX": 0.8778468780931784, - "velocityY": -3.2411276994439615, - "timestamp": 0.4336189950077097 - }, - { - "x": 1.968948566315896, - "y": 4.529734226845009, - "heading": -4.2094144486999917e-19, - "angularVelocity": -1.793448263863688e-17, - "velocityX": 1.222536155604247, - "velocityY": -3.2022000734735956, - "timestamp": 0.45327682804490765 - }, - { - "x": 1.9996904062675076, - "y": 4.46800295896022, - "heading": -7.262291331492882e-19, - "angularVelocity": -1.5530077760537348e-17, - "velocityX": 1.5638468336484368, - "velocityY": -3.140288543908866, - "timestamp": 0.47293466108210563 - }, - { - "x": 2.0369930888071974, - "y": 4.408129903333294, - "heading": -9.67326202918917e-19, - "angularVelocity": -1.2264681442804184e-17, - "velocityX": 1.8975989097629915, - "velocityY": -3.0457607160274165, - "timestamp": 0.4925924941193036 - }, - { - "x": 2.080512421569209, - "y": 4.351058782117612, - "heading": -1.6795534015194326e-18, - "angularVelocity": -3.623121376951248e-17, - "velocityX": 2.2138418145917167, - "velocityY": -2.9032254525554837, - "timestamp": 0.5122503271565015 - }, - { - "x": 2.1294148840951923, - "y": 4.298173284699749, - "heading": -1.923535200935003e-18, - "angularVelocity": -1.2411428555077921e-17, - "velocityX": 2.4876832778794475, - "velocityY": -2.690301485305471, - "timestamp": 0.5319081601936995 - }, - { - "x": 2.1817785731576067, - "y": 4.25116293816539, - "heading": -2.1365644616328657e-18, - "angularVelocity": -1.0836863419398235e-17, - "velocityX": 2.663756934110092, - "velocityY": -2.391430756655736, - "timestamp": 0.5515659932308974 - }, - { - "x": 2.2344726220787043, - "y": 4.210963490410107, - "heading": -2.140655615689113e-18, - "angularVelocity": -2.0811842087119213e-19, - "velocityX": 2.6805624415155918, - "velocityY": -2.0449582453576474, - "timestamp": 0.5712238262680953 - }, - { - "x": 2.2847273587342762, - "y": 4.177131716956, - "heading": -1.978722549738529e-18, - "angularVelocity": 8.23758723812157e-18, - "velocityX": 2.556473877892601, - "velocityY": -1.7210326992852008, - "timestamp": 0.5908816593052932 - }, - { - "x": 2.330941601494517, - "y": 4.148792845801302, - "heading": -1.6558729417207766e-18, - "angularVelocity": 1.6423460741170202e-17, - "velocityX": 2.3509327133254683, - "velocityY": -1.4416070734283617, - "timestamp": 0.6105394923424912 - }, - { - "x": 2.372265900292984, - "y": 4.125206452882029, - "heading": -1.2164556479802771e-18, - "angularVelocity": 2.2353294405952366e-17, - "velocityX": 2.1021797631645622, - "velocityY": -1.1998470469579758, - "timestamp": 0.6301973253796891 - }, - { - "x": 2.408221763010669, - "y": 4.105824524230037, - "heading": -9.665698088983262e-19, - "angularVelocity": 1.2711770275248128e-17, - "velocityX": 1.8290857720505087, - "velocityY": -0.9859646592438476, - "timestamp": 0.649855158416887 - }, - { - "x": 2.4385166924875037, - "y": 4.0902441055405845, - "heading": -6.950831801501574e-19, - "angularVelocity": 1.3810611494728119e-17, - "velocityX": 1.5411123606303396, - "velocityY": -0.7925806806870744, - "timestamp": 0.6695129914540849 - }, - { - "x": 2.462958637558451, - "y": 4.078162290794493, - "heading": -2.530600057548883e-19, - "angularVelocity": 2.248585116626235e-17, - "velocityX": 1.2433692475002198, - "velocityY": -0.6146056242939526, - "timestamp": 0.6891708244912829 - }, - { - "x": 2.4814140310104698, - "y": 4.069345267851417, - "heading": 2.628695897183014e-19, - "angularVelocity": 2.6245493348697748e-17, - "velocityX": 0.9388315292481915, - "velocityY": -0.44852466324204665, - "timestamp": 0.7088286575284808 - }, - { - "x": 2.4937856312994655, - "y": 4.063608039419202, - "heading": 6.100758780348518e-20, - "angularVelocity": -1.0268781955269092e-17, - "velocityX": 0.6293471038025783, - "velocityY": -0.2918545712215822, - "timestamp": 0.7284864905656787 - }, - { - "x": 2.5, - "y": 4.060801029205322, - "heading": -3.1744174721418913e-35, - "angularVelocity": -3.1034754196134877e-18, - "velocityX": 0.31612684311516714, - "velocityY": -0.14279347110988308, - "timestamp": 0.7481443236028766 - }, - { - "x": 2.5, - "y": 4.060801029205322, - "heading": -7.2806203593007275e-34, - "angularVelocity": -3.5421900256621765e-32, - "velocityX": -1.0821894867257684e-36, - "velocityY": -1.3445064216515772e-31, - "timestamp": 0.7678021566400746 + "heading": -3.928320201048709e-18, + "angularVelocity": 1.562513559934564e-17, + "velocityX": -0.2912228879283196, + "velocityY": -1.3313897800911476, + "timestamp": 0.7503639799258989 + }, + { + "x": 1.9102995444308852, + "y": 4.724875635653645, + "heading": -3.1966305278465913e-18, + "angularVelocity": 1.6658528200090963e-17, + "velocityX": -0.07453084358795621, + "velocityY": -1.3659741160792864, + "timestamp": 0.7942867544276485 + }, + { + "x": 1.9165641847665207, + "y": 4.663493945133946, + "heading": -2.417664167727648e-18, + "angularVelocity": 1.773488716240096e-17, + "velocityX": 0.1426285203035159, + "velocityY": -1.3974911926117468, + "timestamp": 0.8382095289293982 + }, + { + "x": 1.9323905364900036, + "y": 4.600900619777049, + "heading": -1.5888861332178403e-18, + "angularVelocity": 1.8868961285191166e-17, + "velocityX": 0.3603222224050521, + "velocityY": -1.4250767641310782, + "timestamp": 0.8821323034311478 + }, + { + "x": 1.957804843659842, + "y": 4.53732470372903, + "heading": -9.396794624081822e-19, + "angularVelocity": 1.4780630111570506e-17, + "velocityX": 0.5786134290771148, + "velocityY": -1.4474476344227822, + "timestamp": 0.9260550779328974 + }, + { + "x": 1.992834361356742, + "y": 4.47308389106805, + "heading": -3.81904550909219e-19, + "angularVelocity": 1.2698987129083589e-17, + "velocityX": 0.797525158496715, + "velocityY": -1.4625854899100572, + "timestamp": 0.9699778524346471 + }, + { + "x": 2.0375000412705417, + "y": 4.4086463054393965, + "heading": 2.8296907647234686e-19, + "angularVelocity": 1.513732728897157e-17, + "velocityX": 1.0169138999011074, + "velocityY": -1.46706546572779, + "timestamp": 1.0139006269363968 + }, + { + "x": 2.0917879496023337, + "y": 4.344762824527069, + "heading": 7.985525057259883e-19, + "angularVelocity": 1.1738423181491662e-17, + "velocityX": 1.2359854072719534, + "velocityY": -1.4544500350568836, + "timestamp": 1.0578234014381465 + }, + { + "x": 2.155522146006728, + "y": 4.282792952951991, + "heading": 1.4108637952195666e-18, + "angularVelocity": 1.3940628383244673e-17, + "velocityX": 1.4510512399669449, + "velocityY": -1.4108824471777495, + "timestamp": 1.101746175939896 + }, + { + "x": 2.2276450184159087, + "y": 4.225569055680285, + "heading": 1.830932906447906e-18, + "angularVelocity": 9.56381088968155e-18, + "velocityX": 1.6420381732654001, + "velocityY": -1.3028297488099847, + "timestamp": 1.1456689504416457 + }, + { + "x": 2.305355038471454, + "y": 4.176198679539435, + "heading": 2.738350143074857e-18, + "angularVelocity": 2.065937898967755e-17, + "velocityX": 1.7692420602721368, + "velocityY": -1.1240268106739357, + "timestamp": 1.1895917249433954 + }, + { + "x": 2.381463509354403, + "y": 4.136332462140534, + "heading": 2.7713161781139262e-18, + "angularVelocity": 7.504862268589612e-19, + "velocityX": 1.7327792187086972, + "velocityY": -0.9076434229925813, + "timestamp": 1.233514499445145 + }, + { + "x": 2.450950341414104, + "y": 4.103469678408031, + "heading": 2.4984339271026296e-18, + "angularVelocity": -6.212841183256414e-18, + "velocityX": 1.5820228309719393, + "velocityY": -0.7481946234592841, + "timestamp": 1.2774372739468947 + }, + { + "x": 2.512532327834692, + "y": 4.076121211708663, + "heading": 1.986026033701587e-18, + "angularVelocity": -1.1666154831128573e-17, + "velocityX": 1.402051375863395, + "velocityY": -0.6226488878991892, + "timestamp": 1.3213600484486443 + }, + { + "x": 2.565724514235354, + "y": 4.053516749448511, + "heading": 1.2912808268140822e-18, + "angularVelocity": -1.581745371606986e-17, + "velocityX": 1.211038851827205, + "velocityY": -0.5146410379281966, + "timestamp": 1.365282822950394 + }, + { + "x": 2.6102852013760414, + "y": 4.035200826485555, + "heading": 6.452681315987112e-19, + "angularVelocity": -1.4707917382894904e-17, + "velocityX": 1.0145235051044834, + "velocityY": -0.41700286854164376, + "timestamp": 1.4092055974521436 + }, + { + "x": 2.6460725889006635, + "y": 4.020875339537886, + "heading": -1.281828609245258e-19, + "angularVelocity": -1.76093223951591e-17, + "velocityX": 0.8147797567690056, + "velocityY": -0.3261516857519213, + "timestamp": 1.4531283719538932 + }, + { + "x": 2.672994367091148, + "y": 4.010330771618785, + "heading": -5.651109953509987e-20, + "angularVelocity": 1.6317494913316837e-18, + "velocityX": 0.6129343716612188, + "velocityY": -0.2400706248188071, + "timestamp": 1.4970511464556429 + }, + { + "x": 2.6909860044770397, + "y": 4.003412078969364, + "heading": -8.628796629492157e-20, + "angularVelocity": -6.779449938633521e-19, + "velocityX": 0.4096197835907762, + "velocityY": -0.157519481123181, + "timestamp": 1.5409739209573925 + }, + { + "x": 2.7, + "y": 4, + "heading": -2.226129737777408e-30, + "angularVelocity": 1.964529426955296e-18, + "velocityX": 0.20522372790232943, + "velocityY": -0.07768359371383228, + "timestamp": 1.5848966954591421 + }, + { + "x": 2.7, + "y": 4, + "heading": -4.734988782185317e-30, + "angularVelocity": -5.711965804051055e-29, + "velocityX": -1.2601722232887796e-27, + "velocityY": 8.717706191659183e-28, + "timestamp": 1.6288194699608918 } ], "constraints": [ @@ -2000,14 +2000,14 @@ "first" ], "type": "StopPoint", - "uuid": "0498379b-9433-48fb-a153-04829dd4bcea" + "uuid": "f0527566-c4a5-4ee5-8697-324fe290bc3f" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "1a867367-34fe-4cbd-91dd-8d118df4d8a6" + "uuid": "28acfdba-1c51-4531-9ce3-61072fbcb55d" } ], "usesControlIntervalGuessing": false, @@ -2161,14 +2161,14 @@ "first" ], "type": "StopPoint", - "uuid": "6be9f004-a1ce-47b1-a451-0e8325368113" + "uuid": "ee0a7aaa-424b-4c5a-89d3-39da003ef04a" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "c57bde0b-2e10-4f0b-b509-ed56d7ea4973" + "uuid": "9b2e9480-c747-40cc-a2e9-76cc2b379a23" }, { "scope": [ @@ -2176,7 +2176,7 @@ 1 ], "type": "MaxVelocity", - "uuid": "4fe79f92-f09d-44b7-8817-f4921b7f2a7b", + "uuid": "b07c4d77-1552-4b6d-beb8-7ac893eecd74", "velocity": 2 } ], @@ -2592,14 +2592,14 @@ "first" ], "type": "StopPoint", - "uuid": "d199f124-875a-4c27-993a-f119d7538bf2" + "uuid": "b8a3b02c-4e21-48a3-adf7-4a3f2a711922" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "b1ec7568-c771-4e68-93a2-5dc613b7aa69" + "uuid": "3808cb7e-74aa-4d96-81e3-86a48488fcc1" }, { "scope": [ @@ -2607,7 +2607,7 @@ 1 ], "type": "MaxVelocity", - "uuid": "a8dbdb03-b7ef-471d-bad8-97109686f010", + "uuid": "8e657f11-b388-44b1-b0ae-a204ede169e0", "velocity": 2 } ], @@ -3023,14 +3023,14 @@ "first" ], "type": "StopPoint", - "uuid": "77a22270-117d-41d3-99c9-6182509515f5" + "uuid": "8510f842-1d3b-45ee-959f-4c4d9ddea113" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "deab6515-8453-4e44-9963-a538ebb8cd65" + "uuid": "5d534716-586a-4413-bc22-876fb197f722" }, { "scope": [ @@ -3038,7 +3038,7 @@ 1 ], "type": "MaxVelocity", - "uuid": "54152c78-ddb3-4336-8ea0-10a144529ba9", + "uuid": "39022f3f-8bec-48e3-992c-b18d82005533", "velocity": 2 } ], @@ -3050,8 +3050,8 @@ "n1 to n2": { "waypoints": [ { - "x": 2.5, - "y": 6.957, + "x": 2.91, + "y": 6.66, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -3068,8 +3068,8 @@ "controlIntervalCount": 20 }, { - "x": 2.5, - "y": 5.509, + "x": 2.7, + "y": 5.27, "heading": 0, "isInitialGuess": false, "translationConstrained": true, @@ -3079,373 +3079,373 @@ ], "trajectory": [ { - "x": 2.5, - "y": 6.957, + "x": 2.91, + "y": 6.66, "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": -4.757446226639569e-33, + "velocityY": -6.423240989695239e-30, "timestamp": 0 }, { - "x": 2.4943446221143897, - "y": 6.954479923641855, - "heading": 3.6897126781530414e-20, - "angularVelocity": 1.9697832873556895e-18, - "velocityX": -0.3019169811583678, - "velocityY": -0.13453634075911872, - "timestamp": 0.018731566087842332 - }, - { - "x": 2.483082635763188, - "y": 6.949333058137429, - "heading": 5.2597079936043524e-20, - "angularVelocity": 8.38155582821411e-19, - "velocityX": -0.601230366878399, - "velocityY": -0.27476963112913483, - "timestamp": 0.037463132175684664 - }, - { - "x": 2.466275958073038, - "y": 6.941431111397211, - "heading": -1.0918552393928563e-19, - "angularVelocity": -8.636895506035071e-18, - "velocityX": -0.8972382560717649, - "velocityY": -0.4218518997912323, - "timestamp": 0.05619469826352699 - }, - { - "x": 2.4440053256580105, - "y": 6.930617246523635, - "heading": -2.9636578779285724e-19, - "angularVelocity": -9.992772710780623e-18, - "velocityX": -1.1889359549856489, - "velocityY": -0.5773070347064156, - "timestamp": 0.07492626435136933 - }, - { - "x": 2.416379494426775, - "y": 6.916695986231933, - "heading": -2.002041575820117e-19, - "angularVelocity": 5.1336640905466516e-18, - "velocityX": -1.4748276306252714, - "velocityY": -0.7431978845986646, - "timestamp": 0.09365783043921166 - }, - { - "x": 2.3835509550151985, - "y": 6.899418257829668, - "heading": -2.0906128139802138e-19, - "angularVelocity": -4.728480031302008e-19, - "velocityX": -1.7525784687528718, - "velocityY": -0.9223856842105105, - "timestamp": 0.112389396527054 - }, - { - "x": 2.345744441595137, - "y": 6.878458979207731, - "heading": -5.664854088249963e-19, - "angularVelocity": -1.908138066098496e-17, - "velocityX": -2.01833168901968, - "velocityY": -1.118928258515608, - "timestamp": 0.13112096261489634 - }, - { - "x": 2.30331220769535, - "y": 6.853384274840582, - "heading": -1.0549068383945228e-18, - "angularVelocity": -2.6074781886752536e-17, - "velocityX": -2.265279566097002, - "velocityY": -1.3386336331709632, - "timestamp": 0.14985252870273866 - }, - { - "x": 2.2568479900211824, - "y": 6.823610942973657, - "heading": -1.8345727365867855e-18, - "angularVelocity": -4.162310478476867e-17, - "velocityX": -2.4805303227861466, - "velocityY": -1.589473711237126, - "timestamp": 0.16858409479058098 - }, - { - "x": 2.207423790946718, - "y": 6.7883995536652755, - "heading": -2.6797153220676564e-18, - "angularVelocity": -4.5118628896476544e-17, - "velocityX": -2.638551354579043, - "velocityY": -1.8797888624611248, - "timestamp": 0.1873156608784233 - }, - { - "x": 2.156971931826493, - "y": 6.747082606718596, - "heading": -3.7107530407855426e-18, - "angularVelocity": -5.5042786812058804e-17, - "velocityX": -2.6934138279537794, - "velocityY": -2.20573905849193, - "timestamp": 0.20604722696626562 - }, - { - "x": 2.1082465925805094, - "y": 6.699819818236992, - "heading": -3.742581424210628e-18, - "angularVelocity": -1.6991823567672568e-18, - "velocityX": -2.6012421501482437, - "velocityY": -2.5231626795090287, - "timestamp": 0.22477879305410794 - }, - { - "x": 2.063564819070666, - "y": 6.647868372787038, - "heading": -4.086119143230017e-18, - "angularVelocity": -1.8340038541638217e-17, - "velocityX": -2.3853730809429283, - "velocityY": -2.7734704725873653, - "timestamp": 0.24351035914195027 - }, - { - "x": 2.024129444946568, - "y": 6.59262918323462, - "heading": -4.07128100349463e-18, - "angularVelocity": 7.921454718696707e-19, - "velocityX": -2.1052897520241163, - "velocityY": -2.9489893847297766, - "timestamp": 0.2622419252297926 - }, - { - "x": 1.9904640390685304, - "y": 6.535144672578362, - "heading": -4.10027571754238e-18, - "angularVelocity": -1.5479071290730936e-18, - "velocityX": -1.7972552706037406, - "velocityY": -3.0688576911656473, - "timestamp": 0.28097349131763494 - }, - { - "x": 1.9627985342838827, - "y": 6.476132022270179, - "heading": -4.1608406747535095e-18, - "angularVelocity": -3.23330929267101e-18, - "velocityX": -1.476945635773801, - "velocityY": -3.1504386782949707, - "timestamp": 0.29970505740547726 - }, - { - "x": 1.9412387198217298, - "y": 6.416092441347258, - "heading": -4.249951515089872e-18, - "angularVelocity": -4.757254479220396e-18, - "velocityX": -1.150988356288367, - "velocityY": -3.2052622103970294, - "timestamp": 0.3184366234933196 - }, - { - "x": 1.9258346453328115, - "y": 6.355388843391961, - "heading": -4.362967285061641e-18, - "angularVelocity": -6.033438571589532e-18, - "velocityX": -0.8223591351988351, - "velocityY": -3.240711303615491, - "timestamp": 0.3371681895811619 - }, - { - "x": 1.916609614204421, - "y": 6.294293472758921, - "heading": -3.873034512043015e-18, - "angularVelocity": 2.615545924478944e-17, - "velocityX": -0.4924858436891372, - "velocityY": -3.261626408946662, - "timestamp": 0.3558997556690042 + "x": 2.9017252775702578, + "y": 6.660334904568681, + "heading": 3.557918002387283e-19, + "angularVelocity": 8.739618338819358e-18, + "velocityX": -0.20323880003406047, + "velocityY": 0.00822572639175432, + "timestamp": 0.04071428501035364 + }, + { + "x": 2.8851720980286473, + "y": 6.660894166924014, + "heading": 2.4593746412602857e-20, + "angularVelocity": -8.132857702537941e-18, + "velocityX": -0.40656932910438337, + "velocityY": 0.01373626861677896, + "timestamp": 0.08142857002070727 + }, + { + "x": 2.860337882655417, + "y": 6.6615407759417495, + "heading": -4.101778730884207e-19, + "angularVelocity": -1.067574406396209e-17, + "velocityX": -0.6099631951521888, + "velocityY": 0.015881625276030417, + "timestamp": 0.12214285503106091 + }, + { + "x": 2.827222625614269, + "y": 6.6621006275797185, + "heading": -1.2458891712985336e-18, + "angularVelocity": -2.0522253121744445e-17, + "velocityX": -0.8133572045477079, + "velocityY": 0.013750742229429767, + "timestamp": 0.16285714004141455 + }, + { + "x": 2.7858318663934507, + "y": 6.662345465133598, + "heading": -2.427446145131975e-18, + "angularVelocity": -2.9015444749893876e-17, + "velocityX": -1.0166151563348516, + "velocityY": 0.006013554067767749, + "timestamp": 0.20357142505176817 + }, + { + "x": 2.7361833409133944, + "y": 6.661963839528372, + "heading": -3.213541175655737e-18, + "angularVelocity": -1.930093113455471e-17, + "velocityX": -1.219437489016265, + "velocityY": -0.009373260629573183, + "timestamp": 0.2442857100621218 + }, + { + "x": 2.678323290528662, + "y": 6.66050796841237, + "heading": -4.672307495132643e-18, + "angularVelocity": -3.582098491837782e-17, + "velocityX": -1.4211240691100409, + "velocityY": -0.03575823854035497, + "timestamp": 0.2849999950724754 + }, + { + "x": 2.6123718841616017, + "y": 6.657287704995394, + "heading": -6.297598317790764e-18, + "angularVelocity": -3.990899540694541e-17, + "velocityX": -1.6198591317620556, + "velocityY": -0.07909419053847176, + "timestamp": 0.32571428008282904 + }, + { + "x": 2.5386735260706663, + "y": 6.651140357174973, + "heading": -8.461722991277948e-18, + "angularVelocity": -5.314082366829448e-17, + "velocityX": -1.8101351423058039, + "velocityY": -0.1509874929170222, + "timestamp": 0.36642856509318267 + }, + { + "x": 2.4583977191648256, + "y": 6.639961092612853, + "heading": -1.212386291709007e-17, + "angularVelocity": -8.99629845611936e-17, + "velocityX": -1.9716865194971132, + "velocityY": -0.27457843257359255, + "timestamp": 0.4071428501035363 + }, + { + "x": 2.3755519490058536, + "y": 6.620909202377741, + "heading": -1.484617881375725e-17, + "angularVelocity": -6.687476222529868e-17, + "velocityX": -2.0348084250243854, + "velocityY": -0.4679411717542808, + "timestamp": 0.4478571351138899 + }, + { + "x": 2.2959295648381173, + "y": 6.5942288936053375, + "heading": -1.5912583784909486e-17, + "angularVelocity": -2.620018728179701e-17, + "velocityX": -1.9556375396786705, + "velocityY": -0.6553058408302904, + "timestamp": 0.48857142012424354 + }, + { + "x": 2.2222017269017074, + "y": 6.561731593537055, + "heading": -1.7998920552716176e-17, + "angularVelocity": -5.124912608019232e-17, + "velocityX": -1.8108592086924549, + "velocityY": -0.7981793137317525, + "timestamp": 0.5292857051345972 + }, + { + "x": 2.1553520416825007, + "y": 6.5246219052837455, + "heading": -1.798702462065007e-17, + "angularVelocity": 2.877248094927204e-19, + "velocityX": -1.6419221214911441, + "velocityY": -0.9114660430449456, + "timestamp": 0.5699999901449508 + }, + { + "x": 2.0958188014057453, + "y": 6.483632427032881, + "heading": -1.839733676343214e-17, + "angularVelocity": -1.0081331759415523e-17, + "velocityX": -1.462219961903324, + "velocityY": -1.006759132352994, + "timestamp": 0.6107142751553045 + }, + { + "x": 2.043834658344306, + "y": 6.43923791049145, + "heading": -1.8859562986854868e-17, + "angularVelocity": -1.1355707850407907e-17, + "velocityX": -1.2768035358651197, + "velocityY": -1.0903916532167117, + "timestamp": 0.6514285601656582 + }, + { + "x": 1.9995393069281948, + "y": 6.391766764373333, + "heading": -1.7387549289815112e-17, + "angularVelocity": 3.6152451058564913e-17, + "velocityX": -1.087956018503923, + "velocityY": -1.1659579950029253, + "timestamp": 0.6921428451760119 + }, + { + "x": 1.963024249856703, + "y": 6.341458331680076, + "heading": -1.5310581750209048e-17, + "angularVelocity": 5.1011383274234203e-17, + "velocityX": -0.8968610663843175, + "velocityY": -1.2356457366492464, + "timestamp": 0.7328571301863656 + }, + { + "x": 1.9343533533387092, + "y": 6.288494277220297, + "heading": -1.3195006752942667e-17, + "angularVelocity": 5.1959977949498825e-17, + "velocityX": -0.7041974705106752, + "velocityY": -1.3008715355503284, + "timestamp": 0.7735714151967192 }, { "x": 1.9135733842849731, "y": 6.2330169677734375, - "heading": -3.400719263853574e-18, - "angularVelocity": 2.5214933197049766e-17, - "velocityX": -0.162091621448467, - "velocityY": -3.2712964147321877, - "timestamp": 0.37463132175684655 - }, - { - "x": 1.917205979581219, - "y": 6.168694726765038, - "heading": -2.9220139710460773e-18, - "angularVelocity": 2.4351738826537176e-17, - "velocityX": 0.18479014101467978, - "velocityY": -3.272072724039553, - "timestamp": 0.39428927145210496 - }, - { - "x": 1.927655154396695, - "y": 6.104554186928345, - "heading": -2.4618364262321162e-18, - "angularVelocity": 2.3409232177474112e-17, - "velocityX": 0.5315495754878562, - "velocityY": -3.2628295845197286, - "timestamp": 0.4139472211473634 - }, - { - "x": 1.9449095427250505, - "y": 6.040847042418775, - "heading": -1.9605813341962127e-18, - "angularVelocity": 2.5498846810319266e-17, - "velocityX": 0.8777308211607325, - "velocityY": -3.2407827620462775, - "timestamp": 0.4336051708426218 - }, - { - "x": 1.9689399188226782, - "y": 5.977904581007848, - "heading": -1.4825326953016808e-18, - "angularVelocity": 2.4318334570481832e-17, - "velocityX": 1.2224253531091398, - "velocityY": -3.2018833289672246, - "timestamp": 0.4532631205378802 - }, - { - "x": 1.999679925253729, - "y": 5.916178505713037, - "heading": -1.0314098682566954e-18, - "angularVelocity": 2.2948618572175324e-17, - "velocityX": 1.5637442819616925, - "velocityY": -3.1400057611143075, - "timestamp": 0.4729210702331386 - }, - { - "x": 2.036981086275461, - "y": 5.856309837875825, - "heading": -6.124142555239329e-19, - "angularVelocity": 2.1314307251887193e-17, - "velocityX": 1.8975102490332074, - "velocityY": -3.045519434391944, - "timestamp": 0.49257901992839703 - }, - { - "x": 2.080499423518141, - "y": 5.799242134164281, - "heading": -1.1443208997745532e-19, - "angularVelocity": 2.5332353415157017e-17, - "velocityX": 2.213778034704021, - "velocityY": -2.903034375212876, - "timestamp": 0.5122369696236555 - }, - { - "x": 2.1294018036645377, - "y": 5.746359011482999, - "heading": 2.485888925042906e-20, - "angularVelocity": 7.085733120179258e-18, - "velocityX": 2.487664324331455, - "velocityY": -2.6901647171289556, - "timestamp": 0.531894919318914 - }, - { - "x": 2.181766698039788, - "y": 5.699350374820504, - "heading": 1.0628834205133463e-19, - "angularVelocity": 4.142317505357256e-18, - "velocityX": 2.663802440591291, - "velocityY": -2.3913295837679795, - "timestamp": 0.5515528690141724 - }, - { - "x": 2.2344628035738343, - "y": 5.659152676616759, - "heading": 1.821790588441185e-19, - "angularVelocity": 3.860562344858861e-18, - "velocityX": 2.6806511539072937, - "velocityY": -2.0448571100698536, - "timestamp": 0.5712108187094309 - }, - { - "x": 2.2847196675943717, - "y": 5.62532276648187, - "heading": -2.177096233228433e-22, - "angularVelocity": -9.278521924332595e-18, - "velocityX": 2.5565669258304347, - "velocityY": -1.7209276989374398, - "timestamp": 0.5908687684046894 - }, - { - "x": 2.3309357944226123, - "y": 5.596985680144109, - "heading": -2.179376064772025e-19, - "angularVelocity": -1.1075409221490475e-17, - "velocityX": 2.3510146045081415, - "velocityY": -1.441507724714413, - "timestamp": 0.6105267180999479 - }, - { - "x": 2.3722616758048822, - "y": 5.573400877946735, - "heading": -2.1185463417811083e-19, - "angularVelocity": 3.0944314555748793e-19, - "velocityX": 2.10224779404314, - "velocityY": -1.199759006559285, - "timestamp": 0.6301846677952063 - }, - { - "x": 2.4082188315941186, - "y": 5.554020301597982, - "heading": -1.4180298734171178e-19, - "angularVelocity": 3.5635284124926384e-18, - "velocityX": 1.8291406960873096, - "velocityY": -0.9858900164664522, - "timestamp": 0.6498426174904648 - }, - { - "x": 2.4385147898608133, - "y": 5.538440986818481, - "heading": -8.901703862511827e-20, - "angularVelocity": 2.6852225163959986e-18, - "velocityX": 1.541155549604471, - "velocityY": -0.7925198212945216, - "timestamp": 0.6695005671857233 - }, - { - "x": 2.4629575237386443, - "y": 5.526360032228848, - "heading": -5.049125518872738e-20, - "angularVelocity": 1.9598080581490267e-18, - "velocityX": 1.243401995464726, - "velocityY": -0.6145582208173452, - "timestamp": 0.6891585168809817 - }, - { - "x": 2.481413486551153, - "y": 5.517543636073691, - "heading": -2.3943762824284567e-20, - "angularVelocity": 1.3504725300916025e-18, - "velocityX": 0.9388549212209834, - "velocityY": -0.4484901168144495, - "timestamp": 0.7088164665762402 - }, - { - "x": 2.4937854535518733, - "y": 5.511806813298085, - "heading": -7.590378603659992e-21, - "angularVelocity": 8.318983835108739e-19, - "velocityX": 0.6293620236348159, - "velocityY": -0.29183220348720296, - "timestamp": 0.7284744162714987 - }, - { - "x": 2.5, - "y": 5.509, - "heading": -1.243215768494891e-34, - "angularVelocity": 3.8612244066878563e-19, - "velocityX": 0.316134009114162, - "velocityY": -0.14278260660937586, - "timestamp": 0.7481323659667571 - }, - { - "x": 2.5, - "y": 5.509, - "heading": -1.2427239750636687e-34, - "angularVelocity": 2.5055071878670228e-36, - "velocityX": -3.816803690129975e-32, - "velocityY": -1.5853074900155989e-31, - "timestamp": 0.7677903156620156 + "heading": -1.3350384577684241e-17, + "angularVelocity": -3.817492839589226e-18, + "velocityX": -0.5103852136511203, + "velocityY": -1.3626006064747218, + "timestamp": 0.8142857002070729 + }, + { + "x": 1.9001367061426089, + "y": 6.165492022212096, + "heading": -1.383548050805982e-17, + "angularVelocity": -1.027860766237449e-17, + "velocityX": -0.28473055086618276, + "velocityY": -1.430890488180813, + "timestamp": 0.8614765552156922 + }, + { + "x": 1.8973992844227976, + "y": 6.094915987268133, + "heading": -1.3450172270417929e-17, + "angularVelocity": 8.165532188269098e-18, + "velocityX": -0.05800746181448894, + "velocityY": -1.4955447391395353, + "timestamp": 0.9086674102243115 + }, + { + "x": 1.9054231738705003, + "y": 6.021515522090684, + "heading": -1.1899282513418807e-17, + "angularVelocity": 3.2864668502442287e-17, + "velocityX": 0.1700306011860797, + "velocityY": -1.5553959588946442, + "timestamp": 0.9558582652329308 + }, + { + "x": 1.9242856526601226, + "y": 5.9456034456185245, + "heading": -1.2144170175518223e-17, + "angularVelocity": -5.189037232604421e-18, + "velocityX": 0.39970623092267704, + "velocityY": -1.6086183744309348, + "timestamp": 1.0030491202415501 + }, + { + "x": 1.9540827644853827, + "y": 5.867637837934222, + "heading": -1.1171530883996655e-17, + "angularVelocity": 2.0610883683650603e-17, + "velocityX": 0.631417079001948, + "velocityY": -1.6521338227490547, + "timestamp": 1.0502399752501694 + }, + { + "x": 1.994926600446804, + "y": 5.788348204365135, + "heading": -1.0788163245821945e-17, + "angularVelocity": 8.123783832404324e-18, + "velocityX": 0.8655031987415301, + "velocityY": -1.6801906546208838, + "timestamp": 1.0974308302587887 + }, + { + "x": 2.0468962184582224, + "y": 5.709049736165374, + "heading": -1.1071817614552285e-17, + "angularVelocity": -6.010855361331949e-18, + "velocityX": 1.1012645988681842, + "velocityY": -1.6803778652732546, + "timestamp": 1.144621685267408 + }, + { + "x": 2.1096287050594387, + "y": 5.632569717255143, + "heading": -1.2529583423352228e-17, + "angularVelocity": -3.0890853936578006e-17, + "velocityX": 1.3293356644984888, + "velocityY": -1.620653384990821, + "timestamp": 1.1918125402760273 + }, + { + "x": 2.1805529929426006, + "y": 5.563618124657303, + "heading": -1.4564273177036572e-17, + "angularVelocity": -4.311618751793354e-17, + "velocityX": 1.5029244091916873, + "velocityY": -1.4611219183320618, + "timestamp": 1.2390033952846466 + }, + { + "x": 2.258771762653696, + "y": 5.503067309910026, + "heading": -1.2683515932063761e-17, + "angularVelocity": 3.985427355851309e-17, + "velocityX": 1.657498464411, + "velocityY": -1.28310484597711, + "timestamp": 1.286194250293266 + }, + { + "x": 2.3432954510907233, + "y": 5.451683284908294, + "heading": -8.227330488458654e-18, + "angularVelocity": 9.442900415248569e-17, + "velocityX": 1.7911031368585526, + "velocityY": -1.088855563062623, + "timestamp": 1.3333851053018853 + }, + { + "x": 2.421508421025739, + "y": 5.409462082859973, + "heading": -5.9044582907102845e-18, + "angularVelocity": 4.922210076409921e-17, + "velocityX": 1.65737556399112, + "velocityY": -0.8946903386379944, + "timestamp": 1.3805759603105046 + }, + { + "x": 2.490640531911526, + "y": 5.373669021135148, + "heading": -2.911052283946568e-18, + "angularVelocity": 6.343094747832186e-17, + "velocityX": 1.4649471994784071, + "velocityY": -0.7584745332178959, + "timestamp": 1.4277668153191239 + }, + { + "x": 2.550203208974518, + "y": 5.34355126090593, + "heading": -1.895452255610691e-18, + "angularVelocity": 2.1520132293222772e-17, + "velocityX": 1.262165668584904, + "velocityY": -0.6382117938684191, + "timestamp": 1.4749576703277432 + }, + { + "x": 2.600002875223201, + "y": 5.318768927935795, + "heading": 1.3941409252743104e-19, + "angularVelocity": 4.31190709100587e-17, + "velocityX": 1.0552821354820594, + "velocityY": -0.5251511752783364, + "timestamp": 1.5221485253363625 + }, + { + "x": 2.639936653486858, + "y": 5.299129303239457, + "heading": 1.8461191920786168e-18, + "angularVelocity": 3.616528875478909e-17, + "velocityX": 0.8462185789256751, + "velocityY": -0.41617437727683365, + "timestamp": 1.5693393803449818 + }, + { + "x": 2.669940905017083, + "y": 5.284508414647291, + "heading": 2.4777675775438873e-18, + "angularVelocity": 1.338439536164493e-17, + "velocityX": 0.6358064825220862, + "velocityY": -0.3098246172808112, + "timestamp": 1.616530235353601 + }, + { + "x": 2.6899724293774416, + "y": 5.2748198621430635, + "heading": 1.135223590946834e-18, + "angularVelocity": -2.8449673154135896e-17, + "velocityX": 0.4244789452682728, + "velocityY": -0.2053057208326553, + "timestamp": 1.6637210903622204 + }, + { + "x": 2.7, + "y": 5.27, + "heading": -9.164578473792561e-35, + "angularVelocity": -2.405623852622706e-17, + "velocityX": 0.21248969997959466, + "velocityY": -0.10213551210711598, + "timestamp": 1.7109119453708397 + }, + { + "x": 2.7, + "y": 5.27, + "heading": -1.7616390663298781e-35, + "angularVelocity": -1.0799207912382592e-34, + "velocityX": 2.69466690896049e-30, + "velocityY": -2.081869618544005e-30, + "timestamp": 1.758102800379459 } ], "constraints": [ @@ -3454,14 +3454,14 @@ "first" ], "type": "StopPoint", - "uuid": "a78be2a2-d0bd-487c-ba69-e496be5b7879" + "uuid": "2f34b2d5-7995-40e1-b5fa-9dac489b7735" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "5be57b63-c2c7-4a54-a5ce-cd76464f2a4f" + "uuid": "48cb16fd-245e-4c72-a2f4-0ddaf7494e87" } ], "usesControlIntervalGuessing": false, @@ -3606,14 +3606,14 @@ "first" ], "type": "StopPoint", - "uuid": "b7c7182a-9e67-4c59-a7c1-078c8b54aa32" + "uuid": "b0c308f0-970d-457d-baab-4504289cabae" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "36f08ccc-807a-4a33-b862-b48692585417" + "uuid": "42db2edf-e132-40be-933f-d4fa086169b9" }, { "scope": [ @@ -3621,7 +3621,7 @@ "last" ], "type": "MaxVelocity", - "uuid": "06a3c835-6227-48f9-99b5-6dd1b38a98e2", + "uuid": "e8c76491-5433-4e8d-867a-30ed8f7a0f1a", "velocity": 2 } ], @@ -3911,14 +3911,14 @@ "first" ], "type": "StopPoint", - "uuid": "44beb270-e5a3-4b24-b447-85823b6edec8" + "uuid": "7ce5ab16-600c-42d7-a26c-2e58013f5d46" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "a4f14c60-8ee7-4eaa-9078-389f48626365" + "uuid": "107edbe4-6860-4931-8afb-3bf58a8b4f77" }, { "scope": [ @@ -3926,7 +3926,7 @@ 1 ], "type": "MaxVelocity", - "uuid": "d8c170d1-c514-4114-8fd5-9cb91c32825f", + "uuid": "1fc426e1-bc03-4124-b5c1-5a22c30c062e", "velocity": 2.5 } ], @@ -4396,14 +4396,14 @@ "first" ], "type": "StopPoint", - "uuid": "960e0494-e84a-4278-81ef-00b1ad98467f" + "uuid": "17ff44e7-f4ea-4c88-8d3a-5a05b20f47e8" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "87efa241-9708-4981-b449-1938ee2d0179" + "uuid": "8d654cba-0c58-42cc-8f8e-4a4abbf7e55e" } ], "usesControlIntervalGuessing": true, @@ -4863,14 +4863,14 @@ "first" ], "type": "StopPoint", - "uuid": "84617f45-75f5-4861-9b7c-3154b05ad12f" + "uuid": "4d2bb53c-29be-4377-8355-dfe688eee709" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "e7097864-78fb-43fd-952f-092e63aad890" + "uuid": "d0fdee95-081b-4719-9b17-083f3ffdb4e5" } ], "usesControlIntervalGuessing": true, @@ -5474,14 +5474,14 @@ "first" ], "type": "StopPoint", - "uuid": "eabc20a4-7721-4b98-b969-92364e1280e8" + "uuid": "94fe74a9-58d7-47b4-a7b7-7ac42f105cb1" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "a07c39e1-ab75-4d9c-bd29-a729173b6737" + "uuid": "edd4bb7f-c9d7-417e-a01c-9e361225f2e5" }, { "scope": [ @@ -5489,7 +5489,7 @@ 1 ], "type": "MaxVelocity", - "uuid": "0cc2cf0c-efd3-4799-913f-83f10f3c3688", + "uuid": "5c37dad0-dc2a-416f-93bc-3952d7b8558b", "velocity": 2 }, { @@ -5498,7 +5498,7 @@ 1 ], "type": "MaxVelocity", - "uuid": "8bd9e104-2f44-4339-91f6-8e25a4ef4ad3", + "uuid": "c6640ac4-d745-4acb-8f3c-2470a3049906", "velocity": 2 } ], @@ -6013,14 +6013,14 @@ "first" ], "type": "StopPoint", - "uuid": "6c397019-87d5-484c-b1a1-5b23e475feee" + "uuid": "1904c8ef-1284-4529-b3e5-998f74402e4e" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "16544c24-9053-427d-83ea-e1340101d757" + "uuid": "96611d6d-eb53-40d6-a25d-b259987b2dc9" }, { "scope": [ @@ -6028,7 +6028,7 @@ 1 ], "type": "MaxVelocity", - "uuid": "c4aa52a5-f9cd-4256-b9af-1f9489fb982c", + "uuid": "451b6584-a0e0-4d74-b5d3-605f7abccc30", "velocity": 2 } ], @@ -6498,14 +6498,14 @@ "first" ], "type": "StopPoint", - "uuid": "01063a4f-7d56-4bc0-bbf0-31869fed2227" + "uuid": "18aa8c7b-ac34-417f-bc78-8177d27e2f68" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "caac5203-9568-4567-a8ae-2c53bc8d6ee3" + "uuid": "5bc17c24-3360-4e12-9d21-9b2b387d362f" } ], "usesControlIntervalGuessing": true, @@ -7001,14 +7001,14 @@ "first" ], "type": "StopPoint", - "uuid": "4a497b39-65f7-443e-89ce-c33472faba7c" + "uuid": "f5add8e5-f7be-4c53-bf20-076c2d5e2d35" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "b2eb186f-2d2f-4a16-bf05-474e20f28057" + "uuid": "77f7f669-75b6-4b73-b5d6-5f49d3ae32f4" } ], "usesControlIntervalGuessing": true, @@ -7504,14 +7504,14 @@ "first" ], "type": "StopPoint", - "uuid": "2d97d0c9-152a-4cf5-82ac-0f42cf996f0a" + "uuid": "07bd1325-f2ef-424b-adbf-93bd3a014fbf" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "641d9666-1209-4f6b-b7b5-bcf0bfc32c89" + "uuid": "694fbdac-9686-452e-ab8a-adf412df0003" } ], "usesControlIntervalGuessing": true, @@ -7674,14 +7674,14 @@ "first" ], "type": "StopPoint", - "uuid": "c6e250dd-7f41-4aaf-8e22-27f2621256db" + "uuid": "75bc80d5-2ac3-4817-8483-4dd6287edf98" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "4e4d0b67-d59a-4885-856f-8778c5698bf3" + "uuid": "25ab28e0-976c-459c-9221-807de20eb9ae" } ], "usesControlIntervalGuessing": true, @@ -8159,14 +8159,14 @@ "first" ], "type": "StopPoint", - "uuid": "621ae88a-b38a-4f43-9d28-4d8a5c4b32cd" + "uuid": "6b6cf8ac-70f0-46f9-9488-e81571a0ba29" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "5ebb9a55-65c0-4c1a-ba01-5d69db4e2480" + "uuid": "e9e5272a-bceb-4ee6-9c75-c5d204c25cd4" } ], "usesControlIntervalGuessing": true, @@ -8644,14 +8644,14 @@ "first" ], "type": "StopPoint", - "uuid": "31ae09ac-35fe-4698-83a5-9a5a6a20d8e3" + "uuid": "c441fbfb-39da-4773-928f-0959aa97436a" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "25cc0f0a-4dc3-4c07-a786-b53e56e30e28" + "uuid": "19f6fcd5-6dfd-4498-8d69-4d92ef0d9f7b" } ], "usesControlIntervalGuessing": true, @@ -9138,14 +9138,14 @@ "first" ], "type": "StopPoint", - "uuid": "0c4ba41c-28bb-42d0-be7d-a3f100176128" + "uuid": "36c35567-c90c-4358-8ea2-cb102a9f67db" }, { "scope": [ "last" ], "type": "StopPoint", - "uuid": "dc5d8b68-e54a-42ed-993b-00e7f345ff37" + "uuid": "64f87544-4eba-44fd-a51d-645e99b11c0c" } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/n1 to n2.traj b/src/main/deploy/choreo/n1 to n2.traj index ccd5b6d..fb70ad2 100644 --- a/src/main/deploy/choreo/n1 to n2.traj +++ b/src/main/deploy/choreo/n1 to n2.traj @@ -1,373 +1,373 @@ { "samples": [ { - "x": 2.5, - "y": 6.957, + "x": 2.91, + "y": 6.66, "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": -4.757446226639569e-33, + "velocityY": -6.423240989695239e-30, "timestamp": 0 }, { - "x": 2.4943446221143897, - "y": 6.954479923641855, - "heading": 3.6897126781530414e-20, - "angularVelocity": 1.9697832873556895e-18, - "velocityX": -0.3019169811583678, - "velocityY": -0.13453634075911872, - "timestamp": 0.018731566087842332 - }, - { - "x": 2.483082635763188, - "y": 6.949333058137429, - "heading": 5.2597079936043524e-20, - "angularVelocity": 8.38155582821411e-19, - "velocityX": -0.601230366878399, - "velocityY": -0.27476963112913483, - "timestamp": 0.037463132175684664 - }, - { - "x": 2.466275958073038, - "y": 6.941431111397211, - "heading": -1.0918552393928563e-19, - "angularVelocity": -8.636895506035071e-18, - "velocityX": -0.8972382560717649, - "velocityY": -0.4218518997912323, - "timestamp": 0.05619469826352699 - }, - { - "x": 2.4440053256580105, - "y": 6.930617246523635, - "heading": -2.9636578779285724e-19, - "angularVelocity": -9.992772710780623e-18, - "velocityX": -1.1889359549856489, - "velocityY": -0.5773070347064156, - "timestamp": 0.07492626435136933 - }, - { - "x": 2.416379494426775, - "y": 6.916695986231933, - "heading": -2.002041575820117e-19, - "angularVelocity": 5.1336640905466516e-18, - "velocityX": -1.4748276306252714, - "velocityY": -0.7431978845986646, - "timestamp": 0.09365783043921166 - }, - { - "x": 2.3835509550151985, - "y": 6.899418257829668, - "heading": -2.0906128139802138e-19, - "angularVelocity": -4.728480031302008e-19, - "velocityX": -1.7525784687528718, - "velocityY": -0.9223856842105105, - "timestamp": 0.112389396527054 - }, - { - "x": 2.345744441595137, - "y": 6.878458979207731, - "heading": -5.664854088249963e-19, - "angularVelocity": -1.908138066098496e-17, - "velocityX": -2.01833168901968, - "velocityY": -1.118928258515608, - "timestamp": 0.13112096261489634 - }, - { - "x": 2.30331220769535, - "y": 6.853384274840582, - "heading": -1.0549068383945228e-18, - "angularVelocity": -2.6074781886752536e-17, - "velocityX": -2.265279566097002, - "velocityY": -1.3386336331709632, - "timestamp": 0.14985252870273866 - }, - { - "x": 2.2568479900211824, - "y": 6.823610942973657, - "heading": -1.8345727365867855e-18, - "angularVelocity": -4.162310478476867e-17, - "velocityX": -2.4805303227861466, - "velocityY": -1.589473711237126, - "timestamp": 0.16858409479058098 - }, - { - "x": 2.207423790946718, - "y": 6.7883995536652755, - "heading": -2.6797153220676564e-18, - "angularVelocity": -4.5118628896476544e-17, - "velocityX": -2.638551354579043, - "velocityY": -1.8797888624611248, - "timestamp": 0.1873156608784233 - }, - { - "x": 2.156971931826493, - "y": 6.747082606718596, - "heading": -3.7107530407855426e-18, - "angularVelocity": -5.5042786812058804e-17, - "velocityX": -2.6934138279537794, - "velocityY": -2.20573905849193, - "timestamp": 0.20604722696626562 - }, - { - "x": 2.1082465925805094, - "y": 6.699819818236992, - "heading": -3.742581424210628e-18, - "angularVelocity": -1.6991823567672568e-18, - "velocityX": -2.6012421501482437, - "velocityY": -2.5231626795090287, - "timestamp": 0.22477879305410794 - }, - { - "x": 2.063564819070666, - "y": 6.647868372787038, - "heading": -4.086119143230017e-18, - "angularVelocity": -1.8340038541638217e-17, - "velocityX": -2.3853730809429283, - "velocityY": -2.7734704725873653, - "timestamp": 0.24351035914195027 - }, - { - "x": 2.024129444946568, - "y": 6.59262918323462, - "heading": -4.07128100349463e-18, - "angularVelocity": 7.921454718696707e-19, - "velocityX": -2.1052897520241163, - "velocityY": -2.9489893847297766, - "timestamp": 0.2622419252297926 - }, - { - "x": 1.9904640390685304, - "y": 6.535144672578362, - "heading": -4.10027571754238e-18, - "angularVelocity": -1.5479071290730936e-18, - "velocityX": -1.7972552706037406, - "velocityY": -3.0688576911656473, - "timestamp": 0.28097349131763494 - }, - { - "x": 1.9627985342838827, - "y": 6.476132022270179, - "heading": -4.1608406747535095e-18, - "angularVelocity": -3.23330929267101e-18, - "velocityX": -1.476945635773801, - "velocityY": -3.1504386782949707, - "timestamp": 0.29970505740547726 - }, - { - "x": 1.9412387198217298, - "y": 6.416092441347258, - "heading": -4.249951515089872e-18, - "angularVelocity": -4.757254479220396e-18, - "velocityX": -1.150988356288367, - "velocityY": -3.2052622103970294, - "timestamp": 0.3184366234933196 - }, - { - "x": 1.9258346453328115, - "y": 6.355388843391961, - "heading": -4.362967285061641e-18, - "angularVelocity": -6.033438571589532e-18, - "velocityX": -0.8223591351988351, - "velocityY": -3.240711303615491, - "timestamp": 0.3371681895811619 - }, - { - "x": 1.916609614204421, - "y": 6.294293472758921, - "heading": -3.873034512043015e-18, - "angularVelocity": 2.615545924478944e-17, - "velocityX": -0.4924858436891372, - "velocityY": -3.261626408946662, - "timestamp": 0.3558997556690042 + "x": 2.9017252775702578, + "y": 6.660334904568681, + "heading": 3.557918002387283e-19, + "angularVelocity": 8.739618338819358e-18, + "velocityX": -0.20323880003406047, + "velocityY": 0.00822572639175432, + "timestamp": 0.04071428501035364 + }, + { + "x": 2.8851720980286473, + "y": 6.660894166924014, + "heading": 2.4593746412602857e-20, + "angularVelocity": -8.132857702537941e-18, + "velocityX": -0.40656932910438337, + "velocityY": 0.01373626861677896, + "timestamp": 0.08142857002070727 + }, + { + "x": 2.860337882655417, + "y": 6.6615407759417495, + "heading": -4.101778730884207e-19, + "angularVelocity": -1.067574406396209e-17, + "velocityX": -0.6099631951521888, + "velocityY": 0.015881625276030417, + "timestamp": 0.12214285503106091 + }, + { + "x": 2.827222625614269, + "y": 6.6621006275797185, + "heading": -1.2458891712985336e-18, + "angularVelocity": -2.0522253121744445e-17, + "velocityX": -0.8133572045477079, + "velocityY": 0.013750742229429767, + "timestamp": 0.16285714004141455 + }, + { + "x": 2.7858318663934507, + "y": 6.662345465133598, + "heading": -2.427446145131975e-18, + "angularVelocity": -2.9015444749893876e-17, + "velocityX": -1.0166151563348516, + "velocityY": 0.006013554067767749, + "timestamp": 0.20357142505176817 + }, + { + "x": 2.7361833409133944, + "y": 6.661963839528372, + "heading": -3.213541175655737e-18, + "angularVelocity": -1.930093113455471e-17, + "velocityX": -1.219437489016265, + "velocityY": -0.009373260629573183, + "timestamp": 0.2442857100621218 + }, + { + "x": 2.678323290528662, + "y": 6.66050796841237, + "heading": -4.672307495132643e-18, + "angularVelocity": -3.582098491837782e-17, + "velocityX": -1.4211240691100409, + "velocityY": -0.03575823854035497, + "timestamp": 0.2849999950724754 + }, + { + "x": 2.6123718841616017, + "y": 6.657287704995394, + "heading": -6.297598317790764e-18, + "angularVelocity": -3.990899540694541e-17, + "velocityX": -1.6198591317620556, + "velocityY": -0.07909419053847176, + "timestamp": 0.32571428008282904 + }, + { + "x": 2.5386735260706663, + "y": 6.651140357174973, + "heading": -8.461722991277948e-18, + "angularVelocity": -5.314082366829448e-17, + "velocityX": -1.8101351423058039, + "velocityY": -0.1509874929170222, + "timestamp": 0.36642856509318267 + }, + { + "x": 2.4583977191648256, + "y": 6.639961092612853, + "heading": -1.212386291709007e-17, + "angularVelocity": -8.99629845611936e-17, + "velocityX": -1.9716865194971132, + "velocityY": -0.27457843257359255, + "timestamp": 0.4071428501035363 + }, + { + "x": 2.3755519490058536, + "y": 6.620909202377741, + "heading": -1.484617881375725e-17, + "angularVelocity": -6.687476222529868e-17, + "velocityX": -2.0348084250243854, + "velocityY": -0.4679411717542808, + "timestamp": 0.4478571351138899 + }, + { + "x": 2.2959295648381173, + "y": 6.5942288936053375, + "heading": -1.5912583784909486e-17, + "angularVelocity": -2.620018728179701e-17, + "velocityX": -1.9556375396786705, + "velocityY": -0.6553058408302904, + "timestamp": 0.48857142012424354 + }, + { + "x": 2.2222017269017074, + "y": 6.561731593537055, + "heading": -1.7998920552716176e-17, + "angularVelocity": -5.124912608019232e-17, + "velocityX": -1.8108592086924549, + "velocityY": -0.7981793137317525, + "timestamp": 0.5292857051345972 + }, + { + "x": 2.1553520416825007, + "y": 6.5246219052837455, + "heading": -1.798702462065007e-17, + "angularVelocity": 2.877248094927204e-19, + "velocityX": -1.6419221214911441, + "velocityY": -0.9114660430449456, + "timestamp": 0.5699999901449508 + }, + { + "x": 2.0958188014057453, + "y": 6.483632427032881, + "heading": -1.839733676343214e-17, + "angularVelocity": -1.0081331759415523e-17, + "velocityX": -1.462219961903324, + "velocityY": -1.006759132352994, + "timestamp": 0.6107142751553045 + }, + { + "x": 2.043834658344306, + "y": 6.43923791049145, + "heading": -1.8859562986854868e-17, + "angularVelocity": -1.1355707850407907e-17, + "velocityX": -1.2768035358651197, + "velocityY": -1.0903916532167117, + "timestamp": 0.6514285601656582 + }, + { + "x": 1.9995393069281948, + "y": 6.391766764373333, + "heading": -1.7387549289815112e-17, + "angularVelocity": 3.6152451058564913e-17, + "velocityX": -1.087956018503923, + "velocityY": -1.1659579950029253, + "timestamp": 0.6921428451760119 + }, + { + "x": 1.963024249856703, + "y": 6.341458331680076, + "heading": -1.5310581750209048e-17, + "angularVelocity": 5.1011383274234203e-17, + "velocityX": -0.8968610663843175, + "velocityY": -1.2356457366492464, + "timestamp": 0.7328571301863656 + }, + { + "x": 1.9343533533387092, + "y": 6.288494277220297, + "heading": -1.3195006752942667e-17, + "angularVelocity": 5.1959977949498825e-17, + "velocityX": -0.7041974705106752, + "velocityY": -1.3008715355503284, + "timestamp": 0.7735714151967192 }, { "x": 1.9135733842849731, "y": 6.2330169677734375, - "heading": -3.400719263853574e-18, - "angularVelocity": 2.5214933197049766e-17, - "velocityX": -0.162091621448467, - "velocityY": -3.2712964147321877, - "timestamp": 0.37463132175684655 - }, - { - "x": 1.917205979581219, - "y": 6.168694726765038, - "heading": -2.9220139710460773e-18, - "angularVelocity": 2.4351738826537176e-17, - "velocityX": 0.18479014101467978, - "velocityY": -3.272072724039553, - "timestamp": 0.39428927145210496 - }, - { - "x": 1.927655154396695, - "y": 6.104554186928345, - "heading": -2.4618364262321162e-18, - "angularVelocity": 2.3409232177474112e-17, - "velocityX": 0.5315495754878562, - "velocityY": -3.2628295845197286, - "timestamp": 0.4139472211473634 - }, - { - "x": 1.9449095427250505, - "y": 6.040847042418775, - "heading": -1.9605813341962127e-18, - "angularVelocity": 2.5498846810319266e-17, - "velocityX": 0.8777308211607325, - "velocityY": -3.2407827620462775, - "timestamp": 0.4336051708426218 - }, - { - "x": 1.9689399188226782, - "y": 5.977904581007848, - "heading": -1.4825326953016808e-18, - "angularVelocity": 2.4318334570481832e-17, - "velocityX": 1.2224253531091398, - "velocityY": -3.2018833289672246, - "timestamp": 0.4532631205378802 - }, - { - "x": 1.999679925253729, - "y": 5.916178505713037, - "heading": -1.0314098682566954e-18, - "angularVelocity": 2.2948618572175324e-17, - "velocityX": 1.5637442819616925, - "velocityY": -3.1400057611143075, - "timestamp": 0.4729210702331386 - }, - { - "x": 2.036981086275461, - "y": 5.856309837875825, - "heading": -6.124142555239329e-19, - "angularVelocity": 2.1314307251887193e-17, - "velocityX": 1.8975102490332074, - "velocityY": -3.045519434391944, - "timestamp": 0.49257901992839703 - }, - { - "x": 2.080499423518141, - "y": 5.799242134164281, - "heading": -1.1443208997745532e-19, - "angularVelocity": 2.5332353415157017e-17, - "velocityX": 2.213778034704021, - "velocityY": -2.903034375212876, - "timestamp": 0.5122369696236555 - }, - { - "x": 2.1294018036645377, - "y": 5.746359011482999, - "heading": 2.485888925042906e-20, - "angularVelocity": 7.085733120179258e-18, - "velocityX": 2.487664324331455, - "velocityY": -2.6901647171289556, - "timestamp": 0.531894919318914 - }, - { - "x": 2.181766698039788, - "y": 5.699350374820504, - "heading": 1.0628834205133463e-19, - "angularVelocity": 4.142317505357256e-18, - "velocityX": 2.663802440591291, - "velocityY": -2.3913295837679795, - "timestamp": 0.5515528690141724 - }, - { - "x": 2.2344628035738343, - "y": 5.659152676616759, - "heading": 1.821790588441185e-19, - "angularVelocity": 3.860562344858861e-18, - "velocityX": 2.6806511539072937, - "velocityY": -2.0448571100698536, - "timestamp": 0.5712108187094309 - }, - { - "x": 2.2847196675943717, - "y": 5.62532276648187, - "heading": -2.177096233228433e-22, - "angularVelocity": -9.278521924332595e-18, - "velocityX": 2.5565669258304347, - "velocityY": -1.7209276989374398, - "timestamp": 0.5908687684046894 - }, - { - "x": 2.3309357944226123, - "y": 5.596985680144109, - "heading": -2.179376064772025e-19, - "angularVelocity": -1.1075409221490475e-17, - "velocityX": 2.3510146045081415, - "velocityY": -1.441507724714413, - "timestamp": 0.6105267180999479 - }, - { - "x": 2.3722616758048822, - "y": 5.573400877946735, - "heading": -2.1185463417811083e-19, - "angularVelocity": 3.0944314555748793e-19, - "velocityX": 2.10224779404314, - "velocityY": -1.199759006559285, - "timestamp": 0.6301846677952063 - }, - { - "x": 2.4082188315941186, - "y": 5.554020301597982, - "heading": -1.4180298734171178e-19, - "angularVelocity": 3.5635284124926384e-18, - "velocityX": 1.8291406960873096, - "velocityY": -0.9858900164664522, - "timestamp": 0.6498426174904648 - }, - { - "x": 2.4385147898608133, - "y": 5.538440986818481, - "heading": -8.901703862511827e-20, - "angularVelocity": 2.6852225163959986e-18, - "velocityX": 1.541155549604471, - "velocityY": -0.7925198212945216, - "timestamp": 0.6695005671857233 - }, - { - "x": 2.4629575237386443, - "y": 5.526360032228848, - "heading": -5.049125518872738e-20, - "angularVelocity": 1.9598080581490267e-18, - "velocityX": 1.243401995464726, - "velocityY": -0.6145582208173452, - "timestamp": 0.6891585168809817 - }, - { - "x": 2.481413486551153, - "y": 5.517543636073691, - "heading": -2.3943762824284567e-20, - "angularVelocity": 1.3504725300916025e-18, - "velocityX": 0.9388549212209834, - "velocityY": -0.4484901168144495, - "timestamp": 0.7088164665762402 - }, - { - "x": 2.4937854535518733, - "y": 5.511806813298085, - "heading": -7.590378603659992e-21, - "angularVelocity": 8.318983835108739e-19, - "velocityX": 0.6293620236348159, - "velocityY": -0.29183220348720296, - "timestamp": 0.7284744162714987 - }, - { - "x": 2.5, - "y": 5.509, - "heading": -1.243215768494891e-34, - "angularVelocity": 3.8612244066878563e-19, - "velocityX": 0.316134009114162, - "velocityY": -0.14278260660937586, - "timestamp": 0.7481323659667571 - }, - { - "x": 2.5, - "y": 5.509, - "heading": -1.2427239750636687e-34, - "angularVelocity": 2.5055071878670228e-36, - "velocityX": -3.816803690129975e-32, - "velocityY": -1.5853074900155989e-31, - "timestamp": 0.7677903156620156 + "heading": -1.3350384577684241e-17, + "angularVelocity": -3.817492839589226e-18, + "velocityX": -0.5103852136511203, + "velocityY": -1.3626006064747218, + "timestamp": 0.8142857002070729 + }, + { + "x": 1.9001367061426089, + "y": 6.165492022212096, + "heading": -1.383548050805982e-17, + "angularVelocity": -1.027860766237449e-17, + "velocityX": -0.28473055086618276, + "velocityY": -1.430890488180813, + "timestamp": 0.8614765552156922 + }, + { + "x": 1.8973992844227976, + "y": 6.094915987268133, + "heading": -1.3450172270417929e-17, + "angularVelocity": 8.165532188269098e-18, + "velocityX": -0.05800746181448894, + "velocityY": -1.4955447391395353, + "timestamp": 0.9086674102243115 + }, + { + "x": 1.9054231738705003, + "y": 6.021515522090684, + "heading": -1.1899282513418807e-17, + "angularVelocity": 3.2864668502442287e-17, + "velocityX": 0.1700306011860797, + "velocityY": -1.5553959588946442, + "timestamp": 0.9558582652329308 + }, + { + "x": 1.9242856526601226, + "y": 5.9456034456185245, + "heading": -1.2144170175518223e-17, + "angularVelocity": -5.189037232604421e-18, + "velocityX": 0.39970623092267704, + "velocityY": -1.6086183744309348, + "timestamp": 1.0030491202415501 + }, + { + "x": 1.9540827644853827, + "y": 5.867637837934222, + "heading": -1.1171530883996655e-17, + "angularVelocity": 2.0610883683650603e-17, + "velocityX": 0.631417079001948, + "velocityY": -1.6521338227490547, + "timestamp": 1.0502399752501694 + }, + { + "x": 1.994926600446804, + "y": 5.788348204365135, + "heading": -1.0788163245821945e-17, + "angularVelocity": 8.123783832404324e-18, + "velocityX": 0.8655031987415301, + "velocityY": -1.6801906546208838, + "timestamp": 1.0974308302587887 + }, + { + "x": 2.0468962184582224, + "y": 5.709049736165374, + "heading": -1.1071817614552285e-17, + "angularVelocity": -6.010855361331949e-18, + "velocityX": 1.1012645988681842, + "velocityY": -1.6803778652732546, + "timestamp": 1.144621685267408 + }, + { + "x": 2.1096287050594387, + "y": 5.632569717255143, + "heading": -1.2529583423352228e-17, + "angularVelocity": -3.0890853936578006e-17, + "velocityX": 1.3293356644984888, + "velocityY": -1.620653384990821, + "timestamp": 1.1918125402760273 + }, + { + "x": 2.1805529929426006, + "y": 5.563618124657303, + "heading": -1.4564273177036572e-17, + "angularVelocity": -4.311618751793354e-17, + "velocityX": 1.5029244091916873, + "velocityY": -1.4611219183320618, + "timestamp": 1.2390033952846466 + }, + { + "x": 2.258771762653696, + "y": 5.503067309910026, + "heading": -1.2683515932063761e-17, + "angularVelocity": 3.985427355851309e-17, + "velocityX": 1.657498464411, + "velocityY": -1.28310484597711, + "timestamp": 1.286194250293266 + }, + { + "x": 2.3432954510907233, + "y": 5.451683284908294, + "heading": -8.227330488458654e-18, + "angularVelocity": 9.442900415248569e-17, + "velocityX": 1.7911031368585526, + "velocityY": -1.088855563062623, + "timestamp": 1.3333851053018853 + }, + { + "x": 2.421508421025739, + "y": 5.409462082859973, + "heading": -5.9044582907102845e-18, + "angularVelocity": 4.922210076409921e-17, + "velocityX": 1.65737556399112, + "velocityY": -0.8946903386379944, + "timestamp": 1.3805759603105046 + }, + { + "x": 2.490640531911526, + "y": 5.373669021135148, + "heading": -2.911052283946568e-18, + "angularVelocity": 6.343094747832186e-17, + "velocityX": 1.4649471994784071, + "velocityY": -0.7584745332178959, + "timestamp": 1.4277668153191239 + }, + { + "x": 2.550203208974518, + "y": 5.34355126090593, + "heading": -1.895452255610691e-18, + "angularVelocity": 2.1520132293222772e-17, + "velocityX": 1.262165668584904, + "velocityY": -0.6382117938684191, + "timestamp": 1.4749576703277432 + }, + { + "x": 2.600002875223201, + "y": 5.318768927935795, + "heading": 1.3941409252743104e-19, + "angularVelocity": 4.31190709100587e-17, + "velocityX": 1.0552821354820594, + "velocityY": -0.5251511752783364, + "timestamp": 1.5221485253363625 + }, + { + "x": 2.639936653486858, + "y": 5.299129303239457, + "heading": 1.8461191920786168e-18, + "angularVelocity": 3.616528875478909e-17, + "velocityX": 0.8462185789256751, + "velocityY": -0.41617437727683365, + "timestamp": 1.5693393803449818 + }, + { + "x": 2.669940905017083, + "y": 5.284508414647291, + "heading": 2.4777675775438873e-18, + "angularVelocity": 1.338439536164493e-17, + "velocityX": 0.6358064825220862, + "velocityY": -0.3098246172808112, + "timestamp": 1.616530235353601 + }, + { + "x": 2.6899724293774416, + "y": 5.2748198621430635, + "heading": 1.135223590946834e-18, + "angularVelocity": -2.8449673154135896e-17, + "velocityX": 0.4244789452682728, + "velocityY": -0.2053057208326553, + "timestamp": 1.6637210903622204 + }, + { + "x": 2.7, + "y": 5.27, + "heading": -9.164578473792561e-35, + "angularVelocity": -2.405623852622706e-17, + "velocityX": 0.21248969997959466, + "velocityY": -0.10213551210711598, + "timestamp": 1.7109119453708397 + }, + { + "x": 2.7, + "y": 5.27, + "heading": -1.7616390663298781e-35, + "angularVelocity": -1.0799207912382592e-34, + "velocityX": 2.69466690896049e-30, + "velocityY": -2.081869618544005e-30, + "timestamp": 1.758102800379459 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/n2 to n3.traj b/src/main/deploy/choreo/n2 to n3.traj index a35b37d..61dbbd2 100644 --- a/src/main/deploy/choreo/n2 to n3.traj +++ b/src/main/deploy/choreo/n2 to n3.traj @@ -1,373 +1,373 @@ { "samples": [ { - "x": 2.5, - "y": 5.509, + "x": 2.7, + "y": 5.27, "heading": 0, "angularVelocity": 0, "velocityX": 0, - "velocityY": 1.5949944891681758e-33, + "velocityY": -5.00840856616175e-28, "timestamp": 0 }, { - "x": 2.4943444648641293, - "y": 5.5064791255479575, - "heading": -1.6665950731162323e-20, - "angularVelocity": -8.896918377207487e-19, - "velocityX": -0.3019139532075592, - "velocityY": -0.13457385606700714, - "timestamp": 0.01873227479480579 - }, - { - "x": 2.483082181980465, - "y": 5.501330655009401, - "heading": -6.635323653535645e-20, - "angularVelocity": -2.6524961112440185e-18, - "velocityX": -0.6012234502766525, - "velocityY": -0.2748449184605893, - "timestamp": 0.03746454958961158 - }, - { - "x": 2.46627509012907, - "y": 5.49342628716381, - "heading": 3.593397333313667e-19, - "angularVelocity": 2.2725103255105993e-17, - "velocityX": -0.8972264199355992, - "velocityY": -0.42196518747333667, - "timestamp": 0.05619682438441737 - }, - { - "x": 2.4440039523453803, - "y": 5.482609176230366, - "heading": 1.0617530792760623e-18, - "angularVelocity": 3.749748835023413e-17, - "velocityX": -1.1889179519117046, - "velocityY": -0.5774584801866853, - "timestamp": 0.07492909917922316 - }, - { - "x": 2.416377557137626, - "y": 5.468683837447711, - "heading": 1.4162827940624112e-18, - "angularVelocity": 1.8926140859171294e-17, - "velocityX": -1.474801939987141, - "velocityY": -0.7433874921864966, - "timestamp": 0.09366137397402896 - }, - { - "x": 2.3835484355934935, - "y": 5.451401194530706, - "heading": 2.0446425305173568e-18, - "angularVelocity": 3.3544221150580245e-17, - "velocityX": -1.7525432390748785, - "velocityY": -0.9226131426280576, - "timestamp": 0.11239364876883476 - }, - { - "x": 2.3457413715260427, - "y": 5.430436170960037, - "heading": 2.625153686840878e-18, - "angularVelocity": 3.098988185000503e-17, - "velocityX": -2.0182847241773043, - "velocityY": -1.1191926127672032, - "timestamp": 0.13112592356364056 - }, - { - "x": 2.303308675591435, - "y": 5.405354917239582, - "heading": 3.474873201601955e-18, - "angularVelocity": 4.536124688156702e-17, - "velocityX": -2.2652185278838264, - "velocityY": -1.3389326173781921, - "timestamp": 0.14985819835844635 - }, - { - "x": 2.256844133996144, - "y": 5.375574300168143, - "heading": 4.0955382647030466e-18, - "angularVelocity": 3.31334537599372e-17, - "velocityX": -2.4804537678559324, - "velocityY": -1.5898024878269215, - "timestamp": 0.16859047315325215 - }, - { - "x": 2.2074197303799292, - "y": 5.3403550272659315, - "heading": 4.355462667028749e-18, - "angularVelocity": 1.3875749071945138e-17, - "velocityX": -2.6384624482403214, - "velocityY": -1.8801385997164786, - "timestamp": 0.18732274794805795 - }, - { - "x": 2.156967572169077, - "y": 5.2990297375325435, - "heading": 4.29698949681871e-18, - "angularVelocity": -3.1215197468412938e-18, - "velocityX": -2.6933278933555838, - "velocityY": -2.2061009773807925, - "timestamp": 0.20605502274286375 - }, - { - "x": 2.108241591757582, - "y": 5.251758019086516, - "heading": 4.0630092494696724e-18, - "angularVelocity": -1.2490750274876772e-17, - "velocityX": -2.6011779639814714, - "velocityY": -2.5235439349379956, - "timestamp": 0.22478729753766954 - }, - { - "x": 2.0635590541012263, - "y": 5.199796918965488, - "heading": 3.723409843166543e-18, - "angularVelocity": -1.8129101367482953e-17, - "velocityX": -2.3853236270452767, - "velocityY": -2.773880945598276, - "timestamp": 0.24351957233247534 - }, - { - "x": 2.024123123965078, - "y": 5.144547524590311, - "heading": 3.368607724278227e-18, - "angularVelocity": -1.894068001303013e-17, - "velocityX": -2.105239783642413, - "velocityY": -2.949422586438396, - "timestamp": 0.2622518471272811 - }, - { - "x": 1.9904575467629446, - "y": 5.087052505755269, - "heading": 3.0083094680873597e-18, - "angularVelocity": -1.9234087711961826e-17, - "velocityX": -1.7971964201308785, - "velocityY": -3.0693025521374286, - "timestamp": 0.2809841219220869 - }, - { - "x": 1.9627923272283192, - "y": 5.028029241661778, - "heading": 2.6490143792152282e-18, - "angularVelocity": -1.918053490889217e-17, - "velocityX": -1.4768745300648793, - "velocityY": -3.150886090452353, - "timestamp": 0.29971639671689265 - }, - { - "x": 1.941233279386563, - "y": 4.967979084271092, - "heading": 2.295124883149197e-18, - "angularVelocity": -1.889196390427566e-17, - "velocityX": -1.1509038852950093, - "velocityY": -3.205705556237633, - "timestamp": 0.3184486715116984 - }, - { - "x": 1.9258304578861423, - "y": 4.907265050190802, - "heading": 1.8766425205004917e-18, - "angularVelocity": -2.2340176069469045e-17, - "velocityX": -0.822261133212249, - "velocityY": -3.241145816263607, - "timestamp": 0.3371809463065042 - }, - { - "x": 1.9166071629104302, - "y": 4.846159459726292, - "heading": 1.4687210226502946e-18, - "angularVelocity": -2.1776397057375608e-17, - "velocityX": -0.49237452881426474, - "velocityY": -3.262048583733586, - "timestamp": 0.35591322110130996 + "x": 2.6930041825612396, + "y": 5.26928432609697, + "heading": -1.1816834704103417e-20, + "angularVelocity": -3.1498607863339557e-19, + "velocityX": -0.18646463918650766, + "velocityY": -0.01907538000761375, + "timestamp": 0.03751819899629495 + }, + { + "x": 2.679023019715013, + "y": 5.267757112086426, + "heading": -1.1982484405963989e-18, + "angularVelocity": -3.162288568366952e-17, + "velocityX": -0.37265015966331894, + "velocityY": -0.040705952083294986, + "timestamp": 0.0750363979925899 + }, + { + "x": 2.658071304417103, + "y": 5.265299821378877, + "heading": -2.3024750226762525e-18, + "angularVelocity": -2.9431811326509344e-17, + "velocityX": -0.5584413926695573, + "velocityY": -0.06549596658054355, + "timestamp": 0.11255459698888486 + }, + { + "x": 2.6301707261184073, + "y": 5.26176227954264, + "heading": -3.3161342568916012e-18, + "angularVelocity": -2.7017849555311248e-17, + "velocityX": -0.7436545208766203, + "velocityY": -0.0942886900483679, + "timestamp": 0.1500727959851798 + }, + { + "x": 2.595354617234827, + "y": 5.25694838737037, + "heading": -4.228322057875787e-18, + "angularVelocity": -2.43132528127072e-17, + "velocityX": -0.927979215822151, + "velocityY": -0.1283081891234354, + "timestamp": 0.18759099498147475 + }, + { + "x": 2.5536774231155643, + "y": 5.250592050828935, + "heading": -4.902812238816008e-18, + "angularVelocity": -1.79777158616401e-17, + "velocityX": -1.1108527390521832, + "velocityY": -0.16942008709418127, + "timestamp": 0.22510919397776968 + }, + { + "x": 2.505235624376603, + "y": 5.24231380432782, + "heading": -5.418282495230493e-18, + "angularVelocity": -1.3739235384430708e-17, + "velocityX": -1.2911546938550877, + "velocityY": -0.22064615900354675, + "timestamp": 0.2626273929740646 + }, + { + "x": 2.4502205541283706, + "y": 5.2315364184801245, + "heading": -5.7648703440253685e-18, + "angularVelocity": -9.237882353094148e-18, + "velocityX": -1.4663569073180298, + "velocityY": -0.28725754796798325, + "timestamp": 0.30014559197035956 + }, + { + "x": 2.3890767277206044, + "y": 5.217310567080376, + "heading": -5.890943886177935e-18, + "angularVelocity": -3.360353857609014e-18, + "velocityX": -1.6297111280302612, + "velocityY": -0.37917202265783617, + "timestamp": 0.3376637909666545 + }, + { + "x": 2.3230664799507315, + "y": 5.198008155784784, + "heading": -6.1785882537098945e-18, + "angularVelocity": -7.666822033208596e-18, + "velocityX": -1.7594194160703869, + "velocityY": -0.5144812867392538, + "timestamp": 0.37518198996294944 + }, + { + "x": 2.255862321531191, + "y": 5.17177551793778, + "heading": -6.695668627212835e-18, + "angularVelocity": -1.3782112845000978e-17, + "velocityX": -1.7912415898848488, + "velocityY": -0.6991976840308759, + "timestamp": 0.4127001889592444 + }, + { + "x": 2.192202860236617, + "y": 5.139469280223623, + "heading": -6.883874082813852e-18, + "angularVelocity": -5.01635811715478e-18, + "velocityX": -1.696762184690978, + "velocityY": -0.8610817837371232, + "timestamp": 0.4502183879555393 + }, + { + "x": 2.134134507416613, + "y": 5.102897710542656, + "heading": -6.85446046853456e-18, + "angularVelocity": 7.840090786572423e-19, + "velocityX": -1.5477382809785758, + "velocityY": -0.974768796461879, + "timestamp": 0.48773658695183425 + }, + { + "x": 2.0823628902154088, + "y": 5.063194893980567, + "heading": -6.674766174603623e-18, + "angularVelocity": 4.7895544482881795e-18, + "velocityX": -1.3799067808711225, + "velocityY": -1.058228209887372, + "timestamp": 0.5252547859481292 + }, + { + "x": 2.0371815070623285, + "y": 5.0210380148882745, + "heading": -6.384013416846825e-18, + "angularVelocity": 7.749679994101778e-18, + "velocityX": -1.2042524524521976, + "velocityY": -1.1236381334912477, + "timestamp": 0.5627729849444242 + }, + { + "x": 1.9987362683691992, + "y": 4.976861735113259, + "heading": -6.007530542513076e-18, + "angularVelocity": 1.0034709668421525e-17, + "velocityX": -1.0247090671030967, + "velocityY": -1.1774626969472783, + "timestamp": 0.6002911839407191 + }, + { + "x": 1.9671096330518483, + "y": 4.930964909309413, + "heading": -5.562569285242097e-18, + "angularVelocity": 1.1859879305883021e-17, + "velocityX": -0.8429678439572205, + "velocityY": -1.2233216687202904, + "timestamp": 0.6378093829370141 + }, + { + "x": 1.9423526866284888, + "y": 4.883564417353199, + "heading": -5.061777213472376e-18, + "angularVelocity": 1.3347986496029381e-17, + "velocityX": -0.6598650011288002, + "velocityY": -1.2633999825124016, + "timestamp": 0.675327581933309 + }, + { + "x": 1.9244993041283396, + "y": 4.8348243554396655, + "heading": -4.514546757319931e-18, + "angularVelocity": 1.4585743490895e-17, + "velocityX": -0.47585926236927334, + "velocityY": -1.2991045203921598, + "timestamp": 0.7128457809296039 }, { "x": 1.913573145866394, "y": 4.784873008728027, - "heading": 1.0730296908708793e-18, - "angularVelocity": -2.1123504400879342e-17, - "velocityX": -0.16196735726282194, - "velocityY": -3.2717036061874047, - "timestamp": 0.37464549589611573 - }, - { - "x": 1.9172081226819078, - "y": 4.720543496953144, - "heading": 6.726891716838813e-19, - "angularVelocity": -2.0365443785309124e-17, - "velocityX": 0.18491238625516085, - "velocityY": -3.272462008053034, - "timestamp": 0.3943033289333137 - }, - { - "x": 1.9276595873253093, - "y": 4.656396088408969, - "heading": 3.1960685238797674e-19, - "angularVelocity": -1.7961405046966803e-17, - "velocityX": 0.5316692141816879, - "velocityY": -3.26319836081565, - "timestamp": 0.4139611619705117 - }, - { - "x": 1.9449161546870903, - "y": 4.592682541241059, - "heading": -6.838836479966531e-20, - "angularVelocity": -1.9737434946765602e-17, - "velocityX": 0.8778468780931784, - "velocityY": -3.2411276994439615, - "timestamp": 0.4336189950077097 - }, - { - "x": 1.968948566315896, - "y": 4.529734226845009, - "heading": -4.2094144486999917e-19, - "angularVelocity": -1.793448263863688e-17, - "velocityX": 1.222536155604247, - "velocityY": -3.2022000734735956, - "timestamp": 0.45327682804490765 - }, - { - "x": 1.9996904062675076, - "y": 4.46800295896022, - "heading": -7.262291331492882e-19, - "angularVelocity": -1.5530077760537348e-17, - "velocityX": 1.5638468336484368, - "velocityY": -3.140288543908866, - "timestamp": 0.47293466108210563 - }, - { - "x": 2.0369930888071974, - "y": 4.408129903333294, - "heading": -9.67326202918917e-19, - "angularVelocity": -1.2264681442804184e-17, - "velocityX": 1.8975989097629915, - "velocityY": -3.0457607160274165, - "timestamp": 0.4925924941193036 - }, - { - "x": 2.080512421569209, - "y": 4.351058782117612, - "heading": -1.6795534015194326e-18, - "angularVelocity": -3.623121376951248e-17, - "velocityX": 2.2138418145917167, - "velocityY": -2.9032254525554837, - "timestamp": 0.5122503271565015 - }, - { - "x": 2.1294148840951923, - "y": 4.298173284699749, - "heading": -1.923535200935003e-18, - "angularVelocity": -1.2411428555077921e-17, - "velocityX": 2.4876832778794475, - "velocityY": -2.690301485305471, - "timestamp": 0.5319081601936995 - }, - { - "x": 2.1817785731576067, - "y": 4.25116293816539, - "heading": -2.1365644616328657e-18, - "angularVelocity": -1.0836863419398235e-17, - "velocityX": 2.663756934110092, - "velocityY": -2.391430756655736, - "timestamp": 0.5515659932308974 - }, - { - "x": 2.2344726220787043, - "y": 4.210963490410107, - "heading": -2.140655615689113e-18, - "angularVelocity": -2.0811842087119213e-19, - "velocityX": 2.6805624415155918, - "velocityY": -2.0449582453576474, - "timestamp": 0.5712238262680953 - }, - { - "x": 2.2847273587342762, - "y": 4.177131716956, - "heading": -1.978722549738529e-18, - "angularVelocity": 8.23758723812157e-18, - "velocityX": 2.556473877892601, - "velocityY": -1.7210326992852008, - "timestamp": 0.5908816593052932 - }, - { - "x": 2.330941601494517, - "y": 4.148792845801302, - "heading": -1.6558729417207766e-18, - "angularVelocity": 1.6423460741170202e-17, - "velocityX": 2.3509327133254683, - "velocityY": -1.4416070734283617, - "timestamp": 0.6105394923424912 - }, - { - "x": 2.372265900292984, - "y": 4.125206452882029, - "heading": -1.2164556479802771e-18, - "angularVelocity": 2.2353294405952366e-17, - "velocityX": 2.1021797631645622, - "velocityY": -1.1998470469579758, - "timestamp": 0.6301973253796891 - }, - { - "x": 2.408221763010669, - "y": 4.105824524230037, - "heading": -9.665698088983262e-19, - "angularVelocity": 1.2711770275248128e-17, - "velocityX": 1.8290857720505087, - "velocityY": -0.9859646592438476, - "timestamp": 0.649855158416887 - }, - { - "x": 2.4385166924875037, - "y": 4.0902441055405845, - "heading": -6.950831801501574e-19, - "angularVelocity": 1.3810611494728119e-17, - "velocityX": 1.5411123606303396, - "velocityY": -0.7925806806870744, - "timestamp": 0.6695129914540849 - }, - { - "x": 2.462958637558451, - "y": 4.078162290794493, - "heading": -2.530600057548883e-19, - "angularVelocity": 2.248585116626235e-17, - "velocityX": 1.2433692475002198, - "velocityY": -0.6146056242939526, - "timestamp": 0.6891708244912829 - }, - { - "x": 2.4814140310104698, - "y": 4.069345267851417, - "heading": 2.628695897183014e-19, - "angularVelocity": 2.6245493348697748e-17, - "velocityX": 0.9388315292481915, - "velocityY": -0.44852466324204665, - "timestamp": 0.7088286575284808 - }, - { - "x": 2.4937856312994655, - "y": 4.063608039419202, - "heading": 6.100758780348518e-20, - "angularVelocity": -1.0268781955269092e-17, - "velocityX": 0.6293471038025783, - "velocityY": -0.2918545712215822, - "timestamp": 0.7284864905656787 - }, - { - "x": 2.5, - "y": 4.060801029205322, - "heading": -3.1744174721418913e-35, - "angularVelocity": -3.1034754196134877e-18, - "velocityX": 0.31612684311516714, - "velocityY": -0.14279347110988308, - "timestamp": 0.7481443236028766 - }, - { - "x": 2.5, - "y": 4.060801029205322, - "heading": -7.2806203593007275e-34, - "angularVelocity": -3.5421900256621765e-32, - "velocityX": -1.0821894867257684e-36, - "velocityY": -1.3445064216515772e-31, - "timestamp": 0.7678021566400746 + "heading": -3.928320201048709e-18, + "angularVelocity": 1.562513559934564e-17, + "velocityX": -0.2912228879283196, + "velocityY": -1.3313897800911476, + "timestamp": 0.7503639799258989 + }, + { + "x": 1.9102995444308852, + "y": 4.724875635653645, + "heading": -3.1966305278465913e-18, + "angularVelocity": 1.6658528200090963e-17, + "velocityX": -0.07453084358795621, + "velocityY": -1.3659741160792864, + "timestamp": 0.7942867544276485 + }, + { + "x": 1.9165641847665207, + "y": 4.663493945133946, + "heading": -2.417664167727648e-18, + "angularVelocity": 1.773488716240096e-17, + "velocityX": 0.1426285203035159, + "velocityY": -1.3974911926117468, + "timestamp": 0.8382095289293982 + }, + { + "x": 1.9323905364900036, + "y": 4.600900619777049, + "heading": -1.5888861332178403e-18, + "angularVelocity": 1.8868961285191166e-17, + "velocityX": 0.3603222224050521, + "velocityY": -1.4250767641310782, + "timestamp": 0.8821323034311478 + }, + { + "x": 1.957804843659842, + "y": 4.53732470372903, + "heading": -9.396794624081822e-19, + "angularVelocity": 1.4780630111570506e-17, + "velocityX": 0.5786134290771148, + "velocityY": -1.4474476344227822, + "timestamp": 0.9260550779328974 + }, + { + "x": 1.992834361356742, + "y": 4.47308389106805, + "heading": -3.81904550909219e-19, + "angularVelocity": 1.2698987129083589e-17, + "velocityX": 0.797525158496715, + "velocityY": -1.4625854899100572, + "timestamp": 0.9699778524346471 + }, + { + "x": 2.0375000412705417, + "y": 4.4086463054393965, + "heading": 2.8296907647234686e-19, + "angularVelocity": 1.513732728897157e-17, + "velocityX": 1.0169138999011074, + "velocityY": -1.46706546572779, + "timestamp": 1.0139006269363968 + }, + { + "x": 2.0917879496023337, + "y": 4.344762824527069, + "heading": 7.985525057259883e-19, + "angularVelocity": 1.1738423181491662e-17, + "velocityX": 1.2359854072719534, + "velocityY": -1.4544500350568836, + "timestamp": 1.0578234014381465 + }, + { + "x": 2.155522146006728, + "y": 4.282792952951991, + "heading": 1.4108637952195666e-18, + "angularVelocity": 1.3940628383244673e-17, + "velocityX": 1.4510512399669449, + "velocityY": -1.4108824471777495, + "timestamp": 1.101746175939896 + }, + { + "x": 2.2276450184159087, + "y": 4.225569055680285, + "heading": 1.830932906447906e-18, + "angularVelocity": 9.56381088968155e-18, + "velocityX": 1.6420381732654001, + "velocityY": -1.3028297488099847, + "timestamp": 1.1456689504416457 + }, + { + "x": 2.305355038471454, + "y": 4.176198679539435, + "heading": 2.738350143074857e-18, + "angularVelocity": 2.065937898967755e-17, + "velocityX": 1.7692420602721368, + "velocityY": -1.1240268106739357, + "timestamp": 1.1895917249433954 + }, + { + "x": 2.381463509354403, + "y": 4.136332462140534, + "heading": 2.7713161781139262e-18, + "angularVelocity": 7.504862268589612e-19, + "velocityX": 1.7327792187086972, + "velocityY": -0.9076434229925813, + "timestamp": 1.233514499445145 + }, + { + "x": 2.450950341414104, + "y": 4.103469678408031, + "heading": 2.4984339271026296e-18, + "angularVelocity": -6.212841183256414e-18, + "velocityX": 1.5820228309719393, + "velocityY": -0.7481946234592841, + "timestamp": 1.2774372739468947 + }, + { + "x": 2.512532327834692, + "y": 4.076121211708663, + "heading": 1.986026033701587e-18, + "angularVelocity": -1.1666154831128573e-17, + "velocityX": 1.402051375863395, + "velocityY": -0.6226488878991892, + "timestamp": 1.3213600484486443 + }, + { + "x": 2.565724514235354, + "y": 4.053516749448511, + "heading": 1.2912808268140822e-18, + "angularVelocity": -1.581745371606986e-17, + "velocityX": 1.211038851827205, + "velocityY": -0.5146410379281966, + "timestamp": 1.365282822950394 + }, + { + "x": 2.6102852013760414, + "y": 4.035200826485555, + "heading": 6.452681315987112e-19, + "angularVelocity": -1.4707917382894904e-17, + "velocityX": 1.0145235051044834, + "velocityY": -0.41700286854164376, + "timestamp": 1.4092055974521436 + }, + { + "x": 2.6460725889006635, + "y": 4.020875339537886, + "heading": -1.281828609245258e-19, + "angularVelocity": -1.76093223951591e-17, + "velocityX": 0.8147797567690056, + "velocityY": -0.3261516857519213, + "timestamp": 1.4531283719538932 + }, + { + "x": 2.672994367091148, + "y": 4.010330771618785, + "heading": -5.651109953509987e-20, + "angularVelocity": 1.6317494913316837e-18, + "velocityX": 0.6129343716612188, + "velocityY": -0.2400706248188071, + "timestamp": 1.4970511464556429 + }, + { + "x": 2.6909860044770397, + "y": 4.003412078969364, + "heading": -8.628796629492157e-20, + "angularVelocity": -6.779449938633521e-19, + "velocityX": 0.4096197835907762, + "velocityY": -0.157519481123181, + "timestamp": 1.5409739209573925 + }, + { + "x": 2.7, + "y": 4, + "heading": -2.226129737777408e-30, + "angularVelocity": 1.964529426955296e-18, + "velocityX": 0.20522372790232943, + "velocityY": -0.07768359371383228, + "timestamp": 1.5848966954591421 + }, + { + "x": 2.7, + "y": 4, + "heading": -4.734988782185317e-30, + "angularVelocity": -5.711965804051055e-29, + "velocityX": -1.2601722232887796e-27, + "velocityY": 8.717706191659183e-28, + "timestamp": 1.6288194699608918 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/s1 to n1.traj b/src/main/deploy/choreo/s1 to n1.traj index 8ba5ee3..c03e330 100644 --- a/src/main/deploy/choreo/s1 to n1.traj +++ b/src/main/deploy/choreo/s1 to n1.traj @@ -4,190 +4,190 @@ "x": 0.7777742147445679, "y": 6.587954998016357, "heading": 1.04, - "angularVelocity": 3.456261362722889e-34, - "velocityX": -1.1558052906437545e-31, + "angularVelocity": 0, + "velocityX": 0, "velocityY": 0, "timestamp": 0 }, { - "x": 0.7963313164649566, - "y": 6.590858044372771, - "heading": 1.0212479427537267, - "angularVelocity": -0.5403793813658812, - "velocityX": 0.5347613339651155, - "velocityY": 0.08365729549312781, - "timestamp": 0.034701652011361266 - }, - { - "x": 0.8334366047936193, - "y": 6.597354793156111, - "heading": 0.9842134904961697, - "angularVelocity": -1.0672244723516933, - "velocityX": 1.0692657605036897, - "velocityY": 0.1872172766072533, - "timestamp": 0.06940330402272253 - }, - { - "x": 0.8888948620431416, - "y": 6.60825583696797, - "heading": 0.9287093637187599, - "angularVelocity": -1.5994664103956173, - "velocityX": 1.5981445849138671, - "velocityY": 0.3141361629783576, - "timestamp": 0.10410495603408379 - }, - { - "x": 0.9624427298967876, - "y": 6.624092354477085, - "heading": 0.8539129665807309, - "angularVelocity": -2.1554131519024216, - "velocityX": 2.1194341937832384, - "velocityY": 0.4563620632219429, - "timestamp": 0.13880660804544506 - }, - { - "x": 1.0542492622786634, - "y": 6.644882073171641, - "heading": 0.7600424721123056, - "angularVelocity": -2.705072785517315, - "velocityX": 2.645595441733396, - "velocityY": 0.5990988177666401, - "timestamp": 0.17350826005680633 - }, - { - "x": 1.1664507744572048, - "y": 6.671052140823148, - "heading": 0.6611723277837077, - "angularVelocity": -2.8491480548599815, - "velocityX": 3.233319040309866, - "velocityY": 0.7541447203418256, - "timestamp": 0.2082099120681676 - }, - { - "x": 1.2913236884116448, - "y": 6.70181970935028, - "heading": 0.6001895893740606, - "angularVelocity": -1.7573439555466015, - "velocityX": 3.598471735972595, - "velocityY": 0.8866312334945337, - "timestamp": 0.24291156407952888 - }, - { - "x": 1.4286273378821592, - "y": 6.734134961997154, - "heading": 0.577040600299819, - "angularVelocity": -0.6670860818574766, - "velocityX": 3.9566891347294235, - "velocityY": 0.9312309580044206, - "timestamp": 0.2776132160908901 - }, - { - "x": 1.5744661161236646, - "y": 6.762134785686582, - "heading": 0.577039742085532, - "angularVelocity": -0.00002473122278590041, - "velocityX": 4.202646553938067, - "velocityY": 0.8068729315901393, - "timestamp": 0.3123148681022514 - }, - { - "x": 1.7203049375629715, - "y": 6.790134384466256, - "heading": 0.577038883923436, - "angularVelocity": -0.000024729718795994622, - "velocityX": 4.202647798772216, - "velocityY": 0.8068664503495967, - "timestamp": 0.34701652011361267 - }, - { - "x": 1.8657539829456187, - "y": 6.820090502206212, - "heading": 0.5770365067477514, - "angularVelocity": -0.0000685032425500324, - "velocityX": 4.191415594134462, - "velocityY": 0.8632475978419899, - "timestamp": 0.38171817212497394 - }, - { - "x": 1.9989519675183611, - "y": 6.8523001934695875, - "heading": 0.5408190014813394, - "angularVelocity": -1.0436824521943329, - "velocityX": 3.838375894298458, - "velocityY": 0.9281889880294489, - "timestamp": 0.4164198241363352 - }, - { - "x": 2.1201153456577337, - "y": 6.881905888069826, - "heading": 0.4637582440967774, - "angularVelocity": -2.2206653838650827, - "velocityX": 3.491573775787503, - "velocityY": 0.8531494290400246, - "timestamp": 0.4511214761476965 - }, - { - "x": 2.230128544512573, - "y": 6.907090743568618, - "heading": 0.3438736710895332, - "angularVelocity": -3.4547223563879386, - "velocityX": 3.1702582579878573, - "velocityY": 0.7257537909303905, - "timestamp": 0.48582312815905776 - }, - { - "x": 2.320682619145, - "y": 6.924998011921803, - "heading": 0.23280303009322398, - "angularVelocity": -3.2007306441763914, - "velocityX": 2.6095032767540833, - "velocityY": 0.5160350391192005, - "timestamp": 0.520524780170419 - }, - { - "x": 2.3926583087168334, - "y": 6.938338116341775, - "heading": 0.14104839688844084, - "angularVelocity": -2.644099859417151, - "velocityX": 2.0741286192446693, - "velocityY": 0.38442274781631036, - "timestamp": 0.5552264321817802 - }, - { - "x": 2.4464370134331435, - "y": 6.947890317145862, - "heading": 0.07108140123042413, - "angularVelocity": -2.016243942366481, - "velocityX": 1.5497447988557767, - "velocityY": 0.27526645708275965, - "timestamp": 0.5899280841931415 - }, - { - "x": 2.4821822846008574, - "y": 6.954062116580002, - "heading": 0.02387349227336643, - "angularVelocity": -1.3603937052219282, - "velocityX": 1.0300740482329511, - "velocityY": 0.17785318785745177, - "timestamp": 0.6246297362045027 - }, - { - "x": 2.5, - "y": 6.957088470458984, - "heading": 2.0093661788817544e-33, - "angularVelocity": -0.6879641426163315, - "velocityX": 0.5134543852064684, - "velocityY": 0.08721065723302289, - "timestamp": 0.6593313882158639 - }, - { - "x": 2.5, - "y": 6.957088470458984, - "heading": 0, - "angularVelocity": -8.025462815650884e-32, - "velocityX": -3.4298034808089893e-33, + "x": 0.8022597960341636, + "y": 6.587072803769289, + "heading": 0.9986739281683281, + "angularVelocity": -0.5541098437511959, + "velocityX": 0.3283085234375779, + "velocityY": -0.011828671217327015, + "timestamp": 0.07458101006095233 + }, + { + "x": 0.8519509596093587, + "y": 6.58624052081795, + "heading": 0.92130341357734, + "angularVelocity": -1.0374023431403834, + "velocityX": 0.6662709922349006, + "velocityY": -0.011159448640586789, + "timestamp": 0.14916202012190466 + }, + { + "x": 0.9274840945472737, + "y": 6.586388414118182, + "heading": 0.8127223882915726, + "angularVelocity": -1.455880326601429, + "velocityX": 1.0127663178090307, + "velocityY": 0.001982988700662192, + "timestamp": 0.223743030182857 + }, + { + "x": 1.0299326460915046, + "y": 6.587708067588331, + "heading": 0.6829443155359307, + "angularVelocity": -1.7400954029669882, + "velocityX": 1.3736546536510383, + "velocityY": 0.017694228987703408, + "timestamp": 0.2983240402438093 + }, + { + "x": 1.1563896366352349, + "y": 6.592337580333048, + "heading": 0.5969389995681239, + "angularVelocity": -1.1531798228204053, + "velocityX": 1.6955655392755504, + "velocityY": 0.062073612852970986, + "timestamp": 0.3729050503047616 + }, + { + "x": 1.303789514545485, + "y": 6.601009577302425, + "heading": 0.5715164500982508, + "angularVelocity": -0.3408716166382244, + "velocityX": 1.9763727762570436, + "velocityY": 0.1162762070688939, + "timestamp": 0.4474860603657139 + }, + { + "x": 1.4600489951884137, + "y": 6.6056939320175365, + "heading": 0.5715163560688652, + "angularVelocity": -0.0000012607685095724167, + "velocityX": 2.095164446220536, + "velocityY": 0.06280894709532578, + "timestamp": 0.5220670704266662 + }, + { + "x": 1.6163084771514713, + "y": 6.610378242604074, + "heading": 0.5715162620402611, + "angularVelocity": -0.000001260758262137434, + "velocityX": 2.095164463921114, + "velocityY": 0.0628083554069323, + "timestamp": 0.5966480804876185 + }, + { + "x": 1.7725679591142018, + "y": 6.6150625531994836, + "heading": 0.5715161680117671, + "angularVelocity": -0.0000012607567828315742, + "velocityX": 2.095164463916719, + "velocityY": 0.06280835552621343, + "timestamp": 0.6712290905485708 + }, + { + "x": 1.9288274410769612, + "y": 6.619746863793874, + "heading": 0.5715160739832708, + "angularVelocity": -0.0000012607568122906995, + "velocityX": 2.0951644639171065, + "velocityY": 0.06280835551253054, + "timestamp": 0.7458101006095231 + }, + { + "x": 2.085086923039661, + "y": 6.624431174390149, + "heading": 0.5715159799547684, + "angularVelocity": -0.0000012607568901512097, + "velocityX": 2.09516446391631, + "velocityY": 0.06280835553779388, + "timestamp": 0.8203911106704754 + }, + { + "x": 2.241346405003104, + "y": 6.629115484970691, + "heading": 0.5715158859260843, + "angularVelocity": -0.0000012607593264970524, + "velocityX": 2.0951644639262916, + "velocityY": 0.06280835532631046, + "timestamp": 0.8949721207314277 + }, + { + "x": 2.397605884569563, + "y": 6.633799875776661, + "heading": 0.5715157918938158, + "angularVelocity": -0.0000012608069432354713, + "velocityX": 2.095164431787018, + "velocityY": 0.06280943101139673, + "timestamp": 0.96955313079238 + }, + { + "x": 2.541564258519034, + "y": 6.643991369670441, + "heading": 0.5358742044161485, + "angularVelocity": -0.4778909195321637, + "velocityX": 1.9302282689900117, + "velocityY": 0.1366499848331686, + "timestamp": 1.0441341408533324 + }, + { + "x": 2.6647430298386063, + "y": 6.651108311020372, + "heading": 0.4363244672087119, + "angularVelocity": -1.3347866585073835, + "velocityX": 1.6516103927652543, + "velocityY": 0.09542564982805292, + "timestamp": 1.1187151509142848 + }, + { + "x": 2.765146548502248, + "y": 6.655141686978524, + "heading": 0.28217231220283606, + "angularVelocity": -2.0669089206452895, + "velocityX": 1.3462343642383414, + "velocityY": 0.054080468404030795, + "timestamp": 1.193296160975237 + }, + { + "x": 2.838497759398012, + "y": 6.6577675266394865, + "heading": 0.14782655645686552, + "angularVelocity": -1.8013399877003722, + "velocityX": 0.9835105589992653, + "velocityY": 0.03520788547662426, + "timestamp": 1.2678771710361894 + }, + { + "x": 2.88643730414052, + "y": 6.659290340099078, + "heading": 0.050922701669744434, + "angularVelocity": -1.2993100349254527, + "velocityX": 0.6427848684717067, + "velocityY": 0.020418246660172383, + "timestamp": 1.3424581810971417 + }, + { + "x": 2.9100513458251953, + "y": 6.66, + "heading": -9.944840194397303e-31, + "angularVelocity": -0.6827837492158759, + "velocityX": 0.3166227122075712, + "velocityY": 0.00951528948644474, + "timestamp": 1.417039191158094 + }, + { + "x": 2.9100513458251953, + "y": 6.66, + "heading": -9.944840194397303e-31, + "angularVelocity": 0, + "velocityX": -3.154930727150579e-32, "velocityY": 0, - "timestamp": 0.6940330402272251 + "timestamp": 1.4916202012190463 } ] } \ No newline at end of file diff --git a/src/main/java/team3647/frc2023/auto/AutoCommands.java b/src/main/java/team3647/frc2023/auto/AutoCommands.java index 9fc0a9e..466026e 100644 --- a/src/main/java/team3647/frc2023/auto/AutoCommands.java +++ b/src/main/java/team3647/frc2023/auto/AutoCommands.java @@ -19,12 +19,13 @@ import team3647.frc2023.constants.FieldConstants; import team3647.frc2023.subsystems.Superstructure; import team3647.frc2023.subsystems.SwerveDrive; -import team3647.lib.GeomUtil; +import team3647.frc2023.util.TargetingUtil; public class AutoCommands { private final SwerveDrive swerve; private final Supplier autoDriveVelocities; private final Superstructure superstructure; + private final TargetingUtil targeting; private final String s1_to_n1 = "s1 to n1"; private final String s1_to_n1_to_f1 = "s1 to n1 to f1"; @@ -57,10 +58,12 @@ public class AutoCommands { public AutoCommands( SwerveDrive swerve, Supplier autoDriveVelocities, - Superstructure superstructure) { + Superstructure superstructure, + TargetingUtil targeting) { this.swerve = swerve; this.autoDriveVelocities = autoDriveVelocities; this.superstructure = superstructure; + this.targeting = targeting; this.blueFive_S1N1F1N2N3 = new AutonomousMode(five_S1N1F1N2N3(Alliance.Blue), getInitial(s1_to_n1_to_f1)); @@ -82,11 +85,14 @@ public Command five_S1N1F1N2N3(Alliance color) { } public Command four_S1N1N2N3(Alliance color) { - return Commands.sequence( - shoot(), - Commands.waitSeconds(1), - followChoreoPathWithIntake(s1_to_n1, color), - shoot()); + return Commands.parallel( + superstructure.spinUp(), + Commands.sequence(Commands.waitSeconds(1), superstructure.intake()), + Commands.sequence( + Commands.waitSeconds(1.5), + followChoreoPathWithOverrideAndPivot(s1_to_n1, color), + followChoreoPathWithOverrideAndPivot(n1_to_n2, color), + followChoreoPathWithOverrideAndPivot(n2_to_n3, color))); } // , @@ -100,15 +106,29 @@ public Pose2d getInitial(String path) { return traj.getInitialPose(); } + public Pose2d getFinal(String path) { + ChoreoTrajectory traj = Choreo.getTrajectory(path); + return traj.getFinalPose(); + } + + public Command spinUp() { + return Commands.parallel(superstructure.spinUp(), superstructure.feed()); + } + + public double getPivotAngleByPose(Pose2d pose) { + return targeting.getPivotAngleByPose(pose) * 180 / Math.PI; + } + + public Command target() { + return Commands.run(() -> swerve.drive(0, 0, deeThetaOnTheMove()), swerve).withTimeout(2); + } + public Command shoot() { - return Commands.parallel( - superstructure.shoot(), - Commands.run(() -> swerve.drive(0, 0, autoDriveVelocities.get().dtheta))) - .until(() -> !superstructure.getIsShooting()); + return Commands.parallel(superstructure.shootStow()); } public Command intakeAndShoot(String path, Alliance color) { - return Commands.parallel(followChoreoPathWithIntake(path, color), superstructure.shoot()); + return Commands.parallel(overrideChoreoPathWithIntake(path, color), superstructure.shoot()); } public Pose2d flipForPP(Pose2d pose) { @@ -130,6 +150,12 @@ public Command pathAndShootWithOverride(String path, Alliance color) { superstructure.shootStow(), followChoreoPathWithOverride(path, color)); } + public Command followChoreoPathWithOverrideAndPivot(String path, Alliance color) { + return Commands.deadline( + followChoreoPathWithOverride(path, color), + superstructure.pivotCommands.setAngle(() -> getPivotAngleByPose(getFinal(path)))); + } + public Command followChoreoPathWithOverride(String path, Alliance color) { ChoreoTrajectory traj = Choreo.getTrajectory(path); boolean mirror = color == Alliance.Red; @@ -142,8 +168,8 @@ public Command followChoreoPathWithOverride(String path, Alliance color) { new PIDController(0, 0, 0)), (ChassisSpeeds speeds) -> swerve.drive( - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, + speeds.vxMetersPerSecond * 0.4, + speeds.vyMetersPerSecond * 0.4, deeThetaOnTheMove()), () -> mirror); } @@ -167,14 +193,15 @@ public Command customChoreoFolloweForOverride( }, (interrupted) -> { timer.stop(); - if (interrupted) { - outputChassisSpeeds.accept(new ChassisSpeeds()); - } - superstructure.setPiece(); + // if (interrupted) { + outputChassisSpeeds.accept(new ChassisSpeeds()); + // } + // superstructure.setPiece(); }, () -> - GeomUtil.distance(swerve.getOdoPose().minus(trajectory.getFinalPose())) - < 0.05, + // GeomUtil.distance(swerve.getOdoPose().minus(trajectory.getFinalPose())) + // < 0.05, + timer.hasElapsed(0.5) && swerve.getVel() < 0.1, swerve); } diff --git a/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java b/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java index c3cf5af..cf28286 100644 --- a/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java +++ b/src/main/java/team3647/frc2023/commands/DrivetrainCommands.java @@ -68,6 +68,7 @@ public Command characterize() { return Commands.runEnd( () -> { double desiredVoltage = filter.calculate(12); + swerve.drive(desiredVoltage, 0, 0); voltageVelocityMap.put( desiredVoltage, swerve.getChassisSpeeds().vxMetersPerSecond); diff --git a/src/main/java/team3647/frc2023/commands/ShooterCommands.java b/src/main/java/team3647/frc2023/commands/ShooterCommands.java index 671c8bc..b2872a5 100644 --- a/src/main/java/team3647/frc2023/commands/ShooterCommands.java +++ b/src/main/java/team3647/frc2023/commands/ShooterCommands.java @@ -19,7 +19,7 @@ public Command shoot(DoubleSupplier bill) { } public Command setVelocity(DoubleSupplier bill) { - return Commands.run(() -> shooter.setVelocity(bill.getAsDouble())); + return Commands.run(() -> shooter.setVelocity(bill.getAsDouble()), shooter); } public Command kill() { diff --git a/src/main/java/team3647/frc2023/constants/FieldConstants.java b/src/main/java/team3647/frc2023/constants/FieldConstants.java index c67dfff..4c4c886 100644 --- a/src/main/java/team3647/frc2023/constants/FieldConstants.java +++ b/src/main/java/team3647/frc2023/constants/FieldConstants.java @@ -25,7 +25,7 @@ public class FieldConstants { public static final double kSpeakerHeight = Units.inchesToMeters(83); - public static final Pose3d kBlueSpeaker = new Pose3d(0, 5.1, kSpeakerHeight, new Rotation3d()); + public static final Pose3d kBlueSpeaker = new Pose3d(0, 5.5, kSpeakerHeight, new Rotation3d()); public static final Pose2d kBlueAmpAlign = new Pose2d( diff --git a/src/main/java/team3647/frc2023/constants/ShooterConstants.java b/src/main/java/team3647/frc2023/constants/ShooterConstants.java index 325d8fe..8116602 100644 --- a/src/main/java/team3647/frc2023/constants/ShooterConstants.java +++ b/src/main/java/team3647/frc2023/constants/ShooterConstants.java @@ -34,8 +34,8 @@ public class ShooterConstants { public static final double masterKD = 0; public static final double kNominalVoltage = 11.0; - public static final double kStallCurrent = 25.0; - public static final double kMaxCurrent = 20.0; + public static final double kStallCurrent = 55.0; + public static final double kMaxCurrent = 40.0; static { Slot0Configs kTopSlot0 = new Slot0Configs(); @@ -67,6 +67,8 @@ public class ShooterConstants { kTopCurrent.StatorCurrentLimitEnable = false; kBottomCurrent.StatorCurrentLimitEnable = false; + // kTopCurrent.StatorCurrentLimit = kMaxCurrent; + // kBottomCurrent.StatorCurrentLimit = kMaxCurrent; kTopRollerConfigurator.apply(kTopMotor); kTopRollerConfigurator.apply(kTopCurrent); diff --git a/src/main/java/team3647/frc2023/constants/SwerveDriveConstants.java b/src/main/java/team3647/frc2023/constants/SwerveDriveConstants.java index 7e11ef1..df3d707 100644 --- a/src/main/java/team3647/frc2023/constants/SwerveDriveConstants.java +++ b/src/main/java/team3647/frc2023/constants/SwerveDriveConstants.java @@ -90,8 +90,8 @@ public class SwerveDriveConstants { // rotation. // from motor to output shaft - public static final double kDriveMotorGearRatio = 6.12; - public static final double kTurnMotorGearRatio = 12.8; // 7.0 / 150.0; + public static final double kDriveMotorGearRatio = 1 / 6.12; + public static final double kTurnMotorGearRatio = 1 / 12.8; // 7.0 / 150.0; public static final double kCouplingGearRatio = 3.57; public static final double kWheelDiameterMeters = 0.097; // 97mm public static final double kWheelRadiusInches = 1.9; diff --git a/src/main/java/team3647/frc2023/constants/TunerConstants.java b/src/main/java/team3647/frc2023/constants/TunerConstants.java index fd74a71..6e2335b 100644 --- a/src/main/java/team3647/frc2023/constants/TunerConstants.java +++ b/src/main/java/team3647/frc2023/constants/TunerConstants.java @@ -23,7 +23,7 @@ public class TunerConstants { .withKA(0); // When using closed-loop control, the drive motor uses the control // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput public static final Slot0Configs driveGains = - new Slot0Configs().withKP(3).withKI(0).withKD(0).withKS(0).withKV(0).withKA(0); + new Slot0Configs().withKP(0.25).withKI(0).withKD(0).withKS(0.2).withKV(0.1).withKA(0); // The closed-loop output type to use for the steer motors; // This affects the PID/FF gains for the steer motors public static final ClosedLoopOutputType steerClosedLoopOutput = ClosedLoopOutputType.Voltage; diff --git a/src/main/java/team3647/frc2023/constants/VisionConstants.java b/src/main/java/team3647/frc2023/constants/VisionConstants.java index 9a8bebb..d3ee574 100644 --- a/src/main/java/team3647/frc2023/constants/VisionConstants.java +++ b/src/main/java/team3647/frc2023/constants/VisionConstants.java @@ -23,6 +23,6 @@ public class VisionConstants { Units.inchesToMeters(11), Units.inchesToMeters(7.5)), new Rotation3d(Math.PI, 0, 0) - .rotateBy(new Rotation3d(0, Math.PI / 6, 0)) + .rotateBy(new Rotation3d(0, -Math.PI / 6, 0)) .rotateBy(new Rotation3d(0, 0, Math.PI * 3 / 4))); } diff --git a/src/main/java/team3647/frc2023/robot/RobotContainer.java b/src/main/java/team3647/frc2023/robot/RobotContainer.java index 1803383..92badb6 100644 --- a/src/main/java/team3647/frc2023/robot/RobotContainer.java +++ b/src/main/java/team3647/frc2023/robot/RobotContainer.java @@ -1,6 +1,7 @@ package team3647.frc2023.robot; import com.playingwithfusion.TimeOfFlight; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.PowerDistribution; import edu.wpi.first.wpilibj.PowerDistribution.ModuleType; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; @@ -87,10 +88,9 @@ private void configureButtonBindings() { mainController.buttonA.onTrue(superstructure.ejectPiece()); tofPiece.onTrue(superstructure.setPiece()); - tofPiece.and(() -> !mainController.leftBumper.getAsBoolean()) - .onTrue(superstructure.slightReverse()); + tofPiece.and(() -> !superstructure.getIsShooting()).onTrue(superstructure.slightReverse()); - mainController.buttonY.whileTrue(drivetrainCommands.characterize()); + // mainController.buttonY.whileTrue(drivetrainCommands.characterize()); // mainController.leftBumper.onTrue(superstructure.shootStow()); // mainController.rightBumper.onTrue(superstructure.shootStow()); @@ -110,8 +110,9 @@ private void configureDefaultCommands() { autoDrive::getEnabled, autoDrive::getVelocities)); pivot.setDefaultCommand(superstructure.pivotCommands.holdPositionAtCall()); - // intake.setDefaultCommand(superstructure.intakeCommands.intake()); - // kicker.setDefaultCommand(superstructure.kickerCommands.kick()); + intake.setDefaultCommand(superstructure.intakeCommands.kill()); + kicker.setDefaultCommand(superstructure.kickerCommands.kill()); + shooter.setDefaultCommand(superstructure.shooterCommands.kill()); } public void teleopInit() {} @@ -122,6 +123,7 @@ public void configureSmartDashboardLogging() { SmartDashboard.putNumber("pivot interp angle", 40); printer.addBoolean("tof", () -> tofPiece.getAsBoolean()); printer.addBoolean("shooht", () -> superstructure.getIsShooting()); + // printer.addDouble("auto drive", () -> autoDrive.getVelocities().dtheta); } public Command getAutonomousCommand() { @@ -211,7 +213,7 @@ public Command getAutonomousCommand() { targetingUtil.exitVelocity()); public final AutoCommands autoCommands = - new AutoCommands(swerve, autoDrive::getVelocities, superstructure); + new AutoCommands(swerve, autoDrive::getVelocities, superstructure, targetingUtil); private final CommandScheduler scheduler = CommandScheduler.getInstance(); @@ -219,5 +221,10 @@ public Command getAutonomousCommand() { Trigger piece = new Trigger(() -> superstructure.getPiece()); - Trigger tofPiece = new Trigger(() -> (tof.getRange() < 100 && !superstructure.getIsShooting())); + Trigger tofPiece = + new Trigger( + () -> + (tof.getRange() < 100 + && !superstructure.getIsShooting() + && !DriverStation.isAutonomous())); } diff --git a/src/main/java/team3647/frc2023/subsystems/Superstructure.java b/src/main/java/team3647/frc2023/subsystems/Superstructure.java index f5654d0..bb7ad27 100644 --- a/src/main/java/team3647/frc2023/subsystems/Superstructure.java +++ b/src/main/java/team3647/frc2023/subsystems/Superstructure.java @@ -86,14 +86,15 @@ public boolean getIsShooting() { public Command shoot() { return Commands.parallel( - prep(), - spinUp(), - Commands.sequence( - Commands.waitSeconds(0.5), - Commands.waitUntil(() -> shooter.velocityReached(shootSpeed, 1)) - .withTimeout(2), - Commands.parallel(isSHooting(), intake(), ejectPiece()).withTimeout(0.8), - doneShooting())); + prep(), + spinUp(), + Commands.sequence( + Commands.waitSeconds(1), + // Commands.waitUntil(() -> shooter.velocityReached(shootSpeed, 1)) + // .withTimeout(3), + Commands.parallel(isSHooting(), intake(), ejectPiece()) + .withTimeout(1))) + .finallyDo((interrupted) -> doneShooting()); } public Command shootManual() { diff --git a/src/main/java/team3647/frc2023/subsystems/SwerveDrive.java b/src/main/java/team3647/frc2023/subsystems/SwerveDrive.java index 7dee036..d4c22d8 100644 --- a/src/main/java/team3647/frc2023/subsystems/SwerveDrive.java +++ b/src/main/java/team3647/frc2023/subsystems/SwerveDrive.java @@ -224,7 +224,7 @@ public ChassisSpeeds getFieldRelativeChassisSpeeds() { public void addVisionData(VisionMeasurement data) { periodicIO.visionPose = data.pose; SmartDashboard.putNumber("timestamped viison", data.timestamp); - this.m_odometry.addVisionMeasurement(data.pose, data.timestamp); + addVisionMeasurement(data.pose, data.timestamp, data.stdDevs); } @Override @@ -255,6 +255,13 @@ public SwerveModuleState[] getModuleStates() { }; } + public double getVel() { + return Math.sqrt( + getChassisSpeeds().vxMetersPerSecond * getChassisSpeeds().vxMetersPerSecond + + getChassisSpeeds().vyMetersPerSecond + * getChassisSpeeds().vyMetersPerSecond); + } + public double getMaxSpeedMpS() { return this.maxSpeedMpS; } diff --git a/src/main/java/team3647/frc2023/util/AprilTagCamera.java b/src/main/java/team3647/frc2023/util/AprilTagCamera.java index 96f2c3d..9109041 100644 --- a/src/main/java/team3647/frc2023/util/AprilTagCamera.java +++ b/src/main/java/team3647/frc2023/util/AprilTagCamera.java @@ -3,7 +3,7 @@ import java.util.Optional; public interface AprilTagCamera { - public Optional QueueToInputs(); + public Optional QueueToInputs(); public int getTagNum(); } diff --git a/src/main/java/team3647/frc2023/util/AprilTagLimelight.java b/src/main/java/team3647/frc2023/util/AprilTagLimelight.java index 457dc96..fc56693 100644 --- a/src/main/java/team3647/frc2023/util/AprilTagLimelight.java +++ b/src/main/java/team3647/frc2023/util/AprilTagLimelight.java @@ -1,110 +1,110 @@ -package team3647.frc2023.util; +// package team3647.frc2023.util; -import edu.wpi.first.math.VecBuilder; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Pose3d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Rotation3d; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.networktables.DoubleArraySubscriber; -import edu.wpi.first.networktables.IntegerSubscriber; -import edu.wpi.first.networktables.NetworkTable; -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.networktables.PubSubOption; -import edu.wpi.first.networktables.TimestampedDoubleArray; -import java.util.Optional; -import team3647.lib.vision.AprilTagCamera.AprilTagId; +// import edu.wpi.first.math.VecBuilder; +// import edu.wpi.first.math.geometry.Pose2d; +// import edu.wpi.first.math.geometry.Pose3d; +// import edu.wpi.first.math.geometry.Rotation2d; +// import edu.wpi.first.math.geometry.Rotation3d; +// import edu.wpi.first.math.util.Units; +// import edu.wpi.first.networktables.DoubleArraySubscriber; +// import edu.wpi.first.networktables.IntegerSubscriber; +// import edu.wpi.first.networktables.NetworkTable; +// import edu.wpi.first.networktables.NetworkTableEntry; +// import edu.wpi.first.networktables.NetworkTableInstance; +// import edu.wpi.first.networktables.PubSubOption; +// import edu.wpi.first.networktables.TimestampedDoubleArray; +// import java.util.Optional; +// import team3647.lib.vision.AprilTagCamera.AprilTagId; -public class AprilTagLimelight implements AprilTagCamera { - private final NetworkTable table; - private final NetworkTableEntry botPoseEntry; - private final String limelightIdentifier; - private final DoubleArraySubscriber botPoseSubscriber; - private final DoubleArraySubscriber botToTargetSubscriber; - private final IntegerSubscriber tagNumberSubscriber; - private final double stdDevs = 0.01; +// public class AprilTagLimelight implements AprilTagCamera { +// private final NetworkTable table; +// private final NetworkTableEntry botPoseEntry; +// private final String limelightIdentifier; +// private final DoubleArraySubscriber botPoseSubscriber; +// private final DoubleArraySubscriber botToTargetSubscriber; +// private final IntegerSubscriber tagNumberSubscriber; +// private final double stdDevs = 0.01; - public AprilTagLimelight(String limelight) { - this.limelightIdentifier = limelight; - table = NetworkTableInstance.getDefault().getTable(limelightIdentifier); - botPoseEntry = table.getEntry("botpose_wpiblue"); - tagNumberSubscriber = table.getIntegerTopic("tid").subscribe(-1); - botToTargetSubscriber = - table.getDoubleArrayTopic("botpose_targetspace") - .subscribe( - new double[] {}, - PubSubOption.keepDuplicates(true), - PubSubOption.sendAll(true)); - botPoseSubscriber = - table.getDoubleArrayTopic("botpose_wpiblue") - .subscribe( - new double[] {}, - PubSubOption.keepDuplicates(true), - PubSubOption.sendAll(true)); - } +// public AprilTagLimelight(String limelight) { +// this.limelightIdentifier = limelight; +// table = NetworkTableInstance.getDefault().getTable(limelightIdentifier); +// botPoseEntry = table.getEntry("botpose_wpiblue"); +// tagNumberSubscriber = table.getIntegerTopic("tid").subscribe(-1); +// botToTargetSubscriber = +// table.getDoubleArrayTopic("botpose_targetspace") +// .subscribe( +// new double[] {}, +// PubSubOption.keepDuplicates(true), +// PubSubOption.sendAll(true)); +// botPoseSubscriber = +// table.getDoubleArrayTopic("botpose_wpiblue") +// .subscribe( +// new double[] {}, +// PubSubOption.keepDuplicates(true), +// PubSubOption.sendAll(true)); +// } - public DoubleArraySubscriber getBotPoseSubscriber() { - return botPoseSubscriber; - } +// public DoubleArraySubscriber getBotPoseSubscriber() { +// return botPoseSubscriber; +// } - public DoubleArraySubscriber getBotToTargetSubscriber() { - return botToTargetSubscriber; - } +// public DoubleArraySubscriber getBotToTargetSubscriber() { +// return botToTargetSubscriber; +// } - public IntegerSubscriber getTagNumSubscriber() { - return tagNumberSubscriber; - } +// public IntegerSubscriber getTagNumSubscriber() { +// return tagNumberSubscriber; +// } - public int getTagNum() { - return ((int) tagNumberSubscriber.get(-1)); - } +// public int getTagNum() { +// return ((int) tagNumberSubscriber.get(-1)); +// } - public static AprilTagId getId(int id) { - var adjustedId = id - 1; - var possibleValues = AprilTagId.values(); - if (adjustedId < 0 || adjustedId > possibleValues.length) { - return AprilTagId.ID_DNE; - } +// public static AprilTagId getId(int id) { +// var adjustedId = id - 1; +// var possibleValues = AprilTagId.values(); +// if (adjustedId < 0 || adjustedId > possibleValues.length) { +// return AprilTagId.ID_DNE; +// } - return possibleValues[adjustedId]; - } +// return possibleValues[adjustedId]; +// } - public Optional QueueToInputs() { - var botPoseQueue = this.getBotPoseSubscriber().readQueue(); - var botToTargetQueue = this.getBotToTargetSubscriber().readQueue(); - if (botPoseQueue.length < 1 || botToTargetQueue.length < 1) { - return Optional.empty(); - } - var inputs = new VisionMeasurement[botPoseQueue.length]; - var length = - botPoseQueue.length <= botToTargetQueue.length - ? botPoseQueue.length - : botToTargetQueue.length; - for (TimestampedDoubleArray botPose : botPoseQueue) {} +// public Optional QueueToInputs() { +// var botPoseQueue = this.getBotPoseSubscriber().readQueue(); +// var botToTargetQueue = this.getBotToTargetSubscriber().readQueue(); +// if (botPoseQueue.length < 1 || botToTargetQueue.length < 1) { +// return Optional.empty(); +// } +// var inputs = new VisionMeasurement[botPoseQueue.length]; +// var length = +// botPoseQueue.length <= botToTargetQueue.length +// ? botPoseQueue.length +// : botToTargetQueue.length; +// for (TimestampedDoubleArray botPose : botPoseQueue) {} - for (int i = 0; i < length; i++) { - var botPose = botPoseQueue[i].value; - var timestamp = botPoseQueue[i].timestamp; - var botToTag = botToTargetQueue[i].value; - Pose2d tagDistance = new Pose2d(botToTag[0], botToTag[1], new Rotation2d()); - double distanceSquared = - Math.pow(tagDistance.getX(), 2) + Math.pow(tagDistance.getY(), 2); - double stdDev = stdDevs * distanceSquared; - inputs[i] = - new VisionMeasurement( - new Pose3d( - botPose[0], - botPose[1], - botPose[2], - new Rotation3d( - Units.degreesToRadians(botPose[3]), - Units.degreesToRadians(botPose[4]), - Units.degreesToRadians(botPose[5]))) - .toPose2d(), - (timestamp - (botPose[6] / 1000.0)) / 1000000, - VecBuilder.fill(stdDev, stdDev, stdDev)); - } - return Optional.of(inputs); - } -} +// for (int i = 0; i < length; i++) { +// var botPose = botPoseQueue[i].value; +// var timestamp = botPoseQueue[i].timestamp; +// var botToTag = botToTargetQueue[i].value; +// Pose2d tagDistance = new Pose2d(botToTag[0], botToTag[1], new Rotation2d()); +// double distanceSquared = +// Math.pow(tagDistance.getX(), 2) + Math.pow(tagDistance.getY(), 2); +// double stdDev = stdDevs * distanceSquared; +// inputs[i] = +// new VisionMeasurement( +// new Pose3d( +// botPose[0], +// botPose[1], +// botPose[2], +// new Rotation3d( +// Units.degreesToRadians(botPose[3]), +// Units.degreesToRadians(botPose[4]), +// Units.degreesToRadians(botPose[5]))) +// .toPose2d(), +// (timestamp - (botPose[6] / 1000.0)) / 1000000, +// VecBuilder.fill(stdDev, stdDev, stdDev)); +// } +// return Optional.of(inputs); +// } +// } diff --git a/src/main/java/team3647/frc2023/util/AprilTagPhotonVision.java b/src/main/java/team3647/frc2023/util/AprilTagPhotonVision.java index c4ab3f2..53b7318 100644 --- a/src/main/java/team3647/frc2023/util/AprilTagPhotonVision.java +++ b/src/main/java/team3647/frc2023/util/AprilTagPhotonVision.java @@ -44,7 +44,7 @@ public static AprilTagId getId(int id) { return possibleValues[adjustedId]; } - public Optional QueueToInputs() { + public Optional QueueToInputs() { var update = photonPoseEstimator.update(); if (update.isEmpty()) { return Optional.empty(); @@ -55,7 +55,7 @@ public Optional QueueToInputs() { .times(GeomUtil.distance(drivePose.get(), visionPose)); VisionMeasurement measurement = VisionMeasurement.fromEstimatedRobotPose(update.get(), stdDevs); - return Optional.of(new VisionMeasurement[] {measurement}); + return Optional.of(measurement); } public int getTagNum() { diff --git a/src/main/java/team3647/frc2023/util/AutoDrive.java b/src/main/java/team3647/frc2023/util/AutoDrive.java index 83b8994..2971b53 100644 --- a/src/main/java/team3647/frc2023/util/AutoDrive.java +++ b/src/main/java/team3647/frc2023/util/AutoDrive.java @@ -11,6 +11,7 @@ import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.trajectory.TrapezoidProfile; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import java.util.List; @@ -61,9 +62,10 @@ public enum DriveMode { @Override public void periodic() { - if (this.mode == DriveMode.SHOOT_ON_THE_MOVE) { - targetRot = targeting.shootAtSpeaker().rotation; - } + // if (this.mode == DriveMode.SHOOT_ON_THE_MOVE) { + targetRot = targeting.shootAtSpeaker().rotation; + // } + SmartDashboard.putNumber("auto drive", getRot()); detector.pieceCoordinate(swerve::getOdoPose).ifPresent(this::setTargetPose); } diff --git a/src/main/java/team3647/frc2023/util/TargetingUtil.java b/src/main/java/team3647/frc2023/util/TargetingUtil.java index dd0bc1e..249df9e 100644 --- a/src/main/java/team3647/frc2023/util/TargetingUtil.java +++ b/src/main/java/team3647/frc2023/util/TargetingUtil.java @@ -17,7 +17,7 @@ public class TargetingUtil { private final Supplier drivePose; private final Supplier fieldRelativeSpeeds; private final Transform3d robotToShooter; - private final double shootSpeed = 8; + private final double shootSpeed = 9; double kDt = 0.02; public TargetingUtil( @@ -84,13 +84,6 @@ public double robotAngleToPose(Pose3d pose) { var rot = currentPose.getRotation().getRadians(); SmartDashboard.putNumber("rot", rot); final var fieldAngle = fieldAngleToPose(pose); - // if (Math.signum(rot * fieldAngle) < 0) { - // if (rot < 0) { - // rot += 2 * Math.PI; - // } else { - // rot -= 2 * Math.PI; - // } - // } var newAngle = rot - fieldAngle; double distance = GeomUtil.distance(pose.toPose2d().minus(currentPose)); @@ -124,29 +117,18 @@ public double getPivotAngle(Pose3d pose) { double shooterDistance = GeomUtil.distance(pose.toPose2d().minus(shooter2D)); double pivotAngle = Math.atan((pose.getZ() - shooterPose.getZ()) / shooterDistance) - + shooterDistance * Math.PI / 180 * 1.6; + + shooterDistance * Math.PI / 180 * 3; double newPivotAngle = Math.atan( (exitVelocity() * Math.sin(pivotAngle) * Math.cos(angle)) / (exitVelocity() * Math.cos(pivotAngle) * Math.cos(angleOnTheMove) + fieldRelativeSpeeds.get().vxMetersPerSecond)); + SmartDashboard.putNumber("new pibotr angle", newPivotAngle * 180 / Math.PI); return newPivotAngle; } // returns the pivot angle not accounting for movement public double getPivotAngleStationary(Pose3d pose) { - double angleOnTheMove = fieldAngleToPoseStationary(pose); - if (angleOnTheMove < 0) { - angleOnTheMove += Math.PI; - } else { - angleOnTheMove -= Math.PI; - } - double angle = fieldAngleToPoseStationary(pose); - if (angle < 0) { - angle += Math.PI; - } else { - angle -= Math.PI; - } var currentPose = compensatedPose(); var pose3D = new Pose3d( @@ -163,6 +145,24 @@ public double getPivotAngleStationary(Pose3d pose) { return pivotAngle; } + public double getPivotAngleByPose(Pose2d botPose) { + var currentPose = botPose; + var pose3D = + new Pose3d( + currentPose.getX(), + currentPose.getY(), + 0, + new Rotation3d(0, 0, currentPose.getRotation().getRadians())); + var shooterPose = pose3D.transformBy(robotToShooter); + var shooter2D = shooterPose.toPose2d(); + var pose = speakerPose; + double shooterDistance = GeomUtil.distance(pose.toPose2d().minus(shooter2D)); + double pivotAngle = + Math.atan((pose.getZ() - shooterPose.getZ()) / shooterDistance) + + (shooterDistance) * Math.PI / 180 * 2.6; + return pivotAngle; + } + // returns latency compensated pose; public Pose2d compensatedPose() { var speeds = fieldRelativeSpeeds.get(); diff --git a/src/main/java/team3647/frc2023/util/VisionController.java b/src/main/java/team3647/frc2023/util/VisionController.java index 0eb2938..520fdd8 100644 --- a/src/main/java/team3647/frc2023/util/VisionController.java +++ b/src/main/java/team3647/frc2023/util/VisionController.java @@ -1,6 +1,7 @@ package team3647.frc2023.util; import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.wpilibj.Timer; import java.util.function.Consumer; import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; @@ -37,17 +38,14 @@ public void periodic() { break; } - for (int j = 0; j < inputs.get().length; j++) { + var getInputs = inputs.get(); - Logger.recordOutput("Robot/Vision", inputs.get()[j].pose); + Logger.recordOutput("Robot/Vision", getInputs.pose); - if (shouldAddData(inputs.get()[j].pose, swerve::getOdoPose)) { - botPoseAcceptor.accept( - new VisionMeasurement( - inputs.get()[j].pose, - inputs.get()[j].timestamp, - inputs.get()[j].stdDevs)); - } + if (shouldAddData(getInputs.pose, swerve::getOdoPose)) { + botPoseAcceptor.accept( + new VisionMeasurement( + getInputs.pose, Timer.getFPGATimestamp(), getInputs.stdDevs)); } } }