From 93bb7b090cb67e87f4092e88683cf86c2a79b330 Mon Sep 17 00:00:00 2001 From: suryatho Date: Wed, 17 Jan 2024 05:26:34 -0500 Subject: [PATCH] Change Drive subsystem - move control loop into IO implementation - include timestamp queue for future - added kraken implementation with FOC --- src/main/Potential Autos.chor | 2919 +++++++++++++++++ src/main/Test.chor | 2802 ++++++++++++++++ src/main/deploy/choreo/3Spike.traj | 490 +++ src/main/deploy/choreo/6Note.traj | 1759 ++++++++++ src/main/deploy/choreo/FiveNoteCitrus.traj | 796 +++++ src/main/deploy/choreo/New Path.traj | 283 ++ src/main/deploy/choreo/Test.traj | 1048 ++++++ src/main/java/frc/robot/Constants.java | 9 +- src/main/java/frc/robot/RobotContainer.java | 16 +- .../frc/robot/commands/DriveCommands.java | 6 +- .../frc/robot/commands/DriveTrajectory.java | 8 +- .../frc/robot/subsystems/drive/Drive.java | 65 +- .../subsystems/drive/DriveConstants.java | 138 +- .../frc/robot/subsystems/drive/GyroIO.java | 4 +- .../robot/subsystems/drive/GyroIOPigeon2.java | 2 +- .../frc/robot/subsystems/drive/Module.java | 202 -- .../frc/robot/subsystems/drive/ModuleIO.java | 62 +- .../subsystems/drive/ModuleIOKrakenFOC.java | 308 ++ .../robot/subsystems/drive/ModuleIOSim.java | 98 +- .../subsystems/drive/ModuleIOSparkMax.java | 254 +- .../subsystems/drive/ModuleIOTalonFX.java | 211 -- .../drive/PhoenixOdometryThread.java | 21 +- .../drive/SparkMaxOdometryThread.java | 10 +- 23 files changed, 10910 insertions(+), 601 deletions(-) create mode 100644 src/main/Potential Autos.chor create mode 100644 src/main/Test.chor create mode 100644 src/main/deploy/choreo/3Spike.traj create mode 100644 src/main/deploy/choreo/6Note.traj create mode 100644 src/main/deploy/choreo/FiveNoteCitrus.traj create mode 100644 src/main/deploy/choreo/New Path.traj create mode 100644 src/main/deploy/choreo/Test.traj delete mode 100644 src/main/java/frc/robot/subsystems/drive/Module.java create mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java delete mode 100644 src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java diff --git a/src/main/Potential Autos.chor b/src/main/Potential Autos.chor new file mode 100644 index 00000000..3221b480 --- /dev/null +++ b/src/main/Potential Autos.chor @@ -0,0 +1,2919 @@ +{ + "version": "v0.2", + "robotConfiguration": { + "mass": 59.27038160247355, + "rotationalInertia": 6, + "motorMaxTorque": 0.7248618784530386, + "motorMaxVelocity": 4704, + "gearing": 6.75, + "wheelbase": 0.6603996433841925, + "trackWidth": 0.6603996433841925, + "bumperLength": 0.812799561088237, + "bumperWidth": 0.812799561088237, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "6Note": { + "waypoints": [ + { + "x": 1.2134901285171509, + "y": 3.7024037837982178, + "heading": -0.9272952180016122, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 1.9061466455459595, + "y": 2.8476362228393555, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 15 + }, + { + "x": 2.65, + "y": 3.0946526527404785, + "heading": 1.57, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 2.65, + "y": 3.9813501834869385, + "heading": 1.57, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.690741539001465, + "y": 4.745745658874512, + "heading": -0.3547054524853324, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.705155611038208, + "y": 5.005199432373047, + "heading": 1.460139105621001, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "x": 2.7339837551116943, + "y": 5.379965782165527, + "heading": 1.499489025796698, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.777225971221924, + "y": 6.043013572692871, + "heading": 0.07130738082253721, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 2.777225971221924, + "y": 6.547506809234619, + "heading": 1.53235378563829, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 4.431106090545654, + "y": 7.380357265472412, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": false, + "headingConstrained": false, + "controlIntervalCount": 8 + }, + { + "x": 5.288247108459473, + "y": 7.439470291137695, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 7.268537998199463, + "y": 7.7350358963012695, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": false, + "headingConstrained": false, + "controlIntervalCount": 7 + }, + { + "x": 7.830112934112549, + "y": 7.469026565551758, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 5.879378795623779, + "y": 6.833560466766357, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": false, + "headingConstrained": false, + "controlIntervalCount": 7 + }, + { + "x": 5.376916885375977, + "y": 6.759668827056885, + "heading": 0.21109341192044445, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 7.711886405944824, + "y": 5.976419448852539, + "heading": -0.2837940575825868, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 5.376916885375977, + "y": 6.227650165557861, + "heading": 0.06885690933268979, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2134901285191984, + "y": 3.702403783800421, + "heading": -0.9272952180015092, + "angularVelocity": 2.9275724560072123e-13, + "velocityX": 2.454991007161089e-12, + "velocityY": -1.0608084392054969e-12, + "timestamp": 0 + }, + { + "x": 1.2214199688965524, + "y": 3.6828844138661974, + "heading": -0.9068235786762225, + "angularVelocity": 0.11838487813596572, + "velocityX": 0.045857255063974624, + "velocityY": -0.1128780258036785, + "timestamp": 0.1729244447864887 + }, + { + "x": 1.2371939281344813, + "y": 3.6459016271317672, + "heading": -0.8661542466206761, + "angularVelocity": 0.23518555807832484, + "velocityX": 0.09121879355263159, + "velocityY": -0.2138667369457007, + "timestamp": 0.3458488895729774 + }, + { + "x": 1.2607987600293624, + "y": 3.5935252227252095, + "heading": -0.8055681239046617, + "angularVelocity": 0.3503618172041365, + "velocityX": 0.13650373094344487, + "velocityY": -0.30288606382088756, + "timestamp": 0.518773334359466 + }, + { + "x": 1.2923153971748729, + "y": 3.5278914158276553, + "heading": -0.7252939199511473, + "angularVelocity": 0.46421547893157705, + "velocityX": 0.18225669120552063, + "velocityY": -0.37955193086711103, + "timestamp": 0.6916977791459548 + }, + { + "x": 1.3319437170467292, + "y": 3.45130898950986, + "heading": -0.6254055185731093, + "angularVelocity": 0.5776418800095864, + "velocityX": 0.2291655174159904, + "velocityY": -0.44286639999442523, + "timestamp": 0.8646222239324435 + }, + { + "x": 1.380003817262134, + "y": 3.3664624405251735, + "heading": -0.5056820412512726, + "angularVelocity": 0.6923455933948506, + "velocityX": 0.2779254273134932, + "velocityY": -0.490656766910279, + "timestamp": 1.037546668718932 + }, + { + "x": 1.436809995729507, + "y": 3.2767232226728136, + "heading": -0.36592886657710183, + "angularVelocity": 0.808174777547772, + "velocityX": 0.3285028819178693, + "velocityY": -0.5189504466638519, + "timestamp": 1.2104711135054207 + }, + { + "x": 1.5024117560477557, + "y": 3.186433452722628, + "heading": -0.2093004084573229, + "angularVelocity": 0.9057623884898333, + "velocityX": 0.3793666095115132, + "velocityY": -0.5221342191413152, + "timestamp": 1.3833955582919093 + }, + { + "x": 1.575388792180146, + "y": 3.100609774230474, + "heading": -0.043535810037648476, + "angularVelocity": 0.9585955220739105, + "velocityX": 0.4220168884847946, + "velocityY": -0.49630738203272023, + "timestamp": 1.556320003078398 + }, + { + "x": 1.6534260730218766, + "y": 3.0220856356001904, + "heading": 0.1259077975683872, + "angularVelocity": 0.9798707627458886, + "velocityX": 0.4512796379646867, + "velocityY": -0.4540950743631267, + "timestamp": 1.7292444478648865 + }, + { + "x": 1.7350915783546517, + "y": 2.9526358276231015, + "heading": 0.29512622138663475, + "angularVelocity": 0.978568553623787, + "velocityX": 0.4722611972532152, + "velocityY": -0.401619378011739, + "timestamp": 1.9021688926513751 + }, + { + "x": 1.819540714989021, + "y": 2.893956167629996, + "heading": 0.4612947569724265, + "angularVelocity": 0.9609314391291418, + "velocityX": 0.48835858190283854, + "velocityY": -0.33933698641658866, + "timestamp": 2.0750933374378637 + }, + { + "x": 1.906146645544586, + "y": 2.847636222828339, + "heading": 0.6223817773774516, + "angularVelocity": 0.9315456852260934, + "velocityX": 0.5008310459701584, + "velocityY": -0.2678623304812785, + "timestamp": 2.2480177822243523 + }, + { + "x": 1.9665251142175801, + "y": 2.8219435002912077, + "heading": 0.7304227409187306, + "angularVelocity": 0.9070944374295804, + "velocityX": 0.5069278473953359, + "velocityY": -0.21571193983857168, + "timestamp": 2.367124414372505 + }, + { + "x": 2.027199417328043, + "y": 2.802535781230278, + "heading": 0.8348605982998051, + "angularVelocity": 0.8768433412411867, + "velocityX": 0.5094116252951654, + "velocityY": -0.16294406809191087, + "timestamp": 2.486231046520658 + }, + { + "x": 2.0877233838067433, + "y": 2.7894495205117447, + "heading": 0.9349982104943089, + "angularVelocity": 0.8407391786537501, + "velocityX": 0.5081494230967811, + "velocityY": -0.10987012719992924, + "timestamp": 2.605337678668811 + }, + { + "x": 2.147643915001536, + "y": 2.7826815563505396, + "heading": 1.0301447309723348, + "angularVelocity": 0.7988347804694407, + "velocityX": 0.5030830779492154, + "velocityY": -0.05682273215515476, + "timestamp": 2.724444310816964 + }, + { + "x": 2.20651048737849, + "y": 2.782188581989056, + "heading": 1.1196211525892386, + "angularVelocity": 0.7512295482688252, + "velocityX": 0.4942342101420168, + "velocityY": -0.004138933646610826, + "timestamp": 2.8435509429651167 + }, + { + "x": 2.263883758606366, + "y": 2.78789118528915, + "heading": 1.2027687081800864, + "angularVelocity": 0.6980934136595841, + "velocityX": 0.4816966952555762, + "velocityY": 0.047878133400203145, + "timestamp": 2.9626575751132695 + }, + { + "x": 2.3193412784170198, + "y": 2.799681571659277, + "heading": 1.2789478693283394, + "angularVelocity": 0.6395879020676446, + "velocityX": 0.4656123563275106, + "velocityY": 0.09899017450798987, + "timestamp": 3.0817642072614224 + }, + { + "x": 2.372480995706151, + "y": 2.817429968176388, + "heading": 1.3475588619550296, + "angularVelocity": 0.5760467844787401, + "velocityX": 0.4461524614429469, + "velocityY": 0.1490126631712517, + "timestamp": 3.2008708394095753 + }, + { + "x": 2.4229223212854074, + "y": 2.8409891725170064, + "heading": 1.4080386482590237, + "angularVelocity": 0.5077784927315084, + "velocityX": 0.4234972013791647, + "velocityY": 0.19779926497358047, + "timestamp": 3.319977471557728 + }, + { + "x": 2.470306439114864, + "y": 2.8701952854845723, + "heading": 1.45984182318626, + "angularVelocity": 0.434931068507544, + "velocityX": 0.397829379968852, + "velocityY": 0.2452097956790979, + "timestamp": 3.439084103705881 + }, + { + "x": 2.5142942062154736, + "y": 2.904872210821119, + "heading": 1.5024339139907708, + "angularVelocity": 0.3575962976843553, + "velocityX": 0.3693141690481337, + "velocityY": 0.2911418507417147, + "timestamp": 3.558190735854034 + }, + { + "x": 2.5545596425467716, + "y": 2.944842709921586, + "heading": 1.5353104282168073, + "angularVelocity": 0.2760258899137462, + "velocityX": 0.3380620825190745, + "velocityY": 0.33558583920682944, + "timestamp": 3.6772973680021868 + }, + { + "x": 2.5907808083346966, + "y": 2.9899393707705153, + "heading": 1.5580188767710943, + "angularVelocity": 0.19065645700034164, + "velocityX": 0.30410704180422304, + "velocityY": 0.3786242635368564, + "timestamp": 3.7964040001503396 + }, + { + "x": 2.6226553892163267, + "y": 3.0399773739572353, + "heading": 1.5699999995373906, + "angularVelocity": 0.1005915667162924, + "velocityX": 0.26761381867758166, + "velocityY": 0.42011097406612685, + "timestamp": 3.9155106322984925 + }, + { + "x": 2.6499999999952943, + "y": 3.0946526527631186, + "heading": 1.5699999995375231, + "angularVelocity": 1.6268742139378163e-13, + "velocityX": 0.22958092316607345, + "velocityY": 0.45904478970404045, + "timestamp": 4.034617264446646 + }, + { + "x": 2.677778603556993, + "y": 3.1876184634557494, + "heading": 1.5699999996915948, + "angularVelocity": 3.4878134565245447e-9, + "velocityX": 0.15720141808521537, + "velocityY": 0.526101218742126, + "timestamp": 4.211324345228364 + }, + { + "x": 2.694302289976671, + "y": 3.2907293636552515, + "heading": 1.569999999845665, + "angularVelocity": 3.487804657098119e-9, + "velocityX": 0.09350890981424996, + "velocityY": 0.5835131200339685, + "timestamp": 4.388031426010083 + }, + { + "x": 2.7009127461581475, + "y": 3.402378723740959, + "heading": 1.5699999999997312, + "angularVelocity": 3.487781847838159e-9, + "velocityX": 0.037409118885215344, + "velocityY": 0.6318329713426234, + "timestamp": 4.564738506791802 + }, + { + "x": 2.6987671383912533, + "y": 3.5209211039854664, + "heading": 1.570000000153807, + "angularVelocity": 3.4878366830559997e-9, + "velocityX": -0.012142172119808498, + "velocityY": 0.6708411430749418, + "timestamp": 4.74144558757352 + }, + { + "x": 2.6888828222740733, + "y": 3.644196175998938, + "heading": 1.5700000003078753, + "angularVelocity": 3.4877937223103217e-9, + "velocityX": -0.055936163456970826, + "velocityY": 0.6976238377085905, + "timestamp": 4.918152668355239 + }, + { + "x": 2.6722022855076024, + "y": 3.76625962058142, + "heading": 1.5700000004619454, + "angularVelocity": 3.4878037554984787e-9, + "velocityX": -0.0943965389284013, + "velocityY": 0.6907671381579374, + "timestamp": 5.094859749136957 + }, + { + "x": 2.657986617113385, + "y": 3.879155032674401, + "heading": 1.570000000616029, + "angularVelocity": 3.4878815132726272e-9, + "velocityX": -0.08044764465302807, + "velocityY": 0.6388844831997175, + "timestamp": 5.271566829918676 + }, + { + "x": 2.6500000000052806, + "y": 3.9813501834674954, + "heading": 1.5700000004617618, + "angularVelocity": 1.7432220039690896e-9, + "velocityX": -0.04519692738746778, + "velocityY": 0.5783308186700414, + "timestamp": 5.448273910700395 + }, + { + "x": 2.6452136818359064, + "y": 4.05360530425828, + "heading": 1.53966341048279, + "angularVelocity": -0.2382427016177502, + "velocityX": -0.03758844925284465, + "velocityY": 0.5674419970250193, + "timestamp": 5.575608724119742 + }, + { + "x": 2.6414058365018693, + "y": 4.124344962259836, + "heading": 1.4805030233227192, + "angularVelocity": -0.4646049696157924, + "velocityX": -0.029904196935062462, + "velocityY": 0.5555405950865885, + "timestamp": 5.702943537539089 + }, + { + "x": 2.638726148869199, + "y": 4.1926740124341215, + "heading": 1.3885239036867012, + "angularVelocity": -0.7223407103550541, + "velocityX": -0.02104442268552982, + "velocityY": 0.5366093399943983, + "timestamp": 5.830278350958436 + }, + { + "x": 2.6371495018803164, + "y": 4.258402426861184, + "heading": 1.2664490199394642, + "angularVelocity": -0.9586921313529179, + "velocityX": -0.012381900523162376, + "velocityY": 0.5161857363964165, + "timestamp": 5.957613164377784 + }, + { + "x": 2.6367566351923606, + "y": 4.321249747197685, + "heading": 1.117526343536387, + "angularVelocity": -1.1695362196212704, + "velocityX": -0.003085304648377667, + "velocityY": 0.4935596059814381, + "timestamp": 6.084947977797131 + }, + { + "x": 2.6379461563733613, + "y": 4.380543504144595, + "heading": 0.9470242990843019, + "angularVelocity": -1.3390057264463304, + "velocityX": 0.009341680772761525, + "velocityY": 0.46565236445510094, + "timestamp": 6.212282791216478 + }, + { + "x": 2.640763664026479, + "y": 4.4348680141998, + "heading": 0.7637352361799904, + "angularVelocity": -1.439426169991059, + "velocityX": 0.022126766227508896, + "velocityY": 0.4266273189098902, + "timestamp": 6.3396176046358255 + }, + { + "x": 2.6449181726826847, + "y": 4.483606188164319, + "heading": 0.5746573747577438, + "angularVelocity": -1.4848874101138032, + "velocityX": 0.032626652054096365, + "velocityY": 0.3827560794609454, + "timestamp": 6.466952418055173 + }, + { + "x": 2.6500836019606915, + "y": 4.5266472095485755, + "heading": 0.3868086488776482, + "angularVelocity": -1.4752346255840696, + "velocityX": 0.0405657269652761, + "velocityY": 0.33801456355338955, + "timestamp": 6.59428723147452 + }, + { + "x": 2.6558464279561003, + "y": 4.564412547861051, + "heading": 0.20759285673582817, + "angularVelocity": -1.4074375050153292, + "velocityX": 0.0452572697009629, + "velocityY": 0.2965829791062329, + "timestamp": 6.721622044893867 + }, + { + "x": 2.661778734291967, + "y": 4.59791659515814, + "heading": 0.04509455516787029, + "angularVelocity": -1.2761498384118317, + "velocityX": 0.046588251681661745, + "velocityY": 0.263117731793826, + "timestamp": 6.8489568583132145 + }, + { + "x": 2.6675521800942206, + "y": 4.628154547719805, + "heading": -0.09493357899778786, + "angularVelocity": -1.0996846065360344, + "velocityX": 0.04534067037561326, + "velocityY": 0.2374680712369913, + "timestamp": 6.976291671732562 + }, + { + "x": 2.6730273366267054, + "y": 4.655759180593734, + "heading": -0.20883881516584757, + "angularVelocity": -0.8945333420741611, + "velocityX": 0.04299811171875369, + "velocityY": 0.2167877904868582, + "timestamp": 7.103626485151909 + }, + { + "x": 2.678138048749673, + "y": 4.6811080520674, + "heading": -0.2939465026251688, + "angularVelocity": -0.6683772114260695, + "velocityX": 0.040136016118023574, + "velocityY": 0.19907259272871744, + "timestamp": 7.230961298571256 + }, + { + "x": 2.6828300625819463, + "y": 4.704445666215685, + "heading": -0.34796884329444056, + "angularVelocity": -0.4242542889593211, + "velocityX": 0.036847847868648385, + "velocityY": 0.18327756173208992, + "timestamp": 7.358296111990604 + }, + { + "x": 2.6870489868716323, + "y": 4.725946330962835, + "heading": -0.3688433898724138, + "angularVelocity": -0.16393432463684127, + "velocityX": 0.033132528155050814, + "velocityY": 0.16885142523195418, + "timestamp": 7.485630925409951 + }, + { + "x": 2.690741538999899, + "y": 4.745745658881827, + "heading": -0.3547054524849317, + "angularVelocity": 0.11102963078930123, + "velocityX": 0.02899876341795603, + "velocityY": 0.15549029656562582, + "timestamp": 7.612965738829298 + }, + { + "x": 2.6934607424234462, + "y": 4.761417510798841, + "heading": -0.3176562911177192, + "angularVelocity": 0.34403603709867575, + "velocityX": 0.025250341412210373, + "velocityY": 0.1455277699022493, + "timestamp": 7.720655507548441 + }, + { + "x": 2.6957976390226768, + "y": 4.776182991755986, + "heading": -0.2567351376272597, + "angularVelocity": 0.5657097616347546, + "velocityX": 0.021700265768873628, + "velocityY": 0.13711126973577806, + "timestamp": 7.8283452762675845 + }, + { + "x": 2.6977687917894335, + "y": 4.7902101666937575, + "heading": -0.17317582068144213, + "angularVelocity": 0.7759262355558584, + "velocityX": 0.018303992960618302, + "velocityY": 0.1302554093821528, + "timestamp": 7.936035044986728 + }, + { + "x": 2.699387576905781, + "y": 4.803677821280753, + "heading": -0.06825731088311157, + "angularVelocity": 0.9742662738687601, + "velocityX": 0.015031930485161661, + "velocityY": 0.125059740805471, + "timestamp": 8.043724813705872 + }, + { + "x": 2.700669750016591, + "y": 4.816787270497459, + "heading": 0.05667241036907346, + "angularVelocity": 1.1600890469331555, + "velocityX": 0.01190617387623651, + "velocityY": 0.12173346969049074, + "timestamp": 8.151414582425016 + }, + { + "x": 2.701638921088602, + "y": 4.829788325532351, + "heading": 0.20013160725212134, + "angularVelocity": 1.332152520098241, + "velocityX": 0.008999657843294685, + "velocityY": 0.12072692858580873, + "timestamp": 8.25910435114416 + }, + { + "x": 2.7023270420759418, + "y": 4.843037991339417, + "heading": 0.3603318680041595, + "angularVelocity": 1.4876089210741354, + "velocityX": 0.0063898455239930225, + "velocityY": 0.12303551163966073, + "timestamp": 8.366794119863304 + }, + { + "x": 2.7027342342087275, + "y": 4.8571699423090635, + "heading": 0.534542545009005, + "angularVelocity": 1.6177087106318269, + "velocityX": 0.003781158937186622, + "velocityY": 0.13122835275947514, + "timestamp": 8.474483888582448 + }, + { + "x": 2.70256595507304, + "y": 4.874565727425271, + "heading": 0.7112936152404646, + "angularVelocity": 1.6412986324126229, + "velocityX": -0.0015626288292674016, + "velocityY": 0.16153609867638755, + "timestamp": 8.582173657301592 + }, + { + "x": 2.7023906866424063, + "y": 4.895151112055553, + "heading": 0.8857476045173669, + "angularVelocity": 1.6199680921307593, + "velocityX": -0.0016275309601235012, + "velocityY": 0.1911545067457904, + "timestamp": 8.689863426020736 + }, + { + "x": 2.702514003300952, + "y": 4.918783664787565, + "heading": 1.0519954165670216, + "angularVelocity": 1.543766078541766, + "velocityX": 0.0011451102305272795, + "velocityY": 0.21945030624338774, + "timestamp": 8.79755319473988 + }, + { + "x": 2.703054335679489, + "y": 4.945063560715068, + "heading": 1.205028220918099, + "angularVelocity": 1.4210523997151632, + "velocityX": 0.005017490356734875, + "velocityY": 0.24403335836798773, + "timestamp": 8.905242963459024 + }, + { + "x": 2.7039518227222876, + "y": 4.973866114721659, + "heading": 1.3418780223624793, + "angularVelocity": 1.2707781155821471, + "velocityX": 0.008334004718619866, + "velocityY": 0.26745859282506784, + "timestamp": 9.012932732178168 + }, + { + "x": 2.705155611038573, + "y": 5.005199432371511, + "heading": 1.4601391056217587, + "angularVelocity": 1.0981645200204058, + "velocityX": 0.011178297889957256, + "velocityY": 0.2909590950064466, + "timestamp": 9.120622500897312 + }, + { + "x": 2.7075475883170625, + "y": 5.057800568431657, + "heading": 1.5846330579194456, + "angularVelocity": 0.7815302239817514, + "velocityX": 0.015016010847101066, + "velocityY": 0.33021184476934917, + "timestamp": 9.279917622780061 + }, + { + "x": 2.7106355946768605, + "y": 5.11553191139856, + "heading": 1.6606890009799455, + "angularVelocity": 0.4774530579023228, + "velocityX": 0.019385442083753358, + "velocityY": 0.36241751965678165, + "timestamp": 9.439212744662811 + }, + { + "x": 2.714603113036703, + "y": 5.177430582712724, + "heading": 1.689552850964268, + "angularVelocity": 0.1811973250635666, + "velocityX": 0.02490671599190395, + "velocityY": 0.38857857404297325, + "timestamp": 9.59850786654556 + }, + { + "x": 2.719666892119011, + "y": 5.242655325525797, + "heading": 1.6720167349941795, + "angularVelocity": -0.11008570608420444, + "velocityX": 0.03178866384838745, + "velocityY": 0.40945850720964916, + "timestamp": 9.75780298842831 + }, + { + "x": 2.7260456700000577, + "y": 5.310428510061647, + "heading": 1.6086050162662178, + "angularVelocity": -0.3980769653911309, + "velocityX": 0.04004377413266913, + "velocityY": 0.4254567480373902, + "timestamp": 9.91709811031106 + }, + { + "x": 2.733983755111467, + "y": 5.379965782164736, + "heading": 1.4994890257959146, + "angularVelocity": -0.6849926674968776, + "velocityX": 0.0498325687430904, + "velocityY": 0.4365310831844459, + "timestamp": 10.076393232193809 + }, + { + "x": 2.7403519987836336, + "y": 5.428782764015629, + "heading": 1.4030227598298415, + "angularVelocity": -0.86907069684122, + "velocityX": 0.05737191041991601, + "velocityY": 0.43979527998539214, + "timestamp": 10.187392563175258 + }, + { + "x": 2.7474416272846045, + "y": 5.477928317505554, + "heading": 1.2877775765276958, + "angularVelocity": -1.0382511519468594, + "velocityX": 0.06387091201629533, + "velocityY": 0.4427554026788899, + "timestamp": 10.298391894156708 + }, + { + "x": 2.755099437239571, + "y": 5.527335311414416, + "heading": 1.1556172324559857, + "angularVelocity": -1.1906409061595067, + "velocityX": 0.06898969470888307, + "velocityY": 0.44511073595952727, + "timestamp": 10.409391225138158 + }, + { + "x": 2.763073314371291, + "y": 5.576891966790131, + "heading": 1.008839937166576, + "angularVelocity": -1.3223259450318314, + "velocityX": 0.07183716391577571, + "velocityY": 0.4464590455826599, + "timestamp": 10.520390556119608 + }, + { + "x": 2.770756458926381, + "y": 5.626388669002003, + "heading": 0.8509533533965823, + "angularVelocity": -1.4224102282551414, + "velocityX": 0.06921793572996068, + "velocityY": 0.44591892372393505, + "timestamp": 10.631389887101058 + }, + { + "x": 2.7765929371018587, + "y": 5.6751833178331985, + "heading": 0.6976742957975531, + "angularVelocity": -1.380900733356889, + "velocityX": 0.0525812014042681, + "velocityY": 0.43959408035749586, + "timestamp": 10.742389218082508 + }, + { + "x": 2.7812148969457993, + "y": 5.723355185159096, + "heading": 0.5561189909540492, + "angularVelocity": -1.2752807026531834, + "velocityX": 0.041639528855491686, + "velocityY": 0.43398340233557375, + "timestamp": 10.853388549063958 + }, + { + "x": 2.7846341541044826, + "y": 5.770917360704941, + "heading": 0.4290947667682521, + "angularVelocity": -1.1443692771461944, + "velocityX": 0.030804304212884475, + "velocityY": 0.42849065047935536, + "timestamp": 10.964387880045408 + }, + { + "x": 2.7868036331209676, + "y": 5.817867178043778, + "heading": 0.31870033092567784, + "angularVelocity": -0.9945504612345583, + "velocityX": 0.019544973798314574, + "velocityY": 0.42297387661508384, + "timestamp": 11.075387211026857 + }, + { + "x": 2.7876739995139346, + "y": 5.8641980426984786, + "heading": 0.22669269944147122, + "angularVelocity": -0.828902576809249, + "velocityX": 0.007841185918062894, + "velocityY": 0.4173976926896044, + "timestamp": 11.186386542008307 + }, + { + "x": 2.787199598907597, + "y": 5.909896954976253, + "heading": 0.15461009075553808, + "angularVelocity": -0.649396785077588, + "velocityX": -0.00427390512415645, + "velocityY": 0.41170439370904605, + "timestamp": 11.297385872989757 + }, + { + "x": 2.785334928758619, + "y": 5.954945308815798, + "heading": 0.10384044901782247, + "angularVelocity": -0.45738691663173364, + "velocityX": -0.016798931460352096, + "velocityY": 0.40584347154427963, + "timestamp": 11.408385203971207 + }, + { + "x": 2.7820296460936547, + "y": 5.99932303663678, + "heading": 0.0756680850519561, + "angularVelocity": -0.2538066106695057, + "velocityX": -0.029777500831199538, + "velocityY": 0.39980175927762673, + "timestamp": 11.519384534952657 + }, + { + "x": 2.777225971223495, + "y": 6.043013572694398, + "heading": 0.07130738082260937, + "angularVelocity": -0.03928586070851831, + "velocityX": -0.04327661108101771, + "velocityY": 0.39361080516708935, + "timestamp": 11.630383865934107 + }, + { + "x": 2.7708025416380804, + "y": 6.086228577980418, + "heading": 0.0924405017308609, + "angularVelocity": 0.1894267867463071, + "velocityX": -0.0575764289833756, + "velocityY": 0.38735781825339893, + "timestamp": 11.741947395050046 + }, + { + "x": 2.7631104838036147, + "y": 6.128747951743283, + "heading": 0.13792413323079813, + "angularVelocity": 0.407692655411535, + "velocityX": -0.06894778181538898, + "velocityY": 0.3811225231531768, + "timestamp": 11.853510924165985 + }, + { + "x": 2.754514263518267, + "y": 6.170565147177378, + "heading": 0.20653308055445607, + "angularVelocity": 0.6149764874177278, + "velocityX": -0.0770522444451208, + "velocityY": 0.37482854574377555, + "timestamp": 11.965074453281924 + }, + { + "x": 2.745440052449635, + "y": 6.211648913048854, + "heading": 0.29697222751905, + "angularVelocity": 0.8106515419634385, + "velocityX": -0.08133671613909955, + "velocityY": 0.36825444781340366, + "timestamp": 12.076637982397862 + }, + { + "x": 2.7363923538047996, + "y": 6.2519737139034355, + "heading": 0.40783083793060415, + "angularVelocity": 0.9936814587406215, + "velocityX": -0.08109907192317244, + "velocityY": 0.36145146276694384, + "timestamp": 12.188201511513801 + }, + { + "x": 2.7280592025191917, + "y": 6.291485229850295, + "heading": 0.5371181653698212, + "angularVelocity": 1.1588673144286004, + "velocityX": -0.07469422463560056, + "velocityY": 0.35416158122392677, + "timestamp": 12.29976504062974 + }, + { + "x": 2.721516703521013, + "y": 6.329946640679988, + "heading": 0.6809409115442911, + "angularVelocity": 1.2891555796425005, + "velocityX": -0.05864370777231024, + "velocityY": 0.34474896102184066, + "timestamp": 12.41132856974568 + }, + { + "x": 2.7186115194936558, + "y": 6.367345324047673, + "heading": 0.8310949021421773, + "angularVelocity": 1.3459057057109145, + "velocityX": -0.02604062490013896, + "velocityY": 0.3352232005464074, + "timestamp": 12.522892098861618 + }, + { + "x": 2.7201626165622823, + "y": 6.403993224874882, + "heading": 0.9826164331350509, + "angularVelocity": 1.3581636572914138, + "velocityX": 0.013903263009787203, + "velocityY": 0.3284935592311268, + "timestamp": 12.634455627977557 + }, + { + "x": 2.7265981550401013, + "y": 6.440150870382165, + "heading": 1.131579259128451, + "angularVelocity": 1.3352286976688623, + "velocityX": 0.05768496704140498, + "velocityY": 0.32409915437288567, + "timestamp": 12.746019157093496 + }, + { + "x": 2.7381546633881, + "y": 6.476016938175864, + "heading": 1.2747563358948757, + "angularVelocity": 1.283368118388856, + "velocityX": 0.10358679458289283, + "velocityY": 0.3214855967802891, + "timestamp": 12.857582686209435 + }, + { + "x": 2.7549876221362974, + "y": 6.511747596731161, + "heading": 1.4092327359199672, + "angularVelocity": 1.2053795784590957, + "velocityX": 0.1508822722947548, + "velocityY": 0.3202718557414932, + "timestamp": 12.969146215325374 + }, + { + "x": 2.777225971219294, + "y": 6.547506809233031, + "heading": 1.532353785638737, + "angularVelocity": 1.1035958648685134, + "velocityX": 0.19933350348528897, + "velocityY": 0.3205277991599169, + "timestamp": 13.080709744441313 + }, + { + "x": 2.8283739264126173, + "y": 6.60500051454071, + "heading": 1.6910426580508369, + "angularVelocity": 0.8908309216248135, + "velocityX": 0.2871290177610515, + "velocityY": 0.32275212312664076, + "timestamp": 13.258845537171021 + }, + { + "x": 2.894537518108946, + "y": 6.662949818763498, + "heading": 1.8139189760032997, + "angularVelocity": 0.6897901655205115, + "velocityX": 0.37142222056386526, + "velocityY": 0.32530971645546014, + "timestamp": 13.43698132990073 + }, + { + "x": 2.975118854402208, + "y": 6.72132005713001, + "heading": 1.9033257225678124, + "angularVelocity": 0.5019022016528627, + "velocityX": 0.45235904059312243, + "velocityY": 0.3276727123322237, + "timestamp": 13.615117122630439 + }, + { + "x": 3.069506354210641, + "y": 6.779993578108028, + "heading": 1.9614530049830199, + "angularVelocity": 0.3263088317299888, + "velocityX": 0.5298626309923767, + "velocityY": 0.32937524839676924, + "timestamp": 13.793252915360148 + }, + { + "x": 3.177057501791681, + "y": 6.838789011583473, + "heading": 1.990113992675826, + "angularVelocity": 0.16089404186178405, + "velocityX": 0.6037593340125262, + "velocityY": 0.33005962796787114, + "timestamp": 13.971388708089856 + }, + { + "x": 3.2970934581179465, + "y": 6.897474588900029, + "heading": 1.9906701857847593, + "angularVelocity": 0.0031222984451791374, + "velocityX": 0.6738452418130287, + "velocityY": 0.32944292900353384, + "timestamp": 14.149524500819565 + }, + { + "x": 3.428875798780963, + "y": 6.955773952952954, + "heading": 1.9640246482498747, + "angularVelocity": -0.14957991949193578, + "velocityX": 0.7397858602409795, + "velocityY": 0.32727484555599373, + "timestamp": 14.327660293549274 + }, + { + "x": 3.5715750772701043, + "y": 7.013357345374212, + "heading": 1.9106160532989442, + "angularVelocity": -0.29981955974577057, + "velocityX": 0.8010702189812141, + "velocityY": 0.3232555992232119, + "timestamp": 14.505796086278982 + }, + { + "x": 3.7242209599931746, + "y": 7.069819180114435, + "heading": 1.830377783516494, + "angularVelocity": -0.4504331698484236, + "velocityX": 0.8569074209354516, + "velocityY": 0.31695951653343485, + "timestamp": 14.683931879008691 + }, + { + "x": 3.885616535722601, + "y": 7.1246504487670705, + "heading": 1.7226603342509128, + "angularVelocity": -0.6046929009336263, + "velocityX": 0.9060255283762794, + "velocityY": 0.307806015922469, + "timestamp": 14.8620676717384 + }, + { + "x": 4.05416552018741, + "y": 7.177233618565563, + "heading": 1.5860732407671714, + "angularVelocity": -0.7667582768795543, + "velocityX": 0.9461825828669139, + "velocityY": 0.29518587473863683, + "timestamp": 15.040203464468108 + }, + { + "x": 4.2273936305200035, + "y": 7.226946789532177, + "heading": 1.417957586567635, + "angularVelocity": -0.9437499989398298, + "velocityX": 0.972449768119705, + "velocityY": 0.27907457679060355, + "timestamp": 15.218339257197817 + }, + { + "x": 4.400615134292295, + "y": 7.271518848059685, + "heading": 1.2225365130610886, + "angularVelocity": -1.0970342934019721, + "velocityX": 0.9724126809054928, + "velocityY": 0.2502139398555887, + "timestamp": 15.396475049927526 + }, + { + "x": 4.569586786239567, + "y": 7.310085040153136, + "heading": 1.0159093047800025, + "angularVelocity": -1.1599421155910052, + "velocityX": 0.9485553091785852, + "velocityY": 0.2164988377864223, + "timestamp": 15.574610842657234 + }, + { + "x": 4.731916077731233, + "y": 7.343905863563497, + "heading": 0.8050006957827572, + "angularVelocity": -1.183976593168793, + "velocityX": 0.9112671238351473, + "velocityY": 0.18985978557591154, + "timestamp": 15.752746635386943 + }, + { + "x": 4.885947895624515, + "y": 7.373460799757093, + "heading": 0.5949879079305361, + "angularVelocity": -1.1789477265311938, + "velocityX": 0.8646876382009842, + "velocityY": 0.16591239605136865, + "timestamp": 15.930882428116652 + }, + { + "x": 5.030555674163505, + "y": 7.399083556924728, + "heading": 0.389293360584716, + "angularVelocity": -1.1547064416090447, + "velocityX": 0.8117839560724309, + "velocityY": 0.14383834251217997, + "timestamp": 16.10901822084636 + }, + { + "x": 5.164922112553356, + "y": 7.42103691269045, + "heading": 0.1902575355029938, + "angularVelocity": -1.117326406063954, + "velocityX": 0.754292196599486, + "velocityY": 0.12323944239401181, + "timestamp": 16.287154013576068 + }, + { + "x": 5.288247156990128, + "y": 7.439470298396178, + "heading": -7.485335201745413e-8, + "angularVelocity": -1.0680481863931495, + "velocityX": 0.692309179127921, + "velocityY": 0.10347940424527421, + "timestamp": 16.465289806305776 + }, + { + "x": 5.288247150924154, + "y": 7.439470297488981, + "heading": -6.549675275293239e-8, + "angularVelocity": -1.0680481909285577, + "velocityX": 0.6923091791495746, + "velocityY": 0.1034794048722287, + "timestamp": 16.465289797545616 + }, + { + "x": 5.288247144858181, + "y": 7.439470296581783, + "heading": -5.614015360341729e-8, + "angularVelocity": -1.0680481958850412, + "velocityX": 0.6923091791998811, + "velocityY": 0.10347940550159965, + "timestamp": 16.465289788785455 + }, + { + "x": 5.288247138792208, + "y": 7.439470295674586, + "heading": -4.678355456587365e-8, + "angularVelocity": -1.068048201182234, + "velocityX": 0.6923091792135615, + "velocityY": 0.10347940615791178, + "timestamp": 16.465289780025294 + }, + { + "x": 5.288247132726235, + "y": 7.439470294767388, + "heading": -3.742695563880682e-8, + "angularVelocity": -1.0680482066432966, + "velocityX": 0.6923091791656869, + "velocityY": 0.10347940690651845, + "timestamp": 16.465289771265134 + }, + { + "x": 5.288247126660261, + "y": 7.43947029386019, + "heading": -2.8070356820536066e-8, + "angularVelocity": -1.0680482122907076, + "velocityX": 0.6923091790890028, + "velocityY": 0.10347940774772568, + "timestamp": 16.465289762504973 + }, + { + "x": 5.2882471205942885, + "y": 7.4394702929529934, + "heading": -1.871375810835249e-8, + "angularVelocity": -1.0680482182439734, + "velocityX": 0.6923091790087366, + "velocityY": 0.10347940865836429, + "timestamp": 16.465289753744813 + }, + { + "x": 5.288247114528315, + "y": 7.439470292045796, + "heading": -9.357159497718974e-9, + "angularVelocity": -1.0680482247148153, + "velocityX": 0.692309178935693, + "velocityY": 0.10347940958549313, + "timestamp": 16.465289744984652 + }, + { + "x": 5.2882471084623415, + "y": 7.439470291138599, + "heading": -5.609846744710123e-13, + "angularVelocity": -1.0680482316377529, + "velocityX": 0.6923091788453621, + "velocityY": 0.10347941057644157, + "timestamp": 16.46528973622449 + }, + { + "x": 5.418318713107553, + "y": 7.45524667726722, + "heading": -0.14765149788980822, + "angularVelocity": -0.9184906932543151, + "velocityX": 0.8091320441513836, + "velocityY": 0.09813963317439353, + "timestamp": 16.626044220713585 + }, + { + "x": 5.563548987825593, + "y": 7.4700941218212495, + "heading": -0.27422768048539375, + "angularVelocity": -0.7873881901546119, + "velocityX": 0.9034290718221242, + "velocityY": 0.09236099763780091, + "timestamp": 16.786798705202678 + }, + { + "x": 5.720519992249432, + "y": 7.483950977822062, + "heading": -0.3825207221223279, + "angularVelocity": -0.6736548717787152, + "velocityX": 0.9764642331243513, + "velocityY": 0.08619887682187297, + "timestamp": 16.94755318969177 + }, + { + "x": 5.891579981174876, + "y": 7.497541590590127, + "heading": -0.47714146755787396, + "angularVelocity": -0.588604079901089, + "velocityX": 1.0641071038395116, + "velocityY": 0.08454266651145323, + "timestamp": 17.108307674180864 + }, + { + "x": 6.070777918835097, + "y": 7.508754325284448, + "heading": -0.5566348918765167, + "angularVelocity": -0.4945020636066715, + "velocityX": 1.1147305670618215, + "velocityY": 0.06975068056649593, + "timestamp": 17.269062158669957 + }, + { + "x": 6.2553343016634635, + "y": 7.518023579202175, + "heading": -0.6170518299221113, + "angularVelocity": -0.37583360883180356, + "velocityX": 1.148063666191382, + "velocityY": 0.057660935241979815, + "timestamp": 17.42981664315905 + }, + { + "x": 6.442537822955601, + "y": 7.525844356758643, + "heading": -0.6536325541269659, + "angularVelocity": -0.22755647733647813, + "velocityX": 1.1645306311553023, + "velocityY": 0.04865044719085291, + "timestamp": 17.590571127648143 + }, + { + "x": 6.629868252624434, + "y": 7.5325663261121605, + "heading": -0.6631454099336127, + "angularVelocity": -0.05917630132092225, + "velocityX": 1.165320085817266, + "velocityY": 0.04181512805856416, + "timestamp": 17.751325612137236 + }, + { + "x": 6.814379245318763, + "y": 7.537436085797625, + "heading": -0.6469315275958195, + "angularVelocity": 0.1008611509586783, + "velocityX": 1.1477813093220908, + "velocityY": 0.030293149836502442, + "timestamp": 17.91208009662633 + }, + { + "x": 6.991679035452718, + "y": 7.540305977691209, + "heading": -0.6170030764514597, + "angularVelocity": 0.1861749066087869, + "velocityX": 1.1029228247693241, + "velocityY": 0.01785263970713557, + "timestamp": 18.072834581115423 + }, + { + "x": 7.159447389396387, + "y": 7.541387264231915, + "heading": -0.5764605353737351, + "angularVelocity": 0.2522016179067287, + "velocityX": 1.0436309411262277, + "velocityY": 0.006726322705293858, + "timestamp": 18.233589065604516 + }, + { + "x": 7.315440984587148, + "y": 7.540720415566423, + "heading": -0.5273581032751606, + "angularVelocity": 0.3054498433093596, + "velocityX": 0.9703840964748058, + "velocityY": -0.004148243019810784, + "timestamp": 18.39434355009361 + }, + { + "x": 7.470574266382935, + "y": 7.537764395855196, + "heading": -0.4641850757014082, + "angularVelocity": 0.3546341172959247, + "velocityX": 0.8708709486472784, + "velocityY": -0.016594193461135685, + "timestamp": 18.572479337072245 + }, + { + "x": 7.604749650146638, + "y": 7.532550172079166, + "heading": -0.39417620645558443, + "angularVelocity": 0.3930084484061484, + "velocityX": 0.7532196985108242, + "velocityY": -0.029271062624336028, + "timestamp": 18.75061512405088 + }, + { + "x": 7.714483814910743, + "y": 7.524998903950903, + "heading": -0.31905072154824177, + "angularVelocity": 0.4217315688319769, + "velocityX": 0.6160141464196879, + "velocityY": -0.04239051712518056, + "timestamp": 18.928750911029518 + }, + { + "x": 7.796027577811261, + "y": 7.515004663660351, + "heading": -0.24044153160037793, + "angularVelocity": 0.4412880268554748, + "velocityX": 0.45776182474014415, + "velocityY": -0.05610461805755441, + "timestamp": 19.106886698008154 + }, + { + "x": 7.845367727625598, + "y": 7.50244123092944, + "heading": -0.1599630926416538, + "angularVelocity": 0.4517814209261708, + "velocityX": 0.2769805587610722, + "velocityY": -0.07052728115034504, + "timestamp": 19.28502248498679 + }, + { + "x": 7.858234920893501, + "y": 7.487166681771978, + "heading": -0.07925291889731063, + "angularVelocity": 0.4530823093638088, + "velocityX": 0.07223250018704967, + "velocityY": -0.08574666223336481, + "timestamp": 19.463158271965426 + }, + { + "x": 7.830112934107491, + "y": 7.469026565551425, + "heading": 1.9541743370744396e-13, + "angularVelocity": 0.44490172492554647, + "velocityX": -0.157868260300945, + "velocityY": -0.10183308210267149, + "timestamp": 19.641294058944062 + }, + { + "x": 7.754138306062181, + "y": 7.447719681174328, + "heading": 0.07588851944221475, + "angularVelocity": 0.42601499150875954, + "velocityX": -0.4264983789132844, + "velocityY": -0.11961034730517565, + "timestamp": 19.819429854525456 + }, + { + "x": 7.639951170097708, + "y": 7.423862681107234, + "heading": 0.14871254480285817, + "angularVelocity": 0.40881185683628485, + "velocityX": -0.641011738198812, + "velocityY": -0.13392591864598205, + "timestamp": 19.99756565010685 + }, + { + "x": 7.496026995727258, + "y": 7.397999014635099, + "heading": 0.21873314599039687, + "angularVelocity": 0.39307428896591823, + "velocityX": -0.8079463978438458, + "velocityY": -0.14519073152928566, + "timestamp": 20.175701445688244 + }, + { + "x": 7.329549618949632, + "y": 7.370590094001231, + "heading": 0.2861417733138313, + "angularVelocity": 0.37841146471109305, + "velocityX": -0.934553194268821, + "velocityY": -0.15386531687407357, + "timestamp": 20.353837241269638 + }, + { + "x": 7.146249960822764, + "y": 7.342003057760445, + "heading": 0.3510012841680181, + "angularVelocity": 0.36410150269067454, + "velocityX": -1.0289883486090268, + "velocityY": -0.16047889840086563, + "timestamp": 20.53197303685103 + }, + { + "x": 6.9503179785639695, + "y": 7.312500714944177, + "heading": 0.4131524690164208, + "angularVelocity": 0.34889778691337076, + "velocityX": -1.0999023616556318, + "velocityY": -0.16561715021879247, + "timestamp": 20.710108832432425 + }, + { + "x": 6.744669290890069, + "y": 7.282249603442564, + "heading": 0.4720173085707148, + "angularVelocity": 0.3304492472283394, + "velocityX": -1.1544489808985139, + "velocityY": -0.16982050913888352, + "timestamp": 20.88824462801382 + }, + { + "x": 6.528580504402744, + "y": 7.251108531405175, + "heading": 0.5274928016956117, + "angularVelocity": 0.31142249059958194, + "velocityX": -1.2130565099431412, + "velocityY": -0.17481647602398917, + "timestamp": 21.066380423595213 + }, + { + "x": 6.313399103045075, + "y": 7.214538894676935, + "heading": 0.5275460333478232, + "angularVelocity": 0.0002988262523701674, + "velocityX": -1.2079627267049402, + "velocityY": -0.20529078172505943, + "timestamp": 21.244516219176607 + }, + { + "x": 6.120539072312893, + "y": 7.178889186765798, + "heading": 0.517735292666819, + "angularVelocity": -0.05507450453172011, + "velocityX": -1.0826573631611538, + "velocityY": -0.20012658205312084, + "timestamp": 21.422652014758 + }, + { + "x": 5.958628600692053, + "y": 7.144959716647863, + "heading": 0.5008124167051957, + "angularVelocity": -0.09499986179866582, + "velocityX": -0.9089159822650381, + "velocityY": -0.1904696919958043, + "timestamp": 21.600787810339394 + }, + { + "x": 5.837986516704409, + "y": 7.113426483231326, + "heading": 0.4771314716153743, + "angularVelocity": -0.1329375997257422, + "velocityX": -0.6772478467367095, + "velocityY": -0.1770179503423335, + "timestamp": 21.778923605920788 + }, + { + "x": 5.7227454917181495, + "y": 7.076203538098732, + "heading": 0.44815466228660306, + "angularVelocity": -0.1674411973555948, + "velocityX": -0.6659151113565552, + "velocityY": -0.2150911244418225, + "timestamp": 21.951980235828838 + }, + { + "x": 5.614010909083641, + "y": 7.033635647002007, + "heading": 0.41542486169539805, + "angularVelocity": -0.18912768961795748, + "velocityX": -0.6283179251047766, + "velocityY": -0.2459766557929015, + "timestamp": 22.125036865736888 + }, + { + "x": 5.516875858601351, + "y": 6.9866420715294, + "heading": 0.38017449454674707, + "angularVelocity": -0.2036926708051821, + "velocityX": -0.5612905470916604, + "velocityY": -0.27155027514399244, + "timestamp": 22.298093495644938 + }, + { + "x": 5.437243043942288, + "y": 6.935553672024848, + "heading": 0.3424575004944832, + "angularVelocity": -0.21794596411047518, + "velocityX": -0.46015465976713954, + "velocityY": -0.29521203278234875, + "timestamp": 22.471150125552988 + }, + { + "x": 5.382029638407054, + "y": 6.880562557170266, + "heading": 0.301962621519522, + "angularVelocity": -0.23399784796292267, + "velocityX": -0.3190481957280769, + "velocityY": -0.31776369898542106, + "timestamp": 22.64420675546104 + }, + { + "x": 5.359043767990556, + "y": 6.821859309651465, + "heading": 0.2582986739644723, + "angularVelocity": -0.25231016909100673, + "velocityX": -0.13282282465859874, + "velocityY": -0.3392140916541047, + "timestamp": 22.81726338536909 + }, + { + "x": 5.3769168853790115, + "y": 6.759668827057295, + "heading": 0.21109341192044304, + "angularVelocity": -0.27277349656177, + "velocityX": 0.10327900992250337, + "velocityY": -0.3593649236555796, + "timestamp": 22.99032001527714 + }, + { + "x": 5.453420781420232, + "y": 6.691937084119905, + "heading": 0.15775875752405638, + "angularVelocity": -0.2994044744273571, + "velocityX": 0.42946952680603284, + "velocityY": -0.3802253361498929, + "timestamp": 23.16845581133829 + }, + { + "x": 5.578248031578353, + "y": 6.621478294299551, + "heading": 0.10071571878926534, + "angularVelocity": -0.3202222124699196, + "velocityX": 0.7007420906893269, + "velocityY": -0.39553414517797064, + "timestamp": 23.346591607399443 + }, + { + "x": 5.742096614685876, + "y": 6.549230090448469, + "heading": 0.041213467656511724, + "angularVelocity": -0.3340274804303152, + "velocityX": 0.9197959462996009, + "velocityY": -0.40557936949725093, + "timestamp": 23.524727403460595 + }, + { + "x": 5.9357354634248365, + "y": 6.476092053517804, + "heading": -0.019568941838124344, + "angularVelocity": -0.34121389882677833, + "velocityX": 1.087029406896217, + "velocityY": -0.4105746208687261, + "timestamp": 23.702863199521747 + }, + { + "x": 6.148385758343611, + "y": 6.4028961286174635, + "heading": -0.08035607310842466, + "angularVelocity": -0.3412404054344825, + "velocityX": 1.1937538643055912, + "velocityY": -0.4108995862656572, + "timestamp": 23.8809989955829 + }, + { + "x": 6.374102354529772, + "y": 6.330385344994096, + "heading": -0.14018516035925196, + "angularVelocity": -0.33586223866139814, + "velocityX": 1.2671040923692445, + "velocityY": -0.40705341221124436, + "timestamp": 24.059134791644052 + }, + { + "x": 6.612341331128408, + "y": 6.260950902900721, + "heading": -0.19394098279674674, + "angularVelocity": -0.3017687832884667, + "velocityX": 1.337400914733772, + "velocityY": -0.3897837696238762, + "timestamp": 24.237270587705204 + }, + { + "x": 6.84605152207567, + "y": 6.203473880999886, + "heading": -0.2289715660283947, + "angularVelocity": -0.19665100449335837, + "velocityX": 1.3119776940700294, + "velocityY": -0.32265846153343364, + "timestamp": 24.415406383766356 + }, + { + "x": 7.064382598864322, + "y": 6.152325234982081, + "heading": -0.25444586862734997, + "angularVelocity": -0.14300496117137484, + "velocityX": 1.2256440402022253, + "velocityY": -0.28713289046373164, + "timestamp": 24.59354217982751 + }, + { + "x": 7.261910978463682, + "y": 6.1066818733384185, + "heading": -0.2732102079076508, + "angularVelocity": -0.10533727468150804, + "velocityX": 1.1088640462354282, + "velocityY": -0.25622790395414907, + "timestamp": 24.77167797588866 + }, + { + "x": 7.432844706342166, + "y": 6.066712621400304, + "heading": -0.28619204413559407, + "angularVelocity": -0.07287606710718948, + "velocityX": 0.9595697869741047, + "velocityY": -0.22437518355201194, + "timestamp": 24.949813771949813 + }, + { + "x": 7.570743590476923, + "y": 6.032935227158681, + "heading": -0.29375630091954236, + "angularVelocity": -0.04246342931183577, + "velocityX": 0.7741222549554037, + "velocityY": -0.18961598392228185, + "timestamp": 25.127949568010965 + }, + { + "x": 7.668499056236834, + "y": 6.006054729182268, + "heading": -0.29595855831583523, + "angularVelocity": -0.012362800992221083, + "velocityX": 0.5487693541770304, + "velocityY": -0.15089891291403637, + "timestamp": 25.306085364072118 + }, + { + "x": 7.718349321522349, + "y": 5.98690425676214, + "heading": -0.2927031769525776, + "angularVelocity": 0.018274717576348395, + "velocityX": 0.27984417724094734, + "velocityY": -0.10750490829952818, + "timestamp": 25.48422116013327 + }, + { + "x": 7.71188640594573, + "y": 5.976419448852084, + "heading": -0.28379405758259746, + "angularVelocity": 0.050013077477811155, + "velocityX": -0.03628083585189105, + "velocityY": -0.05885851211368899, + "timestamp": 25.662356956194422 + }, + { + "x": 7.64073998767034, + "y": 5.975523422514508, + "heading": -0.2689680472353761, + "angularVelocity": 0.08322869775494246, + "velocityX": -0.39939428102041696, + "velocityY": -0.00503001842507848, + "timestamp": 25.84049275246608 + }, + { + "x": 7.515254770850456, + "y": 5.982745341832985, + "heading": -0.24839985111703908, + "angularVelocity": 0.11546357637731, + "velocityX": -0.7044357139148312, + "velocityY": 0.04054165119978489, + "timestamp": 26.018628548737738 + }, + { + "x": 7.345155681918835, + "y": 5.996704613871788, + "heading": -0.2223274199316461, + "angularVelocity": 0.14636267236058986, + "velocityX": -0.9548843774933711, + "velocityY": 0.07836309338792039, + "timestamp": 26.196764345009395 + }, + { + "x": 7.139560502547719, + "y": 6.016111344560358, + "heading": -0.19106664781409208, + "angularVelocity": 0.1754884350694437, + "velocityX": -1.1541485971641012, + "velocityY": 0.10894346389053551, + "timestamp": 26.374900141281053 + }, + { + "x": 6.907022872723975, + "y": 6.039765038954999, + "heading": -0.15513422497131063, + "angularVelocity": 0.20171365663076973, + "velocityX": -1.30539529219411, + "velocityY": 0.1327846221246523, + "timestamp": 26.55303593755271 + }, + { + "x": 6.655745454364941, + "y": 6.066534208413209, + "heading": -0.11537934692318932, + "angularVelocity": 0.22317175368561765, + "velocityX": -1.4105947463605994, + "velocityY": 0.15027394840704048, + "timestamp": 26.731171733824368 + }, + { + "x": 6.395528884740123, + "y": 6.09504146210893, + "heading": -0.07344943255486255, + "angularVelocity": 0.2353817438489003, + "velocityX": -1.4607764136777701, + "velocityY": 0.16003102291923085, + "timestamp": 26.909307530096026 + }, + { + "x": 6.148227884557905, + "y": 6.126415515383055, + "heading": -0.034920139114795905, + "angularVelocity": 0.21629169569780238, + "velocityX": -1.388272348167526, + "velocityY": 0.1761243609142077, + "timestamp": 27.087443326367683 + }, + { + "x": 5.921399691377473, + "y": 6.155778519380747, + "heading": -0.0014273926700099603, + "angularVelocity": 0.18801805782885908, + "velocityX": -1.2733442571814486, + "velocityY": 0.16483494397205203, + "timestamp": 27.26557912263934 + }, + { + "x": 5.722452569880608, + "y": 6.181830253435625, + "heading": 0.026178625185304717, + "angularVelocity": 0.15497175993321324, + "velocityX": -1.1168284289924366, + "velocityY": 0.14624648498673082, + "timestamp": 27.443714918911 + }, + { + "x": 5.559444959743296, + "y": 6.203351946244675, + "heading": 0.047322653488671644, + "angularVelocity": 0.11869612254198844, + "velocityX": -0.9150749795917061, + "velocityY": 0.12081621582892742, + "timestamp": 27.621850715182656 + }, + { + "x": 5.441124591962054, + "y": 6.21907395052926, + "heading": 0.06162804732685082, + "angularVelocity": 0.08030611554553489, + "velocityX": -0.6642144378512844, + "velocityY": 0.08825853429690561, + "timestamp": 27.799986511454314 + }, + { + "x": 5.376916885374797, + "y": 6.22765016555822, + "heading": 0.06885690933270866, + "angularVelocity": 0.0405806253272207, + "velocityX": -0.3604424710341068, + "velocityY": 0.048144254038901185, + "timestamp": 27.97812230772597 + }, + { + "x": 5.376916885375449, + "y": 6.227650165558026, + "heading": 0.06885690933269815, + "angularVelocity": -1.1945341778272885e-14, + "velocityX": 6.978979046403437e-13, + "velocityY": -1.7270403267086624e-13, + "timestamp": 28.15625810399763 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "b1a6932a-81ed-4f77-9ef6-e63e2a832e36" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint", + "uuid": "ef354677-2b6b-4171-aaef-587377d39b69" + }, + { + "scope": [ + 2, + 8 + ], + "type": "MaxVelocity", + "uuid": "59c28855-22c6-46c7-8545-932700f028ae", + "velocity": 1.2 + }, + { + "scope": [ + 2, + 3 + ], + "type": "ZeroAngularVelocity", + "uuid": "4c477f6c-d3c3-4174-8fd3-c133f4de9207" + }, + { + "scope": [ + 9, + 10 + ], + "type": "MaxVelocity", + "uuid": "9bb07e11-0347-46d1-a512-97901ece5ed2", + "velocity": 0.7 + }, + { + "scope": [ + 11, + 12 + ], + "type": "MaxVelocity", + "uuid": "b5f98f79-332e-476c-a061-2f84d5aaa010", + "velocity": 1.2 + }, + { + "scope": [ + 13, + 14 + ], + "type": "MaxVelocity", + "uuid": "1581cd49-0881-4941-b34a-f384cb5fe29e", + "velocity": 0.7 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "3Spike": { + "waypoints": [ + { + "x": 1.3269914388656616, + "y": 4.39792537689209, + "heading": -0.8519661022091116, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.096055269241333, + "y": 3.8164384365081787, + "heading": 0.23255650839848893, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.499344825744629, + "y": 4.060287952423096, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.5, + "y": 4.45, + "heading": -0.3, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "x": 2.6400270462036133, + "y": 4.9, + "heading": -0.3, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 2.75, + "y": 4.9, + "heading": 1.57, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.75, + "y": 5.5, + "heading": 1.57, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.3269914388656616, + "y": 4.39792537689209, + "heading": -0.8519661022091116, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.333513997411188, + "y": 4.387203709271714, + "heading": -0.8332351514575969, + "angularVelocity": 0.40842901188133685, + "velocityX": 0.14222460872532702, + "velocityY": -0.2337863235028909, + "timestamp": 0.04586097022156875 + }, + { + "x": 1.3467826201629625, + "y": 4.365882340785122, + "heading": -0.7959323522941728, + "angularVelocity": 0.8133887918899778, + "velocityX": 0.2893227659089947, + "velocityY": -0.4649131578242103, + "timestamp": 0.0917219404431375 + }, + { + "x": 1.367096650503852, + "y": 4.334120690383467, + "heading": -0.7403859703551774, + "angularVelocity": 1.21119072864428, + "velocityX": 0.44294811563615344, + "velocityY": -0.6925638565474022, + "timestamp": 0.13758291066470624 + }, + { + "x": 1.394861349124937, + "y": 4.292141697873546, + "heading": -0.6671219475387827, + "angularVelocity": 1.5975244845984082, + "velocityX": 0.6054101883790317, + "velocityY": -0.9153533452761209, + "timestamp": 0.183443880886275 + }, + { + "x": 1.430646384004042, + "y": 4.240291060193454, + "heading": -0.5768914970162948, + "angularVelocity": 1.9674780120559225, + "velocityX": 0.7802938905613228, + "velocityY": -1.130604900628666, + "timestamp": 0.22930485110784377 + }, + { + "x": 1.4753118159731884, + "y": 4.179187224074356, + "heading": -0.4707408110774181, + "angularVelocity": 2.3146192814070323, + "velocityX": 0.9739312481474731, + "velocityY": -1.3323712041826856, + "timestamp": 0.27516582132941253 + }, + { + "x": 1.5302943233607504, + "y": 4.110166528705352, + "heading": -0.3504561598343917, + "angularVelocity": 2.6228108708100475, + "velocityX": 1.1988954250624084, + "velocityY": -1.5049985867185927, + "timestamp": 0.3210267915509813 + }, + { + "x": 1.5980403774204008, + "y": 4.0370266304131235, + "heading": -0.2213773500302043, + "angularVelocity": 2.8145677943699647, + "velocityX": 1.477204990045953, + "velocityY": -1.5948179451692148, + "timestamp": 0.36688776177255006 + }, + { + "x": 1.6779338120627578, + "y": 3.9699528764898666, + "heading": -0.09749875706374012, + "angularVelocity": 2.7011768911117184, + "velocityX": 1.742079032701809, + "velocityY": -1.4625454629329036, + "timestamp": 0.4127487319941188 + }, + { + "x": 1.762553862107012, + "y": 3.914807637304757, + "heading": 0.010619630767232302, + "angularVelocity": 2.3575250874244156, + "velocityX": 1.8451430407038498, + "velocityY": -1.2024437973877604, + "timestamp": 0.4586097022156876 + }, + { + "x": 1.8480147040992523, + "y": 3.872006113916856, + "heading": 0.09844406396935727, + "angularVelocity": 1.9150147233653705, + "velocityX": 1.8634765374424491, + "velocityY": -0.9332886587683153, + "timestamp": 0.5044706724372563 + }, + { + "x": 1.9326347476825954, + "y": 3.8414277812280493, + "heading": 0.16488095802435299, + "angularVelocity": 1.4486587120599115, + "velocityX": 1.8451428998234738, + "velocityY": -0.6667615739717827, + "timestamp": 0.5503316426588251 + }, + { + "x": 2.015500889838408, + "y": 3.8229408161865948, + "heading": 0.20962371332846336, + "angularVelocity": 0.9756172860701385, + "velocityX": 1.8068990201354285, + "velocityY": -0.40310889525751753, + "timestamp": 0.5961926128803938 + }, + { + "x": 2.096055269241333, + "y": 3.8164384365081783, + "heading": 0.23255650839848893, + "angularVelocity": 0.5000503687390396, + "velocityX": 1.7564909554625945, + "velocityY": -0.14178460784849248, + "timestamp": 0.6420535831019626 + }, + { + "x": 2.169861583576719, + "y": 3.8209387927505536, + "heading": 0.23468746359287693, + "angularVelocity": 0.049123667766486946, + "velocityX": 1.7014139358860232, + "velocityY": 0.10374408878397706, + "timestamp": 0.6854329823055731 + }, + { + "x": 2.240363784904268, + "y": 3.8361456579806616, + "heading": 0.21972330792773048, + "angularVelocity": -0.3449599565662249, + "velocityX": 1.6252461449876687, + "velocityY": 0.35055499866955947, + "timestamp": 0.7288123815091837 + }, + { + "x": 2.3064164966295926, + "y": 3.861977794130398, + "heading": 0.19077614253377356, + "angularVelocity": -0.6673021278622857, + "velocityX": 1.5226746552042265, + "velocityY": 0.5954931747322527, + "timestamp": 0.7721917807127943 + }, + { + "x": 2.366685094129252, + "y": 3.8980997052524633, + "heading": 0.1512071128503967, + "angularVelocity": -0.9121617728648345, + "velocityX": 1.3893368420520344, + "velocityY": 0.8326973583133246, + "timestamp": 0.8155711799164048 + }, + { + "x": 2.41978296792177, + "y": 3.943862475881486, + "heading": 0.10416005613005118, + "angularVelocity": -1.084548370518457, + "velocityX": 1.2240343289055675, + "velocityY": 1.0549424719836538, + "timestamp": 0.8589505791200154 + }, + { + "x": 2.4643919768040097, + "y": 3.9983256079338245, + "heading": 0.0526756800424131, + "angularVelocity": -1.18683930697115, + "velocityX": 1.0283454750688807, + "velocityY": 1.2555068316346145, + "timestamp": 0.9023299783236259 + }, + { + "x": 2.499344825744629, + "y": 4.060287952423096, + "heading": 3.33452988603619e-32, + "angularVelocity": -1.214301742519955, + "velocityX": 0.805747649398295, + "velocityY": 1.4283818039627032, + "timestamp": 0.9457093775272365 + }, + { + "x": 2.523403938669763, + "y": 4.12670444317368, + "heading": -0.049468562293879026, + "angularVelocity": -1.1662999834075207, + "velocityX": 0.5672318277350724, + "velocityY": 1.5658743344958055, + "timestamp": 0.9881243333684852 + }, + { + "x": 2.5364666185648104, + "y": 4.197030421591671, + "heading": -0.09653835267359968, + "angularVelocity": -1.109745122826349, + "velocityX": 0.3079734408763396, + "velocityY": 1.658046720152395, + "timestamp": 1.0305392892097338 + }, + { + "x": 2.537870729336966, + "y": 4.267128606265824, + "heading": -0.14073751847846447, + "angularVelocity": -1.0420655857875718, + "velocityX": 0.03310414320388964, + "velocityY": 1.65267612057683, + "timestamp": 1.0729542450509824 + }, + { + "x": 2.5301525844049313, + "y": 4.329958394850495, + "heading": -0.1822098664897901, + "angularVelocity": -0.9777765221906387, + "velocityX": -0.1819675343037494, + "velocityY": 1.4813121300852452, + "timestamp": 1.115369200892231 + }, + { + "x": 2.5191102993462846, + "y": 4.381587731788539, + "heading": -0.22206682765430108, + "angularVelocity": -0.9396912097159363, + "velocityX": -0.26033942131110027, + "velocityY": 1.217243680066123, + "timestamp": 1.1577841567334795 + }, + { + "x": 2.5084288948755984, + "y": 4.421531111089479, + "heading": -0.2611721666603998, + "angularVelocity": -0.9219705226727761, + "velocityX": -0.25183108785176694, + "velocityY": 0.94172865463871, + "timestamp": 1.200199112574728 + }, + { + "x": 2.5, + "y": 4.45, + "heading": -0.3, + "angularVelocity": -0.9154278855063672, + "velocityX": -0.1987245939179124, + "velocityY": 0.6711993056665078, + "timestamp": 1.2426140684159765 + }, + { + "x": 2.5193699694803637, + "y": 4.526734764577753, + "heading": -0.6391389887177107, + "angularVelocity": -2.999645914621537, + "velocityX": 0.17132518451448528, + "velocityY": 0.6787102949897058, + "timestamp": 1.3556737422476777 + }, + { + "x": 2.5387399844003316, + "y": 4.6034695176852845, + "heading": -1.2190938747349336, + "angularVelocity": -5.12963523033465, + "velocityX": 0.17132558642263376, + "velocityY": 0.6787101935368887, + "timestamp": 1.4687334160793788 + }, + { + "x": 2.558109999348224, + "y": 4.680204270785767, + "heading": -1.511597411804606, + "angularVelocity": -2.58716063081064, + "velocityX": 0.17132558666962455, + "velocityY": 0.6787101934745414, + "timestamp": 1.58179308991108 + }, + { + "x": 2.577480014296659, + "y": 4.756939023886113, + "heading": -1.4587662970367927, + "angularVelocity": 0.46728522184188176, + "velocityX": 0.1713255866744186, + "velocityY": 0.6787101934733313, + "timestamp": 1.694852763742781 + }, + { + "x": 2.5968501077533794, + "y": 4.833673757168711, + "heading": -1.0457294063544558, + "angularVelocity": 3.6532644813497406, + "velocityX": 0.17132628107130865, + "velocityY": 0.6787100181875982, + "timestamp": 1.8079124375744822 + }, + { + "x": 2.6400270462036133, + "y": 4.9, + "heading": -0.3, + "angularVelocity": 6.5958920725752055, + "velocityX": 0.3818951265904639, + "velocityY": 0.5866480999230684, + "timestamp": 1.9209721114061833 + }, + { + "x": 2.65028611673258, + "y": 4.911101822443869, + "heading": -0.12187108053489694, + "angularVelocity": 6.729487849665297, + "velocityX": 0.3875748569117997, + "velocityY": 0.41941297050192267, + "timestamp": 1.94744201869251 + }, + { + "x": 2.660785716281329, + "y": 4.917835219089741, + "heading": 0.060531873273454315, + "angularVelocity": 6.890955523012834, + "velocityX": 0.3966617425279967, + "velocityY": 0.25437930601858355, + "timestamp": 1.973911925978837 + }, + { + "x": 2.6714281364807704, + "y": 4.920121500261387, + "heading": 0.24615978486608456, + "angularVelocity": 7.012790395700312, + "velocityX": 0.4020573281319579, + "velocityY": 0.0863728439587885, + "timestamp": 2.0003818332651635 + }, + { + "x": 2.6820026092586784, + "y": 4.9178796942564835, + "heading": 0.4333660134100134, + "angularVelocity": 7.072417236634274, + "velocityX": 0.3994903595061169, + "velocityY": -0.08469262776982427, + "timestamp": 2.02685174055149 + }, + { + "x": 2.692177602021045, + "y": 4.9111165494503615, + "heading": 0.6193545789499477, + "angularVelocity": 7.02641545087723, + "velocityX": 0.3843985040183, + "velocityY": -0.25550315431651316, + "timestamp": 2.0533216478378167 + }, + { + "x": 2.701026406563282, + "y": 4.901195651201101, + "heading": 0.7946225445644793, + "angularVelocity": 6.621404590452321, + "velocityX": 0.3342967712927192, + "velocityY": -0.3747991310262394, + "timestamp": 2.0797915551241433 + }, + { + "x": 2.7092405777751063, + "y": 4.892429852217793, + "heading": 0.9506886625791671, + "angularVelocity": 5.89598279761655, + "velocityX": 0.3103211175986155, + "velocityY": -0.3311609250643644, + "timestamp": 2.10626146241047 + }, + { + "x": 2.717843250419752, + "y": 4.886627760240061, + "heading": 1.093906101120617, + "angularVelocity": 5.410575752769251, + "velocityX": 0.3249982159586029, + "velocityY": -0.21919578013514107, + "timestamp": 2.1327313696967964 + }, + { + "x": 2.7263555670649233, + "y": 4.884431942213397, + "heading": 1.2272956538012554, + "angularVelocity": 5.039290513478366, + "velocityX": 0.3215846792772284, + "velocityY": -0.08295525945410186, + "timestamp": 2.159201276983123 + }, + { + "x": 2.7346242420728504, + "y": 4.885911244753091, + "heading": 1.3510610914689725, + "angularVelocity": 4.675703482031048, + "velocityX": 0.3123802028652458, + "velocityY": 0.05588620026853448, + "timestamp": 2.1856711842694496 + }, + { + "x": 2.742537461432789, + "y": 4.891093691725606, + "heading": 1.4652638899797559, + "angularVelocity": 4.31443893155514, + "velocityX": 0.298951532936642, + "velocityY": 0.1957863666259237, + "timestamp": 2.212141091555776 + }, + { + "x": 2.75, + "y": 4.9, + "heading": 1.57, + "angularVelocity": 3.956799277281427, + "velocityX": 0.2819253761068593, + "velocityY": 0.33646919039249157, + "timestamp": 2.238610998842103 + }, + { + "x": 2.7674728869027185, + "y": 4.97015192039149, + "heading": 1.8148137759040575, + "angularVelocity": 2.798622019433345, + "velocityX": 0.19974368618937863, + "velocityY": 0.801951231658216, + "timestamp": 2.326087540584466 + }, + { + "x": 2.7760292561397533, + "y": 5.08021838751503, + "heading": 1.9563256399870756, + "angularVelocity": 1.6177121461865793, + "velocityX": 0.09781330019007135, + "velocityY": 1.2582398084243995, + "timestamp": 2.413564082326829 + }, + { + "x": 2.7742938765095486, + "y": 5.226701796198958, + "heading": 1.976040818465192, + "angularVelocity": 0.22537674770204888, + "velocityX": -0.019838228576937924, + "velocityY": 1.6745450353461948, + "timestamp": 2.501040624069192 + }, + { + "x": 2.762838623484245, + "y": 5.364431983374006, + "heading": 1.782825401942879, + "angularVelocity": -2.2087683471915627, + "velocityX": -0.13095228500277664, + "velocityY": 1.5744813916020168, + "timestamp": 2.588517165811555 + }, + { + "x": 2.7543895903343842, + "y": 5.455020359988401, + "heading": 1.6426814209537794, + "angularVelocity": -1.6020750043120497, + "velocityX": -0.09658627309188043, + "velocityY": 1.0355733641276863, + "timestamp": 2.675993707553918 + }, + { + "x": 2.75, + "y": 5.5, + "heading": 1.57, + "angularVelocity": -0.830867561818363, + "velocityX": -0.05018019970785374, + "velocityY": 0.5141908803856656, + "timestamp": 2.7634702492962813 + }, + { + "x": 2.75, + "y": 5.5, + "heading": 1.57, + "angularVelocity": -2.798206983013458e-30, + "velocityX": 7.872829382238201e-31, + "velocityY": 0, + "timestamp": 2.8509467910386443 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "a03a1a0f-af50-4b3e-be17-249ea7f9b21c" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "2948cc63-8cc5-47ed-9e06-66af134206c2" + }, + { + "scope": [ + 3, + 4 + ], + "type": "MaxVelocity", + "uuid": "4dd86342-9f10-4fdc-8160-e1af8ae08f40", + "velocity": 0.7 + }, + { + "scope": [ + 3, + 4 + ], + "type": "ZeroAngularVelocity", + "uuid": "bc7e19cf-6df0-4b6e-9ad8-7c982f4ad86c" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "New Path": { + "waypoints": [ + { + "x": 1.4851783514022827, + "y": 7.326318264007568, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 30 + }, + { + "x": 10.1687593460083, + "y": 7.326318740844727, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.4851783514022827, + "y": 7.326318264007568, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.566665314770093, + "y": 7.326318268482221, + "heading": -9.745639154608945e-23, + "angularVelocity": -8.704126162754158e-22, + "velocityX": 0.7277847634744247, + "velocityY": 3.996448202047959e-8, + "timestamp": 0.11196574517278132 + }, + { + "x": 1.7296392389410207, + "y": 7.326318277431527, + "heading": -3.4658654410217392e-22, + "angularVelocity": -2.2250569735274297e-21, + "velocityX": 1.4555695040427974, + "velocityY": 7.992896278313055e-8, + "timestamp": 0.22393149034556264 + }, + { + "x": 1.9741001187856775, + "y": 7.326318290855485, + "heading": -8.24362198504553e-22, + "angularVelocity": -4.267159205128636e-21, + "velocityX": 2.1833541987990532, + "velocityY": 1.1989344103012352e-7, + "timestamp": 0.33589723551834394 + }, + { + "x": 2.3000479389158897, + "y": 7.326318308754096, + "heading": -1.7621745704477776e-21, + "angularVelocity": -8.375886381665278e-21, + "velocityX": 2.9111387561188593, + "velocityY": 1.5985791173013713e-7, + "timestamp": 0.4478629806911253 + }, + { + "x": 2.6359451684196418, + "y": 7.326318327199053, + "heading": 5.860691675515718e-19, + "angularVelocity": 5.250099783101951e-18, + "velocityX": 2.999999946281858, + "velocityY": 1.6473750198101658e-7, + "timestamp": 0.5598287258639066 + }, + { + "x": 2.9718423979233948, + "y": 7.326318345644011, + "heading": 4.8428473870582435e-18, + "angularVelocity": 3.801857621890219e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 0.671794471036688 + }, + { + "x": 3.307739627427147, + "y": 7.3263183640889675, + "heading": 1.0521971096433735e-17, + "angularVelocity": 5.072197480886782e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101613e-7, + "timestamp": 0.7837602162094693 + }, + { + "x": 3.6436368569308994, + "y": 7.326318382533925, + "heading": 1.4717398190746304e-17, + "angularVelocity": 3.747063076821502e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 0.8957259613822507 + }, + { + "x": 3.979534086434652, + "y": 7.326318400978883, + "heading": 1.7426356992707997e-17, + "angularVelocity": 2.4194532030809352e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.007691706555032 + }, + { + "x": 4.315431315938405, + "y": 7.326318419423839, + "heading": 1.8985412935500457e-17, + "angularVelocity": 1.3924401153114026e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.1196574517278133 + }, + { + "x": 4.651328545442158, + "y": 7.326318437868797, + "heading": 2.022868920510175e-17, + "angularVelocity": 1.1104077197839936e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.2316231969005946 + }, + { + "x": 4.98722577494591, + "y": 7.3263184563137544, + "heading": 2.188100699427991e-17, + "angularVelocity": 1.4757350915348522e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.343588942073376 + }, + { + "x": 5.323123004449662, + "y": 7.326318474758711, + "heading": 2.3719859301334404e-17, + "angularVelocity": 1.6423347194890602e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.4555546872461573 + }, + { + "x": 5.659020233953415, + "y": 7.326318493203669, + "heading": 2.5354180700870405e-17, + "angularVelocity": 1.4596619698171647e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.5675204324189387 + }, + { + "x": 5.994917463457167, + "y": 7.326318511648626, + "heading": 2.6401814540471825e-17, + "angularVelocity": 9.356735339113599e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.67948617759172 + }, + { + "x": 6.330814692960919, + "y": 7.326318530093584, + "heading": 2.6961840683369917e-17, + "angularVelocity": 5.001763244165286e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.7914519227645014 + }, + { + "x": 6.666711922464672, + "y": 7.3263185485385405, + "heading": 2.746703941908865e-17, + "angularVelocity": 4.512083019401423e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.9034176679372827 + }, + { + "x": 7.002609151968424, + "y": 7.326318566983498, + "heading": 2.75257745202433e-17, + "angularVelocity": 5.245810017426919e-19, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.015383413110064 + }, + { + "x": 7.338506381472176, + "y": 7.326318585428456, + "heading": 2.6894889559197837e-17, + "angularVelocity": -5.634624707407713e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.127349158282845 + }, + { + "x": 7.674403610975929, + "y": 7.326318603873412, + "heading": 2.614291653422174e-17, + "angularVelocity": -6.716098963685828e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.2393149034556266 + }, + { + "x": 8.010300840479681, + "y": 7.32631862231837, + "heading": 2.4766232846828968e-17, + "angularVelocity": -1.2295579197538016e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.351280648628408 + }, + { + "x": 8.346198069983435, + "y": 7.326318640763327, + "heading": 2.2029876482173888e-17, + "angularVelocity": -2.4439227918755626e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.4632463938011893 + }, + { + "x": 8.68209529948719, + "y": 7.326318659208284, + "heading": 1.6814351481469152e-17, + "angularVelocity": -4.6581434349974976e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101613e-7, + "timestamp": 2.5752121389739706 + }, + { + "x": 9.017992528990941, + "y": 7.326318677653242, + "heading": 6.264884173064017e-18, + "angularVelocity": -9.42204897841851e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.687177884146752 + }, + { + "x": 9.353889758494695, + "y": 7.326318696098199, + "heading": -5.6652372475915536e-21, + "angularVelocity": -5.600417688760007e-17, + "velocityX": 2.999999946281858, + "velocityY": 1.6473750198101658e-7, + "timestamp": 2.7991436293195333 + }, + { + "x": 9.679837578624907, + "y": 7.32631871399681, + "heading": -2.7181194656762927e-21, + "angularVelocity": 2.6321601978014413e-20, + "velocityX": 2.9111387561188593, + "velocityY": 1.5985791173013718e-7, + "timestamp": 2.9111093744923147 + }, + { + "x": 9.924298458469563, + "y": 7.326318727420768, + "heading": -1.1801505322960497e-21, + "angularVelocity": 1.3736066636473078e-20, + "velocityX": 2.1833541987990532, + "velocityY": 1.1989344103012357e-7, + "timestamp": 3.023075119665096 + }, + { + "x": 10.08727238264049, + "y": 7.326318736370074, + "heading": -3.536469486075474e-22, + "angularVelocity": 7.381753952033097e-21, + "velocityX": 1.4555695040427974, + "velocityY": 7.992896278313059e-8, + "timestamp": 3.1350408648378774 + }, + { + "x": 10.1687593460083, + "y": 7.326318740844727, + "heading": 0, + "angularVelocity": 3.1585280732860345e-21, + "velocityX": 0.7277847634744247, + "velocityY": 3.996448202047962e-8, + "timestamp": 3.2470066100106587 + }, + { + "x": 10.1687593460083, + "y": 7.326318740844727, + "heading": 0, + "angularVelocity": 0, + "velocityX": 4.0858250360262775e-40, + "velocityY": 0, + "timestamp": 3.35897235518344 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "b98a06ab-cdf4-4ac8-9cf9-5f0472a6bff2" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "01b32ae9-f6f6-4a82-86d8-8d628cef4f6a" + }, + { + "scope": [ + 0, + 1 + ], + "type": "MaxVelocity", + "uuid": "fe88f063-4a67-428e-9628-e902d396ad29", + "velocity": 1.5 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + } +} \ No newline at end of file diff --git a/src/main/Test.chor b/src/main/Test.chor new file mode 100644 index 00000000..f8e87cb5 --- /dev/null +++ b/src/main/Test.chor @@ -0,0 +1,2802 @@ +{ + "version": "v0.2", + "robotConfiguration": { + "mass": 59.27038160247355, + "rotationalInertia": 6, + "motorMaxTorque": 0.7759834368530021, + "motorMaxVelocity": 4640, + "gearing": 6.75, + "wheelbase": 0.6603996433841925, + "trackWidth": 0.6603996433841925, + "bumperLength": 0.812799561088237, + "bumperWidth": 0.812799561088237, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "3Spike": { + "waypoints": [ + { + "x": 1.898240327835083, + "y": 6.697559356689453, + "heading": 0.8156915418947346, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 2.9706242084503174, + "y": 6.795048713684082, + "heading": -0.08673824690926328, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.444181203842163, + "y": 5.6446733474731445, + "heading": 0.03446907195457146, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "x": 3.087611436843872, + "y": 5.508187770843506, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 3.107109308242798, + "y": 4.1628336906433105, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 2.249202251434326, + "y": 4.747770309448242, + "heading": -0.16514851217251245, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.898240327835083, + "y": 6.697559356689453, + "heading": 0.8156915418947346, + "angularVelocity": -1.0203651299070483e-18, + "velocityX": 7.059276162786864e-17, + "velocityY": 8.05331973081768e-18, + "timestamp": 0 + }, + { + "x": 1.928011425667035, + "y": 6.700257705488313, + "heading": 0.7906543885401096, + "angularVelocity": -0.4125180197131184, + "velocityX": 0.4905156008901217, + "velocityY": 0.04445862863220824, + "timestamp": 0.06069347800136739 + }, + { + "x": 1.987561656316453, + "y": 6.7056814751882055, + "heading": 0.7406973162292918, + "angularVelocity": -0.8231044579401747, + "velocityX": 0.981163588088925, + "velocityY": 0.08936330357883958, + "timestamp": 0.12138695600273477 + }, + { + "x": 2.0768993924383765, + "y": 6.713863123454453, + "heading": 0.665908700412107, + "angularVelocity": -1.2322348014971582, + "velocityX": 1.4719495251188295, + "velocityY": 0.13480275864341493, + "timestamp": 0.18208043400410218 + }, + { + "x": 2.196043819991789, + "y": 6.724832813976885, + "heading": 0.5664907713553188, + "angularVelocity": -1.638033151676551, + "velocityX": 1.9630515745156076, + "velocityY": 0.18073919774683922, + "timestamp": 0.24277391200546955 + }, + { + "x": 2.3450703903963706, + "y": 6.738620084887391, + "heading": 0.44354978680104007, + "angularVelocity": -2.0256045394451108, + "velocityX": 2.4553967792259948, + "velocityY": 0.22716231404953768, + "timestamp": 0.3034673900068369 + }, + { + "x": 2.524366013731002, + "y": 6.755404219318462, + "heading": 0.3052638198889322, + "angularVelocity": -2.2784320731956287, + "velocityX": 2.9541168052783493, + "velocityY": 0.27653934135544966, + "timestamp": 0.3641608680082043 + }, + { + "x": 2.673270314372625, + "y": 6.768792785441263, + "heading": 0.17727124299711214, + "angularVelocity": -2.1088357613800035, + "velocityX": 2.453382233891238, + "velocityY": 0.22059316031451623, + "timestamp": 0.42485434600957167 + }, + { + "x": 2.7922668700063817, + "y": 6.779362583190905, + "heading": 0.07259539139052823, + "angularVelocity": -1.7246639186540365, + "velocityX": 1.9606152020333387, + "velocityY": 0.17415047049050045, + "timestamp": 0.48554782401093904 + }, + { + "x": 2.8814664055883235, + "y": 6.787228518212843, + "heading": -0.006725211090079159, + "angularVelocity": -1.306904878293822, + "velocityX": 1.4696725005598144, + "velocityY": 0.12960099307146533, + "timestamp": 0.5462413020123065 + }, + { + "x": 2.9409112103177457, + "y": 6.792446959885125, + "heading": -0.059963007280896694, + "angularVelocity": -0.877158435204807, + "velocityX": 0.9794265658673137, + "velocityY": 0.08598027076589665, + "timestamp": 0.6069347800136738 + }, + { + "x": 2.9706242084503174, + "y": 6.795048713684082, + "heading": -0.0867382469092633, + "angularVelocity": -0.4411551374228896, + "velocityX": 0.48955833659593734, + "velocityY": 0.04286710672435541, + "timestamp": 0.6676282580150412 + }, + { + "x": 2.9706242084503174, + "y": 6.795048713684082, + "heading": -0.0867382469092633, + "angularVelocity": 4.1300797018606364e-18, + "velocityX": -6.787421352957282e-17, + "velocityY": -6.122691775796301e-18, + "timestamp": 0.7283217360164086 + }, + { + "x": 2.9443020593472835, + "y": 6.73752994711249, + "heading": -0.08067661919484623, + "angularVelocity": 0.06958187507135942, + "velocityX": -0.3021539059782267, + "velocityY": -0.6602621966248707, + "timestamp": 0.8154367743536277 + }, + { + "x": 2.8916577620939425, + "y": 6.622492415321145, + "heading": -0.06855357159018967, + "angularVelocity": 0.13916136451353756, + "velocityX": -0.6043078010200357, + "velocityY": -1.3205243777318816, + "timestamp": 0.9025518126908468 + }, + { + "x": 2.8126913183903137, + "y": 6.449936121363781, + "heading": -0.0503694568413589, + "angularVelocity": 0.2087368047574143, + "velocityX": -0.9064616765471906, + "velocityY": -1.9807865237848399, + "timestamp": 0.989666851028066 + }, + { + "x": 2.707402733848941, + "y": 6.219861077435211, + "heading": -0.026124774152275536, + "angularVelocity": 0.27830651460237066, + "velocityX": -1.208615487647548, + "velocityY": -2.641048529852652, + "timestamp": 1.0767818893652852 + }, + { + "x": 2.602114129325041, + "y": 5.98978600061254, + "heading": -0.00188700512612124, + "angularVelocity": 0.2782271521517419, + "velocityX": -1.2086157170284564, + "velocityY": -2.6410489074464856, + "timestamp": 1.1638969277025044 + }, + { + "x": 2.5231476684834186, + "y": 5.817229677874622, + "heading": 0.016291079692932007, + "angularVelocity": 0.20866758674531646, + "velocityX": -0.9064618732755036, + "velocityY": -1.9807868541589642, + "timestamp": 1.2510119660397236 + }, + { + "x": 2.4705033590732923, + "y": 5.7021921250404635, + "heading": 0.028409733760042313, + "angularVelocity": 0.13911093076948938, + "velocityX": -0.6043079405686759, + "velocityY": -1.3205246192838944, + "timestamp": 1.3381270043769429 + }, + { + "x": 2.444181203842163, + "y": 5.6446733474731445, + "heading": 0.03446907195457146, + "angularVelocity": 0.06955559350239472, + "velocityX": -0.30215397632309554, + "velocityY": -0.6602623228456396, + "timestamp": 1.425242042714162 + }, + { + "x": 2.444181203842163, + "y": 5.6446733474731445, + "heading": 0.03446907195457146, + "angularVelocity": -1.0356554389384789e-17, + "velocityX": 2.4614269190125946e-15, + "velocityY": -5.221227236415707e-16, + "timestamp": 1.5123570810513813 + }, + { + "x": 2.5156734582113587, + "y": 5.629508282219497, + "heading": 0.030639214226098638, + "angularVelocity": -0.04090586229477797, + "velocityX": 0.7635929373114598, + "velocityY": -0.1619747037455608, + "timestamp": 1.6059832146572177 + }, + { + "x": 2.6586579607167553, + "y": 5.599178152927087, + "heading": 0.022979469316454657, + "angularVelocity": -0.08181203916730466, + "velocityX": 1.5271858080496832, + "velocityY": -0.32394939451519356, + "timestamp": 1.699609348263054 + }, + { + "x": 2.8731346805000304, + "y": 5.5536829658728095, + "heading": 0.011489763977113574, + "angularVelocity": -0.12271899839111648, + "velocityX": 2.2907783491969966, + "velocityY": -0.485924018242721, + "timestamp": 1.7932354818688905 + }, + { + "x": 3.0161191826976204, + "y": 5.523352836293968, + "heading": 0.0038299237880790897, + "angularVelocity": -0.08181305682537549, + "velocityX": 1.5271858047620634, + "velocityY": -0.32394939757450636, + "timestamp": 1.8868616154747269 + }, + { + "x": 3.087611436843872, + "y": 5.508187770843506, + "heading": -9.681628847292082e-19, + "angularVelocity": -0.04090656786280392, + "velocityX": 0.7635929349302473, + "velocityY": -0.16197470584769585, + "timestamp": 1.9804877490805632 + }, + { + "x": 3.087611436843872, + "y": 5.508187770843506, + "heading": 7.657382594875245e-22, + "angularVelocity": 1.035618611822264e-17, + "velocityX": -2.4614264121936132e-15, + "velocityY": 5.221220564767035e-16, + "timestamp": 2.0741138826863996 + }, + { + "x": 3.08858633048636, + "y": 5.440920061829133, + "heading": 2.324556482364219e-16, + "angularVelocity": 2.5878970789813725e-15, + "velocityX": 0.010853393925855314, + "velocityY": -0.7488847117307628, + "timestamp": 2.1639377340224875 + }, + { + "x": 3.0905361177310438, + "y": 5.306384646580583, + "heading": 6.8687655980783295e-16, + "angularVelocity": 5.0590227964198126e-15, + "velocityX": 0.021706787403138753, + "velocityY": -1.4977693925098754, + "timestamp": 2.2537615853585753 + }, + { + "x": 3.0934607984973375, + "y": 5.104581530658249, + "heading": 1.3632643561200468e-15, + "angularVelocity": 7.530158039918729e-15, + "velocityX": 0.03256017998327358, + "velocityY": -2.2466540113856897, + "timestamp": 2.343585436694663 + }, + { + "x": 3.0973603725434855, + "y": 4.835510730743306, + "heading": 2.2709770866681227e-15, + "angularVelocity": 1.0105475517379967e-14, + "velocityX": 0.043413569871963154, + "velocityY": -2.9955384445516495, + "timestamp": 2.433409288030751 + }, + { + "x": 3.1012599465895736, + "y": 4.566439930828403, + "heading": 2.264075262752123e-15, + "angularVelocity": -7.683731903225901e-17, + "velocityX": 0.04341356987129279, + "velocityY": -2.995538444551191, + "timestamp": 2.523233139366839 + }, + { + "x": 3.1041846273558074, + "y": 4.3646368149061106, + "heading": 2.0256507625049035e-15, + "angularVelocity": -2.6543562395670722e-15, + "velocityX": 0.032560179982603216, + "velocityY": -2.246654011385231, + "timestamp": 2.6130569907029266 + }, + { + "x": 3.106134414600431, + "y": 4.230101399657602, + "heading": 1.5650617308478489e-15, + "angularVelocity": -5.127691863765037e-15, + "velocityX": 0.021706787402468397, + "velocityY": -1.497769392509417, + "timestamp": 2.7028808420390145 + }, + { + "x": 3.1071093082428582, + "y": 4.16283369064327, + "heading": 8.822473275380185e-16, + "angularVelocity": -7.601704816337083e-15, + "velocityX": 0.010853393925184958, + "velocityY": -0.7488847117303047, + "timestamp": 2.7927046933751023 + }, + { + "x": 3.107109308242798, + "y": 4.1628336906433105, + "heading": -3.209207761292708e-17, + "angularVelocity": -1.0179249626542184e-14, + "velocityX": -6.728159002851988e-13, + "velocityY": 4.5874167966636e-13, + "timestamp": 2.88252854471119 + }, + { + "x": 3.0534901012801763, + "y": 4.1993922875336205, + "heading": -0.010317012651529421, + "angularVelocity": -0.11688600366277159, + "velocityX": -0.6074757328604911, + "velocityY": 0.4141885286325489, + "timestamp": 2.9707941396348567 + }, + { + "x": 2.946251691134035, + "y": 4.272509454128145, + "heading": -0.030952692489374736, + "angularVelocity": -0.2337907522825489, + "velocityX": -1.2149514229058669, + "velocityY": 0.828376749261782, + "timestamp": 3.0590597345585233 + }, + { + "x": 2.7853940913065474, + "y": 4.3821851470513975, + "heading": -0.06191007575465608, + "angularVelocity": -0.3507298998219544, + "velocityX": -1.8224269599791396, + "velocityY": 1.2425644784709402, + "timestamp": 3.14732532948219 + }, + { + "x": 2.5709173398047853, + "y": 4.5284193116673395, + "heading": -0.10319240644132736, + "angularVelocity": -0.46770580000323475, + "velocityX": -2.429902066453476, + "velocityY": 1.6567515886842268, + "timestamp": 3.2355909244058565 + }, + { + "x": 2.410059790521505, + "y": 4.63809482563131, + "heading": -0.1341686316183575, + "angularVelocity": -0.3509433681815447, + "velocityX": -1.8224263873414401, + "velocityY": 1.2425624509620008, + "timestamp": 3.323856519329523 + }, + { + "x": 2.3028214309916533, + "y": 4.711211816793368, + "heading": -0.15482166754318777, + "angularVelocity": -0.2339873870755679, + "velocityX": -1.2149508494515129, + "velocityY": 0.8283747617097077, + "timestamp": 3.4121221142531897 + }, + { + "x": 2.2492022514342964, + "y": 4.747770309448263, + "heading": -0.16514851217251283, + "angularVelocity": -0.11699739449184227, + "velocityX": -0.6074754223741123, + "velocityY": 0.4141873477033858, + "timestamp": 3.5003877091768563 + }, + { + "x": 2.249202251434326, + "y": 4.747770309448242, + "heading": -0.16514851217251242, + "angularVelocity": 5.079807183482213e-15, + "velocityX": 3.3640849188303873e-13, + "velocityY": -2.2937202025389454e-13, + "timestamp": 3.588653304100523 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "115e1b38-6d75-4aa8-8426-67e43b58195c" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "9d06e358-6b8f-4b0e-aaa5-2018887d7c7d" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint", + "uuid": "8a71d32e-0b3d-42cd-a985-4cae03ffa9f0" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint", + "uuid": "7bd301ab-555d-4391-9547-86bd36bb53e7" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint", + "uuid": "3c075286-f878-408e-bc4c-4ab023481cfd" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint", + "uuid": "38910e11-e23a-4562-90df-c353509e0337" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint", + "uuid": "68bc3dc9-f4b4-43df-a29b-dbe7d1a55538" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "3Centerline": { + "waypoints": [ + { + "x": 1.8608553409576416, + "y": 5.938887119293213, + "heading": 0.305879568562575, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 8.029805183410645, + "y": 7.180410385131836, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 4.923573017120361, + "y": 6.213200092315674, + "heading": 0.23554532977562306, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 8.085606575012207, + "y": 5.673794269561768, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 4.867772102355957, + "y": 6.045798301696777, + "heading": 0.33473735609186744, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 + }, + { + "x": 8.011205673217773, + "y": 4.14857816696167, + "heading": -0.288054962130024, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 5.07237434387207, + "y": 6.064398765563965, + "heading": 0.13552729798344068, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.8608553409576416, + "y": 5.938887119293213, + "heading": 0.305879568562575, + "angularVelocity": 5.829173690613896e-22, + "velocityX": -4.239164801235723e-21, + "velocityY": -8.56131924454696e-22, + "timestamp": 0 + }, + { + "x": 1.920785185674491, + "y": 5.950948447445072, + "heading": 0.29568066409022925, + "angularVelocity": -0.1087415369182898, + "velocityX": 0.638976807700777, + "velocityY": 0.12859884746103709, + "timestamp": 0.09379032852928489 + }, + { + "x": 2.040644873990048, + "y": 5.975071063417899, + "heading": 0.2752821629251994, + "angularVelocity": -0.21749045434530723, + "velocityX": 1.2779536034798316, + "velocityY": 0.2571972649109091, + "timestamp": 0.18758065705856977 + }, + { + "x": 2.2204344060834567, + "y": 6.01125490522267, + "heading": 0.24468324187657475, + "angularVelocity": -0.3262481486997939, + "velocityX": 1.9169304011689379, + "velocityY": 0.3857950214288154, + "timestamp": 0.28137098558785467 + }, + { + "x": 2.4601537876635544, + "y": 6.059499889251536, + "heading": 0.2038834521735011, + "angularVelocity": -0.4350106278850961, + "velocityX": 2.5559072597261285, + "velocityY": 0.514391886513143, + "timestamp": 0.37516131411713954 + }, + { + "x": 2.7598030292882076, + "y": 6.119805911541678, + "heading": 0.15288343864942966, + "angularVelocity": -0.5437662318044585, + "velocityX": 3.1948842308521352, + "velocityY": 0.6429876431375527, + "timestamp": 0.4689516426464244 + }, + { + "x": 3.096037782022018, + "y": 6.187474552253286, + "heading": 0.1528834334531576, + "angularVelocity": -5.540306902040579e-8, + "velocityX": 3.5849618825977916, + "velocityY": 0.7214884708552811, + "timestamp": 0.5627419711757093 + }, + { + "x": 3.432272534725669, + "y": 6.255143193114989, + "heading": 0.152883428256934, + "angularVelocity": -5.540255286722503e-8, + "velocityX": 3.584961882276224, + "velocityY": 0.7214884724556129, + "timestamp": 0.6565322997049943 + }, + { + "x": 3.768507287429323, + "y": 6.322811833976674, + "heading": 0.1528834230607103, + "angularVelocity": -5.540255361921963e-8, + "velocityX": 3.584961882276264, + "velocityY": 0.7214884724554109, + "timestamp": 0.7503226282342792 + }, + { + "x": 4.104742040132986, + "y": 6.390480474838318, + "heading": 0.15288341786448656, + "angularVelocity": -5.5402553936553215e-8, + "velocityX": 3.584961882276352, + "velocityY": 0.7214884724549767, + "timestamp": 0.8441129567635641 + }, + { + "x": 4.440976792836646, + "y": 6.458149115699975, + "heading": 0.15288341266826286, + "angularVelocity": -5.540255357687288e-8, + "velocityX": 3.584961882276326, + "velocityY": 0.7214884724551046, + "timestamp": 0.937903285292849 + }, + { + "x": 4.777211545540301, + "y": 6.525817756561653, + "heading": 0.1528834074720391, + "angularVelocity": -5.5402554130547034e-8, + "velocityX": 3.5849618822762768, + "velocityY": 0.7214884724553482, + "timestamp": 1.031693613822134 + }, + { + "x": 5.113446298243962, + "y": 6.593486397423306, + "heading": 0.15288340227581537, + "angularVelocity": -5.540255388249398e-8, + "velocityX": 3.584961882276334, + "velocityY": 0.7214884724550655, + "timestamp": 1.125483942351419 + }, + { + "x": 5.449681050947619, + "y": 6.661155038284979, + "heading": 0.15288339707959164, + "angularVelocity": -5.540255387272659e-8, + "velocityX": 3.584961882276292, + "velocityY": 0.7214884724552731, + "timestamp": 1.2192742708807038 + }, + { + "x": 5.785915803651279, + "y": 6.728823679146634, + "heading": 0.15288339188336802, + "angularVelocity": -5.5402552746810195e-8, + "velocityX": 3.584961882276327, + "velocityY": 0.7214884724551002, + "timestamp": 1.3130645994099888 + }, + { + "x": 6.122150556354927, + "y": 6.796492320008345, + "heading": 0.1528833866871443, + "angularVelocity": -5.540255396525083e-8, + "velocityX": 3.5849618822762097, + "velocityY": 0.7214884724556808, + "timestamp": 1.4068549279392737 + }, + { + "x": 6.458385309058613, + "y": 6.864160960869874, + "heading": 0.15288338149092062, + "angularVelocity": -5.540255313060849e-8, + "velocityX": 3.5849618822765965, + "velocityY": 0.7214884724537611, + "timestamp": 1.5006452564685586 + }, + { + "x": 6.794620061762232, + "y": 6.931829601731737, + "heading": 0.1528833762946969, + "angularVelocity": -5.540255368032922e-8, + "velocityX": 3.5849618822758833, + "velocityY": 0.7214884724573051, + "timestamp": 1.5944355849978435 + }, + { + "x": 7.130854814534018, + "y": 6.999498242254649, + "heading": 0.1528833710984248, + "angularVelocity": -5.5403069453392605e-8, + "velocityX": 3.5849618830026935, + "velocityY": 0.7214884688433891, + "timestamp": 1.6882259135271285 + }, + { + "x": 7.430504858402931, + "y": 7.059802356369048, + "heading": 0.10191849540154471, + "angularVelocity": -0.5433915894746751, + "velocityX": 3.1948927844447264, + "velocityY": 0.6429672980148516, + "timestamp": 1.7820162420564134 + }, + { + "x": 7.67022495954135, + "y": 7.108045593634433, + "heading": 0.06114961629640668, + "angularVelocity": -0.4346810566124464, + "velocityX": 2.555914931714623, + "velocityY": 0.5143732623808938, + "timestamp": 1.8758065705856983 + }, + { + "x": 7.8500150651920775, + "y": 7.144227995763732, + "heading": 0.030574531833697825, + "angularVelocity": -0.32599400111027615, + "velocityX": 1.916936516483045, + "velocityY": 0.3857796714935467, + "timestamp": 1.9695968991149833 + }, + { + "x": 7.969875144582322, + "y": 7.168349588803029, + "heading": 0.010191587226393475, + "angularVelocity": -0.21732458907999266, + "velocityX": 1.2779577731494982, + "velocityY": 0.25718635831161385, + "timestamp": 2.063387227644268 + }, + { + "x": 8.029805183410645, + "y": 7.180410385131836, + "heading": -1.5403204326632027e-19, + "angularVelocity": -0.10866351985547505, + "velocityX": 0.6389788773328549, + "velocityY": 0.1285931771210457, + "timestamp": 2.157177556173553 + }, + { + "x": 8.029805183410645, + "y": 7.180410385131836, + "heading": -1.5143595129762703e-19, + "angularVelocity": -2.854862345939135e-20, + "velocityX": 1.5619014793820894e-19, + "velocityY": 5.756387803346198e-20, + "timestamp": 2.250967884702838 + }, + { + "x": 7.977782055764913, + "y": 7.164211731514623, + "heading": 0.007845907070085154, + "angularVelocity": 0.08862253925843301, + "velocityX": -0.5876212439105888, + "velocityY": -0.1829700254287652, + "timestamp": 2.3394996186808683 + }, + { + "x": 7.873735810540943, + "y": 7.131814420329787, + "heading": 0.023538488906928237, + "angularVelocity": 0.17725374994617482, + "velocityX": -1.1752423741049713, + "velocityY": -0.36594009547893014, + "timestamp": 2.428031352658899 + }, + { + "x": 7.717666465286569, + "y": 7.0832184446557225, + "heading": 0.04707901098544824, + "angularVelocity": 0.2658992546600486, + "velocityX": -1.7628633060898011, + "velocityY": -0.548910243711303, + "timestamp": 2.5165630866369293 + }, + { + "x": 7.50957404571599, + "y": 7.018423792773334, + "heading": 0.07846909232757103, + "angularVelocity": 0.3545630468495328, + "velocityX": -2.3504839476228767, + "velocityY": -0.7318805243153491, + "timestamp": 2.60509482061496 + }, + { + "x": 7.249458590154704, + "y": 6.937430446851037, + "heading": 0.11771030908646642, + "angularVelocity": 0.4432446422955327, + "velocityX": -2.9381041562546777, + "velocityY": -0.9148510063340223, + "timestamp": 2.6936265545929903 + }, + { + "x": 6.9403504261875275, + "y": 6.841180717415368, + "heading": 0.11771031338168697, + "angularVelocity": 4.8516168957241446e-8, + "velocityX": -3.4914956488244515, + "velocityY": -1.0871777283787833, + "timestamp": 2.7821582885710208 + }, + { + "x": 6.631242265256751, + "y": 6.74493097822586, + "heading": 0.1177103176763249, + "angularVelocity": 4.850958782691505e-8, + "velocityX": -3.4914956145271363, + "velocityY": -1.0871778385521202, + "timestamp": 2.8706900225490513 + }, + { + "x": 6.3221341042798125, + "y": 6.6486812391846035, + "heading": 0.11771032197096283, + "angularVelocity": 4.850958799660226e-8, + "velocityX": -3.4914956150485508, + "velocityY": -1.087177836877585, + "timestamp": 2.9592217565270817 + }, + { + "x": 6.013025943341647, + "y": 6.552431500018828, + "heading": 0.11771032626560081, + "angularVelocity": 4.8509588352896574e-8, + "velocityX": -3.4914956146105998, + "velocityY": -1.0871778382840753, + "timestamp": 3.0477534905051122 + }, + { + "x": 5.703917779977134, + "y": 6.456181768647687, + "heading": 0.11771033056081937, + "angularVelocity": 4.851614628573155e-8, + "velocityX": -3.491495642017128, + "velocityY": -1.0871777502406696, + "timestamp": 3.1362852244831427 + }, + { + "x": 5.443802852291196, + "y": 6.375187913058336, + "heading": 0.15698715456634274, + "angularVelocity": 0.44364683984694225, + "velocityX": -2.9380981936995307, + "velocityY": -0.9148567632195014, + "timestamp": 3.224816958461173 + }, + { + "x": 5.235710915275229, + "y": 6.310392799606434, + "heading": 0.18840994792999097, + "angularVelocity": 0.35493254171940114, + "velocityX": -2.3504784969828454, + "velocityY": -0.7318857379206117, + "timestamp": 3.3133486924392037 + }, + { + "x": 5.079641966218732, + "y": 6.261796449920312, + "heading": 0.21197753584446782, + "angularVelocity": 0.2662049736914137, + "velocityX": -1.7628588308823359, + "velocityY": -0.548914468321395, + "timestamp": 3.401880426417234 + }, + { + "x": 4.975596000715217, + "y": 6.229398878335517, + "heading": 0.2276893987993674, + "angularVelocity": 0.17747153759349776, + "velocityX": -1.1752392145548016, + "velocityY": -0.3659430367966726, + "timestamp": 3.4904121603952647 + }, + { + "x": 4.923573017120361, + "y": 6.213200092315674, + "heading": 0.23554532977562304, + "angularVelocity": 0.08873576313556827, + "velocityX": -0.5876196168005193, + "velocityY": -0.18297152096741656, + "timestamp": 3.578943894373295 + }, + { + "x": 4.923573017120361, + "y": 6.213200092315674, + "heading": 0.23554532977562304, + "angularVelocity": -4.4557329542893123e-20, + "velocityX": 9.178744124267652e-20, + "velocityY": 4.4873769036430355e-20, + "timestamp": 3.6674756283513257 + }, + { + "x": 4.976262359083494, + "y": 6.204211295952416, + "heading": 0.22497404740616994, + "angularVelocity": -0.12050665436480094, + "velocityX": 0.600628769410924, + "velocityY": -0.10246720678209494, + "timestamp": 3.7551992683874307 + }, + { + "x": 5.081663106400117, + "y": 6.186229734618521, + "heading": 0.2051184155284092, + "angularVelocity": -0.2263430002401704, + "velocityX": 1.20150904902308, + "velocityY": -0.2049796534491144, + "timestamp": 3.8429229084235357 + }, + { + "x": 5.23980338600934, + "y": 6.1592505270678295, + "heading": 0.17793825550147288, + "angularVelocity": -0.30983848841372325, + "velocityX": 1.8027099598709833, + "velocityY": -0.3075477435681723, + "timestamp": 3.9306465484596407 + }, + { + "x": 5.450715913523779, + "y": 6.123268329544682, + "heading": 0.14677650230362854, + "angularVelocity": -0.35522640402312194, + "velocityX": 2.404283810243538, + "velocityY": -0.4101767494866671, + "timestamp": 4.018370188495745 + }, + { + "x": 5.71440675063786, + "y": 6.078282986612711, + "heading": 0.11861550472466739, + "angularVelocity": -0.321019483087692, + "velocityX": 3.0059267605123567, + "velocityY": -0.5128075272920207, + "timestamp": 4.10609382853185 + }, + { + "x": 6.03025451353813, + "y": 6.024436884344235, + "heading": 0.11767251558753837, + "angularVelocity": -0.010749544099411572, + "velocityX": 3.6004862859119227, + "velocityY": -0.6138151842116195, + "timestamp": 4.193817468567954 + }, + { + "x": 6.346475636674256, + "y": 5.970479408408474, + "heading": 0.11767250521570034, + "angularVelocity": -1.1823310146086215e-7, + "velocityX": 3.6047423819392157, + "velocityY": -0.6150847811781829, + "timestamp": 4.281541108604059 + }, + { + "x": 6.662696759700643, + "y": 5.916521931829571, + "heading": 0.11767249484386287, + "angularVelocity": -1.1823309505995875e-7, + "velocityX": 3.6047423806882386, + "velocityY": -0.6150847885096368, + "timestamp": 4.3692647486401635 + }, + { + "x": 6.978917882822774, + "y": 5.862564455811782, + "heading": 0.11767248447202486, + "angularVelocity": -1.182331011358331e-7, + "velocityX": 3.6047423817796624, + "velocityY": -0.6150847821132567, + "timestamp": 4.456988388676268 + }, + { + "x": 7.29476901407188, + "y": 5.808714720950228, + "heading": 0.11673860844372616, + "angularVelocity": -0.010645659800645866, + "velocityX": 3.6005246831881257, + "velocityY": -0.6138565937230951, + "timestamp": 4.544712028712373 + }, + { + "x": 7.558460925573039, + "y": 5.76372860364344, + "heading": 0.08862558626816161, + "angularVelocity": -0.3204725905581884, + "velocityX": 3.005939007918847, + "velocityY": -0.5128163547280221, + "timestamp": 4.632435668748477 + }, + { + "x": 7.769374450560142, + "y": 5.727745631951145, + "heading": 0.05752619060297154, + "angularVelocity": -0.3545155633349269, + "velocityX": 2.404295180869109, + "velocityY": -0.4101855745781148, + "timestamp": 4.720159308784582 + }, + { + "x": 7.927515620226354, + "y": 5.700765664089877, + "heading": 0.030393120465028865, + "angularVelocity": -0.3093016902487785, + "velocityX": 1.8027201060184643, + "velocityY": -0.3075564106797736, + "timestamp": 4.807882948820686 + }, + { + "x": 8.032916964145462, + "y": 5.682783456079591, + "heading": 0.010562795048926609, + "angularVelocity": -0.22605452085595826, + "velocityX": 1.2015158499547787, + "velocityY": -0.20498702519508957, + "timestamp": 4.895606588856791 + }, + { + "x": 8.085606575012207, + "y": 5.673794269561768, + "heading": 1.3549285413243613e-17, + "angularVelocity": -0.12040990369960944, + "velocityX": 0.6006318347603821, + "velocityY": -0.1024716543240064, + "timestamp": 4.983330228892895 + }, + { + "x": 8.085606575012207, + "y": 5.673794269561768, + "heading": 1.3789902561459072e-17, + "angularVelocity": 2.7952519766367576e-18, + "velocityX": -1.7194569329753236e-17, + "velocityY": 1.867684279393307e-18, + "timestamp": 5.071053868929 + }, + { + "x": 8.031831521176109, + "y": 5.680016482032018, + "heading": 0.01228794346008448, + "angularVelocity": 0.13914898483607424, + "velocityX": -0.6089500798164236, + "velocityY": 0.07046049255370945, + "timestamp": 5.1593616885984055 + }, + { + "x": 7.9242724093739, + "y": 5.6924629124746104, + "heading": 0.03642124685929677, + "angularVelocity": 0.27328614260390355, + "velocityX": -1.2180021226304998, + "velocityY": 0.1409436954642003, + "timestamp": 5.247669508267811 + }, + { + "x": 7.762915260533646, + "y": 5.711136820956851, + "heading": 0.07167786000455947, + "angularVelocity": 0.39924678558764254, + "velocityX": -1.8272124648114023, + "velocityY": 0.2114638154599267, + "timestamp": 5.335977327937217 + }, + { + "x": 7.5477357103225025, + "y": 5.736044315869435, + "heading": 0.11667225209980614, + "angularVelocity": 0.5095176425337026, + "velocityX": -2.4366987093181898, + "velocityY": 0.28205310702755476, + "timestamp": 5.424285147606622 + }, + { + "x": 7.278684003301139, + "y": 5.767200005582985, + "heading": 0.1676389581166344, + "angularVelocity": 0.5771482775549128, + "velocityX": -3.0467483856877364, + "velocityY": 0.3528078241562921, + "timestamp": 5.512592967276028 + }, + { + "x": 6.9578776989840305, + "y": 5.804154953260779, + "heading": 0.16763897467157093, + "angularVelocity": 1.8746852304717014e-7, + "velocityX": -3.6328187641604055, + "velocityY": 0.4184787691072228, + "timestamp": 5.6009007869454335 + }, + { + "x": 6.637087948695037, + "y": 5.84125338261765, + "heading": 0.16763898257122964, + "angularVelocity": 8.945593599492644e-8, + "velocityX": -3.632631305924235, + "velocityY": 0.42010355929694115, + "timestamp": 5.689208606614839 + }, + { + "x": 6.316298197633932, + "y": 5.878351805298053, + "heading": 0.16763899047088832, + "angularVelocity": 8.94559361551517e-8, + "velocityX": -3.632631314667653, + "velocityY": 0.4201034836924587, + "timestamp": 5.777516426284245 + }, + { + "x": 5.995508447484614, + "y": 5.915450235862703, + "heading": 0.16763899837054705, + "angularVelocity": 8.945593649758392e-8, + "velocityX": -3.632631304342544, + "velocityY": 0.4201035729738787, + "timestamp": 5.86582424595365 + }, + { + "x": 5.674700004894931, + "y": 5.952386616366171, + "heading": 0.16763901501362968, + "angularVelocity": 1.884666917047776e-7, + "velocityX": -3.6328429780135245, + "velocityY": 0.4182685139520664, + "timestamp": 5.954132065623056 + }, + { + "x": 5.405646774421004, + "y": 5.9835463265602655, + "heading": 0.21842500256096314, + "angularVelocity": 0.575101817001684, + "velocityX": -3.0467656373033782, + "velocityY": 0.35285335218042607, + "timestamp": 6.0424398852924615 + }, + { + "x": 5.190465936804369, + "y": 6.008455477656158, + "heading": 0.2632886887773245, + "angularVelocity": 0.5080375258308467, + "velocityX": -2.4367132879307802, + "velocityY": 0.28207186168953324, + "timestamp": 6.130747704961867 + }, + { + "x": 5.029107646558979, + "y": 6.027129895947731, + "heading": 0.29843729202517444, + "angularVelocity": 0.3980236787572713, + "velocityX": -1.8272253901122453, + "velocityY": 0.21146958855381004, + "timestamp": 6.219055524631273 + }, + { + "x": 4.921547644616536, + "y": 6.0395762783958595, + "heading": 0.3224912860382225, + "angularVelocity": 0.27238804109419384, + "velocityX": -1.218012202601224, + "velocityY": 0.1409431519736808, + "timestamp": 6.307363344300678 + }, + { + "x": 4.867772102355957, + "y": 6.045798301696777, + "heading": 0.3347373560918674, + "angularVelocity": 0.13867480931462675, + "velocityX": -0.608955610747675, + "velocityY": 0.0704583503953688, + "timestamp": 6.395671163970084 + }, + { + "x": 4.867772102355957, + "y": 6.045798301696777, + "heading": 0.33473735609186744, + "angularVelocity": 4.382681997898838e-18, + "velocityX": -1.543981625318142e-17, + "velocityY": 3.906065285900497e-18, + "timestamp": 6.483978983639489 + }, + { + "x": 4.903863667608455, + "y": 6.02320970068788, + "heading": 0.3176283286495271, + "angularVelocity": -0.21789742702603002, + "velocityX": 0.45965553754386096, + "velocityY": -0.28768426823466187, + "timestamp": 6.562497705387848 + }, + { + "x": 4.97607330221684, + "y": 5.978022456807334, + "heading": 0.28416125038798334, + "angularVelocity": -0.4262305538900727, + "velocityX": 0.9196486264741721, + "velocityY": -0.5754964277865501, + "timestamp": 6.641016427136207 + }, + { + "x": 5.084438498484905, + "y": 5.910221807895856, + "heading": 0.23546843743464, + "angularVelocity": -0.6201427107970054, + "velocityX": 1.3801192104904565, + "velocityY": -0.8634965954841877, + "timestamp": 6.719535148884566 + }, + { + "x": 5.22901600346336, + "y": 5.819784226720369, + "heading": 0.1734672887277426, + "angularVelocity": -0.7896352274506186, + "velocityX": 1.8413125145083822, + "velocityY": -1.151796401695455, + "timestamp": 6.798053870632924 + }, + { + "x": 5.409900027598653, + "y": 5.706672008533781, + "heading": 0.10216278395557017, + "angularVelocity": -0.9081210593403839, + "velocityX": 2.3037056654462664, + "velocityY": -1.4405764086315203, + "timestamp": 6.876572592381283 + }, + { + "x": 5.6271877913909565, + "y": 5.5708949038667654, + "heading": 0.034869610187120266, + "angularVelocity": -0.8570334853910997, + "velocityX": 2.767336998794774, + "velocityY": -1.7292322346021154, + "timestamp": 6.955091314129642 + }, + { + "x": 5.870534449133084, + "y": 5.418492861386192, + "heading": 0.03486959490632962, + "angularVelocity": -1.946133394861258e-7, + "velocityX": 3.0992182797119168, + "velocityY": -1.9409643851437186, + "timestamp": 7.0336100358780005 + }, + { + "x": 6.113881141917006, + "y": 5.266090874858927, + "heading": 0.03486957962493756, + "angularVelocity": -1.9462099894460953e-7, + "velocityX": 3.0992187259977815, + "velocityY": -1.9409636725326747, + "timestamp": 7.112128757626359 + }, + { + "x": 6.357227836710708, + "y": 5.11368889154076, + "heading": 0.034869564343545455, + "angularVelocity": -1.946209994358398e-7, + "velocityX": 3.0992187515939538, + "velocityY": -1.940963631662183, + "timestamp": 7.190647479374718 + }, + { + "x": 6.600574531479444, + "y": 4.961286908182731, + "heading": 0.03486954906215342, + "angularVelocity": -1.9462099865061453e-7, + "velocityX": 3.099218751276004, + "velocityY": -1.9409636321698667, + "timestamp": 7.269166201123077 + }, + { + "x": 6.843921226325159, + "y": 4.808884924947618, + "heading": 0.0348695337807613, + "angularVelocity": -1.9462099972826922e-7, + "velocityX": 3.0992187522563985, + "velocityY": -1.94096363060443, + "timestamp": 7.347684922871435 + }, + { + "x": 7.0872679867661, + "y": 4.656483046451265, + "heading": 0.03486951849933531, + "angularVelocity": -1.9462143091831554e-7, + "velocityX": 3.09921958766512, + "velocityY": -1.9409622966708406, + "timestamp": 7.426203644619794 + }, + { + "x": 7.326944519044576, + "y": 4.516245452265137, + "heading": 0.014066081529308494, + "angularVelocity": -0.2649487473407811, + "velocityX": 3.0524762367707154, + "velocityY": -1.7860402087997493, + "timestamp": 7.504722366368153 + }, + { + "x": 7.531642013688098, + "y": 4.397938693744427, + "heading": -0.04490743964327847, + "angularVelocity": -0.7510759199772535, + "velocityX": 2.6069896463616553, + "velocityY": -1.5067331190116233, + "timestamp": 7.5832410881165115 + }, + { + "x": 7.700074517764121, + "y": 4.302497700076849, + "heading": -0.10898814253347225, + "angularVelocity": -0.8161200470833309, + "velocityX": 2.1451253959001613, + "velocityY": -1.2155189430293092, + "timestamp": 7.66175980986487 + }, + { + "x": 7.832208824413671, + "y": 4.229887486816899, + "heading": -0.16889030145230455, + "angularVelocity": -0.7629028795299307, + "velocityX": 1.6828382289897996, + "velocityY": -0.9247503225110407, + "timestamp": 7.740278531613229 + }, + { + "x": 7.928084111700808, + "y": 4.180056487400386, + "heading": -0.2202565054701057, + "angularVelocity": -0.6541905277371027, + "velocityX": 1.2210500266981292, + "velocityY": -0.6346384442708259, + "timestamp": 7.818797253361588 + }, + { + "x": 7.987739128273294, + "y": 4.1529638156446245, + "heading": -0.26053803654554536, + "angularVelocity": -0.5130181716984267, + "velocityX": 0.7597553200582032, + "velocityY": -0.34504728493402975, + "timestamp": 7.897315975109946 + }, + { + "x": 8.011205673217773, + "y": 4.14857816696167, + "heading": -0.288054962130024, + "angularVelocity": -0.3504505036730943, + "velocityX": 0.2988656007376954, + "velocityY": -0.05585481507213175, + "timestamp": 7.975834696858305 + }, + { + "x": 7.99065265340485, + "y": 4.173303490266494, + "heading": -0.30126760937320984, + "angularVelocity": -0.14693899986760597, + "velocityX": -0.22857192203690077, + "velocityY": 0.27497247227940913, + "timestamp": 8.06575396441211 + }, + { + "x": 7.922647232931338, + "y": 4.22778256807063, + "heading": -0.29702135633462856, + "angularVelocity": 0.04722294958689644, + "velocityX": -0.7562941994920132, + "velocityY": 0.6058665654892884, + "timestamp": 8.155673231965915 + }, + { + "x": 7.807152216745808, + "y": 4.312023146342467, + "heading": -0.2765961187816114, + "angularVelocity": 0.22715084440382624, + "velocityX": -1.2844301263510791, + "velocityY": 0.9368468022877369, + "timestamp": 8.24559249951972 + }, + { + "x": 7.644109928873724, + "y": 4.426034622592684, + "heading": -0.2421410152647003, + "angularVelocity": 0.3831782047857269, + "velocityX": -1.813207472742426, + "velocityY": 1.2679315496202845, + "timestamp": 8.335511767073525 + }, + { + "x": 7.433422032287237, + "y": 4.569822041127431, + "heading": -0.19801570209656016, + "angularVelocity": 0.49072144790036093, + "velocityX": -2.343078433778598, + "velocityY": 1.5990723951205463, + "timestamp": 8.42543103462733 + }, + { + "x": 7.174950855403372, + "y": 4.743283480361943, + "heading": -0.1581400022906659, + "angularVelocity": 0.44346112786155, + "velocityX": -2.8744804524703853, + "velocityY": 1.9290797618065416, + "timestamp": 8.515350302181135 + }, + { + "x": 6.8965540900738835, + "y": 4.91826413098903, + "heading": -0.1581399896292387, + "angularVelocity": 1.4080883424863374e-7, + "velocityX": -3.0960746556671572, + "velocityY": 1.9459750439179786, + "timestamp": 8.60526956973494 + }, + { + "x": 6.618157289478816, + "y": 5.093244725508082, + "heading": -0.15813997696783524, + "angularVelocity": 1.4080856979053156e-7, + "velocityX": -3.096075047858723, + "velocityY": 1.9459744199356437, + "timestamp": 8.695188837288745 + }, + { + "x": 6.339760488657226, + "y": 5.268225319666733, + "heading": -0.15813996430643187, + "angularVelocity": 1.4080856886565356e-7, + "velocityX": -3.0960750503779084, + "velocityY": 1.9459744159275851, + "timestamp": 8.78510810484255 + }, + { + "x": 6.061363707929087, + "y": 5.443205945794379, + "heading": -0.1581399516450285, + "angularVelocity": 1.4080856880826848e-7, + "velocityX": -3.0960748269168854, + "velocityY": 1.945974771457558, + "timestamp": 8.875027372396355 + }, + { + "x": 5.782315347271137, + "y": 5.617145582087979, + "heading": -0.1581399318842445, + "angularVelocity": 2.1976139881035488e-7, + "velocityX": -3.1033211040223123, + "velocityY": 1.9343978329174265, + "timestamp": 8.96494663995016 + }, + { + "x": 5.545600487148874, + "y": 5.7664037944649005, + "heading": -0.0652854431922894, + "angularVelocity": 1.0326428497250173, + "velocityX": -2.63252656034616, + "velocityY": 1.6599135695540421, + "timestamp": 9.054865907503965 + }, + { + "x": 5.356283433091156, + "y": 5.885661177144069, + "heading": 0.01339587840576429, + "angularVelocity": 0.8750218250050805, + "velocityX": -2.105411434144947, + "velocityY": 1.326271731559752, + "timestamp": 9.14478517505777 + }, + { + "x": 5.2143192278784625, + "y": 5.975050723105011, + "heading": 0.07389681820108311, + "angularVelocity": 0.6728362167665074, + "velocityX": -1.5787962810945682, + "velocityY": 0.9941089200649296, + "timestamp": 9.234704442611575 + }, + { + "x": 5.119686824011737, + "y": 6.034621449602944, + "heading": 0.11484048218322652, + "angularVelocity": 0.45533805040886816, + "velocityX": -1.0524151991129291, + "velocityY": 0.6624912337312708, + "timestamp": 9.32462371016538 + }, + { + "x": 5.07237434387207, + "y": 6.064398765563965, + "heading": 0.13552729798344068, + "angularVelocity": 0.2300598788556157, + "velocityX": -0.5261662091648605, + "velocityY": 0.3311561222760583, + "timestamp": 9.414542977719185 + }, + { + "x": 5.07237434387207, + "y": 6.064398765563965, + "heading": 0.13552729798344068, + "angularVelocity": -1.77685785505055e-19, + "velocityX": 4.577974587806203e-18, + "velocityY": -2.482755140410445e-18, + "timestamp": 9.50446224527299 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "2ec21105-4b70-4b7c-b2f4-7deba5a7964c" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "7871c372-e258-469f-8065-11d2e0383713" + }, + { + "scope": [ + 0 + ], + "type": "StopPoint", + "uuid": "ebd8def5-8ade-470c-b55f-e28f299064fa" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint", + "uuid": "e6642415-db27-4b66-a654-d83064b34cdc" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint", + "uuid": "b6ba2f7c-8701-47cf-a560-b035a97d1a25" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint", + "uuid": "b8a8188a-b568-4775-ab73-460fa8627938" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint", + "uuid": "10d9c9fe-2435-4082-abeb-22b01e5bbf48" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "6Note": { + "waypoints": [ + { + "x": 2.20794415473938, + "y": 3.664973258972168, + "heading": -0.7354400074149414, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 3.175154209136963, + "y": 4.036977291107178, + "heading": 0.4964219619537652, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.9901087284088135, + "y": 4.7824883460998535, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 5 + }, + { + "x": 2.9455201625823975, + "y": 4.960843563079834, + "heading": 0.29423418735375584, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 6 + }, + { + "x": 2.8775510787963867, + "y": 5.338991165161133, + "heading": 0.18998808677928647, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.9705522060394287, + "y": 6.715405464172363, + "heading": 0.2551827989840428, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 7.862403869628906, + "y": 7.236210823059082, + "heading": 0.04995916288883634, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 6.09645414352417, + "y": 6.149875164031982, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 8.122806549072266, + "y": 5.580793380737305, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 5.977550983428955, + "y": 6.595761775970459, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 7.865139007568359, + "y": 6.432270050048828, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 2.20794415473938, + "y": 3.664973258972168, + "heading": -0.7354400074149414, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 2.2471188331781433, + "y": 3.6784892919371552, + "heading": -0.7354399814683132, + "angularVelocity": 3.3622066506767566e-7, + "velocityX": 0.5076319105372495, + "velocityY": 0.17514297271453227, + "timestamp": 0.07717142603841304 + }, + { + "x": 2.3200702936853683, + "y": 3.7036589776847935, + "heading": -0.7236015966081355, + "angularVelocity": 0.15340373332333, + "velocityX": 0.9453169942837708, + "velocityY": 0.32615291746856667, + "timestamp": 0.15434285207682608 + }, + { + "x": 2.393021754194593, + "y": 3.7288286634266474, + "heading": -0.6554112716508567, + "angularVelocity": 0.883621418675562, + "velocityX": 0.9453169943096871, + "velocityY": 0.3261529173936128, + "timestamp": 0.2315142781152391 + }, + { + "x": 2.4659732147038027, + "y": 3.7539983491685462, + "heading": -0.5475903529607853, + "angularVelocity": 1.3971611543941431, + "velocityX": 0.9453169943094869, + "velocityY": 0.3261529173941928, + "timestamp": 0.30868570415365215 + }, + { + "x": 2.538924675213058, + "y": 3.779168034910313, + "heading": -0.4137634894095547, + "angularVelocity": 1.7341504546594302, + "velocityX": 0.9453169943100757, + "velocityY": 0.32615291739248603, + "timestamp": 0.3858571301920652 + }, + { + "x": 2.6118761357224596, + "y": 3.804337720651655, + "heading": -0.2645452299064341, + "angularVelocity": 1.933594688646098, + "velocityX": 0.9453169943119766, + "velocityY": 0.3261529173869763, + "timestamp": 0.4630285562304782 + }, + { + "x": 2.684827596231993, + "y": 3.829507406392615, + "heading": -0.10835582804191357, + "angularVelocity": 2.023927895109458, + "velocityX": 0.9453169943136835, + "velocityY": 0.32615291738202923, + "timestamp": 0.5401999822688912 + }, + { + "x": 2.7577790567415006, + "y": 3.8546770921336493, + "heading": 0.04747751815628348, + "angularVelocity": 2.0193140673677465, + "velocityX": 0.9453169943133516, + "velocityY": 0.3261529173829912, + "timestamp": 0.6173714083073043 + }, + { + "x": 2.830730517250857, + "y": 3.879846777875123, + "heading": 0.19556311960827935, + "angularVelocity": 1.918917519786202, + "velocityX": 0.945316994311386, + "velocityY": 0.32615291738868823, + "timestamp": 0.6945428343457174 + }, + { + "x": 2.903681977760091, + "y": 3.905016463616951, + "heading": 0.32729474861452895, + "angularVelocity": 1.7070000616637375, + "velocityX": 0.9453169943098034, + "velocityY": 0.3261529173932753, + "timestamp": 0.7717142603841305 + }, + { + "x": 2.9766334382693023, + "y": 3.930186149358844, + "heading": 0.4317749140071391, + "angularVelocity": 1.3538711250535118, + "velocityX": 0.9453169943095098, + "velocityY": 0.3261529173941263, + "timestamp": 0.8488856864225436 + }, + { + "x": 3.049584898778474, + "y": 3.9553558351008515, + "heading": 0.49505305343016526, + "angularVelocity": 0.8199685125881792, + "velocityX": 0.9453169943090014, + "velocityY": 0.3261529173955999, + "timestamp": 0.9260571124609567 + }, + { + "x": 3.1225363031680886, + "y": 3.980525683498396, + "heading": 0.5001345013216448, + "angularVelocity": 0.06584623548293991, + "velocityX": 0.9453162671025689, + "velocityY": 0.3261550251127433, + "timestamp": 1.0032285384993698 + }, + { + "x": 3.175154209136963, + "y": 4.036977291107178, + "heading": 0.4964219619537652, + "angularVelocity": -0.04810769423946735, + "velocityX": 0.6818314584815773, + "velocityY": 0.7315091933208836, + "timestamp": 1.0803999645377829 + }, + { + "x": 3.190817987773919, + "y": 4.113035844022939, + "heading": 0.4926861160438778, + "angularVelocity": -0.04810841089874421, + "velocityX": 0.2017105408173478, + "velocityY": 0.9794451388776884, + "timestamp": 1.1580546997513483 + }, + { + "x": 3.168517113834866, + "y": 4.187419502094125, + "heading": 0.48074118507947583, + "angularVelocity": -0.1538210249709965, + "velocityX": -0.28717983362793426, + "velocityY": 0.957876655771434, + "timestamp": 1.2357094349649138 + }, + { + "x": 3.1462160636182532, + "y": 4.261803107315525, + "heading": 0.4134749167724562, + "angularVelocity": -0.8662223639295713, + "velocityX": -0.287182103644815, + "velocityY": 0.9578759751975264, + "timestamp": 1.3133641701784793 + }, + { + "x": 3.1239150134014713, + "y": 4.336186712536874, + "heading": 0.3115418578649691, + "angularVelocity": -1.312644472061511, + "velocityX": -0.2871821036469928, + "velocityY": 0.9578759751968734, + "timestamp": 1.3910189053920448 + }, + { + "x": 3.1016139631847954, + "y": 4.410570317758255, + "heading": 0.19368216783727144, + "angularVelocity": -1.5177398996154035, + "velocityX": -0.2871821036456218, + "velocityY": 0.9578759751972845, + "timestamp": 1.4686736406056102 + }, + { + "x": 3.0793129129682044, + "y": 4.484953922979661, + "heading": 0.07726837869911063, + "angularVelocity": -1.4991202895483509, + "velocityX": -0.28718210364453783, + "velocityY": 0.9578759751976095, + "timestamp": 1.5463283758191757 + }, + { + "x": 3.0570118627515384, + "y": 4.559337528201045, + "heading": -0.020189040323946267, + "angularVelocity": -1.2550093533257207, + "velocityX": -0.2871821036455004, + "velocityY": 0.9578759751973209, + "timestamp": 1.6239831110327412 + }, + { + "x": 3.0347108125347813, + "y": 4.6337211334224016, + "heading": -0.07962773247584773, + "angularVelocity": -0.765422636345789, + "velocityX": -0.28718210364667085, + "velocityY": 0.95787597519697, + "timestamp": 1.7016378462463067 + }, + { + "x": 3.0124097623180286, + "y": 4.708104738643759, + "heading": -0.08012314690355973, + "angularVelocity": -0.006379706612217704, + "velocityX": -0.2871821036466172, + "velocityY": 0.9578759751969861, + "timestamp": 1.7792925814598721 + }, + { + "x": 2.9901087284088135, + "y": 4.7824883460998535, + "heading": 0, + "angularVelocity": 1.0317870080067317, + "velocityX": -0.2871818936460597, + "velocityY": 0.9578760039748432, + "timestamp": 1.8569473166734376 + }, + { + "x": 2.981191001018429, + "y": 4.8181593864561965, + "heading": 0.050738184558630633, + "angularVelocity": 1.3799226553830732, + "velocityX": -0.24253477272728013, + "velocityY": 0.9701426481257134, + "timestamp": 1.8937161778561287 + }, + { + "x": 2.972273289175913, + "y": 4.853830430699488, + "heading": 0.1106008636938567, + "angularVelocity": 1.628080859991564, + "velocityX": -0.24253434987302874, + "velocityY": 0.9701427538387914, + "timestamp": 1.9304850390388197 + }, + { + "x": 2.963355577333478, + "y": 4.889501474942801, + "heading": 0.1746065344176323, + "angularVelocity": 1.7407574960169372, + "velocityX": -0.24253434987083392, + "velocityY": 0.9701427538393401, + "timestamp": 1.9672539002215108 + }, + { + "x": 2.954437865490993, + "y": 4.9251725191861, + "heading": 0.23753611441876185, + "angularVelocity": 1.7114911361669713, + "velocityX": -0.24253434987220052, + "velocityY": 0.9701427538389984, + "timestamp": 2.004022761404202 + }, + { + "x": 2.9455201625823975, + "y": 4.960843563079834, + "heading": 0.29423418735375584, + "angularVelocity": 1.5420132990598214, + "velocityX": -0.2425341068978591, + "velocityY": 0.9701427443318675, + "timestamp": 2.0407916225868927 + }, + { + "x": 2.9341919742056994, + "y": 5.0238681624588315, + "heading": 0.3430288217750536, + "angularVelocity": 0.762004365842858, + "velocityX": -0.17690734037688408, + "velocityY": 0.9842274760712535, + "timestamp": 2.1048262090125567 + }, + { + "x": 2.9228637942769513, + "y": 5.086892763356283, + "heading": 0.35143884659831104, + "angularVelocity": 0.1313356623770872, + "velocityX": -0.1769072084489637, + "velocityY": 0.9842274997842765, + "timestamp": 2.1688607954382206 + }, + { + "x": 2.9115356143482174, + "y": 5.149917364253737, + "heading": 0.33056040182371793, + "angularVelocity": -0.32604949824779894, + "velocityX": -0.17690720844873878, + "velocityY": 0.9842274997843169, + "timestamp": 2.2328953818638846 + }, + { + "x": 2.9002074344194813, + "y": 5.212941965151191, + "heading": 0.29058860737305475, + "angularVelocity": -0.6242219506963693, + "velocityX": -0.1769072084487773, + "velocityY": 0.98422749978431, + "timestamp": 2.2969299682895485 + }, + { + "x": 2.888879254490749, + "y": 5.275966566048645, + "heading": 0.2408226493649336, + "angularVelocity": -0.7771730995076082, + "velocityX": -0.17690720844871077, + "velocityY": 0.9842274997843219, + "timestamp": 2.3609645547152125 + }, + { + "x": 2.8775510787963867, + "y": 5.338991165161133, + "heading": 0.18998808677928647, + "angularVelocity": -0.7938610276597882, + "velocityX": -0.1769071423224252, + "velocityY": 0.9842274719092861, + "timestamp": 2.4249991411408764 + }, + { + "x": 2.879336047375825, + "y": 5.478935829486119, + "heading": 0.15080970804902258, + "angularVelocity": -0.2799334432597178, + "velocityX": 0.012753779424941127, + "velocityY": 0.999918654624096, + "timestamp": 2.5649551902432868 + }, + { + "x": 2.8811210197460353, + "y": 5.618880493762755, + "heading": 0.15294624001981122, + "angularVelocity": 0.015265735096775236, + "velocityX": 0.01275380651038498, + "velocityY": 0.9999186542786257, + "timestamp": 2.704911239345697 + }, + { + "x": 2.8829059921162457, + "y": 5.75882515803939, + "heading": 0.17635279292699202, + "angularVelocity": 0.16724216678947132, + "velocityX": 0.012753806510388455, + "velocityY": 0.9999186542786257, + "timestamp": 2.8448672884481074 + }, + { + "x": 2.8846909644864565, + "y": 5.898769822316026, + "heading": 0.209990869687123, + "angularVelocity": 0.24034743032447933, + "velocityX": 0.012753806510387863, + "velocityY": 0.9999186542786257, + "timestamp": 2.9848233375505178 + }, + { + "x": 2.886475936856667, + "y": 6.038714486592662, + "heading": 0.24721381499294584, + "angularVelocity": 0.26596167542987414, + "velocityX": 0.012753806510387369, + "velocityY": 0.9999186542786257, + "timestamp": 3.124779386652928 + }, + { + "x": 2.888260909226877, + "y": 6.178659150869297, + "heading": 0.28292073715992244, + "angularVelocity": 0.25512953813699535, + "velocityX": 0.012753806510387482, + "velocityY": 0.9999186542786257, + "timestamp": 3.2647354357553384 + }, + { + "x": 2.890045881597088, + "y": 6.318603815145933, + "heading": 0.311356671114116, + "angularVelocity": 0.203177598514417, + "velocityX": 0.01275380651038814, + "velocityY": 0.9999186542786257, + "timestamp": 3.4046914848577488 + }, + { + "x": 2.8918308539673, + "y": 6.458548479422569, + "heading": 0.32363800040319796, + "angularVelocity": 0.08775132884821014, + "velocityX": 0.012753806510403596, + "velocityY": 0.9999186542786255, + "timestamp": 3.544647533960159 + }, + { + "x": 2.893615842051929, + "y": 6.598493143498768, + "heading": 0.3039916671103851, + "angularVelocity": -0.14037502072123356, + "velocityX": 0.012753918791481205, + "velocityY": 0.9999186528464911, + "timestamp": 3.6846035830625694 + }, + { + "x": 2.9705522060394287, + "y": 6.715405464172363, + "heading": 0.2551827989840428, + "angularVelocity": -0.348744255352816, + "velocityX": 0.5497180327747238, + "velocityY": 0.8353502504778957, + "timestamp": 3.8245596321649797 + }, + { + "x": 3.067512358448162, + "y": 6.793764446329018, + "heading": 0.22544622618853677, + "angularVelocity": -0.34874429599630347, + "velocityX": 1.1371283545018185, + "velocityY": 0.9189777266914475, + "timestamp": 3.90982718531751 + }, + { + "x": 3.2146009111068476, + "y": 6.878954519586076, + "heading": 0.19634651748332468, + "angularVelocity": -0.34127528736702184, + "velocityX": 1.7250237308389438, + "velocityY": 0.999091332017781, + "timestamp": 3.9950947384700406 + }, + { + "x": 3.4118873367487743, + "y": 6.970423443263398, + "heading": 0.16905588214468348, + "angularVelocity": -0.32005885392094063, + "velocityX": 2.313733868837685, + "velocityY": 1.0727283743406777, + "timestamp": 4.080362291622571 + }, + { + "x": 3.6595052637221444, + "y": 7.066815632279669, + "heading": 0.1464467829149484, + "angularVelocity": -0.2651547792076474, + "velocityX": 2.904011172109284, + "velocityY": 1.1304673988221574, + "timestamp": 4.165629844775101 + }, + { + "x": 3.9571287842762186, + "y": 7.15979793947491, + "heading": 0.14644676578757745, + "angularVelocity": -2.0086621808526402e-7, + "velocityX": 3.490466297557207, + "velocityY": 1.090477019188179, + "timestamp": 4.2508973979276305 + }, + { + "x": 4.264875107150024, + "y": 7.209974468067706, + "heading": 0.14644676444758695, + "angularVelocity": -1.5715127861540166e-8, + "velocityX": 3.6091844024572333, + "velocityY": 0.5884598154592168, + "timestamp": 4.3361649510801605 + }, + { + "x": 4.572621681248356, + "y": 7.260149455807815, + "heading": 0.14644676310759733, + "angularVelocity": -1.57151175667415e-8, + "velocityX": 3.609187348765855, + "velocityY": 0.5884417446616913, + "timestamp": 4.4214325042326905 + }, + { + "x": 4.88036825534869, + "y": 7.310324443535645, + "heading": 0.14644676176760774, + "angularVelocity": -1.571511728249981e-8, + "velocityX": 3.6091873487893316, + "velocityY": 0.5884417445176963, + "timestamp": 4.5067000573852205 + }, + { + "x": 5.188114829449024, + "y": 7.3604994312634755, + "heading": 0.14644676042761817, + "angularVelocity": -1.571511667882576e-8, + "velocityX": 3.609187348789331, + "velocityY": 0.5884417445176952, + "timestamp": 4.5919676105377505 + }, + { + "x": 5.495861403549358, + "y": 7.410674418991307, + "heading": 0.14644675908762866, + "angularVelocity": -1.5715116325516118e-8, + "velocityX": 3.6091873487893316, + "velocityY": 0.5884417445176952, + "timestamp": 4.6772351636902805 + }, + { + "x": 5.803607977649693, + "y": 7.460849406719137, + "heading": 0.14644675774763907, + "angularVelocity": -1.571511716726206e-8, + "velocityX": 3.6091873487893316, + "velocityY": 0.5884417445176947, + "timestamp": 4.7625027168428105 + }, + { + "x": 6.111354551750398, + "y": 7.511024394444695, + "heading": 0.14644675640764948, + "angularVelocity": -1.571511704808039e-8, + "velocityX": 3.6091873487936774, + "velocityY": 0.588441744491041, + "timestamp": 4.84777026999534 + }, + { + "x": 6.419101172352554, + "y": 7.561199096954343, + "heading": 0.14644675506765978, + "angularVelocity": -1.571511840266027e-8, + "velocityX": 3.6091878941529654, + "velocityY": 0.5884383995386058, + "timestamp": 4.93303782314787 + }, + { + "x": 6.729953533870276, + "y": 7.585618195359195, + "heading": 0.14644675369982535, + "angularVelocity": -1.6041675590865194e-8, + "velocityX": 3.6456113729645336, + "velocityY": 0.28638207034239754, + "timestamp": 5.0183053763004 + }, + { + "x": 7.0406630536048125, + "y": 7.559445844562609, + "heading": 0.14644659261395782, + "angularVelocity": -0.0000018891813071650758, + "velocityX": 3.6439361544563957, + "velocityY": -0.306943847089974, + "timestamp": 5.10357292945293 + }, + { + "x": 7.303768657890303, + "y": 7.516650807168708, + "heading": 0.13291855766036426, + "angularVelocity": -0.15865395984089825, + "velocityX": 3.085647406990028, + "velocityY": -0.5018912330854348, + "timestamp": 5.18884048260546 + }, + { + "x": 7.517742289575736, + "y": 7.46187898590858, + "heading": 0.11459098071417309, + "angularVelocity": -0.21494198283613922, + "velocityX": 2.5094379253813877, + "velocityY": -0.6423524451575228, + "timestamp": 5.27410803575799 + }, + { + "x": 7.682254663626309, + "y": 7.3964939427656935, + "heading": 0.09395275194683736, + "angularVelocity": -0.24204082331783083, + "velocityX": 1.9293666578691, + "velocityY": -0.766822087950889, + "timestamp": 5.35937558891052 + }, + { + "x": 7.79716709924449, + "y": 7.321144775788464, + "heading": 0.07218032971430863, + "angularVelocity": -0.25534240666647107, + "velocityX": 1.3476689710166752, + "velocityY": -0.88367924481708, + "timestamp": 5.44464314206305 + }, + { + "x": 7.862403869628906, + "y": 7.236210823059082, + "heading": 0.04995916288883634, + "angularVelocity": -0.26060518924146586, + "velocityX": 0.765083176102371, + "velocityY": -0.9960876041258929, + "timestamp": 5.52991069521558 + }, + { + "x": 7.849595091966555, + "y": 7.089448693119536, + "heading": 0.016951434853172975, + "angularVelocity": -0.2606051434315723, + "velocityX": -0.10112884280544625, + "velocityY": -1.1587276131788378, + "timestamp": 5.656568702041625 + }, + { + "x": 7.725201262748097, + "y": 6.945847730654978, + "heading": -0.0160562821430182, + "angularVelocity": -0.2606050562718856, + "velocityX": -0.9821236914719388, + "velocityY": -1.133769321522416, + "timestamp": 5.783226708867669 + }, + { + "x": 7.545167611462926, + "y": 6.885160438976653, + "heading": -0.04514530953155864, + "angularVelocity": -0.2296659178323204, + "velocityX": -1.421415477763183, + "velocityY": -0.4791429551048147, + "timestamp": 5.909884715693714 + }, + { + "x": 7.365133889793344, + "y": 6.8244733560996425, + "heading": -0.05406183700742751, + "angularVelocity": -0.0703984509096719, + "velocityX": -1.4214160334675636, + "velocityY": -0.4791413065606847, + "timestamp": 6.0365427225197585 + }, + { + "x": 7.185100168123733, + "y": 6.76378627322272, + "heading": -0.05250329807242255, + "angularVelocity": 0.01230509601454327, + "velocityX": -1.421416033467796, + "velocityY": -0.4791413065599946, + "timestamp": 6.163200729345803 + }, + { + "x": 7.005066446454122, + "y": 6.703099190345796, + "heading": -0.04562573507290899, + "angularVelocity": 0.05430026235100232, + "velocityX": -1.421416033467796, + "velocityY": -0.47914130655999465, + "timestamp": 6.289858736171848 + }, + { + "x": 6.825032724784511, + "y": 6.642412107468873, + "heading": -0.036269834978042204, + "angularVelocity": 0.07386741927582839, + "velocityX": -1.421416033467796, + "velocityY": -0.47914130655999443, + "timestamp": 6.416516742997892 + }, + { + "x": 6.6449990031149, + "y": 6.58172502459195, + "heading": -0.026195987787784713, + "angularVelocity": 0.07953581019228782, + "velocityX": -1.421416033467796, + "velocityY": -0.4791413065599945, + "timestamp": 6.543174749823937 + }, + { + "x": 6.464965281445298, + "y": 6.5210379417150035, + "heading": -0.01685149454173795, + "angularVelocity": 0.07377735904902329, + "velocityX": -1.4214160334677322, + "velocityY": -0.47914130656018417, + "timestamp": 6.669832756649981 + }, + { + "x": 6.284931579179647, + "y": 6.460350801274506, + "heading": -0.010001719998819724, + "angularVelocity": 0.054080864799318604, + "velocityX": -1.4214158802681582, + "velocityY": -0.4791417610403343, + "timestamp": 6.796490763476026 + }, + { + "x": 6.142981170634808, + "y": 6.3340767179594515, + "heading": -0.005000858650557523, + "angularVelocity": 0.03948318368160243, + "velocityX": -1.1207377417501938, + "velocityY": -0.9969688176802427, + "timestamp": 6.92314877030207 + }, + { + "x": 6.09645414352417, + "y": 6.149875164031982, + "heading": 0, + "angularVelocity": 0.039483162382477774, + "velocityX": -0.3673437493334494, + "velocityY": -1.4543222220484673, + "timestamp": 7.049806777128115 + }, + { + "x": 6.165600746854775, + "y": 5.968104158414488, + "heading": 0.00511908966767943, + "angularVelocity": 0.03948315266730385, + "velocityX": 0.5333225383693347, + "velocityY": -1.4019860622035414, + "timestamp": 7.179459282611575 + }, + { + "x": 6.326507175654422, + "y": 5.858874551218305, + "heading": 0.010238176429586542, + "angularVelocity": 0.03948313025530103, + "velocityX": 1.2410591542336487, + "velocityY": -0.8424797252392536, + "timestamp": 7.309111788095034 + }, + { + "x": 6.5166850085952825, + "y": 5.8182005038026094, + "heading": 0.013042835442353568, + "angularVelocity": 0.021632123515804805, + "velocityX": 1.466827287537449, + "velocityY": -0.31371586121378253, + "timestamp": 7.438764293578494 + }, + { + "x": 6.706862852151511, + "y": 5.777526506020753, + "heading": 0.012869451393311492, + "angularVelocity": -0.0013372980985972803, + "velocityX": 1.46682736941299, + "velocityY": -0.31371547839172276, + "timestamp": 7.5684167990619535 + }, + { + "x": 6.897040695707745, + "y": 5.736852508238917, + "heading": 0.011189038201757389, + "angularVelocity": -0.012960900256479425, + "velocityX": 1.4668273694130223, + "velocityY": -0.31371547839157227, + "timestamp": 7.698069304545413 + }, + { + "x": 7.087218539263978, + "y": 5.69617851045708, + "heading": 0.00878429042310287, + "angularVelocity": -0.018547638317558075, + "velocityX": 1.4668273694130223, + "velocityY": -0.3137154783915722, + "timestamp": 7.827721810028873 + }, + { + "x": 7.277396382820212, + "y": 5.655504512675243, + "heading": 0.006107038226975533, + "angularVelocity": -0.020649444344774476, + "velocityX": 1.4668273694130223, + "velocityY": -0.3137154783915722, + "timestamp": 7.957374315512332 + }, + { + "x": 7.467574226376446, + "y": 5.614830514893406, + "heading": 0.0034846320252397466, + "angularVelocity": -0.020226421324935615, + "velocityX": 1.4668273694130223, + "velocityY": -0.31371547839157227, + "timestamp": 8.087026820995792 + }, + { + "x": 7.657752069932681, + "y": 5.574156517111579, + "heading": 0.001269475989498469, + "angularVelocity": -0.017085331498009396, + "velocityX": 1.466827369413038, + "velocityY": -0.3137154783914988, + "timestamp": 8.216679326479252 + }, + { + "x": 7.847929918666442, + "y": 5.533482543538116, + "heading": 4.380350364958381e-9, + "angularVelocity": -0.009791338812742587, + "velocityX": 1.4668274093469016, + "velocityY": -0.31371529167421114, + "timestamp": 8.346331831962711 + }, + { + "x": 8.042166177633085, + "y": 5.5431907443044945, + "heading": 1.962321228485874e-9, + "angularVelocity": -1.865007678883919e-8, + "velocityX": 1.4981296215002158, + "velocityY": 0.07487862061103963, + "timestamp": 8.475984337446171 + }, + { + "x": 8.122806549072266, + "y": 5.580793380737305, + "heading": 0, + "angularVelocity": -1.5135236051494907e-8, + "velocityX": 0.621973105248157, + "velocityY": 0.2900262998767247, + "timestamp": 8.60563684292963 + }, + { + "x": 8.11718410945889, + "y": 5.625307353924722, + "heading": -1.3275801458767835e-9, + "angularVelocity": -1.3277978561229321e-8, + "velocityX": -0.05623360725094295, + "velocityY": 0.4452126578849198, + "timestamp": 8.705620462913968 + }, + { + "x": 8.043750642119495, + "y": 5.685331009202492, + "heading": -2.46038264579736e-9, + "angularVelocity": -1.132988316080594e-8, + "velocityX": -0.7344549773005545, + "velocityY": 0.6003348878380743, + "timestamp": 8.805604082898306 + }, + { + "x": 7.902503945412699, + "y": 5.760854708948911, + "heading": -3.384756088443973e-9, + "angularVelocity": -9.245251518787126e-9, + "velocityX": -1.4126983673696807, + "velocityY": 0.7553607257902549, + "timestamp": 8.905587702882643 + }, + { + "x": 7.693440340935021, + "y": 5.8518623369658735, + "heading": -4.077873413911931e-9, + "angularVelocity": -6.932312274821422e-9, + "velocityX": -2.090978547497895, + "velocityY": 0.910225375367175, + "timestamp": 9.005571322866981 + }, + { + "x": 7.416552437959587, + "y": 5.958321458738986, + "heading": -4.493798325640354e-9, + "angularVelocity": -4.159935597905052e-9, + "velocityX": -2.769332646998966, + "velocityY": 1.0647656266808398, + "timestamp": 9.105554942851319 + }, + { + "x": 7.071817789543997, + "y": 6.0801331226760436, + "heading": -4.492422504241304e-9, + "angularVelocity": 1.3751436668540922e-11, + "velocityX": -3.447911252686312, + "velocityY": 1.2183162000152656, + "timestamp": 9.205538562835656 + }, + { + "x": 6.719237507782601, + "y": 6.176923161975723, + "heading": -4.492422414840064e-9, + "angularVelocity": 8.941669409693866e-16, + "velocityX": -3.5263804392355262, + "velocityY": 0.9680589613760336, + "timestamp": 9.305522182819994 + }, + { + "x": 6.434609797569398, + "y": 6.288588608635183, + "heading": -3.44266509070979e-9, + "angularVelocity": 1.0499298806305677e-8, + "velocityX": -2.846743398964724, + "velocityY": 1.1168374043775104, + "timestamp": 9.405505802804331 + }, + { + "x": 6.218300222393017, + "y": 6.387157705629217, + "heading": -2.434030100368673e-9, + "angularVelocity": 1.0088007894569008e-8, + "velocityX": -2.163450125018647, + "velocityY": 0.9858524524149839, + "timestamp": 9.505489422788669 + }, + { + "x": 6.070072189647912, + "y": 6.471451374683713, + "heading": -1.514234809843918e-9, + "angularVelocity": 9.199464869761595e-9, + "velocityX": -1.482523164732402, + "velocityY": 0.8430747860496444, + "timestamp": 9.605473042773006 + }, + { + "x": 5.9898377265253275, + "y": 6.541056287298709, + "heading": -7.000413265076412e-10, + "angularVelocity": 8.143273212076663e-9, + "velocityX": -0.8024760766578407, + "velocityY": 0.6961631575356222, + "timestamp": 9.705456662757344 + }, + { + "x": 5.977550983428955, + "y": 6.595761775970459, + "heading": 0, + "angularVelocity": 7.001564000000435e-9, + "velocityX": -0.12288755966089779, + "velocityY": 0.5471445088509955, + "timestamp": 9.805440282741682 + }, + { + "x": 6.0304601481193325, + "y": 6.634882466873651, + "heading": 5.703476949438232e-10, + "angularVelocity": 5.834627878284655e-9, + "velocityX": 0.5412584535856627, + "velocityY": 0.40020296696956553, + "timestamp": 9.903192408927895 + }, + { + "x": 6.148290994089988, + "y": 6.659639301678495, + "heading": 1.0266247362892406e-9, + "angularVelocity": 4.667690900343943e-9, + "velocityX": 1.205404429904499, + "velocityY": 0.25326134366988606, + "timestamp": 10.000944535114108 + }, + { + "x": 6.33104351533712, + "y": 6.670032267176693, + "heading": 1.3688309489429736e-9, + "angularVelocity": 3.5007521312183917e-9, + "velocityX": 1.869550344804717, + "velocityY": 0.10631958524989513, + "timestamp": 10.098696661300322 + }, + { + "x": 6.578717699898933, + "y": 6.66606133715871, + "heading": 1.5969659946550133e-9, + "angularVelocity": 2.333809901831043e-9, + "velocityX": 2.5336961373362707, + "velocityY": -0.040622441292512496, + "timestamp": 10.196448787486535 + }, + { + "x": 6.891313512118934, + "y": 6.647726434030782, + "heading": 1.7110289169133522e-9, + "angularVelocity": 1.1668578874288627e-9, + "velocityX": 3.1978415651034835, + "velocityY": -0.18756526161575324, + "timestamp": 10.294200913672748 + }, + { + "x": 7.215922010510692, + "y": 6.575907742022885, + "heading": 1.1406885772349463e-9, + "angularVelocity": -5.834552635062462e-9, + "velocityX": 3.3207308226516776, + "velocityY": -0.7347020963342302, + "timestamp": 10.391953039858961 + }, + { + "x": 7.475608809304632, + "y": 6.518452697971138, + "heading": 6.844139951960176e-10, + "angularVelocity": -4.667665741975464e-9, + "velocityX": 2.656584658945199, + "velocityY": -0.5877626023202006, + "timestamp": 10.489705166045175 + }, + { + "x": 7.670373908426623, + "y": 6.475361385107564, + "heading": 3.4220728009986415e-10, + "angularVelocity": -3.500757271088463e-9, + "velocityX": 1.9924384944803746, + "velocityY": -0.4408222568461895, + "timestamp": 10.587457292231388 + }, + { + "x": 7.80021730785198, + "y": 6.446633831176134, + "heading": 1.1406915587464051e-10, + "angularVelocity": -2.3338413945406575e-9, + "velocityX": 1.3282923297629874, + "velocityY": -0.293881627552551, + "timestamp": 10.685209418417601 + }, + { + "x": 7.865139007568359, + "y": 6.432270050048828, + "heading": 0, + "angularVelocity": -1.1669216564398507e-9, + "velocityX": 0.6641461649193595, + "velocityY": -0.1469408563491806, + "timestamp": 10.782961544603815 + }, + { + "x": 7.865139007568359, + "y": 6.432270050048828, + "heading": 0, + "angularVelocity": 0, + "velocityX": -1.888057920334398e-31, + "velocityY": -2.3966916927967252e-29, + "timestamp": 10.880713670790028 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "uuid": "b1a6932a-81ed-4f77-9ef6-e63e2a832e36" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "uuid": "ef354677-2b6b-4171-aaef-587377d39b69" + }, + { + "scope": [ + 0, + 5 + ], + "type": "MaxVelocity", + "uuid": "a1406cd1-958e-4f06-b3b2-a0ee3d2846df", + "velocity": 1 + }, + { + "scope": [ + 6, + 8 + ], + "type": "MaxVelocity", + "uuid": "8014ce42-8982-4933-be8d-91d8f4c2a730", + "velocity": 1.5 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + } +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3Spike.traj b/src/main/deploy/choreo/3Spike.traj new file mode 100644 index 00000000..5b447cef --- /dev/null +++ b/src/main/deploy/choreo/3Spike.traj @@ -0,0 +1,490 @@ +{ + "samples": [ + { + "x": 1.3269914388656616, + "y": 4.39792537689209, + "heading": -0.8519661022091116, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.333513997411188, + "y": 4.387203709271714, + "heading": -0.8332351514575969, + "angularVelocity": 0.40842901188133685, + "velocityX": 0.14222460872532702, + "velocityY": -0.2337863235028909, + "timestamp": 0.04586097022156875 + }, + { + "x": 1.3467826201629625, + "y": 4.365882340785122, + "heading": -0.7959323522941728, + "angularVelocity": 0.8133887918899778, + "velocityX": 0.2893227659089947, + "velocityY": -0.4649131578242103, + "timestamp": 0.0917219404431375 + }, + { + "x": 1.367096650503852, + "y": 4.334120690383467, + "heading": -0.7403859703551774, + "angularVelocity": 1.21119072864428, + "velocityX": 0.44294811563615344, + "velocityY": -0.6925638565474022, + "timestamp": 0.13758291066470624 + }, + { + "x": 1.394861349124937, + "y": 4.292141697873546, + "heading": -0.6671219475387827, + "angularVelocity": 1.5975244845984082, + "velocityX": 0.6054101883790317, + "velocityY": -0.9153533452761209, + "timestamp": 0.183443880886275 + }, + { + "x": 1.430646384004042, + "y": 4.240291060193454, + "heading": -0.5768914970162948, + "angularVelocity": 1.9674780120559225, + "velocityX": 0.7802938905613228, + "velocityY": -1.130604900628666, + "timestamp": 0.22930485110784377 + }, + { + "x": 1.4753118159731884, + "y": 4.179187224074356, + "heading": -0.4707408110774181, + "angularVelocity": 2.3146192814070323, + "velocityX": 0.9739312481474731, + "velocityY": -1.3323712041826856, + "timestamp": 0.27516582132941253 + }, + { + "x": 1.5302943233607504, + "y": 4.110166528705352, + "heading": -0.3504561598343917, + "angularVelocity": 2.6228108708100475, + "velocityX": 1.1988954250624084, + "velocityY": -1.5049985867185927, + "timestamp": 0.3210267915509813 + }, + { + "x": 1.5980403774204008, + "y": 4.0370266304131235, + "heading": -0.2213773500302043, + "angularVelocity": 2.8145677943699647, + "velocityX": 1.477204990045953, + "velocityY": -1.5948179451692148, + "timestamp": 0.36688776177255006 + }, + { + "x": 1.6779338120627578, + "y": 3.9699528764898666, + "heading": -0.09749875706374012, + "angularVelocity": 2.7011768911117184, + "velocityX": 1.742079032701809, + "velocityY": -1.4625454629329036, + "timestamp": 0.4127487319941188 + }, + { + "x": 1.762553862107012, + "y": 3.914807637304757, + "heading": 0.010619630767232302, + "angularVelocity": 2.3575250874244156, + "velocityX": 1.8451430407038498, + "velocityY": -1.2024437973877604, + "timestamp": 0.4586097022156876 + }, + { + "x": 1.8480147040992523, + "y": 3.872006113916856, + "heading": 0.09844406396935727, + "angularVelocity": 1.9150147233653705, + "velocityX": 1.8634765374424491, + "velocityY": -0.9332886587683153, + "timestamp": 0.5044706724372563 + }, + { + "x": 1.9326347476825954, + "y": 3.8414277812280493, + "heading": 0.16488095802435299, + "angularVelocity": 1.4486587120599115, + "velocityX": 1.8451428998234738, + "velocityY": -0.6667615739717827, + "timestamp": 0.5503316426588251 + }, + { + "x": 2.015500889838408, + "y": 3.8229408161865948, + "heading": 0.20962371332846336, + "angularVelocity": 0.9756172860701385, + "velocityX": 1.8068990201354285, + "velocityY": -0.40310889525751753, + "timestamp": 0.5961926128803938 + }, + { + "x": 2.096055269241333, + "y": 3.8164384365081783, + "heading": 0.23255650839848893, + "angularVelocity": 0.5000503687390396, + "velocityX": 1.7564909554625945, + "velocityY": -0.14178460784849248, + "timestamp": 0.6420535831019626 + }, + { + "x": 2.169861583576719, + "y": 3.8209387927505536, + "heading": 0.23468746359287693, + "angularVelocity": 0.049123667766486946, + "velocityX": 1.7014139358860232, + "velocityY": 0.10374408878397706, + "timestamp": 0.6854329823055731 + }, + { + "x": 2.240363784904268, + "y": 3.8361456579806616, + "heading": 0.21972330792773048, + "angularVelocity": -0.3449599565662249, + "velocityX": 1.6252461449876687, + "velocityY": 0.35055499866955947, + "timestamp": 0.7288123815091837 + }, + { + "x": 2.3064164966295926, + "y": 3.861977794130398, + "heading": 0.19077614253377356, + "angularVelocity": -0.6673021278622857, + "velocityX": 1.5226746552042265, + "velocityY": 0.5954931747322527, + "timestamp": 0.7721917807127943 + }, + { + "x": 2.366685094129252, + "y": 3.8980997052524633, + "heading": 0.1512071128503967, + "angularVelocity": -0.9121617728648345, + "velocityX": 1.3893368420520344, + "velocityY": 0.8326973583133246, + "timestamp": 0.8155711799164048 + }, + { + "x": 2.41978296792177, + "y": 3.943862475881486, + "heading": 0.10416005613005118, + "angularVelocity": -1.084548370518457, + "velocityX": 1.2240343289055675, + "velocityY": 1.0549424719836538, + "timestamp": 0.8589505791200154 + }, + { + "x": 2.4643919768040097, + "y": 3.9983256079338245, + "heading": 0.0526756800424131, + "angularVelocity": -1.18683930697115, + "velocityX": 1.0283454750688807, + "velocityY": 1.2555068316346145, + "timestamp": 0.9023299783236259 + }, + { + "x": 2.499344825744629, + "y": 4.060287952423096, + "heading": 3.33452988603619e-32, + "angularVelocity": -1.214301742519955, + "velocityX": 0.805747649398295, + "velocityY": 1.4283818039627032, + "timestamp": 0.9457093775272365 + }, + { + "x": 2.523403938669763, + "y": 4.12670444317368, + "heading": -0.049468562293879026, + "angularVelocity": -1.1662999834075207, + "velocityX": 0.5672318277350724, + "velocityY": 1.5658743344958055, + "timestamp": 0.9881243333684852 + }, + { + "x": 2.5364666185648104, + "y": 4.197030421591671, + "heading": -0.09653835267359968, + "angularVelocity": -1.109745122826349, + "velocityX": 0.3079734408763396, + "velocityY": 1.658046720152395, + "timestamp": 1.0305392892097338 + }, + { + "x": 2.537870729336966, + "y": 4.267128606265824, + "heading": -0.14073751847846447, + "angularVelocity": -1.0420655857875718, + "velocityX": 0.03310414320388964, + "velocityY": 1.65267612057683, + "timestamp": 1.0729542450509824 + }, + { + "x": 2.5301525844049313, + "y": 4.329958394850495, + "heading": -0.1822098664897901, + "angularVelocity": -0.9777765221906387, + "velocityX": -0.1819675343037494, + "velocityY": 1.4813121300852452, + "timestamp": 1.115369200892231 + }, + { + "x": 2.5191102993462846, + "y": 4.381587731788539, + "heading": -0.22206682765430108, + "angularVelocity": -0.9396912097159363, + "velocityX": -0.26033942131110027, + "velocityY": 1.217243680066123, + "timestamp": 1.1577841567334795 + }, + { + "x": 2.5084288948755984, + "y": 4.421531111089479, + "heading": -0.2611721666603998, + "angularVelocity": -0.9219705226727761, + "velocityX": -0.25183108785176694, + "velocityY": 0.94172865463871, + "timestamp": 1.200199112574728 + }, + { + "x": 2.5, + "y": 4.45, + "heading": -0.3, + "angularVelocity": -0.9154278855063672, + "velocityX": -0.1987245939179124, + "velocityY": 0.6711993056665078, + "timestamp": 1.2426140684159765 + }, + { + "x": 2.5193699694803637, + "y": 4.526734764577753, + "heading": -0.6391389887177107, + "angularVelocity": -2.999645914621537, + "velocityX": 0.17132518451448528, + "velocityY": 0.6787102949897058, + "timestamp": 1.3556737422476777 + }, + { + "x": 2.5387399844003316, + "y": 4.6034695176852845, + "heading": -1.2190938747349336, + "angularVelocity": -5.12963523033465, + "velocityX": 0.17132558642263376, + "velocityY": 0.6787101935368887, + "timestamp": 1.4687334160793788 + }, + { + "x": 2.558109999348224, + "y": 4.680204270785767, + "heading": -1.511597411804606, + "angularVelocity": -2.58716063081064, + "velocityX": 0.17132558666962455, + "velocityY": 0.6787101934745414, + "timestamp": 1.58179308991108 + }, + { + "x": 2.577480014296659, + "y": 4.756939023886113, + "heading": -1.4587662970367927, + "angularVelocity": 0.46728522184188176, + "velocityX": 0.1713255866744186, + "velocityY": 0.6787101934733313, + "timestamp": 1.694852763742781 + }, + { + "x": 2.5968501077533794, + "y": 4.833673757168711, + "heading": -1.0457294063544558, + "angularVelocity": 3.6532644813497406, + "velocityX": 0.17132628107130865, + "velocityY": 0.6787100181875982, + "timestamp": 1.8079124375744822 + }, + { + "x": 2.6400270462036133, + "y": 4.9, + "heading": -0.3, + "angularVelocity": 6.5958920725752055, + "velocityX": 0.3818951265904639, + "velocityY": 0.5866480999230684, + "timestamp": 1.9209721114061833 + }, + { + "x": 2.65028611673258, + "y": 4.911101822443869, + "heading": -0.12187108053489694, + "angularVelocity": 6.729487849665297, + "velocityX": 0.3875748569117997, + "velocityY": 0.41941297050192267, + "timestamp": 1.94744201869251 + }, + { + "x": 2.660785716281329, + "y": 4.917835219089741, + "heading": 0.060531873273454315, + "angularVelocity": 6.890955523012834, + "velocityX": 0.3966617425279967, + "velocityY": 0.25437930601858355, + "timestamp": 1.973911925978837 + }, + { + "x": 2.6714281364807704, + "y": 4.920121500261387, + "heading": 0.24615978486608456, + "angularVelocity": 7.012790395700312, + "velocityX": 0.4020573281319579, + "velocityY": 0.0863728439587885, + "timestamp": 2.0003818332651635 + }, + { + "x": 2.6820026092586784, + "y": 4.9178796942564835, + "heading": 0.4333660134100134, + "angularVelocity": 7.072417236634274, + "velocityX": 0.3994903595061169, + "velocityY": -0.08469262776982427, + "timestamp": 2.02685174055149 + }, + { + "x": 2.692177602021045, + "y": 4.9111165494503615, + "heading": 0.6193545789499477, + "angularVelocity": 7.02641545087723, + "velocityX": 0.3843985040183, + "velocityY": -0.25550315431651316, + "timestamp": 2.0533216478378167 + }, + { + "x": 2.701026406563282, + "y": 4.901195651201101, + "heading": 0.7946225445644793, + "angularVelocity": 6.621404590452321, + "velocityX": 0.3342967712927192, + "velocityY": -0.3747991310262394, + "timestamp": 2.0797915551241433 + }, + { + "x": 2.7092405777751063, + "y": 4.892429852217793, + "heading": 0.9506886625791671, + "angularVelocity": 5.89598279761655, + "velocityX": 0.3103211175986155, + "velocityY": -0.3311609250643644, + "timestamp": 2.10626146241047 + }, + { + "x": 2.717843250419752, + "y": 4.886627760240061, + "heading": 1.093906101120617, + "angularVelocity": 5.410575752769251, + "velocityX": 0.3249982159586029, + "velocityY": -0.21919578013514107, + "timestamp": 2.1327313696967964 + }, + { + "x": 2.7263555670649233, + "y": 4.884431942213397, + "heading": 1.2272956538012554, + "angularVelocity": 5.039290513478366, + "velocityX": 0.3215846792772284, + "velocityY": -0.08295525945410186, + "timestamp": 2.159201276983123 + }, + { + "x": 2.7346242420728504, + "y": 4.885911244753091, + "heading": 1.3510610914689725, + "angularVelocity": 4.675703482031048, + "velocityX": 0.3123802028652458, + "velocityY": 0.05588620026853448, + "timestamp": 2.1856711842694496 + }, + { + "x": 2.742537461432789, + "y": 4.891093691725606, + "heading": 1.4652638899797559, + "angularVelocity": 4.31443893155514, + "velocityX": 0.298951532936642, + "velocityY": 0.1957863666259237, + "timestamp": 2.212141091555776 + }, + { + "x": 2.75, + "y": 4.9, + "heading": 1.57, + "angularVelocity": 3.956799277281427, + "velocityX": 0.2819253761068593, + "velocityY": 0.33646919039249157, + "timestamp": 2.238610998842103 + }, + { + "x": 2.7674728869027185, + "y": 4.97015192039149, + "heading": 1.8148137759040575, + "angularVelocity": 2.798622019433345, + "velocityX": 0.19974368618937863, + "velocityY": 0.801951231658216, + "timestamp": 2.326087540584466 + }, + { + "x": 2.7760292561397533, + "y": 5.08021838751503, + "heading": 1.9563256399870756, + "angularVelocity": 1.6177121461865793, + "velocityX": 0.09781330019007135, + "velocityY": 1.2582398084243995, + "timestamp": 2.413564082326829 + }, + { + "x": 2.7742938765095486, + "y": 5.226701796198958, + "heading": 1.976040818465192, + "angularVelocity": 0.22537674770204888, + "velocityX": -0.019838228576937924, + "velocityY": 1.6745450353461948, + "timestamp": 2.501040624069192 + }, + { + "x": 2.762838623484245, + "y": 5.364431983374006, + "heading": 1.782825401942879, + "angularVelocity": -2.2087683471915627, + "velocityX": -0.13095228500277664, + "velocityY": 1.5744813916020168, + "timestamp": 2.588517165811555 + }, + { + "x": 2.7543895903343842, + "y": 5.455020359988401, + "heading": 1.6426814209537794, + "angularVelocity": -1.6020750043120497, + "velocityX": -0.09658627309188043, + "velocityY": 1.0355733641276863, + "timestamp": 2.675993707553918 + }, + { + "x": 2.75, + "y": 5.5, + "heading": 1.57, + "angularVelocity": -0.830867561818363, + "velocityX": -0.05018019970785374, + "velocityY": 0.5141908803856656, + "timestamp": 2.7634702492962813 + }, + { + "x": 2.75, + "y": 5.5, + "heading": 1.57, + "angularVelocity": -2.798206983013458e-30, + "velocityX": 7.872829382238201e-31, + "velocityY": 0, + "timestamp": 2.8509467910386443 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/6Note.traj b/src/main/deploy/choreo/6Note.traj new file mode 100644 index 00000000..3e97bf68 --- /dev/null +++ b/src/main/deploy/choreo/6Note.traj @@ -0,0 +1,1759 @@ +{ + "samples": [ + { + "x": 1.2134901285191984, + "y": 3.702403783800421, + "heading": -0.9272952180015092, + "angularVelocity": 2.9275724560072123e-13, + "velocityX": 2.454991007161089e-12, + "velocityY": -1.0608084392054969e-12, + "timestamp": 0 + }, + { + "x": 1.2214199688965524, + "y": 3.6828844138661974, + "heading": -0.9068235786762225, + "angularVelocity": 0.11838487813596572, + "velocityX": 0.045857255063974624, + "velocityY": -0.1128780258036785, + "timestamp": 0.1729244447864887 + }, + { + "x": 1.2371939281344813, + "y": 3.6459016271317672, + "heading": -0.8661542466206761, + "angularVelocity": 0.23518555807832484, + "velocityX": 0.09121879355263159, + "velocityY": -0.2138667369457007, + "timestamp": 0.3458488895729774 + }, + { + "x": 1.2607987600293624, + "y": 3.5935252227252095, + "heading": -0.8055681239046617, + "angularVelocity": 0.3503618172041365, + "velocityX": 0.13650373094344487, + "velocityY": -0.30288606382088756, + "timestamp": 0.518773334359466 + }, + { + "x": 1.2923153971748729, + "y": 3.5278914158276553, + "heading": -0.7252939199511473, + "angularVelocity": 0.46421547893157705, + "velocityX": 0.18225669120552063, + "velocityY": -0.37955193086711103, + "timestamp": 0.6916977791459548 + }, + { + "x": 1.3319437170467292, + "y": 3.45130898950986, + "heading": -0.6254055185731093, + "angularVelocity": 0.5776418800095864, + "velocityX": 0.2291655174159904, + "velocityY": -0.44286639999442523, + "timestamp": 0.8646222239324435 + }, + { + "x": 1.380003817262134, + "y": 3.3664624405251735, + "heading": -0.5056820412512726, + "angularVelocity": 0.6923455933948506, + "velocityX": 0.2779254273134932, + "velocityY": -0.490656766910279, + "timestamp": 1.037546668718932 + }, + { + "x": 1.436809995729507, + "y": 3.2767232226728136, + "heading": -0.36592886657710183, + "angularVelocity": 0.808174777547772, + "velocityX": 0.3285028819178693, + "velocityY": -0.5189504466638519, + "timestamp": 1.2104711135054207 + }, + { + "x": 1.5024117560477557, + "y": 3.186433452722628, + "heading": -0.2093004084573229, + "angularVelocity": 0.9057623884898333, + "velocityX": 0.3793666095115132, + "velocityY": -0.5221342191413152, + "timestamp": 1.3833955582919093 + }, + { + "x": 1.575388792180146, + "y": 3.100609774230474, + "heading": -0.043535810037648476, + "angularVelocity": 0.9585955220739105, + "velocityX": 0.4220168884847946, + "velocityY": -0.49630738203272023, + "timestamp": 1.556320003078398 + }, + { + "x": 1.6534260730218766, + "y": 3.0220856356001904, + "heading": 0.1259077975683872, + "angularVelocity": 0.9798707627458886, + "velocityX": 0.4512796379646867, + "velocityY": -0.4540950743631267, + "timestamp": 1.7292444478648865 + }, + { + "x": 1.7350915783546517, + "y": 2.9526358276231015, + "heading": 0.29512622138663475, + "angularVelocity": 0.978568553623787, + "velocityX": 0.4722611972532152, + "velocityY": -0.401619378011739, + "timestamp": 1.9021688926513751 + }, + { + "x": 1.819540714989021, + "y": 2.893956167629996, + "heading": 0.4612947569724265, + "angularVelocity": 0.9609314391291418, + "velocityX": 0.48835858190283854, + "velocityY": -0.33933698641658866, + "timestamp": 2.0750933374378637 + }, + { + "x": 1.906146645544586, + "y": 2.847636222828339, + "heading": 0.6223817773774516, + "angularVelocity": 0.9315456852260934, + "velocityX": 0.5008310459701584, + "velocityY": -0.2678623304812785, + "timestamp": 2.2480177822243523 + }, + { + "x": 1.9665251142175801, + "y": 2.8219435002912077, + "heading": 0.7304227409187306, + "angularVelocity": 0.9070944374295804, + "velocityX": 0.5069278473953359, + "velocityY": -0.21571193983857168, + "timestamp": 2.367124414372505 + }, + { + "x": 2.027199417328043, + "y": 2.802535781230278, + "heading": 0.8348605982998051, + "angularVelocity": 0.8768433412411867, + "velocityX": 0.5094116252951654, + "velocityY": -0.16294406809191087, + "timestamp": 2.486231046520658 + }, + { + "x": 2.0877233838067433, + "y": 2.7894495205117447, + "heading": 0.9349982104943089, + "angularVelocity": 0.8407391786537501, + "velocityX": 0.5081494230967811, + "velocityY": -0.10987012719992924, + "timestamp": 2.605337678668811 + }, + { + "x": 2.147643915001536, + "y": 2.7826815563505396, + "heading": 1.0301447309723348, + "angularVelocity": 0.7988347804694407, + "velocityX": 0.5030830779492154, + "velocityY": -0.05682273215515476, + "timestamp": 2.724444310816964 + }, + { + "x": 2.20651048737849, + "y": 2.782188581989056, + "heading": 1.1196211525892386, + "angularVelocity": 0.7512295482688252, + "velocityX": 0.4942342101420168, + "velocityY": -0.004138933646610826, + "timestamp": 2.8435509429651167 + }, + { + "x": 2.263883758606366, + "y": 2.78789118528915, + "heading": 1.2027687081800864, + "angularVelocity": 0.6980934136595841, + "velocityX": 0.4816966952555762, + "velocityY": 0.047878133400203145, + "timestamp": 2.9626575751132695 + }, + { + "x": 2.3193412784170198, + "y": 2.799681571659277, + "heading": 1.2789478693283394, + "angularVelocity": 0.6395879020676446, + "velocityX": 0.4656123563275106, + "velocityY": 0.09899017450798987, + "timestamp": 3.0817642072614224 + }, + { + "x": 2.372480995706151, + "y": 2.817429968176388, + "heading": 1.3475588619550296, + "angularVelocity": 0.5760467844787401, + "velocityX": 0.4461524614429469, + "velocityY": 0.1490126631712517, + "timestamp": 3.2008708394095753 + }, + { + "x": 2.4229223212854074, + "y": 2.8409891725170064, + "heading": 1.4080386482590237, + "angularVelocity": 0.5077784927315084, + "velocityX": 0.4234972013791647, + "velocityY": 0.19779926497358047, + "timestamp": 3.319977471557728 + }, + { + "x": 2.470306439114864, + "y": 2.8701952854845723, + "heading": 1.45984182318626, + "angularVelocity": 0.434931068507544, + "velocityX": 0.397829379968852, + "velocityY": 0.2452097956790979, + "timestamp": 3.439084103705881 + }, + { + "x": 2.5142942062154736, + "y": 2.904872210821119, + "heading": 1.5024339139907708, + "angularVelocity": 0.3575962976843553, + "velocityX": 0.3693141690481337, + "velocityY": 0.2911418507417147, + "timestamp": 3.558190735854034 + }, + { + "x": 2.5545596425467716, + "y": 2.944842709921586, + "heading": 1.5353104282168073, + "angularVelocity": 0.2760258899137462, + "velocityX": 0.3380620825190745, + "velocityY": 0.33558583920682944, + "timestamp": 3.6772973680021868 + }, + { + "x": 2.5907808083346966, + "y": 2.9899393707705153, + "heading": 1.5580188767710943, + "angularVelocity": 0.19065645700034164, + "velocityX": 0.30410704180422304, + "velocityY": 0.3786242635368564, + "timestamp": 3.7964040001503396 + }, + { + "x": 2.6226553892163267, + "y": 3.0399773739572353, + "heading": 1.5699999995373906, + "angularVelocity": 0.1005915667162924, + "velocityX": 0.26761381867758166, + "velocityY": 0.42011097406612685, + "timestamp": 3.9155106322984925 + }, + { + "x": 2.6499999999952943, + "y": 3.0946526527631186, + "heading": 1.5699999995375231, + "angularVelocity": 1.6268742139378163e-13, + "velocityX": 0.22958092316607345, + "velocityY": 0.45904478970404045, + "timestamp": 4.034617264446646 + }, + { + "x": 2.677778603556993, + "y": 3.1876184634557494, + "heading": 1.5699999996915948, + "angularVelocity": 3.4878134565245447e-9, + "velocityX": 0.15720141808521537, + "velocityY": 0.526101218742126, + "timestamp": 4.211324345228364 + }, + { + "x": 2.694302289976671, + "y": 3.2907293636552515, + "heading": 1.569999999845665, + "angularVelocity": 3.487804657098119e-9, + "velocityX": 0.09350890981424996, + "velocityY": 0.5835131200339685, + "timestamp": 4.388031426010083 + }, + { + "x": 2.7009127461581475, + "y": 3.402378723740959, + "heading": 1.5699999999997312, + "angularVelocity": 3.487781847838159e-9, + "velocityX": 0.037409118885215344, + "velocityY": 0.6318329713426234, + "timestamp": 4.564738506791802 + }, + { + "x": 2.6987671383912533, + "y": 3.5209211039854664, + "heading": 1.570000000153807, + "angularVelocity": 3.4878366830559997e-9, + "velocityX": -0.012142172119808498, + "velocityY": 0.6708411430749418, + "timestamp": 4.74144558757352 + }, + { + "x": 2.6888828222740733, + "y": 3.644196175998938, + "heading": 1.5700000003078753, + "angularVelocity": 3.4877937223103217e-9, + "velocityX": -0.055936163456970826, + "velocityY": 0.6976238377085905, + "timestamp": 4.918152668355239 + }, + { + "x": 2.6722022855076024, + "y": 3.76625962058142, + "heading": 1.5700000004619454, + "angularVelocity": 3.4878037554984787e-9, + "velocityX": -0.0943965389284013, + "velocityY": 0.6907671381579374, + "timestamp": 5.094859749136957 + }, + { + "x": 2.657986617113385, + "y": 3.879155032674401, + "heading": 1.570000000616029, + "angularVelocity": 3.4878815132726272e-9, + "velocityX": -0.08044764465302807, + "velocityY": 0.6388844831997175, + "timestamp": 5.271566829918676 + }, + { + "x": 2.6500000000052806, + "y": 3.9813501834674954, + "heading": 1.5700000004617618, + "angularVelocity": 1.7432220039690896e-9, + "velocityX": -0.04519692738746778, + "velocityY": 0.5783308186700414, + "timestamp": 5.448273910700395 + }, + { + "x": 2.6452136818359064, + "y": 4.05360530425828, + "heading": 1.53966341048279, + "angularVelocity": -0.2382427016177502, + "velocityX": -0.03758844925284465, + "velocityY": 0.5674419970250193, + "timestamp": 5.575608724119742 + }, + { + "x": 2.6414058365018693, + "y": 4.124344962259836, + "heading": 1.4805030233227192, + "angularVelocity": -0.4646049696157924, + "velocityX": -0.029904196935062462, + "velocityY": 0.5555405950865885, + "timestamp": 5.702943537539089 + }, + { + "x": 2.638726148869199, + "y": 4.1926740124341215, + "heading": 1.3885239036867012, + "angularVelocity": -0.7223407103550541, + "velocityX": -0.02104442268552982, + "velocityY": 0.5366093399943983, + "timestamp": 5.830278350958436 + }, + { + "x": 2.6371495018803164, + "y": 4.258402426861184, + "heading": 1.2664490199394642, + "angularVelocity": -0.9586921313529179, + "velocityX": -0.012381900523162376, + "velocityY": 0.5161857363964165, + "timestamp": 5.957613164377784 + }, + { + "x": 2.6367566351923606, + "y": 4.321249747197685, + "heading": 1.117526343536387, + "angularVelocity": -1.1695362196212704, + "velocityX": -0.003085304648377667, + "velocityY": 0.4935596059814381, + "timestamp": 6.084947977797131 + }, + { + "x": 2.6379461563733613, + "y": 4.380543504144595, + "heading": 0.9470242990843019, + "angularVelocity": -1.3390057264463304, + "velocityX": 0.009341680772761525, + "velocityY": 0.46565236445510094, + "timestamp": 6.212282791216478 + }, + { + "x": 2.640763664026479, + "y": 4.4348680141998, + "heading": 0.7637352361799904, + "angularVelocity": -1.439426169991059, + "velocityX": 0.022126766227508896, + "velocityY": 0.4266273189098902, + "timestamp": 6.3396176046358255 + }, + { + "x": 2.6449181726826847, + "y": 4.483606188164319, + "heading": 0.5746573747577438, + "angularVelocity": -1.4848874101138032, + "velocityX": 0.032626652054096365, + "velocityY": 0.3827560794609454, + "timestamp": 6.466952418055173 + }, + { + "x": 2.6500836019606915, + "y": 4.5266472095485755, + "heading": 0.3868086488776482, + "angularVelocity": -1.4752346255840696, + "velocityX": 0.0405657269652761, + "velocityY": 0.33801456355338955, + "timestamp": 6.59428723147452 + }, + { + "x": 2.6558464279561003, + "y": 4.564412547861051, + "heading": 0.20759285673582817, + "angularVelocity": -1.4074375050153292, + "velocityX": 0.0452572697009629, + "velocityY": 0.2965829791062329, + "timestamp": 6.721622044893867 + }, + { + "x": 2.661778734291967, + "y": 4.59791659515814, + "heading": 0.04509455516787029, + "angularVelocity": -1.2761498384118317, + "velocityX": 0.046588251681661745, + "velocityY": 0.263117731793826, + "timestamp": 6.8489568583132145 + }, + { + "x": 2.6675521800942206, + "y": 4.628154547719805, + "heading": -0.09493357899778786, + "angularVelocity": -1.0996846065360344, + "velocityX": 0.04534067037561326, + "velocityY": 0.2374680712369913, + "timestamp": 6.976291671732562 + }, + { + "x": 2.6730273366267054, + "y": 4.655759180593734, + "heading": -0.20883881516584757, + "angularVelocity": -0.8945333420741611, + "velocityX": 0.04299811171875369, + "velocityY": 0.2167877904868582, + "timestamp": 7.103626485151909 + }, + { + "x": 2.678138048749673, + "y": 4.6811080520674, + "heading": -0.2939465026251688, + "angularVelocity": -0.6683772114260695, + "velocityX": 0.040136016118023574, + "velocityY": 0.19907259272871744, + "timestamp": 7.230961298571256 + }, + { + "x": 2.6828300625819463, + "y": 4.704445666215685, + "heading": -0.34796884329444056, + "angularVelocity": -0.4242542889593211, + "velocityX": 0.036847847868648385, + "velocityY": 0.18327756173208992, + "timestamp": 7.358296111990604 + }, + { + "x": 2.6870489868716323, + "y": 4.725946330962835, + "heading": -0.3688433898724138, + "angularVelocity": -0.16393432463684127, + "velocityX": 0.033132528155050814, + "velocityY": 0.16885142523195418, + "timestamp": 7.485630925409951 + }, + { + "x": 2.690741538999899, + "y": 4.745745658881827, + "heading": -0.3547054524849317, + "angularVelocity": 0.11102963078930123, + "velocityX": 0.02899876341795603, + "velocityY": 0.15549029656562582, + "timestamp": 7.612965738829298 + }, + { + "x": 2.6934607424234462, + "y": 4.761417510798841, + "heading": -0.3176562911177192, + "angularVelocity": 0.34403603709867575, + "velocityX": 0.025250341412210373, + "velocityY": 0.1455277699022493, + "timestamp": 7.720655507548441 + }, + { + "x": 2.6957976390226768, + "y": 4.776182991755986, + "heading": -0.2567351376272597, + "angularVelocity": 0.5657097616347546, + "velocityX": 0.021700265768873628, + "velocityY": 0.13711126973577806, + "timestamp": 7.8283452762675845 + }, + { + "x": 2.6977687917894335, + "y": 4.7902101666937575, + "heading": -0.17317582068144213, + "angularVelocity": 0.7759262355558584, + "velocityX": 0.018303992960618302, + "velocityY": 0.1302554093821528, + "timestamp": 7.936035044986728 + }, + { + "x": 2.699387576905781, + "y": 4.803677821280753, + "heading": -0.06825731088311157, + "angularVelocity": 0.9742662738687601, + "velocityX": 0.015031930485161661, + "velocityY": 0.125059740805471, + "timestamp": 8.043724813705872 + }, + { + "x": 2.700669750016591, + "y": 4.816787270497459, + "heading": 0.05667241036907346, + "angularVelocity": 1.1600890469331555, + "velocityX": 0.01190617387623651, + "velocityY": 0.12173346969049074, + "timestamp": 8.151414582425016 + }, + { + "x": 2.701638921088602, + "y": 4.829788325532351, + "heading": 0.20013160725212134, + "angularVelocity": 1.332152520098241, + "velocityX": 0.008999657843294685, + "velocityY": 0.12072692858580873, + "timestamp": 8.25910435114416 + }, + { + "x": 2.7023270420759418, + "y": 4.843037991339417, + "heading": 0.3603318680041595, + "angularVelocity": 1.4876089210741354, + "velocityX": 0.0063898455239930225, + "velocityY": 0.12303551163966073, + "timestamp": 8.366794119863304 + }, + { + "x": 2.7027342342087275, + "y": 4.8571699423090635, + "heading": 0.534542545009005, + "angularVelocity": 1.6177087106318269, + "velocityX": 0.003781158937186622, + "velocityY": 0.13122835275947514, + "timestamp": 8.474483888582448 + }, + { + "x": 2.70256595507304, + "y": 4.874565727425271, + "heading": 0.7112936152404646, + "angularVelocity": 1.6412986324126229, + "velocityX": -0.0015626288292674016, + "velocityY": 0.16153609867638755, + "timestamp": 8.582173657301592 + }, + { + "x": 2.7023906866424063, + "y": 4.895151112055553, + "heading": 0.8857476045173669, + "angularVelocity": 1.6199680921307593, + "velocityX": -0.0016275309601235012, + "velocityY": 0.1911545067457904, + "timestamp": 8.689863426020736 + }, + { + "x": 2.702514003300952, + "y": 4.918783664787565, + "heading": 1.0519954165670216, + "angularVelocity": 1.543766078541766, + "velocityX": 0.0011451102305272795, + "velocityY": 0.21945030624338774, + "timestamp": 8.79755319473988 + }, + { + "x": 2.703054335679489, + "y": 4.945063560715068, + "heading": 1.205028220918099, + "angularVelocity": 1.4210523997151632, + "velocityX": 0.005017490356734875, + "velocityY": 0.24403335836798773, + "timestamp": 8.905242963459024 + }, + { + "x": 2.7039518227222876, + "y": 4.973866114721659, + "heading": 1.3418780223624793, + "angularVelocity": 1.2707781155821471, + "velocityX": 0.008334004718619866, + "velocityY": 0.26745859282506784, + "timestamp": 9.012932732178168 + }, + { + "x": 2.705155611038573, + "y": 5.005199432371511, + "heading": 1.4601391056217587, + "angularVelocity": 1.0981645200204058, + "velocityX": 0.011178297889957256, + "velocityY": 0.2909590950064466, + "timestamp": 9.120622500897312 + }, + { + "x": 2.7075475883170625, + "y": 5.057800568431657, + "heading": 1.5846330579194456, + "angularVelocity": 0.7815302239817514, + "velocityX": 0.015016010847101066, + "velocityY": 0.33021184476934917, + "timestamp": 9.279917622780061 + }, + { + "x": 2.7106355946768605, + "y": 5.11553191139856, + "heading": 1.6606890009799455, + "angularVelocity": 0.4774530579023228, + "velocityX": 0.019385442083753358, + "velocityY": 0.36241751965678165, + "timestamp": 9.439212744662811 + }, + { + "x": 2.714603113036703, + "y": 5.177430582712724, + "heading": 1.689552850964268, + "angularVelocity": 0.1811973250635666, + "velocityX": 0.02490671599190395, + "velocityY": 0.38857857404297325, + "timestamp": 9.59850786654556 + }, + { + "x": 2.719666892119011, + "y": 5.242655325525797, + "heading": 1.6720167349941795, + "angularVelocity": -0.11008570608420444, + "velocityX": 0.03178866384838745, + "velocityY": 0.40945850720964916, + "timestamp": 9.75780298842831 + }, + { + "x": 2.7260456700000577, + "y": 5.310428510061647, + "heading": 1.6086050162662178, + "angularVelocity": -0.3980769653911309, + "velocityX": 0.04004377413266913, + "velocityY": 0.4254567480373902, + "timestamp": 9.91709811031106 + }, + { + "x": 2.733983755111467, + "y": 5.379965782164736, + "heading": 1.4994890257959146, + "angularVelocity": -0.6849926674968776, + "velocityX": 0.0498325687430904, + "velocityY": 0.4365310831844459, + "timestamp": 10.076393232193809 + }, + { + "x": 2.7403519987836336, + "y": 5.428782764015629, + "heading": 1.4030227598298415, + "angularVelocity": -0.86907069684122, + "velocityX": 0.05737191041991601, + "velocityY": 0.43979527998539214, + "timestamp": 10.187392563175258 + }, + { + "x": 2.7474416272846045, + "y": 5.477928317505554, + "heading": 1.2877775765276958, + "angularVelocity": -1.0382511519468594, + "velocityX": 0.06387091201629533, + "velocityY": 0.4427554026788899, + "timestamp": 10.298391894156708 + }, + { + "x": 2.755099437239571, + "y": 5.527335311414416, + "heading": 1.1556172324559857, + "angularVelocity": -1.1906409061595067, + "velocityX": 0.06898969470888307, + "velocityY": 0.44511073595952727, + "timestamp": 10.409391225138158 + }, + { + "x": 2.763073314371291, + "y": 5.576891966790131, + "heading": 1.008839937166576, + "angularVelocity": -1.3223259450318314, + "velocityX": 0.07183716391577571, + "velocityY": 0.4464590455826599, + "timestamp": 10.520390556119608 + }, + { + "x": 2.770756458926381, + "y": 5.626388669002003, + "heading": 0.8509533533965823, + "angularVelocity": -1.4224102282551414, + "velocityX": 0.06921793572996068, + "velocityY": 0.44591892372393505, + "timestamp": 10.631389887101058 + }, + { + "x": 2.7765929371018587, + "y": 5.6751833178331985, + "heading": 0.6976742957975531, + "angularVelocity": -1.380900733356889, + "velocityX": 0.0525812014042681, + "velocityY": 0.43959408035749586, + "timestamp": 10.742389218082508 + }, + { + "x": 2.7812148969457993, + "y": 5.723355185159096, + "heading": 0.5561189909540492, + "angularVelocity": -1.2752807026531834, + "velocityX": 0.041639528855491686, + "velocityY": 0.43398340233557375, + "timestamp": 10.853388549063958 + }, + { + "x": 2.7846341541044826, + "y": 5.770917360704941, + "heading": 0.4290947667682521, + "angularVelocity": -1.1443692771461944, + "velocityX": 0.030804304212884475, + "velocityY": 0.42849065047935536, + "timestamp": 10.964387880045408 + }, + { + "x": 2.7868036331209676, + "y": 5.817867178043778, + "heading": 0.31870033092567784, + "angularVelocity": -0.9945504612345583, + "velocityX": 0.019544973798314574, + "velocityY": 0.42297387661508384, + "timestamp": 11.075387211026857 + }, + { + "x": 2.7876739995139346, + "y": 5.8641980426984786, + "heading": 0.22669269944147122, + "angularVelocity": -0.828902576809249, + "velocityX": 0.007841185918062894, + "velocityY": 0.4173976926896044, + "timestamp": 11.186386542008307 + }, + { + "x": 2.787199598907597, + "y": 5.909896954976253, + "heading": 0.15461009075553808, + "angularVelocity": -0.649396785077588, + "velocityX": -0.00427390512415645, + "velocityY": 0.41170439370904605, + "timestamp": 11.297385872989757 + }, + { + "x": 2.785334928758619, + "y": 5.954945308815798, + "heading": 0.10384044901782247, + "angularVelocity": -0.45738691663173364, + "velocityX": -0.016798931460352096, + "velocityY": 0.40584347154427963, + "timestamp": 11.408385203971207 + }, + { + "x": 2.7820296460936547, + "y": 5.99932303663678, + "heading": 0.0756680850519561, + "angularVelocity": -0.2538066106695057, + "velocityX": -0.029777500831199538, + "velocityY": 0.39980175927762673, + "timestamp": 11.519384534952657 + }, + { + "x": 2.777225971223495, + "y": 6.043013572694398, + "heading": 0.07130738082260937, + "angularVelocity": -0.03928586070851831, + "velocityX": -0.04327661108101771, + "velocityY": 0.39361080516708935, + "timestamp": 11.630383865934107 + }, + { + "x": 2.7708025416380804, + "y": 6.086228577980418, + "heading": 0.0924405017308609, + "angularVelocity": 0.1894267867463071, + "velocityX": -0.0575764289833756, + "velocityY": 0.38735781825339893, + "timestamp": 11.741947395050046 + }, + { + "x": 2.7631104838036147, + "y": 6.128747951743283, + "heading": 0.13792413323079813, + "angularVelocity": 0.407692655411535, + "velocityX": -0.06894778181538898, + "velocityY": 0.3811225231531768, + "timestamp": 11.853510924165985 + }, + { + "x": 2.754514263518267, + "y": 6.170565147177378, + "heading": 0.20653308055445607, + "angularVelocity": 0.6149764874177278, + "velocityX": -0.0770522444451208, + "velocityY": 0.37482854574377555, + "timestamp": 11.965074453281924 + }, + { + "x": 2.745440052449635, + "y": 6.211648913048854, + "heading": 0.29697222751905, + "angularVelocity": 0.8106515419634385, + "velocityX": -0.08133671613909955, + "velocityY": 0.36825444781340366, + "timestamp": 12.076637982397862 + }, + { + "x": 2.7363923538047996, + "y": 6.2519737139034355, + "heading": 0.40783083793060415, + "angularVelocity": 0.9936814587406215, + "velocityX": -0.08109907192317244, + "velocityY": 0.36145146276694384, + "timestamp": 12.188201511513801 + }, + { + "x": 2.7280592025191917, + "y": 6.291485229850295, + "heading": 0.5371181653698212, + "angularVelocity": 1.1588673144286004, + "velocityX": -0.07469422463560056, + "velocityY": 0.35416158122392677, + "timestamp": 12.29976504062974 + }, + { + "x": 2.721516703521013, + "y": 6.329946640679988, + "heading": 0.6809409115442911, + "angularVelocity": 1.2891555796425005, + "velocityX": -0.05864370777231024, + "velocityY": 0.34474896102184066, + "timestamp": 12.41132856974568 + }, + { + "x": 2.7186115194936558, + "y": 6.367345324047673, + "heading": 0.8310949021421773, + "angularVelocity": 1.3459057057109145, + "velocityX": -0.02604062490013896, + "velocityY": 0.3352232005464074, + "timestamp": 12.522892098861618 + }, + { + "x": 2.7201626165622823, + "y": 6.403993224874882, + "heading": 0.9826164331350509, + "angularVelocity": 1.3581636572914138, + "velocityX": 0.013903263009787203, + "velocityY": 0.3284935592311268, + "timestamp": 12.634455627977557 + }, + { + "x": 2.7265981550401013, + "y": 6.440150870382165, + "heading": 1.131579259128451, + "angularVelocity": 1.3352286976688623, + "velocityX": 0.05768496704140498, + "velocityY": 0.32409915437288567, + "timestamp": 12.746019157093496 + }, + { + "x": 2.7381546633881, + "y": 6.476016938175864, + "heading": 1.2747563358948757, + "angularVelocity": 1.283368118388856, + "velocityX": 0.10358679458289283, + "velocityY": 0.3214855967802891, + "timestamp": 12.857582686209435 + }, + { + "x": 2.7549876221362974, + "y": 6.511747596731161, + "heading": 1.4092327359199672, + "angularVelocity": 1.2053795784590957, + "velocityX": 0.1508822722947548, + "velocityY": 0.3202718557414932, + "timestamp": 12.969146215325374 + }, + { + "x": 2.777225971219294, + "y": 6.547506809233031, + "heading": 1.532353785638737, + "angularVelocity": 1.1035958648685134, + "velocityX": 0.19933350348528897, + "velocityY": 0.3205277991599169, + "timestamp": 13.080709744441313 + }, + { + "x": 2.8283739264126173, + "y": 6.60500051454071, + "heading": 1.6910426580508369, + "angularVelocity": 0.8908309216248135, + "velocityX": 0.2871290177610515, + "velocityY": 0.32275212312664076, + "timestamp": 13.258845537171021 + }, + { + "x": 2.894537518108946, + "y": 6.662949818763498, + "heading": 1.8139189760032997, + "angularVelocity": 0.6897901655205115, + "velocityX": 0.37142222056386526, + "velocityY": 0.32530971645546014, + "timestamp": 13.43698132990073 + }, + { + "x": 2.975118854402208, + "y": 6.72132005713001, + "heading": 1.9033257225678124, + "angularVelocity": 0.5019022016528627, + "velocityX": 0.45235904059312243, + "velocityY": 0.3276727123322237, + "timestamp": 13.615117122630439 + }, + { + "x": 3.069506354210641, + "y": 6.779993578108028, + "heading": 1.9614530049830199, + "angularVelocity": 0.3263088317299888, + "velocityX": 0.5298626309923767, + "velocityY": 0.32937524839676924, + "timestamp": 13.793252915360148 + }, + { + "x": 3.177057501791681, + "y": 6.838789011583473, + "heading": 1.990113992675826, + "angularVelocity": 0.16089404186178405, + "velocityX": 0.6037593340125262, + "velocityY": 0.33005962796787114, + "timestamp": 13.971388708089856 + }, + { + "x": 3.2970934581179465, + "y": 6.897474588900029, + "heading": 1.9906701857847593, + "angularVelocity": 0.0031222984451791374, + "velocityX": 0.6738452418130287, + "velocityY": 0.32944292900353384, + "timestamp": 14.149524500819565 + }, + { + "x": 3.428875798780963, + "y": 6.955773952952954, + "heading": 1.9640246482498747, + "angularVelocity": -0.14957991949193578, + "velocityX": 0.7397858602409795, + "velocityY": 0.32727484555599373, + "timestamp": 14.327660293549274 + }, + { + "x": 3.5715750772701043, + "y": 7.013357345374212, + "heading": 1.9106160532989442, + "angularVelocity": -0.29981955974577057, + "velocityX": 0.8010702189812141, + "velocityY": 0.3232555992232119, + "timestamp": 14.505796086278982 + }, + { + "x": 3.7242209599931746, + "y": 7.069819180114435, + "heading": 1.830377783516494, + "angularVelocity": -0.4504331698484236, + "velocityX": 0.8569074209354516, + "velocityY": 0.31695951653343485, + "timestamp": 14.683931879008691 + }, + { + "x": 3.885616535722601, + "y": 7.1246504487670705, + "heading": 1.7226603342509128, + "angularVelocity": -0.6046929009336263, + "velocityX": 0.9060255283762794, + "velocityY": 0.307806015922469, + "timestamp": 14.8620676717384 + }, + { + "x": 4.05416552018741, + "y": 7.177233618565563, + "heading": 1.5860732407671714, + "angularVelocity": -0.7667582768795543, + "velocityX": 0.9461825828669139, + "velocityY": 0.29518587473863683, + "timestamp": 15.040203464468108 + }, + { + "x": 4.2273936305200035, + "y": 7.226946789532177, + "heading": 1.417957586567635, + "angularVelocity": -0.9437499989398298, + "velocityX": 0.972449768119705, + "velocityY": 0.27907457679060355, + "timestamp": 15.218339257197817 + }, + { + "x": 4.400615134292295, + "y": 7.271518848059685, + "heading": 1.2225365130610886, + "angularVelocity": -1.0970342934019721, + "velocityX": 0.9724126809054928, + "velocityY": 0.2502139398555887, + "timestamp": 15.396475049927526 + }, + { + "x": 4.569586786239567, + "y": 7.310085040153136, + "heading": 1.0159093047800025, + "angularVelocity": -1.1599421155910052, + "velocityX": 0.9485553091785852, + "velocityY": 0.2164988377864223, + "timestamp": 15.574610842657234 + }, + { + "x": 4.731916077731233, + "y": 7.343905863563497, + "heading": 0.8050006957827572, + "angularVelocity": -1.183976593168793, + "velocityX": 0.9112671238351473, + "velocityY": 0.18985978557591154, + "timestamp": 15.752746635386943 + }, + { + "x": 4.885947895624515, + "y": 7.373460799757093, + "heading": 0.5949879079305361, + "angularVelocity": -1.1789477265311938, + "velocityX": 0.8646876382009842, + "velocityY": 0.16591239605136865, + "timestamp": 15.930882428116652 + }, + { + "x": 5.030555674163505, + "y": 7.399083556924728, + "heading": 0.389293360584716, + "angularVelocity": -1.1547064416090447, + "velocityX": 0.8117839560724309, + "velocityY": 0.14383834251217997, + "timestamp": 16.10901822084636 + }, + { + "x": 5.164922112553356, + "y": 7.42103691269045, + "heading": 0.1902575355029938, + "angularVelocity": -1.117326406063954, + "velocityX": 0.754292196599486, + "velocityY": 0.12323944239401181, + "timestamp": 16.287154013576068 + }, + { + "x": 5.288247156990128, + "y": 7.439470298396178, + "heading": -7.485335201745413e-8, + "angularVelocity": -1.0680481863931495, + "velocityX": 0.692309179127921, + "velocityY": 0.10347940424527421, + "timestamp": 16.465289806305776 + }, + { + "x": 5.288247150924154, + "y": 7.439470297488981, + "heading": -6.549675275293239e-8, + "angularVelocity": -1.0680481909285577, + "velocityX": 0.6923091791495746, + "velocityY": 0.1034794048722287, + "timestamp": 16.465289797545616 + }, + { + "x": 5.288247144858181, + "y": 7.439470296581783, + "heading": -5.614015360341729e-8, + "angularVelocity": -1.0680481958850412, + "velocityX": 0.6923091791998811, + "velocityY": 0.10347940550159965, + "timestamp": 16.465289788785455 + }, + { + "x": 5.288247138792208, + "y": 7.439470295674586, + "heading": -4.678355456587365e-8, + "angularVelocity": -1.068048201182234, + "velocityX": 0.6923091792135615, + "velocityY": 0.10347940615791178, + "timestamp": 16.465289780025294 + }, + { + "x": 5.288247132726235, + "y": 7.439470294767388, + "heading": -3.742695563880682e-8, + "angularVelocity": -1.0680482066432966, + "velocityX": 0.6923091791656869, + "velocityY": 0.10347940690651845, + "timestamp": 16.465289771265134 + }, + { + "x": 5.288247126660261, + "y": 7.43947029386019, + "heading": -2.8070356820536066e-8, + "angularVelocity": -1.0680482122907076, + "velocityX": 0.6923091790890028, + "velocityY": 0.10347940774772568, + "timestamp": 16.465289762504973 + }, + { + "x": 5.2882471205942885, + "y": 7.4394702929529934, + "heading": -1.871375810835249e-8, + "angularVelocity": -1.0680482182439734, + "velocityX": 0.6923091790087366, + "velocityY": 0.10347940865836429, + "timestamp": 16.465289753744813 + }, + { + "x": 5.288247114528315, + "y": 7.439470292045796, + "heading": -9.357159497718974e-9, + "angularVelocity": -1.0680482247148153, + "velocityX": 0.692309178935693, + "velocityY": 0.10347940958549313, + "timestamp": 16.465289744984652 + }, + { + "x": 5.2882471084623415, + "y": 7.439470291138599, + "heading": -5.609846744710123e-13, + "angularVelocity": -1.0680482316377529, + "velocityX": 0.6923091788453621, + "velocityY": 0.10347941057644157, + "timestamp": 16.46528973622449 + }, + { + "x": 5.418318713107553, + "y": 7.45524667726722, + "heading": -0.14765149788980822, + "angularVelocity": -0.9184906932543151, + "velocityX": 0.8091320441513836, + "velocityY": 0.09813963317439353, + "timestamp": 16.626044220713585 + }, + { + "x": 5.563548987825593, + "y": 7.4700941218212495, + "heading": -0.27422768048539375, + "angularVelocity": -0.7873881901546119, + "velocityX": 0.9034290718221242, + "velocityY": 0.09236099763780091, + "timestamp": 16.786798705202678 + }, + { + "x": 5.720519992249432, + "y": 7.483950977822062, + "heading": -0.3825207221223279, + "angularVelocity": -0.6736548717787152, + "velocityX": 0.9764642331243513, + "velocityY": 0.08619887682187297, + "timestamp": 16.94755318969177 + }, + { + "x": 5.891579981174876, + "y": 7.497541590590127, + "heading": -0.47714146755787396, + "angularVelocity": -0.588604079901089, + "velocityX": 1.0641071038395116, + "velocityY": 0.08454266651145323, + "timestamp": 17.108307674180864 + }, + { + "x": 6.070777918835097, + "y": 7.508754325284448, + "heading": -0.5566348918765167, + "angularVelocity": -0.4945020636066715, + "velocityX": 1.1147305670618215, + "velocityY": 0.06975068056649593, + "timestamp": 17.269062158669957 + }, + { + "x": 6.2553343016634635, + "y": 7.518023579202175, + "heading": -0.6170518299221113, + "angularVelocity": -0.37583360883180356, + "velocityX": 1.148063666191382, + "velocityY": 0.057660935241979815, + "timestamp": 17.42981664315905 + }, + { + "x": 6.442537822955601, + "y": 7.525844356758643, + "heading": -0.6536325541269659, + "angularVelocity": -0.22755647733647813, + "velocityX": 1.1645306311553023, + "velocityY": 0.04865044719085291, + "timestamp": 17.590571127648143 + }, + { + "x": 6.629868252624434, + "y": 7.5325663261121605, + "heading": -0.6631454099336127, + "angularVelocity": -0.05917630132092225, + "velocityX": 1.165320085817266, + "velocityY": 0.04181512805856416, + "timestamp": 17.751325612137236 + }, + { + "x": 6.814379245318763, + "y": 7.537436085797625, + "heading": -0.6469315275958195, + "angularVelocity": 0.1008611509586783, + "velocityX": 1.1477813093220908, + "velocityY": 0.030293149836502442, + "timestamp": 17.91208009662633 + }, + { + "x": 6.991679035452718, + "y": 7.540305977691209, + "heading": -0.6170030764514597, + "angularVelocity": 0.1861749066087869, + "velocityX": 1.1029228247693241, + "velocityY": 0.01785263970713557, + "timestamp": 18.072834581115423 + }, + { + "x": 7.159447389396387, + "y": 7.541387264231915, + "heading": -0.5764605353737351, + "angularVelocity": 0.2522016179067287, + "velocityX": 1.0436309411262277, + "velocityY": 0.006726322705293858, + "timestamp": 18.233589065604516 + }, + { + "x": 7.315440984587148, + "y": 7.540720415566423, + "heading": -0.5273581032751606, + "angularVelocity": 0.3054498433093596, + "velocityX": 0.9703840964748058, + "velocityY": -0.004148243019810784, + "timestamp": 18.39434355009361 + }, + { + "x": 7.470574266382935, + "y": 7.537764395855196, + "heading": -0.4641850757014082, + "angularVelocity": 0.3546341172959247, + "velocityX": 0.8708709486472784, + "velocityY": -0.016594193461135685, + "timestamp": 18.572479337072245 + }, + { + "x": 7.604749650146638, + "y": 7.532550172079166, + "heading": -0.39417620645558443, + "angularVelocity": 0.3930084484061484, + "velocityX": 0.7532196985108242, + "velocityY": -0.029271062624336028, + "timestamp": 18.75061512405088 + }, + { + "x": 7.714483814910743, + "y": 7.524998903950903, + "heading": -0.31905072154824177, + "angularVelocity": 0.4217315688319769, + "velocityX": 0.6160141464196879, + "velocityY": -0.04239051712518056, + "timestamp": 18.928750911029518 + }, + { + "x": 7.796027577811261, + "y": 7.515004663660351, + "heading": -0.24044153160037793, + "angularVelocity": 0.4412880268554748, + "velocityX": 0.45776182474014415, + "velocityY": -0.05610461805755441, + "timestamp": 19.106886698008154 + }, + { + "x": 7.845367727625598, + "y": 7.50244123092944, + "heading": -0.1599630926416538, + "angularVelocity": 0.4517814209261708, + "velocityX": 0.2769805587610722, + "velocityY": -0.07052728115034504, + "timestamp": 19.28502248498679 + }, + { + "x": 7.858234920893501, + "y": 7.487166681771978, + "heading": -0.07925291889731063, + "angularVelocity": 0.4530823093638088, + "velocityX": 0.07223250018704967, + "velocityY": -0.08574666223336481, + "timestamp": 19.463158271965426 + }, + { + "x": 7.830112934107491, + "y": 7.469026565551425, + "heading": 1.9541743370744396e-13, + "angularVelocity": 0.44490172492554647, + "velocityX": -0.157868260300945, + "velocityY": -0.10183308210267149, + "timestamp": 19.641294058944062 + }, + { + "x": 7.754138306062181, + "y": 7.447719681174328, + "heading": 0.07588851944221475, + "angularVelocity": 0.42601499150875954, + "velocityX": -0.4264983789132844, + "velocityY": -0.11961034730517565, + "timestamp": 19.819429854525456 + }, + { + "x": 7.639951170097708, + "y": 7.423862681107234, + "heading": 0.14871254480285817, + "angularVelocity": 0.40881185683628485, + "velocityX": -0.641011738198812, + "velocityY": -0.13392591864598205, + "timestamp": 19.99756565010685 + }, + { + "x": 7.496026995727258, + "y": 7.397999014635099, + "heading": 0.21873314599039687, + "angularVelocity": 0.39307428896591823, + "velocityX": -0.8079463978438458, + "velocityY": -0.14519073152928566, + "timestamp": 20.175701445688244 + }, + { + "x": 7.329549618949632, + "y": 7.370590094001231, + "heading": 0.2861417733138313, + "angularVelocity": 0.37841146471109305, + "velocityX": -0.934553194268821, + "velocityY": -0.15386531687407357, + "timestamp": 20.353837241269638 + }, + { + "x": 7.146249960822764, + "y": 7.342003057760445, + "heading": 0.3510012841680181, + "angularVelocity": 0.36410150269067454, + "velocityX": -1.0289883486090268, + "velocityY": -0.16047889840086563, + "timestamp": 20.53197303685103 + }, + { + "x": 6.9503179785639695, + "y": 7.312500714944177, + "heading": 0.4131524690164208, + "angularVelocity": 0.34889778691337076, + "velocityX": -1.0999023616556318, + "velocityY": -0.16561715021879247, + "timestamp": 20.710108832432425 + }, + { + "x": 6.744669290890069, + "y": 7.282249603442564, + "heading": 0.4720173085707148, + "angularVelocity": 0.3304492472283394, + "velocityX": -1.1544489808985139, + "velocityY": -0.16982050913888352, + "timestamp": 20.88824462801382 + }, + { + "x": 6.528580504402744, + "y": 7.251108531405175, + "heading": 0.5274928016956117, + "angularVelocity": 0.31142249059958194, + "velocityX": -1.2130565099431412, + "velocityY": -0.17481647602398917, + "timestamp": 21.066380423595213 + }, + { + "x": 6.313399103045075, + "y": 7.214538894676935, + "heading": 0.5275460333478232, + "angularVelocity": 0.0002988262523701674, + "velocityX": -1.2079627267049402, + "velocityY": -0.20529078172505943, + "timestamp": 21.244516219176607 + }, + { + "x": 6.120539072312893, + "y": 7.178889186765798, + "heading": 0.517735292666819, + "angularVelocity": -0.05507450453172011, + "velocityX": -1.0826573631611538, + "velocityY": -0.20012658205312084, + "timestamp": 21.422652014758 + }, + { + "x": 5.958628600692053, + "y": 7.144959716647863, + "heading": 0.5008124167051957, + "angularVelocity": -0.09499986179866582, + "velocityX": -0.9089159822650381, + "velocityY": -0.1904696919958043, + "timestamp": 21.600787810339394 + }, + { + "x": 5.837986516704409, + "y": 7.113426483231326, + "heading": 0.4771314716153743, + "angularVelocity": -0.1329375997257422, + "velocityX": -0.6772478467367095, + "velocityY": -0.1770179503423335, + "timestamp": 21.778923605920788 + }, + { + "x": 5.7227454917181495, + "y": 7.076203538098732, + "heading": 0.44815466228660306, + "angularVelocity": -0.1674411973555948, + "velocityX": -0.6659151113565552, + "velocityY": -0.2150911244418225, + "timestamp": 21.951980235828838 + }, + { + "x": 5.614010909083641, + "y": 7.033635647002007, + "heading": 0.41542486169539805, + "angularVelocity": -0.18912768961795748, + "velocityX": -0.6283179251047766, + "velocityY": -0.2459766557929015, + "timestamp": 22.125036865736888 + }, + { + "x": 5.516875858601351, + "y": 6.9866420715294, + "heading": 0.38017449454674707, + "angularVelocity": -0.2036926708051821, + "velocityX": -0.5612905470916604, + "velocityY": -0.27155027514399244, + "timestamp": 22.298093495644938 + }, + { + "x": 5.437243043942288, + "y": 6.935553672024848, + "heading": 0.3424575004944832, + "angularVelocity": -0.21794596411047518, + "velocityX": -0.46015465976713954, + "velocityY": -0.29521203278234875, + "timestamp": 22.471150125552988 + }, + { + "x": 5.382029638407054, + "y": 6.880562557170266, + "heading": 0.301962621519522, + "angularVelocity": -0.23399784796292267, + "velocityX": -0.3190481957280769, + "velocityY": -0.31776369898542106, + "timestamp": 22.64420675546104 + }, + { + "x": 5.359043767990556, + "y": 6.821859309651465, + "heading": 0.2582986739644723, + "angularVelocity": -0.25231016909100673, + "velocityX": -0.13282282465859874, + "velocityY": -0.3392140916541047, + "timestamp": 22.81726338536909 + }, + { + "x": 5.3769168853790115, + "y": 6.759668827057295, + "heading": 0.21109341192044304, + "angularVelocity": -0.27277349656177, + "velocityX": 0.10327900992250337, + "velocityY": -0.3593649236555796, + "timestamp": 22.99032001527714 + }, + { + "x": 5.453420781420232, + "y": 6.691937084119905, + "heading": 0.15775875752405638, + "angularVelocity": -0.2994044744273571, + "velocityX": 0.42946952680603284, + "velocityY": -0.3802253361498929, + "timestamp": 23.16845581133829 + }, + { + "x": 5.578248031578353, + "y": 6.621478294299551, + "heading": 0.10071571878926534, + "angularVelocity": -0.3202222124699196, + "velocityX": 0.7007420906893269, + "velocityY": -0.39553414517797064, + "timestamp": 23.346591607399443 + }, + { + "x": 5.742096614685876, + "y": 6.549230090448469, + "heading": 0.041213467656511724, + "angularVelocity": -0.3340274804303152, + "velocityX": 0.9197959462996009, + "velocityY": -0.40557936949725093, + "timestamp": 23.524727403460595 + }, + { + "x": 5.9357354634248365, + "y": 6.476092053517804, + "heading": -0.019568941838124344, + "angularVelocity": -0.34121389882677833, + "velocityX": 1.087029406896217, + "velocityY": -0.4105746208687261, + "timestamp": 23.702863199521747 + }, + { + "x": 6.148385758343611, + "y": 6.4028961286174635, + "heading": -0.08035607310842466, + "angularVelocity": -0.3412404054344825, + "velocityX": 1.1937538643055912, + "velocityY": -0.4108995862656572, + "timestamp": 23.8809989955829 + }, + { + "x": 6.374102354529772, + "y": 6.330385344994096, + "heading": -0.14018516035925196, + "angularVelocity": -0.33586223866139814, + "velocityX": 1.2671040923692445, + "velocityY": -0.40705341221124436, + "timestamp": 24.059134791644052 + }, + { + "x": 6.612341331128408, + "y": 6.260950902900721, + "heading": -0.19394098279674674, + "angularVelocity": -0.3017687832884667, + "velocityX": 1.337400914733772, + "velocityY": -0.3897837696238762, + "timestamp": 24.237270587705204 + }, + { + "x": 6.84605152207567, + "y": 6.203473880999886, + "heading": -0.2289715660283947, + "angularVelocity": -0.19665100449335837, + "velocityX": 1.3119776940700294, + "velocityY": -0.32265846153343364, + "timestamp": 24.415406383766356 + }, + { + "x": 7.064382598864322, + "y": 6.152325234982081, + "heading": -0.25444586862734997, + "angularVelocity": -0.14300496117137484, + "velocityX": 1.2256440402022253, + "velocityY": -0.28713289046373164, + "timestamp": 24.59354217982751 + }, + { + "x": 7.261910978463682, + "y": 6.1066818733384185, + "heading": -0.2732102079076508, + "angularVelocity": -0.10533727468150804, + "velocityX": 1.1088640462354282, + "velocityY": -0.25622790395414907, + "timestamp": 24.77167797588866 + }, + { + "x": 7.432844706342166, + "y": 6.066712621400304, + "heading": -0.28619204413559407, + "angularVelocity": -0.07287606710718948, + "velocityX": 0.9595697869741047, + "velocityY": -0.22437518355201194, + "timestamp": 24.949813771949813 + }, + { + "x": 7.570743590476923, + "y": 6.032935227158681, + "heading": -0.29375630091954236, + "angularVelocity": -0.04246342931183577, + "velocityX": 0.7741222549554037, + "velocityY": -0.18961598392228185, + "timestamp": 25.127949568010965 + }, + { + "x": 7.668499056236834, + "y": 6.006054729182268, + "heading": -0.29595855831583523, + "angularVelocity": -0.012362800992221083, + "velocityX": 0.5487693541770304, + "velocityY": -0.15089891291403637, + "timestamp": 25.306085364072118 + }, + { + "x": 7.718349321522349, + "y": 5.98690425676214, + "heading": -0.2927031769525776, + "angularVelocity": 0.018274717576348395, + "velocityX": 0.27984417724094734, + "velocityY": -0.10750490829952818, + "timestamp": 25.48422116013327 + }, + { + "x": 7.71188640594573, + "y": 5.976419448852084, + "heading": -0.28379405758259746, + "angularVelocity": 0.050013077477811155, + "velocityX": -0.03628083585189105, + "velocityY": -0.05885851211368899, + "timestamp": 25.662356956194422 + }, + { + "x": 7.64073998767034, + "y": 5.975523422514508, + "heading": -0.2689680472353761, + "angularVelocity": 0.08322869775494246, + "velocityX": -0.39939428102041696, + "velocityY": -0.00503001842507848, + "timestamp": 25.84049275246608 + }, + { + "x": 7.515254770850456, + "y": 5.982745341832985, + "heading": -0.24839985111703908, + "angularVelocity": 0.11546357637731, + "velocityX": -0.7044357139148312, + "velocityY": 0.04054165119978489, + "timestamp": 26.018628548737738 + }, + { + "x": 7.345155681918835, + "y": 5.996704613871788, + "heading": -0.2223274199316461, + "angularVelocity": 0.14636267236058986, + "velocityX": -0.9548843774933711, + "velocityY": 0.07836309338792039, + "timestamp": 26.196764345009395 + }, + { + "x": 7.139560502547719, + "y": 6.016111344560358, + "heading": -0.19106664781409208, + "angularVelocity": 0.1754884350694437, + "velocityX": -1.1541485971641012, + "velocityY": 0.10894346389053551, + "timestamp": 26.374900141281053 + }, + { + "x": 6.907022872723975, + "y": 6.039765038954999, + "heading": -0.15513422497131063, + "angularVelocity": 0.20171365663076973, + "velocityX": -1.30539529219411, + "velocityY": 0.1327846221246523, + "timestamp": 26.55303593755271 + }, + { + "x": 6.655745454364941, + "y": 6.066534208413209, + "heading": -0.11537934692318932, + "angularVelocity": 0.22317175368561765, + "velocityX": -1.4105947463605994, + "velocityY": 0.15027394840704048, + "timestamp": 26.731171733824368 + }, + { + "x": 6.395528884740123, + "y": 6.09504146210893, + "heading": -0.07344943255486255, + "angularVelocity": 0.2353817438489003, + "velocityX": -1.4607764136777701, + "velocityY": 0.16003102291923085, + "timestamp": 26.909307530096026 + }, + { + "x": 6.148227884557905, + "y": 6.126415515383055, + "heading": -0.034920139114795905, + "angularVelocity": 0.21629169569780238, + "velocityX": -1.388272348167526, + "velocityY": 0.1761243609142077, + "timestamp": 27.087443326367683 + }, + { + "x": 5.921399691377473, + "y": 6.155778519380747, + "heading": -0.0014273926700099603, + "angularVelocity": 0.18801805782885908, + "velocityX": -1.2733442571814486, + "velocityY": 0.16483494397205203, + "timestamp": 27.26557912263934 + }, + { + "x": 5.722452569880608, + "y": 6.181830253435625, + "heading": 0.026178625185304717, + "angularVelocity": 0.15497175993321324, + "velocityX": -1.1168284289924366, + "velocityY": 0.14624648498673082, + "timestamp": 27.443714918911 + }, + { + "x": 5.559444959743296, + "y": 6.203351946244675, + "heading": 0.047322653488671644, + "angularVelocity": 0.11869612254198844, + "velocityX": -0.9150749795917061, + "velocityY": 0.12081621582892742, + "timestamp": 27.621850715182656 + }, + { + "x": 5.441124591962054, + "y": 6.21907395052926, + "heading": 0.06162804732685082, + "angularVelocity": 0.08030611554553489, + "velocityX": -0.6642144378512844, + "velocityY": 0.08825853429690561, + "timestamp": 27.799986511454314 + }, + { + "x": 5.376916885374797, + "y": 6.22765016555822, + "heading": 0.06885690933270866, + "angularVelocity": 0.0405806253272207, + "velocityX": -0.3604424710341068, + "velocityY": 0.048144254038901185, + "timestamp": 27.97812230772597 + }, + { + "x": 5.376916885375449, + "y": 6.227650165558026, + "heading": 0.06885690933269815, + "angularVelocity": -1.1945341778272885e-14, + "velocityX": 6.978979046403437e-13, + "velocityY": -1.7270403267086624e-13, + "timestamp": 28.15625810399763 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FiveNoteCitrus.traj b/src/main/deploy/choreo/FiveNoteCitrus.traj new file mode 100644 index 00000000..96428e9f --- /dev/null +++ b/src/main/deploy/choreo/FiveNoteCitrus.traj @@ -0,0 +1,796 @@ +{ + "samples": [ + { + "x": 1.5259017944335938, + "y": 4.065149784088135, + "heading": 3.141592653589793, + "angularVelocity": 2.4697776223335052e-20, + "velocityX": -1.0735280345139992e-20, + "velocityY": -2.239727503402426e-21, + "timestamp": 0 + }, + { + "x": 1.559904788906332, + "y": 4.070979826710794, + "heading": 3.12759620632459, + "angularVelocity": -0.19139297374031616, + "velocityX": 0.4649704389193284, + "velocityY": 0.07972231620217306, + "timestamp": 0.07312936829224484 + }, + { + "x": 1.627911251403576, + "y": 4.082639239190771, + "heading": 3.099617818739539, + "angularVelocity": -0.38258757375342123, + "velocityX": 0.9299473533734328, + "velocityY": 0.159435432744093, + "timestamp": 0.14625873658448968 + }, + { + "x": 1.729922084456728, + "y": 4.100127218174481, + "heading": 3.0576862295792706, + "angularVelocity": -0.5733891887688461, + "velocityX": 1.3949366094000561, + "velocityY": 0.23913756391035812, + "timestamp": 0.21938810487673452 + }, + { + "x": 1.8659386655939116, + "y": 4.123443080384379, + "heading": 3.0018475558370348, + "angularVelocity": -0.763560181719188, + "velocityX": 1.8599447022928484, + "velocityY": 0.3188303516683097, + "timestamp": 0.29251747316897936 + }, + { + "x": 2.0359628195863486, + "y": 4.152586622251226, + "heading": 2.9321666556098673, + "angularVelocity": -0.9528442793147593, + "velocityX": 2.3249777478314058, + "velocityY": 0.398520355739737, + "timestamp": 0.36564684146122417 + }, + { + "x": 2.2060208227531533, + "y": 4.181738546925352, + "heading": 2.8637555344699828, + "angularVelocity": -0.935480816222767, + "velocityX": 2.3254406148731763, + "velocityY": 0.39863498557278454, + "timestamp": 0.438776209753469 + }, + { + "x": 2.3420682321422355, + "y": 4.205061397598442, + "heading": 2.809070281345483, + "angularVelocity": -0.7477878505112007, + "velocityX": 1.8603662600420623, + "velocityY": 0.31892591468704445, + "timestamp": 0.5119055780457138 + }, + { + "x": 2.4441041846583342, + "y": 4.2225542506179865, + "heading": 2.768075278940321, + "angularVelocity": -0.5605819298388371, + "velocityX": 1.3952801029038784, + "velocityY": 0.23920421341037532, + "timestamp": 0.5850349463379586 + }, + { + "x": 2.51212822290642, + "y": 4.234216394083628, + "heading": 2.740749867608366, + "angularVelocity": -0.3736585173654914, + "velocityX": 0.930187691164557, + "velocityY": 0.1594727773257496, + "timestamp": 0.6581643146302034 + }, + { + "x": 2.5461401939392103, + "y": 4.24004745483398, + "heading": 2.727085822860873, + "angularVelocity": -0.18684756981474257, + "velocityX": 0.46509318796340804, + "velocityY": 0.07973623848422588, + "timestamp": 0.7312936829224482 + }, + { + "x": 2.546140193939209, + "y": 4.240047454833984, + "heading": 2.727085822860873, + "angularVelocity": 2.138124901867148e-15, + "velocityX": -1.847172589690238e-14, + "velocityY": 6.573250773560681e-14, + "timestamp": 0.804423051214693 + }, + { + "x": 2.5360168271404553, + "y": 4.276081460503995, + "heading": 2.7395679434791145, + "angularVelocity": 0.16406341439956115, + "velocityX": -0.1330602525820323, + "velocityY": 0.47362641217193957, + "timestamp": 0.8805041242797866 + }, + { + "x": 2.5157705765562905, + "y": 4.348149458488229, + "heading": 2.7645386405288823, + "angularVelocity": 0.32821168319227256, + "velocityX": -0.2661141564977991, + "velocityY": 0.9472526487970788, + "timestamp": 0.9565851973448802 + }, + { + "x": 2.4854021451638255, + "y": 4.45625131889642, + "heading": 2.8020114291266602, + "angularVelocity": 0.4925375929663641, + "velocityX": -0.3991588205713448, + "velocityY": 1.4208771781609923, + "timestamp": 1.0326662704099738 + }, + { + "x": 2.4449124008435135, + "y": 4.600386696447278, + "heading": 2.8520100795366674, + "angularVelocity": 0.6571759360863745, + "velocityX": -0.5321920773339947, + "velocityY": 1.894497169191322, + "timestamp": 1.1087473434750674 + }, + { + "x": 2.3943022789363684, + "y": 4.78055489172573, + "heading": 2.9145725851776083, + "angularVelocity": 0.8223136598956061, + "velocityX": -0.6652130400926888, + "velocityY": 2.3681079671984127, + "timestamp": 1.184828416540161 + }, + { + "x": 2.333572688718061, + "y": 4.996754568834679, + "heading": 2.989758964121752, + "angularVelocity": 0.9882402536543538, + "velocityX": -0.7982220514469898, + "velocityY": 2.8417012063430307, + "timestamp": 1.2609094896052546 + }, + { + "x": 2.28297165827435, + "y": 5.176907113870022, + "heading": 3.0531280508995997, + "angularVelocity": 0.832915260325368, + "velocityX": -0.6650935430474006, + "velocityY": 2.3679022623827497, + "timestamp": 1.3369905626703482 + }, + { + "x": 2.2424908465651967, + "y": 5.32102782630787, + "heading": 3.1038719496251863, + "angularVelocity": 0.6669713856713035, + "velocityX": -0.5320746682229187, + "velocityY": 1.8943044128010853, + "timestamp": 1.4130716357354418 + }, + { + "x": 2.2121301407532674, + "y": 5.429117832939613, + "heading": 3.1419489303428287, + "angularVelocity": 0.5004790177586462, + "velocityX": -0.3990572765180793, + "velocityY": 1.4207213736228852, + "timestamp": 1.4891527088005354 + }, + { + "x": 2.191889625413754, + "y": 5.501177678523276, + "heading": 3.1673392754649017, + "angularVelocity": 0.3337274843685313, + "velocityX": -0.26603877316761987, + "velocityY": 0.9471454946752758, + "timestamp": 1.565233781865629 + }, + { + "x": 2.1817693710327157, + "y": 5.537207603454595, + "heading": 3.1800344341561817, + "angularVelocity": 0.16686355988193977, + "velocityX": -0.13301934335730858, + "velocityY": 0.4735727754587908, + "timestamp": 1.6413148549307226 + }, + { + "x": 2.181769371032715, + "y": 5.53720760345459, + "heading": 3.1800344341561817, + "angularVelocity": -1.6037789217842988e-15, + "velocityX": -1.1616886444830788e-14, + "velocityY": -7.019387777747135e-14, + "timestamp": 1.7173959279958162 + }, + { + "x": 2.225493830898323, + "y": 5.5436853809563225, + "heading": 3.171931437896923, + "angularVelocity": -0.09818466170163322, + "velocityX": 0.5298128201757535, + "velocityY": 0.07849175443705406, + "timestamp": 1.7999240552901006 + }, + { + "x": 2.3129427691879965, + "y": 5.556640895937697, + "heading": 3.155726742160075, + "angularVelocity": -0.19635361019479314, + "velocityX": 1.0596258652258266, + "velocityY": 0.1569830239231864, + "timestamp": 1.882452182584385 + }, + { + "x": 2.444116215881018, + "y": 5.576074046394389, + "heading": 3.131424288027548, + "angularVelocity": -0.29447480427936823, + "velocityX": 1.5894392735373082, + "velocityY": 0.23547305741466587, + "timestamp": 1.9649803098786693 + }, + { + "x": 2.5315652197311147, + "y": 5.589029447702419, + "heading": 3.1152241864923176, + "angularVelocity": -0.19629794188183583, + "velocityX": 1.059626659626784, + "velocityY": 0.1569816465340604, + "timestamp": 2.047508437172954 + }, + { + "x": 2.5752897262573216, + "y": 5.5955071449279785, + "heading": 3.107124271420268, + "angularVelocity": -0.09814732670676915, + "velocityX": 0.5298133855660034, + "velocityY": 0.07849078172416017, + "timestamp": 2.1300365644672383 + }, + { + "x": 2.575289726257324, + "y": 5.5955071449279785, + "heading": 3.107124271420268, + "angularVelocity": -5.215552459180153e-16, + "velocityX": 3.0089532183328406e-14, + "velocityY": 4.458059328942571e-15, + "timestamp": 2.2125646917615227 + }, + { + "x": 2.56266419375648, + "y": 5.630492886376702, + "heading": 3.122550239341884, + "angularVelocity": 0.20311625806558295, + "velocityX": -0.16624246405072463, + "velocityY": 0.46066301478281235, + "timestamp": 2.288511186978244 + }, + { + "x": 2.5374131131977165, + "y": 5.700463628965542, + "heading": 3.1534291742330782, + "angularVelocity": 0.4065880170385428, + "velocityX": -0.3324851329440162, + "velocityY": 0.9213162818003855, + "timestamp": 2.3644576821949657 + }, + { + "x": 2.4995362415427587, + "y": 5.805417996771896, + "heading": 3.19980560300506, + "angularVelocity": 0.6106460691785975, + "velocityX": -0.4987310019622596, + "velocityY": 1.3819514318186215, + "timestamp": 2.440404177411687 + }, + { + "x": 2.4490327389219555, + "y": 5.945354001359259, + "heading": 3.2617363981754393, + "angularVelocity": 0.8154529711167446, + "velocityX": -0.6649879296824168, + "velocityY": 1.8425603997661821, + "timestamp": 2.5163506726284086 + }, + { + "x": 2.385900741582678, + "y": 6.120269213764202, + "heading": 3.3392798440385247, + "angularVelocity": 1.0210273119491182, + "velocityX": -0.8312693977401261, + "velocityY": 2.303137385152583, + "timestamp": 2.59229716784513 + }, + { + "x": 2.3227186283976033, + "y": 6.295145275676175, + "heading": 3.4175350409150376, + "angularVelocity": 1.0303990546660913, + "velocityX": -0.8319292813286185, + "velocityY": 2.3026218841691732, + "timestamp": 2.6682436630618516 + }, + { + "x": 2.272170205530713, + "y": 6.435045438189673, + "heading": 3.48012978179617, + "angularVelocity": 0.8241952535467432, + "velocityX": -0.6655794019545626, + "velocityY": 1.8420884612815527, + "timestamp": 2.744190158278573 + }, + { + "x": 2.2342575093322865, + "y": 6.539970319270381, + "heading": 3.5270674455852307, + "angularVelocity": 0.6180359430032825, + "velocityX": -0.49920270962127516, + "velocityY": 1.381563175249813, + "timestamp": 2.8201366534952945 + }, + { + "x": 2.2089819239602884, + "y": 6.609920169443641, + "heading": 3.558356030031694, + "angularVelocity": 0.4119819401432226, + "velocityX": -0.3328077918522984, + "velocityY": 0.9210411879264638, + "timestamp": 2.896083148712016 + }, + { + "x": 2.1963441371917685, + "y": 6.644895076751706, + "heading": 3.5740008281417315, + "angularVelocity": 0.2059976311664359, + "velocityX": -0.1664038180097232, + "velocityY": 0.4605203598699368, + "timestamp": 2.9720296439287375 + }, + { + "x": 2.1963441371917725, + "y": 6.644895076751709, + "heading": 3.5740008281417315, + "angularVelocity": -1.3637868113221733e-15, + "velocityX": 5.3784491271910236e-14, + "velocityY": 4.156281055322198e-14, + "timestamp": 3.047976139145459 + }, + { + "x": 2.231971396242215, + "y": 6.672425335046828, + "heading": 3.5643443041067155, + "angularVelocity": -0.11590115298775237, + "velocityX": 0.4276114662756766, + "velocityY": 0.33042828525922313, + "timestamp": 3.1312930327749537 + }, + { + "x": 2.30322600256208, + "y": 6.727485843396552, + "heading": 3.545035659057472, + "angularVelocity": -0.2317494593006279, + "velocityX": 0.8552239913879874, + "velocityY": 0.6608564716127653, + "timestamp": 3.2146099264044485 + }, + { + "x": 2.410108149628484, + "y": 6.810076554176806, + "heading": 3.516089233108116, + "angularVelocity": -0.3474256502898277, + "velocityX": 1.2828388386838163, + "velocityY": 0.9912840863646427, + "timestamp": 3.2979268200339433 + }, + { + "x": 2.481362993101861, + "y": 6.8651370460368595, + "heading": 3.496795216328086, + "angularVelocity": -0.23157388543347762, + "velocityX": 0.8552268377916669, + "velocityY": 0.6608562736976711, + "timestamp": 3.381243713663438 + }, + { + "x": 2.516990423202518, + "y": 6.8926672935485875, + "heading": 3.487148512215175, + "angularVelocity": -0.11578329067102924, + "velocityX": 0.42761351928325747, + "velocityY": 0.3304281558329967, + "timestamp": 3.464560607292933 + }, + { + "x": 2.5169904232025133, + "y": 6.892667293548584, + "heading": 3.487148512215175, + "angularVelocity": 1.3602024910621585e-15, + "velocityX": -5.3714331419710114e-14, + "velocityY": -4.155450611774537e-14, + "timestamp": 3.5478775009224277 + }, + { + "x": 2.5718261671456832, + "y": 6.898186939161443, + "heading": 3.477069271138113, + "angularVelocity": -0.10937557091391228, + "velocityX": 0.5950538095494854, + "velocityY": 0.05989681023929627, + "timestamp": 3.640030081181827 + }, + { + "x": 2.681508217454821, + "y": 6.909231755509312, + "heading": 3.4575790243828832, + "angularVelocity": -0.211499739891893, + "velocityX": 1.1902222379492307, + "velocityY": 0.1198535767178713, + "timestamp": 3.732182661441226 + }, + { + "x": 2.846051102653566, + "y": 6.925810022984321, + "heading": 3.4296881080413146, + "angularVelocity": -0.30266017796852446, + "velocityX": 1.7855483236126044, + "velocityY": 0.17990019843549565, + "timestamp": 3.8243352417006253 + }, + { + "x": 3.065475416078285, + "y": 6.947935469069552, + "heading": 3.39510156733926, + "angularVelocity": -0.37531820166833757, + "velocityX": 2.3810978792678914, + "velocityY": 0.24009578487060268, + "timestamp": 3.9164878219600245 + }, + { + "x": 3.3398079518665904, + "y": 6.975634927321895, + "heading": 3.3573060233050853, + "angularVelocity": -0.41014091985036794, + "velocityX": 2.9769381933320886, + "velocityY": 0.30058255747556695, + "timestamp": 4.008640402219424 + }, + { + "x": 3.6689956969032202, + "y": 7.0089807464819325, + "heading": 3.327489371179905, + "angularVelocity": -0.32355743096123224, + "velocityX": 3.5722032319659847, + "velocityY": 0.3618544273657076, + "timestamp": 4.100792982478823 + }, + { + "x": 4.008927875855967, + "y": 7.043053913381558, + "heading": 3.3274893635736054, + "angularVelocity": -8.25402758606233e-8, + "velocityX": 3.6887971882705437, + "velocityY": 0.3697472908920567, + "timestamp": 4.192945562738222 + }, + { + "x": 4.348860055864811, + "y": 7.077127069727228, + "heading": 3.32748935596458, + "angularVelocity": -8.256985814359766e-8, + "velocityX": 3.688797199730849, + "velocityY": 0.36974717636509635, + "timestamp": 4.285098142997621 + }, + { + "x": 4.68879223610328, + "y": 7.111200223782021, + "heading": 3.3274893483555545, + "angularVelocity": -8.2569857954749e-8, + "velocityX": 3.6887972022226583, + "velocityY": 0.3697471515054666, + "timestamp": 4.377250723257021 + }, + { + "x": 5.028724416402198, + "y": 7.145273377233764, + "heading": 3.327489340746529, + "angularVelocity": -8.256985699976269e-8, + "velocityX": 3.688797202878599, + "velocityY": 0.36974714496145045, + "timestamp": 4.46940330351642 + }, + { + "x": 5.368656596583823, + "y": 7.179346531855671, + "heading": 3.3274893331375037, + "angularVelocity": -8.25698575388082e-8, + "velocityX": 3.688797201605803, + "velocityY": 0.36974715765955635, + "timestamp": 4.561555883775819 + }, + { + "x": 5.708588776462647, + "y": 7.21341968949849, + "heading": 3.3274893255284783, + "angularVelocity": -8.256985726962378e-8, + "velocityX": 3.6887971983199312, + "velocityY": 0.3697471904411904, + "timestamp": 4.653708464035218 + }, + { + "x": 6.048520955835552, + "y": 7.24749285218862, + "heading": 3.327489317919453, + "angularVelocity": -8.256985807951504e-8, + "velocityX": 3.688797192829927, + "velocityY": 0.3697472452124313, + "timestamp": 4.745861044294617 + }, + { + "x": 6.388453134480725, + "y": 7.281566022139005, + "heading": 3.3274893103104275, + "angularVelocity": -8.256985851785546e-8, + "velocityX": 3.688797184932882, + "velocityY": 0.3697473239975757, + "timestamp": 4.838013624554017 + }, + { + "x": 6.72838531144346, + "y": 7.315639208914652, + "heading": 3.32748930270771, + "angularVelocity": -8.250140346176779e-8, + "velocityX": 3.688797166675784, + "velocityY": 0.3697475065780538, + "timestamp": 4.930166204813416 + }, + { + "x": 7.057617041415691, + "y": 7.348978040671025, + "heading": 3.297037604087901, + "angularVelocity": -0.3304486812424651, + "velocityX": 3.572680537489909, + "velocityY": 0.3617786030790186, + "timestamp": 5.022318785072815 + }, + { + "x": 7.331971315121928, + "y": 7.376672950557005, + "heading": 3.2604290856918334, + "angularVelocity": -0.39725983030554946, + "velocityX": 2.9771740838288077, + "velocityY": 0.3005332005682574, + "timestamp": 5.114471365332214 + }, + { + "x": 7.551418975241575, + "y": 7.398795917834572, + "heading": 3.227345420945804, + "angularVelocity": -0.3590096408901662, + "velocityX": 2.381351227517736, + "velocityY": 0.24006888592042935, + "timestamp": 5.2066239455916135 + }, + { + "x": 7.715983594309332, + "y": 7.4153728220382265, + "heading": 3.200825390020535, + "angularVelocity": -0.2877839215203561, + "velocityX": 1.7857841701721917, + "velocityY": 0.1798854048035672, + "timestamp": 5.298776525851013 + }, + { + "x": 7.8256824275597925, + "y": 7.426416918352715, + "heading": 3.182359338040438, + "angularVelocity": -0.20038562054494358, + "velocityX": 1.1904043591798632, + "velocityY": 0.11984576322660237, + "timestamp": 5.390929106110412 + }, + { + "x": 7.880527496337892, + "y": 7.431936264038086, + "heading": 3.1728327742859292, + "angularVelocity": -0.1033781553125501, + "velocityX": 0.5951549986307312, + "velocityY": 0.059893555555739525, + "timestamp": 5.483081686369811 + }, + { + "x": 7.8805274963378915, + "y": 7.431936264038086, + "heading": 3.1728327742859292, + "angularVelocity": -1.775341783976986e-17, + "velocityX": 4.113028451724064e-16, + "velocityY": 3.57290670854576e-19, + "timestamp": 5.57523426662921 + }, + { + "x": 7.822408547953597, + "y": 7.41047403420173, + "heading": 3.175349165110715, + "angularVelocity": 0.025773979264846814, + "velocityX": -0.5952797776073485, + "velocityY": -0.2198255777008578, + "timestamp": 5.672867263029485 + }, + { + "x": 7.706170653011586, + "y": 7.367549575191858, + "heading": 3.180381990047099, + "angularVelocity": 0.05154840189222895, + "velocityX": -1.1905595365060768, + "velocityY": -0.43965114861261134, + "timestamp": 5.7705002594297605 + }, + { + "x": 7.531813814524372, + "y": 7.303162888101721, + "heading": 3.1879313119617194, + "angularVelocity": 0.07732346842730987, + "velocityX": -1.7858392645493193, + "velocityY": -0.6594767083268057, + "timestamp": 5.868133255830036 + }, + { + "x": 7.299338038391319, + "y": 7.217313975080838, + "heading": 3.1979972199294466, + "angularVelocity": 0.10309944730630771, + "velocityX": -2.381118932168683, + "velocityY": -0.8793022460247019, + "timestamp": 5.965766252230311 + }, + { + "x": 7.008743341800003, + "y": 7.110002842433714, + "heading": 3.210579829440879, + "angularVelocity": 0.1288766090907016, + "velocityX": -2.9763984237453585, + "velocityY": -1.0991277191490776, + "timestamp": 6.063399248630586 + }, + { + "x": 6.66920204101997, + "y": 6.984616642542402, + "heading": 3.210579830691659, + "angularVelocity": 1.2811041720440323e-8, + "velocityX": -3.4777310263835637, + "velocityY": -1.284260490964078, + "timestamp": 6.161032245030861 + }, + { + "x": 6.329660740249976, + "y": 6.859230442627363, + "heading": 3.210579831942713, + "angularVelocity": 1.281383799244368e-8, + "velocityX": -3.477731026280753, + "velocityY": -1.284260491207098, + "timestamp": 6.258665241431136 + }, + { + "x": 5.990119439479944, + "y": 6.733844242712424, + "heading": 3.210579833193766, + "angularVelocity": 1.28138375701327e-8, + "velocityX": -3.4777310262811256, + "velocityY": -1.2842604912060889, + "timestamp": 6.356298237831411 + }, + { + "x": 5.650578138709994, + "y": 6.608458042797264, + "heading": 3.210579834444819, + "angularVelocity": 1.2813836910428288e-8, + "velocityX": -3.4777310262802943, + "velocityY": -1.2842604912083408, + "timestamp": 6.453931234231686 + }, + { + "x": 5.311036837940111, + "y": 6.483071842881922, + "heading": 3.2105798356958726, + "angularVelocity": 1.2813837803323398e-8, + "velocityX": -3.477731026279606, + "velocityY": -1.2842604912102045, + "timestamp": 6.5515642306319615 + }, + { + "x": 4.971495537170285, + "y": 6.357685642966424, + "heading": 3.210579836946926, + "angularVelocity": 1.2813838051458728e-8, + "velocityX": -3.4777310262790175, + "velocityY": -1.2842604912117968, + "timestamp": 6.649197227032237 + }, + { + "x": 4.631954236400644, + "y": 6.232299443050428, + "heading": 3.210579838197979, + "angularVelocity": 1.2813837699475643e-8, + "velocityX": -3.4777310262771293, + "velocityY": -1.2842604912169104, + "timestamp": 6.746830223432512 + }, + { + "x": 4.292412935621366, + "y": 6.106913243157073, + "heading": 3.2105798394487595, + "angularVelocity": 1.281104174057025e-8, + "velocityX": -3.477731026375836, + "velocityY": -1.2842604909850046, + "timestamp": 6.844463219832787 + }, + { + "x": 4.001818240481474, + "y": 5.999602110130247, + "heading": 3.2231628867856554, + "angularVelocity": 0.12888109349125912, + "velocityX": -2.9763984088792372, + "velocityY": -1.0991277230381526, + "timestamp": 6.942096216233062 + }, + { + "x": 3.769342465671717, + "y": 5.9137531967649375, + "heading": 3.2332291944852316, + "angularVelocity": 0.10310354153534633, + "velocityX": -2.381118918614906, + "velocityY": -0.8793022495524555, + "timestamp": 7.039729212633337 + }, + { + "x": 3.5949856282661736, + "y": 5.849366509394725, + "heading": 3.240778843987889, + "angularVelocity": 0.07732682372776463, + "velocityX": -1.7858392534703766, + "velocityY": -0.6594767111954635, + "timestamp": 7.137362209033612 + }, + { + "x": 3.4787477340833317, + "y": 5.806442050188606, + "heading": 3.245811899594428, + "angularVelocity": 0.05155076451719507, + "velocityX": -1.1905595287303292, + "velocityY": -0.43965115062266896, + "timestamp": 7.234995205433887 + }, + { + "x": 3.4206287860870352, + "y": 5.784979820251465, + "heading": 3.2483284087520983, + "angularVelocity": 0.02577519128218804, + "velocityX": -0.595279773633299, + "velocityY": -0.2198255787331396, + "timestamp": 7.332628201834162 + }, + { + "x": 3.4206287860870357, + "y": 5.784979820251465, + "heading": 3.2483284087520983, + "angularVelocity": 1.6085577646564897e-18, + "velocityX": -5.667153970406663e-17, + "velocityY": -2.0927587524019698e-17, + "timestamp": 7.4302611982344375 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/New Path.traj b/src/main/deploy/choreo/New Path.traj new file mode 100644 index 00000000..bf059d37 --- /dev/null +++ b/src/main/deploy/choreo/New Path.traj @@ -0,0 +1,283 @@ +{ + "samples": [ + { + "x": 1.4851783514022827, + "y": 7.326318264007568, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.566665314770093, + "y": 7.326318268482221, + "heading": -9.745639154608945e-23, + "angularVelocity": -8.704126162754158e-22, + "velocityX": 0.7277847634744247, + "velocityY": 3.996448202047959e-8, + "timestamp": 0.11196574517278132 + }, + { + "x": 1.7296392389410207, + "y": 7.326318277431527, + "heading": -3.4658654410217392e-22, + "angularVelocity": -2.2250569735274297e-21, + "velocityX": 1.4555695040427974, + "velocityY": 7.992896278313055e-8, + "timestamp": 0.22393149034556264 + }, + { + "x": 1.9741001187856775, + "y": 7.326318290855485, + "heading": -8.24362198504553e-22, + "angularVelocity": -4.267159205128636e-21, + "velocityX": 2.1833541987990532, + "velocityY": 1.1989344103012352e-7, + "timestamp": 0.33589723551834394 + }, + { + "x": 2.3000479389158897, + "y": 7.326318308754096, + "heading": -1.7621745704477776e-21, + "angularVelocity": -8.375886381665278e-21, + "velocityX": 2.9111387561188593, + "velocityY": 1.5985791173013713e-7, + "timestamp": 0.4478629806911253 + }, + { + "x": 2.6359451684196418, + "y": 7.326318327199053, + "heading": 5.860691675515718e-19, + "angularVelocity": 5.250099783101951e-18, + "velocityX": 2.999999946281858, + "velocityY": 1.6473750198101658e-7, + "timestamp": 0.5598287258639066 + }, + { + "x": 2.9718423979233948, + "y": 7.326318345644011, + "heading": 4.8428473870582435e-18, + "angularVelocity": 3.801857621890219e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 0.671794471036688 + }, + { + "x": 3.307739627427147, + "y": 7.3263183640889675, + "heading": 1.0521971096433735e-17, + "angularVelocity": 5.072197480886782e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101613e-7, + "timestamp": 0.7837602162094693 + }, + { + "x": 3.6436368569308994, + "y": 7.326318382533925, + "heading": 1.4717398190746304e-17, + "angularVelocity": 3.747063076821502e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 0.8957259613822507 + }, + { + "x": 3.979534086434652, + "y": 7.326318400978883, + "heading": 1.7426356992707997e-17, + "angularVelocity": 2.4194532030809352e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.007691706555032 + }, + { + "x": 4.315431315938405, + "y": 7.326318419423839, + "heading": 1.8985412935500457e-17, + "angularVelocity": 1.3924401153114026e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.1196574517278133 + }, + { + "x": 4.651328545442158, + "y": 7.326318437868797, + "heading": 2.022868920510175e-17, + "angularVelocity": 1.1104077197839936e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.2316231969005946 + }, + { + "x": 4.98722577494591, + "y": 7.3263184563137544, + "heading": 2.188100699427991e-17, + "angularVelocity": 1.4757350915348522e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.343588942073376 + }, + { + "x": 5.323123004449662, + "y": 7.326318474758711, + "heading": 2.3719859301334404e-17, + "angularVelocity": 1.6423347194890602e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.4555546872461573 + }, + { + "x": 5.659020233953415, + "y": 7.326318493203669, + "heading": 2.5354180700870405e-17, + "angularVelocity": 1.4596619698171647e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.5675204324189387 + }, + { + "x": 5.994917463457167, + "y": 7.326318511648626, + "heading": 2.6401814540471825e-17, + "angularVelocity": 9.356735339113599e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.67948617759172 + }, + { + "x": 6.330814692960919, + "y": 7.326318530093584, + "heading": 2.6961840683369917e-17, + "angularVelocity": 5.001763244165286e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.7914519227645014 + }, + { + "x": 6.666711922464672, + "y": 7.3263185485385405, + "heading": 2.746703941908865e-17, + "angularVelocity": 4.512083019401423e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 1.9034176679372827 + }, + { + "x": 7.002609151968424, + "y": 7.326318566983498, + "heading": 2.75257745202433e-17, + "angularVelocity": 5.245810017426919e-19, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.015383413110064 + }, + { + "x": 7.338506381472176, + "y": 7.326318585428456, + "heading": 2.6894889559197837e-17, + "angularVelocity": -5.634624707407713e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.127349158282845 + }, + { + "x": 7.674403610975929, + "y": 7.326318603873412, + "heading": 2.614291653422174e-17, + "angularVelocity": -6.716098963685828e-18, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.2393149034556266 + }, + { + "x": 8.010300840479681, + "y": 7.32631862231837, + "heading": 2.4766232846828968e-17, + "angularVelocity": -1.2295579197538016e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.351280648628408 + }, + { + "x": 8.346198069983435, + "y": 7.326318640763327, + "heading": 2.2029876482173888e-17, + "angularVelocity": -2.4439227918755626e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.4632463938011893 + }, + { + "x": 8.68209529948719, + "y": 7.326318659208284, + "heading": 1.6814351481469152e-17, + "angularVelocity": -4.6581434349974976e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101613e-7, + "timestamp": 2.5752121389739706 + }, + { + "x": 9.017992528990941, + "y": 7.326318677653242, + "heading": 6.264884173064017e-18, + "angularVelocity": -9.42204897841851e-17, + "velocityX": 2.9999999462818616, + "velocityY": 1.6473750198101616e-7, + "timestamp": 2.687177884146752 + }, + { + "x": 9.353889758494695, + "y": 7.326318696098199, + "heading": -5.6652372475915536e-21, + "angularVelocity": -5.600417688760007e-17, + "velocityX": 2.999999946281858, + "velocityY": 1.6473750198101658e-7, + "timestamp": 2.7991436293195333 + }, + { + "x": 9.679837578624907, + "y": 7.32631871399681, + "heading": -2.7181194656762927e-21, + "angularVelocity": 2.6321601978014413e-20, + "velocityX": 2.9111387561188593, + "velocityY": 1.5985791173013718e-7, + "timestamp": 2.9111093744923147 + }, + { + "x": 9.924298458469563, + "y": 7.326318727420768, + "heading": -1.1801505322960497e-21, + "angularVelocity": 1.3736066636473078e-20, + "velocityX": 2.1833541987990532, + "velocityY": 1.1989344103012357e-7, + "timestamp": 3.023075119665096 + }, + { + "x": 10.08727238264049, + "y": 7.326318736370074, + "heading": -3.536469486075474e-22, + "angularVelocity": 7.381753952033097e-21, + "velocityX": 1.4555695040427974, + "velocityY": 7.992896278313059e-8, + "timestamp": 3.1350408648378774 + }, + { + "x": 10.1687593460083, + "y": 7.326318740844727, + "heading": 0, + "angularVelocity": 3.1585280732860345e-21, + "velocityX": 0.7277847634744247, + "velocityY": 3.996448202047962e-8, + "timestamp": 3.2470066100106587 + }, + { + "x": 10.1687593460083, + "y": 7.326318740844727, + "heading": 0, + "angularVelocity": 0, + "velocityX": 4.0858250360262775e-40, + "velocityY": 0, + "timestamp": 3.35897235518344 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test.traj b/src/main/deploy/choreo/Test.traj new file mode 100644 index 00000000..b6769f68 --- /dev/null +++ b/src/main/deploy/choreo/Test.traj @@ -0,0 +1,1048 @@ +{ + "samples": [ + { + "x": 6.262721061706543, + "y": 7.723432540893555, + "heading": -1.3909421726729163, + "angularVelocity": 0, + "velocityX": 2.5794352429543268e-33, + "velocityY": 1.632394658558029e-31, + "timestamp": 0 + }, + { + "x": 6.277581307251211, + "y": 7.725905943496002, + "heading": -1.4026847222039476, + "angularVelocity": -0.24070666808423655, + "velocityX": 0.30461529521496195, + "velocityY": 0.05070146799832148, + "timestamp": 0.04878364868113742 + }, + { + "x": 6.30732784535201, + "y": 7.73075521672328, + "heading": -1.4260187855595086, + "angularVelocity": -0.47831730480172274, + "velocityX": 0.6097645195674349, + "velocityY": 0.09940366000447058, + "timestamp": 0.09756729736227485 + }, + { + "x": 6.3519930192267235, + "y": 7.737855600056714, + "heading": -1.460747062073797, + "angularVelocity": -0.7118835399392471, + "velocityX": 0.9155767369238197, + "velocityY": 0.14554842709376553, + "timestamp": 0.14635094604341226 + }, + { + "x": 6.411617775244888, + "y": 7.747043303169748, + "heading": -1.5066013810126098, + "angularVelocity": -0.9399526312293403, + "velocityX": 1.222228300467835, + "velocityY": 0.188335710046752, + "timestamp": 0.1951345947245497 + }, + { + "x": 6.486254915778551, + "y": 7.758094594512896, + "heading": -1.563206025148217, + "angularVelocity": -1.1603200184057156, + "velocityX": 1.5299622425028392, + "velocityY": 0.2265367934116903, + "timestamp": 0.24391824340568713 + }, + { + "x": 6.575973620033059, + "y": 7.770684943191197, + "heading": -1.6300175780522455, + "angularVelocity": -1.369548090605377, + "velocityX": 1.839114266358666, + "velocityY": 0.2580854244953151, + "timestamp": 0.29270189208682457 + }, + { + "x": 6.68086479391976, + "y": 7.784298068252523, + "heading": -1.706202835651976, + "angularVelocity": -1.5616965860360879, + "velocityX": 2.1501297406493154, + "velocityY": 0.27905098182187754, + "timestamp": 0.341485540767962 + }, + { + "x": 6.801036853159218, + "y": 7.797980718308517, + "heading": -1.7903041482596818, + "angularVelocity": -1.7239651990239508, + "velocityX": 2.463367593205988, + "velocityY": 0.2804761518644329, + "timestamp": 0.39026918944909944 + }, + { + "x": 6.936454641010933, + "y": 7.8094344363751, + "heading": -1.878909218250747, + "angularVelocity": -1.8162862431672349, + "velocityX": 2.7758847792800743, + "velocityY": 0.2347860067099, + "timestamp": 0.4390528381302369 + }, + { + "x": 7.082695182670108, + "y": 7.810233262352739, + "heading": -1.9578629194872545, + "angularVelocity": -1.6184460033436374, + "velocityX": 2.997736856770248, + "velocityY": 0.016374871483226684, + "timestamp": 0.4878364868113743 + }, + { + "x": 7.223835296356726, + "y": 7.796762666019016, + "heading": -2.0471358437953318, + "angularVelocity": -1.8299763695739797, + "velocityX": 2.8931848580893713, + "velocityY": -0.27612933222299074, + "timestamp": 0.5366201354925118 + }, + { + "x": 7.354803052838074, + "y": 7.772991915766418, + "heading": -2.1550287036030027, + "angularVelocity": -2.2116603149733707, + "velocityX": 2.68466504704038, + "velocityY": -0.4872688061520612, + "timestamp": 0.5854037841736491 + }, + { + "x": 7.4742458246638215, + "y": 7.740581654034482, + "heading": -2.2822104384915347, + "angularVelocity": -2.6070566332549223, + "velocityX": 2.4484181699170513, + "velocityY": -0.6643673158556982, + "timestamp": 0.6341874328547865 + }, + { + "x": 7.5802409462968035, + "y": 7.700900341921911, + "heading": -2.417169016751695, + "angularVelocity": -2.766471592608644, + "velocityX": 2.1727592031049605, + "velocityY": -0.8134141907248044, + "timestamp": 0.6829710815359239 + }, + { + "x": 7.672129501069982, + "y": 7.654894008543802, + "heading": -2.5538945641696404, + "angularVelocity": -2.802692113327309, + "velocityX": 1.8835933198392991, + "velocityY": -0.943068725318534, + "timestamp": 0.7317547302170613 + }, + { + "x": 7.749652012651268, + "y": 7.60317305783115, + "heading": -2.688974478195872, + "angularVelocity": -2.7689588146462856, + "velocityX": 1.589108516420976, + "velocityY": -1.0602107901094575, + "timestamp": 0.7805383788981987 + }, + { + "x": 7.81268960914966, + "y": 7.546154786467422, + "heading": -2.820156078567574, + "angularVelocity": -2.6890485627500436, + "velocityX": 1.2921869971313598, + "velocityY": -1.1687988271728462, + "timestamp": 0.829322027579336 + }, + { + "x": 7.861183478872633, + "y": 7.484141055466549, + "heading": -2.945831275109532, + "angularVelocity": -2.5761745982433752, + "velocityX": 0.9940599162629143, + "velocityY": -1.2711991144042833, + "timestamp": 0.8781056762604734 + }, + { + "x": 7.895102500915527, + "y": 7.417360782623291, + "heading": -3.064821050995034, + "angularVelocity": -2.439132354844338, + "velocityX": 0.6952948981859916, + "velocityY": -1.3689068909082454, + "timestamp": 0.9268893249416108 + }, + { + "x": 7.91452287202807, + "y": 7.3443702156057205, + "heading": -3.178476524123731, + "angularVelocity": -2.2811041341650276, + "velocityX": 0.3897734760360104, + "velocityY": -1.4649455903494477, + "timestamp": 0.9767140880580117 + }, + { + "x": 7.918523252900167, + "y": 7.2672649935033595, + "heading": -3.284329104363532, + "angularVelocity": -2.124497410904251, + "velocityX": 0.08028900935783301, + "velocityY": -1.5475281221555204, + "timestamp": 1.0265388511744127 + }, + { + "x": 7.906910588846862, + "y": 7.1868374929011365, + "heading": -3.3824565846985584, + "angularVelocity": -1.9694520193841136, + "velocityX": -0.23307013073333988, + "velocityY": -1.614207385478872, + "timestamp": 1.0763636142908135 + }, + { + "x": 7.879509681599388, + "y": 7.104030694847671, + "heading": -3.4729521992093764, + "angularVelocity": -1.8162778676800646, + "velocityX": -0.549945559870545, + "velocityY": -1.6619606973347105, + "timestamp": 1.1261883774072143 + }, + { + "x": 7.83618598767892, + "y": 7.019972039427727, + "heading": -3.5559401814649787, + "angularVelocity": -1.6655971261043478, + "velocityX": -0.8695213225449279, + "velocityY": -1.6870859019152618, + "timestamp": 1.1760131405236152 + }, + { + "x": 7.7768828446036995, + "y": 6.936010421201204, + "heading": -3.6315993208313064, + "angularVelocity": -1.5185047481206, + "velocityX": -1.1902343205660655, + "velocityY": -1.6851383323262898, + "timestamp": 1.225837903640016 + }, + { + "x": 7.701679271265924, + "y": 6.853750000105601, + "heading": -3.7001967878335145, + "angularVelocity": -1.3767745737571055, + "velocityX": -1.5093613824531287, + "velocityY": -1.6509947253221195, + "timestamp": 1.2756626667564168 + }, + { + "x": 7.610872980802322, + "y": 6.775066830474737, + "heading": -3.7621348050541026, + "angularVelocity": -1.2431171438955075, + "velocityX": -1.8225132400821376, + "velocityY": -1.579198067576135, + "timestamp": 1.3254874298728176 + }, + { + "x": 7.505084820312598, + "y": 6.702084255664981, + "heading": -3.818007374513292, + "angularVelocity": -1.1213815373016187, + "velocityX": -2.1232044845366476, + "velocityY": -1.464785184010744, + "timestamp": 1.3753121929892185 + }, + { + "x": 7.385360854295069, + "y": 6.637078162361786, + "heading": -3.8686466484181117, + "angularVelocity": -1.0163475095008234, + "velocityX": -2.4029008575078388, + "velocityY": -1.3046944779510248, + "timestamp": 1.4251369561056193 + }, + { + "x": 7.253221630232025, + "y": 6.582300627137907, + "heading": -3.915117881921837, + "angularVelocity": -0.9326935161779388, + "velocityX": -2.6520793235752476, + "velocityY": -1.099403826484783, + "timestamp": 1.4749617192220201 + }, + { + "x": 7.110603592349798, + "y": 6.5397609082895665, + "heading": -3.9586390406131264, + "angularVelocity": -0.8734845078882504, + "velocityX": -2.862392693148203, + "velocityY": -0.853786675291383, + "timestamp": 1.524786482338421 + }, + { + "x": 6.959688019467871, + "y": 6.511051041811376, + "heading": -4.000460626922849, + "angularVelocity": -0.8393735101590934, + "velocityX": -3.028927052384833, + "velocityY": -0.57621681835421, + "timestamp": 1.5746112454548218 + }, + { + "x": 6.802686317203808, + "y": 6.4972850308966805, + "heading": -4.041762614282168, + "angularVelocity": -0.828944981896137, + "velocityX": -3.151077746165866, + "velocityY": -0.27628853713828266, + "timestamp": 1.6244360085712226 + }, + { + "x": 6.641666889190674, + "y": 6.499146461486816, + "heading": -4.083591898163471, + "angularVelocity": -0.8395280030497522, + "velocityX": -3.231714873124085, + "velocityY": 0.03735954721526441, + "timestamp": 1.6742607716876234 + }, + { + "x": 6.554025443844433, + "y": 6.504818198402247, + "heading": -4.106607061816192, + "angularVelocity": -0.8548926043630328, + "velocityX": -3.2554199741306173, + "velocityY": 0.2106752754883057, + "timestamp": 1.7011824733551997 + }, + { + "x": 6.465903309522955, + "y": 6.515175795727831, + "heading": -4.129366792972261, + "angularVelocity": -0.8454046269745659, + "velocityX": -3.273275048121351, + "velocityY": 0.38473041019022464, + "timestamp": 1.728104175022776 + }, + { + "x": 6.377492972106861, + "y": 6.530224320088071, + "heading": -4.151061142120187, + "angularVelocity": -0.8058312737989967, + "velocityX": -3.2839802813256105, + "velocityY": 0.5589737434155491, + "timestamp": 1.7550258766903521 + }, + { + "x": 6.289029557445244, + "y": 6.54994168512482, + "heading": -4.170708185144005, + "angularVelocity": -0.7297845903793844, + "velocityX": -3.2859518225837547, + "velocityY": 0.7323966842893936, + "timestamp": 1.7819475783579284 + }, + { + "x": 6.200619876138574, + "y": 6.574316591577067, + "heading": -4.1880502469575855, + "angularVelocity": -0.6441666291264727, + "velocityX": -3.283955910304091, + "velocityY": 0.9053999168862914, + "timestamp": 1.8088692800255046 + }, + { + "x": 6.112380667700324, + "y": 6.603335936568093, + "heading": -4.202876413671976, + "angularVelocity": -0.5507143232412612, + "velocityX": -3.2776237374523784, + "velocityY": 1.0779164463436868, + "timestamp": 1.8357909816930809 + }, + { + "x": 6.024447431839759, + "y": 6.636983961155118, + "heading": -4.21502095735311, + "angularVelocity": -0.45110609392624407, + "velocityX": -3.266258461161022, + "velocityY": 1.2498476137399468, + "timestamp": 1.862712683360657 + }, + { + "x": 5.9369844917299925, + "y": 6.67524088945849, + "heading": -4.224379070642445, + "angularVelocity": -0.34760482100510015, + "velocityX": -3.2487894409405498, + "velocityY": 1.4210442109408232, + "timestamp": 1.8896343850282333 + }, + { + "x": 5.85020068278381, + "y": 6.718079424867887, + "heading": -4.230933343824597, + "angularVelocity": -0.24345686847957956, + "velocityX": -3.2235632805747643, + "velocityY": 1.5912268822518436, + "timestamp": 1.9165560866958096 + }, + { + "x": 5.764374657819795, + "y": 6.765456120737885, + "heading": -4.234801932529108, + "angularVelocity": -0.14369777780998974, + "velocityX": -3.1879866296632713, + "velocityY": 1.7597957385832854, + "timestamp": 1.9434777883633858 + }, + { + "x": 5.679898078081831, + "y": 6.817289036413865, + "heading": -4.23633544279374, + "angularVelocity": -0.056961862350179776, + "velocityX": -3.137861817988381, + "velocityY": 1.9253209294128728, + "timestamp": 1.970399490030962 + }, + { + "x": 5.597353754798229, + "y": 6.8733927437395, + "heading": -4.236336685062471, + "angularVelocity": -0.000046143767116917394, + "velocityX": -3.066088626299918, + "velocityY": 2.0839584368923916, + "timestamp": 1.9973211916985383 + }, + { + "x": 5.517548462205778, + "y": 6.933329466406544, + "heading": -4.236336756314199, + "angularVelocity": -0.0000026466279353108937, + "velocityX": -2.9643480036240453, + "velocityY": 2.226334850862786, + "timestamp": 2.0242428933661145 + }, + { + "x": 5.440660426588225, + "y": 6.9969653533432705, + "heading": -4.2363368216405135, + "angularVelocity": -0.000002426529910474948, + "velocityX": -2.855987209387521, + "velocityY": 2.3637393996300693, + "timestamp": 2.0511645950336908 + }, + { + "x": 5.3668609451848726, + "y": 7.064158606000888, + "heading": -4.236336882465344, + "angularVelocity": -0.000002259323384896465, + "velocityX": -2.741263621245184, + "velocityY": 2.4958768761097163, + "timestamp": 2.078086296701267 + }, + { + "x": 5.296314402820094, + "y": 7.1347594731280335, + "heading": -4.236336939821085, + "angularVelocity": -0.0000021304649147255538, + "velocityX": -2.620433999152856, + "velocityY": 2.622451879116761, + "timestamp": 2.1050079983688432 + }, + { + "x": 5.223355159608529, + "y": 7.2028641408202985, + "heading": -4.2363369981086905, + "angularVelocity": -0.0000021650788218063554, + "velocityX": -2.7100531798643903, + "velocityY": 2.5297311638468525, + "timestamp": 2.1319297000364195 + }, + { + "x": 5.147263517880752, + "y": 7.267450054429454, + "heading": -4.236337279703312, + "angularVelocity": -0.000010459763093908157, + "velocityX": -2.8264053538423908, + "velocityY": 2.399027907174606, + "timestamp": 2.1588514017039957 + }, + { + "x": 5.069609969473459, + "y": 7.327719317887442, + "heading": -4.2397693587594265, + "angularVelocity": -0.12748373407074054, + "velocityX": -2.884421994053331, + "velocityY": 2.2386869969130943, + "timestamp": 2.185773103371572 + }, + { + "x": 4.991315313836009, + "y": 7.3835528893390645, + "heading": -4.247917031261401, + "angularVelocity": -0.3026432950855358, + "velocityX": -2.9082357647454766, + "velocityY": 2.0739243061613366, + "timestamp": 2.212694805039148 + }, + { + "x": 4.9127940327135216, + "y": 7.43493409944564, + "heading": -4.261067305912266, + "angularVelocity": -0.4884637239226296, + "velocityX": -2.9166537127576233, + "velocityY": 1.9085424369160964, + "timestamp": 2.2396165067067244 + }, + { + "x": 4.834263162931679, + "y": 7.481850121813001, + "heading": -4.279189453051294, + "angularVelocity": -0.6731427070551314, + "velocityX": -2.917009881155601, + "velocityY": 1.7426841344079564, + "timestamp": 2.2665382083743006 + }, + { + "x": 4.755852198292673, + "y": 7.524286248366545, + "heading": -4.3021424966386705, + "angularVelocity": -0.8525851697939743, + "velocityX": -2.912556034057846, + "velocityY": 1.5762795040790913, + "timestamp": 2.293459910041877 + }, + { + "x": 4.6776450295688194, + "y": 7.562227922327389, + "heading": -4.329756286764849, + "angularVelocity": -1.0257074559091717, + "velocityX": -2.9049860848150146, + "velocityY": 1.4093341657718208, + "timestamp": 2.320381611709453 + }, + { + "x": 4.599697405926689, + "y": 7.595662442260324, + "heading": -4.36187003653138, + "angularVelocity": -1.1928573521496562, + "velocityX": -2.895345346464651, + "velocityY": 1.2419170357716123, + "timestamp": 2.3473033133770294 + }, + { + "x": 4.522044721957581, + "y": 7.624579942278734, + "heading": -4.398354385251799, + "angularVelocity": -1.355202177445422, + "velocityX": -2.8843898847087246, + "velocityY": 1.0741334398346885, + "timestamp": 2.3742250150446056 + }, + { + "x": 4.444705424600301, + "y": 7.648973992630796, + "heading": -4.439126600600355, + "angularVelocity": -1.5144739308094104, + "velocityX": -2.872749215939051, + "velocityY": 0.9061110123446864, + "timestamp": 2.401146716712182 + }, + { + "x": 4.367682079116374, + "y": 7.668842160746219, + "heading": -4.484163448473746, + "angularVelocity": -1.672882659109441, + "velocityX": -2.8610132611596217, + "velocityY": 0.7379982276283237, + "timestamp": 2.428068418379758 + }, + { + "x": 4.2909608557290015, + "y": 7.68418684326669, + "heading": -4.5335145703740745, + "angularVelocity": -1.833135308819658, + "velocityX": -2.849791010044676, + "velocityY": 0.5699744655791606, + "timestamp": 2.4549901200473343 + }, + { + "x": 4.214573338536296, + "y": 7.694986274202921, + "heading": -4.586873130065289, + "angularVelocity": -1.981990601860873, + "velocityX": -2.837395575358335, + "velocityY": 0.4011422112007421, + "timestamp": 2.4819118217149105 + }, + { + "x": 4.138708584851075, + "y": 7.701178218916884, + "heading": -4.642986117141262, + "angularVelocity": -2.084303130941541, + "velocityX": -2.8179776531949567, + "velocityY": 0.22999826646950333, + "timestamp": 2.5088335233824868 + }, + { + "x": 4.0635267157603705, + "y": 7.702741055577131, + "heading": -4.700895063298387, + "angularVelocity": -2.1510135901588874, + "velocityX": -2.792612072558697, + "velocityY": 0.05805118411648136, + "timestamp": 2.535755225050063 + }, + { + "x": 3.9891603849413086, + "y": 7.69967482001442, + "heading": -4.7598428344029395, + "angularVelocity": -2.1896004878318136, + "velocityX": -2.762319103647825, + "velocityY": -0.11389456731172393, + "timestamp": 2.5626769267176392 + }, + { + "x": 3.9157200481289314, + "y": 7.691991139911297, + "heading": -4.819206245948015, + "angularVelocity": -2.20503934996819, + "velocityX": -2.727923283572248, + "velocityY": -0.28540841132599454, + "timestamp": 2.5895986283852155 + }, + { + "x": 3.8432986736297607, + "y": 7.679708003997803, + "heading": -4.878456047486164, + "angularVelocity": -2.200819334148586, + "velocityX": -2.6900741785717757, + "velocityY": -0.4562540683786034, + "timestamp": 2.6165203300527917 + }, + { + "x": 3.767178908067983, + "y": 7.661359584936258, + "heading": -4.9411055422577075, + "angularVelocity": -2.178173285120295, + "velocityX": -2.6465024246556172, + "velocityY": -0.6379307027101192, + "timestamp": 2.6452827316484333 + }, + { + "x": 3.6924331960366215, + "y": 7.6378276959918, + "heading": -5.002452489289649, + "angularVelocity": -2.132886811552756, + "velocityX": -2.5987298655441697, + "velocityY": -0.818147569013091, + "timestamp": 2.674045133244075 + }, + { + "x": 3.6191036719149707, + "y": 7.609130007162116, + "heading": -5.062253719618226, + "angularVelocity": -2.0791459339622027, + "velocityX": -2.5494923947091537, + "velocityY": -0.9977500917039537, + "timestamp": 2.7028075348397165 + }, + { + "x": 3.5471939024124675, + "y": 7.575268547665926, + "heading": -5.1204729247544165, + "angularVelocity": -2.0241426969367864, + "velocityX": -2.5001309179066533, + "velocityY": -1.177282063307094, + "timestamp": 2.731569936435358 + }, + { + "x": 3.4767154865002805, + "y": 7.536248890999991, + "heading": -5.177030505585369, + "angularVelocity": -1.9663719888923923, + "velocityX": -2.450366172582587, + "velocityY": -1.356620257740998, + "timestamp": 2.7603323380309996 + }, + { + "x": 3.407688760361888, + "y": 7.492080782433884, + "heading": -5.231798300946882, + "angularVelocity": -1.9041454233016037, + "velocityX": -2.3998943867351343, + "velocityY": -1.5356196324299194, + "timestamp": 2.7890947396266412 + }, + { + "x": 3.340143896817427, + "y": 7.442779217512771, + "heading": -5.284591493855207, + "angularVelocity": -1.8354932126501358, + "velocityX": -2.3483735640039547, + "velocityY": -1.7140976478326841, + "timestamp": 2.817857141222283 + }, + { + "x": 3.27412236802275, + "y": 7.388366135741471, + "heading": -5.335157129059403, + "angularVelocity": -1.7580463521464555, + "velocityX": -2.295410853476529, + "velocityY": -1.8918128790587394, + "timestamp": 2.8466195428179244 + }, + { + "x": 3.2096786440607277, + "y": 7.328872976736236, + "heading": -5.383158367228062, + "angularVelocity": -1.6688883926830218, + "velocityX": -2.240554348277964, + "velocityY": -2.0684350299261944, + "timestamp": 2.875381944413566 + }, + { + "x": 3.1468819978399267, + "y": 7.264344584145763, + "heading": -5.428151854626462, + "angularVelocity": -1.5643160828833593, + "velocityX": -2.183289389517786, + "velocityY": -2.2434980742445556, + "timestamp": 2.9041443460092076 + }, + { + "x": 3.086069667816101, + "y": 7.1950511228989145, + "heading": -5.468166242373673, + "angularVelocity": -1.391204681366685, + "velocityX": -2.1142994551974152, + "velocityY": -2.4091681293172447, + "timestamp": 2.932906747604849 + }, + { + "x": 3.0275259564740042, + "y": 7.121347712494754, + "heading": -5.501111724564699, + "angularVelocity": -1.1454357203611945, + "velocityX": -2.0354250025830902, + "velocityY": -2.5624915276658675, + "timestamp": 2.9616691492004907 + }, + { + "x": 2.971279327000811, + "y": 7.04320705740932, + "heading": -5.527298450512104, + "angularVelocity": -0.9104499101136155, + "velocityX": -1.9555609529394808, + "velocityY": -2.716763926183058, + "timestamp": 2.9904315507961323 + }, + { + "x": 2.9174109465754836, + "y": 6.960636890290862, + "heading": -5.546994628200283, + "angularVelocity": -0.6847890508268619, + "velocityX": -1.8728749143637953, + "velocityY": -2.870767478991144, + "timestamp": 3.019193952391774 + }, + { + "x": 2.866075880401147, + "y": 6.8736868629294845, + "heading": -5.560494528251902, + "angularVelocity": -0.4693592781789271, + "velocityX": -1.7847976290728618, + "velocityY": -3.0230447576584725, + "timestamp": 3.0479563539874155 + }, + { + "x": 2.8175498704364297, + "y": 6.782475887169627, + "heading": -5.56823832686593, + "angularVelocity": -0.26923338053963086, + "velocityX": -1.687133454533103, + "velocityY": -3.1711877555340404, + "timestamp": 3.076718755583057 + }, + { + "x": 2.7723325729113526, + "y": 6.687281970257596, + "heading": -5.571091030562386, + "angularVelocity": -0.09918169339842771, + "velocityX": -1.5720974263823406, + "velocityY": -3.309665105519257, + "timestamp": 3.1054811571786987 + }, + { + "x": 2.7313227643886693, + "y": 6.5888533564389276, + "heading": -5.571091329273387, + "angularVelocity": -0.000010385467996944691, + "velocityX": -1.4258130840126306, + "velocityY": -3.4221277903851903, + "timestamp": 3.1342435587743402 + }, + { + "x": 2.695327188108764, + "y": 6.488482360163379, + "heading": -5.571091389406789, + "angularVelocity": -0.0000020906947138437794, + "velocityX": -1.2514802062066948, + "velocityY": -3.489659788727615, + "timestamp": 3.163005960369982 + }, + { + "x": 2.6644374013766363, + "y": 6.386424338113809, + "heading": -5.571091445632036, + "angularVelocity": -0.000001954817572507986, + "velocityX": -1.0739640996048214, + "velocityY": -3.548313645166309, + "timestamp": 3.1917683619656234 + }, + { + "x": 2.6387319200953607, + "y": 6.282938844914323, + "heading": -5.5710914990379115, + "angularVelocity": -0.0000018567947078760147, + "velocityX": -0.8937181826005289, + "velocityY": -3.597943407311477, + "timestamp": 3.220530763561265 + }, + { + "x": 2.618276020321345, + "y": 6.178289069805243, + "heading": -5.571091550510496, + "angularVelocity": -0.0000017895788121366361, + "velocityX": -0.7112027730370062, + "velocityY": -3.6384227082392258, + "timestamp": 3.2492931651569066 + }, + { + "x": 2.6031214015215296, + "y": 6.072741180608147, + "heading": -5.571091600810411, + "angularVelocity": -0.0000017488079015618967, + "velocityX": -0.5268898965001583, + "velocityY": -3.66964798979404, + "timestamp": 3.278055566752548 + }, + { + "x": 2.593303437669885, + "y": 5.966563843011363, + "heading": -5.571091650626611, + "angularVelocity": -0.0000017319903046834782, + "velocityX": -0.34134715138461447, + "velocityY": -3.691532407115573, + "timestamp": 3.3068179683481898 + }, + { + "x": 2.5859763828170177, + "y": 5.860714084500256, + "heading": -5.572220789956159, + "angularVelocity": -0.03925747736271315, + "velocityX": -0.25474419542066634, + "velocityY": -3.6801432647805052, + "timestamp": 3.3355803699438313 + }, + { + "x": 2.5796992915743795, + "y": 5.759227799011453, + "heading": -5.582823913476576, + "angularVelocity": -0.36864527759092536, + "velocityX": -0.21823946869483069, + "velocityY": -3.528435730630647, + "timestamp": 3.364342771539473 + }, + { + "x": 2.573220728209944, + "y": 5.662211251222422, + "heading": -5.602962232214507, + "angularVelocity": -0.7001612390035493, + "velocityX": -0.22524417312217984, + "velocityY": -3.3730336274749, + "timestamp": 3.3931051731351145 + }, + { + "x": 2.566526525209324, + "y": 5.569676172743622, + "heading": -5.632679414321448, + "angularVelocity": -1.0331954377354908, + "velocityX": -0.23274144818346448, + "velocityY": -3.21722364424626, + "timestamp": 3.421867574730756 + }, + { + "x": 2.5596164915421924, + "y": 5.481632761500877, + "heading": -5.672052484273959, + "angularVelocity": -1.3689075935324158, + "velocityX": -0.2402453649128921, + "velocityY": -3.0610591034961283, + "timestamp": 3.4506299763263977 + }, + { + "x": 2.552741284617666, + "y": 5.398278762662877, + "heading": -5.720011428064755, + "angularVelocity": -1.6674179181912085, + "velocityX": -0.23903452226162972, + "velocityY": -2.898019435575762, + "timestamp": 3.4793923779220393 + }, + { + "x": 2.5462952902058125, + "y": 5.320098463558787, + "heading": -5.772531812210918, + "angularVelocity": -1.826008303635769, + "velocityX": -0.22411182843701577, + "velocityY": -2.718142254016118, + "timestamp": 3.508154779517681 + }, + { + "x": 2.5403087000740845, + "y": 5.247241378213319, + "heading": -5.827031586720459, + "angularVelocity": -1.8948269784878506, + "velocityX": -0.2081394389758424, + "velocityY": -2.533066826954782, + "timestamp": 3.5369171811133224 + }, + { + "x": 2.534781390260419, + "y": 5.179741570676487, + "heading": -5.881748515191503, + "angularVelocity": -1.902376903023185, + "velocityX": -0.1921713593795052, + "velocityY": -2.3468070742417884, + "timestamp": 3.565679582708964 + }, + { + "x": 2.5297098084520613, + "y": 5.11759072142905, + "heading": -5.9354503143250374, + "angularVelocity": -1.8670832807526792, + "velocityX": -0.1763267852123539, + "velocityY": -2.160836571340383, + "timestamp": 3.5944419843046056 + }, + { + "x": 2.52508907755234, + "y": 5.060765743374426, + "heading": -5.987224172733467, + "angularVelocity": -1.800053386927794, + "velocityX": -0.16065177604679973, + "velocityY": -1.9756687516399425, + "timestamp": 3.623204385900247 + }, + { + "x": 2.520913062305025, + "y": 5.009238629150355, + "heading": -6.036351212906015, + "angularVelocity": -1.7080298392042115, + "velocityX": -0.14519007508535509, + "velocityY": -1.791474681025325, + "timestamp": 3.6519667874958888 + }, + { + "x": 2.5171749245786357, + "y": 4.962980001646945, + "heading": -6.0822365620723975, + "angularVelocity": -1.5953239862042126, + "velocityX": -0.12996611962149396, + "velocityY": -1.6083019823498301, + "timestamp": 3.6807291890915304 + }, + { + "x": 2.513867921957398, + "y": 4.921960544229186, + "heading": -6.124371621043028, + "angularVelocity": -1.4649353542508017, + "velocityX": -0.114976581848851, + "velocityY": -1.4261485530463565, + "timestamp": 3.709491590687172 + }, + { + "x": 2.510986070347903, + "y": 4.886151736006819, + "heading": -6.162313815889174, + "angularVelocity": -1.3191594839521836, + "velocityX": -0.10019509671006031, + "velocityY": -1.2449867269704884, + "timestamp": 3.7382539922828135 + }, + { + "x": 2.5085244953189276, + "y": 4.855526327600999, + "heading": -6.195675162110924, + "angularVelocity": -1.1598943193531397, + "velocityX": -0.08558308390155527, + "velocityY": -1.0647722967077582, + "timestamp": 3.767016393878455 + }, + { + "x": 2.50647946265891, + "y": 4.830058663888566, + "heading": -6.224114844978648, + "angularVelocity": -0.9887798406939349, + "velocityX": -0.07110090070932439, + "velocityY": -0.8854498337959771, + "timestamp": 3.7957787954740967 + }, + { + "x": 2.5048481619845675, + "y": 4.809724875505472, + "heading": -6.247333434669391, + "angularVelocity": -0.8072549023254009, + "velocityX": -0.056716427830769224, + "velocityY": -0.7069572516565106, + "timestamp": 3.8245411970697383 + }, + { + "x": 2.503628338009038, + "y": 4.794502956668013, + "heading": -6.265067776519374, + "angularVelocity": -0.6165807048832722, + "velocityX": -0.042410365889320165, + "velocityY": -0.5292297580521163, + "timestamp": 3.85330359866538 + }, + { + "x": 2.502817851264885, + "y": 4.78437275996039, + "heading": -6.2770863579082645, + "angularVelocity": -0.4178573666361596, + "velocityX": -0.02817868812018645, + "velocityY": -0.35220274196989104, + "timestamp": 3.8820660002610214 + }, + { + "x": 2.5024142265319824, + "y": 4.779315948486328, + "heading": -6.283185307179586, + "angularVelocity": -0.21204589787255823, + "velocityX": -0.014033067842394916, + "velocityY": -0.17581325597052144, + "timestamp": 3.910828401856663 + }, + { + "x": 2.5024142265319824, + "y": 4.779315948486328, + "heading": -6.283185307179586, + "angularVelocity": -2.714702576575285e-32, + "velocityX": 0, + "velocityY": 5.2982271204903544e-33, + "timestamp": 3.9395908034523046 + } + ] +} \ No newline at end of file diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 01dd542c..1233ebde 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -27,7 +27,7 @@ */ public final class Constants { public static final int loopPeriodMs = 20; - private static RobotType robotType = RobotType.KITBOT; + private static RobotType robotType = RobotType.SIMBOT; public static final boolean tuningMode = false; public static final boolean characterizationMode = false; @@ -45,12 +45,13 @@ public static RobotType getRobot() { public static Mode getMode() { return switch (robotType) { - case KITBOT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; + case RAINBOWT, COMPBOT -> RobotBase.isReal() ? Mode.REAL : Mode.REPLAY; case SIMBOT -> Mode.SIM; }; } - public static final Map logFolders = Map.of(RobotType.KITBOT, "/media/sda1/"); + public static final Map logFolders = + Map.of(RobotType.RAINBOWT, "/media/sda1/"); public enum Mode { /** Running on a real robot. */ @@ -65,7 +66,7 @@ public enum Mode { public enum RobotType { SIMBOT, - KITBOT, + RAINBOWT, COMPBOT } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 04b53680..af3de664 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -64,10 +64,10 @@ public RobotContainer() { drive = new Drive( new GyroIOPigeon2(false), - new ModuleIOSparkMax(0), - new ModuleIOSparkMax(1), - new ModuleIOSparkMax(2), - new ModuleIOSparkMax(3)); + new ModuleIOSparkMax(DriveConstants.moduleConfigs[0]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[1]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[2]), + new ModuleIOSparkMax(DriveConstants.moduleConfigs[3])); shooter = new KitbotShooter(new KitbotFlywheelIO() {}, new KitbotFeederIO() {}); } } @@ -77,10 +77,10 @@ public RobotContainer() { drive = new Drive( new GyroIO() {}, - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim(), - new ModuleIOSim()); + new ModuleIOSim(DriveConstants.moduleConfigs[0]), + new ModuleIOSim(DriveConstants.moduleConfigs[1]), + new ModuleIOSim(DriveConstants.moduleConfigs[2]), + new ModuleIOSim(DriveConstants.moduleConfigs[3])); shooter = new KitbotShooter(new KitbotFlywheelIOSim(), new KitbotFeederIOSim()); } default -> { diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 636c3327..06f30472 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -61,9 +61,9 @@ public static Command joystickDrive( // Convert to field relative speeds & send command drive.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( - linearVelocity.getX() * DriveConstants.maxLinearSpeed, - linearVelocity.getY() * DriveConstants.maxLinearSpeed, - omega * DriveConstants.maxAngularSpeed, + linearVelocity.getX() * DriveConstants.drivetrainConfig.maxLinearVelocity(), + linearVelocity.getY() * DriveConstants.drivetrainConfig.maxLinearVelocity(), + omega * DriveConstants.drivetrainConfig.maxLinearVelocity(), drive.getRotation())); }, drive); diff --git a/src/main/java/frc/robot/commands/DriveTrajectory.java b/src/main/java/frc/robot/commands/DriveTrajectory.java index 1d22c559..9ed1b7f8 100644 --- a/src/main/java/frc/robot/commands/DriveTrajectory.java +++ b/src/main/java/frc/robot/commands/DriveTrajectory.java @@ -28,10 +28,10 @@ public class DriveTrajectory extends Command { // initialize tunable numbers to constants static { - driveKp.initDefault(DriveConstants.trajectoryConstants().drivekp()); - driveKd.initDefault(DriveConstants.trajectoryConstants().drivekd()); - turnKp.initDefault(DriveConstants.trajectoryConstants().turnkp()); - turnKd.initDefault(DriveConstants.trajectoryConstants().turnkd()); + driveKp.initDefault(DriveConstants.trajectoryConstants.drivekp()); + driveKd.initDefault(DriveConstants.trajectoryConstants.drivekd()); + turnKp.initDefault(DriveConstants.trajectoryConstants.turnkp()); + turnKd.initDefault(DriveConstants.trajectoryConstants.turnkd()); } public static final PIDController linearController = new PIDController(0.0, 0.0, 0.0); diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index 54426345..83dbcb3a 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -33,10 +33,12 @@ public class Drive extends SubsystemBase { public static final Lock odometryLock = new ReentrantLock(); private final GyroIO gyroIO; private final GyroIOInputsAutoLogged gyroInputs = new GyroIOInputsAutoLogged(); - private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final ModuleIO[] modules = new ModuleIO[4]; // FL, FR, BL, BR + private final ModuleIOInputsAutoLogged[] moduleInputs = new ModuleIOInputsAutoLogged[4]; private final SwerveDriveKinematics kinematics = new SwerveDriveKinematics(DriveConstants.moduleTranslations); private Pose2d pose = new Pose2d(); + private final double[] lastPositionMeters = new double[4]; private Rotation2d lastGyroRotation = new Rotation2d(); private ChassisSpeeds robotVelocity = new ChassisSpeeds(); private ChassisSpeeds fieldVelocity = new ChassisSpeeds(); @@ -48,22 +50,25 @@ public Drive( ModuleIO blModuleIO, ModuleIO brModuleIO) { this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); + modules[0] = flModuleIO; + modules[1] = frModuleIO; + modules[2] = blModuleIO; + modules[3] = brModuleIO; + Arrays.fill(moduleInputs, new ModuleIOInputsAutoLogged()); + Arrays.fill(lastPositionMeters, 0.0); } public void periodic() { odometryLock.lock(); // Prevents odometry updates while reading data gyroIO.updateInputs(gyroInputs); - for (var module : modules) { - module.updateInputs(); + for (int i = 0; i < modules.length; i++) { + modules[i].updateInputs(moduleInputs[i]); } odometryLock.unlock(); Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); + for (int i = 0; i < modules.length; i++) { + Logger.processInputs("Drive/Module" + i, moduleInputs[i]); + modules[i].periodic(); } // Stop moving when disabled @@ -79,25 +84,31 @@ public void periodic() { } // Update odometry + // FIXME: turning not working? int deltaCount = gyroInputs.connected ? gyroInputs.odometryYawPositions.length : Integer.MAX_VALUE; - for (int i = 0; i < 4; i++) { - deltaCount = Math.min(deltaCount, modules[i].getPositionDeltas().length); + for (int i = 0; i < modules.length; i++) { + int minOdometryDeltaCount = + Math.min( + moduleInputs[i].odometryDrivePositionsMeters.length, + moduleInputs[i].odometryTurnPositions.length); + deltaCount = Math.min(deltaCount, minOdometryDeltaCount); } for (int deltaIndex = 0; deltaIndex < deltaCount; deltaIndex++) { // Read wheel deltas from each module SwerveModulePosition[] wheelDeltas = new SwerveModulePosition[4]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - wheelDeltas[moduleIndex] = modules[moduleIndex].getPositionDeltas()[deltaIndex]; + for (int moduleIndex = 0; moduleIndex < modules.length; moduleIndex++) { + double measure = moduleInputs[moduleIndex].odometryDrivePositionsMeters[deltaIndex]; + wheelDeltas[moduleIndex] = + new SwerveModulePosition( + measure - lastPositionMeters[moduleIndex], + moduleInputs[moduleIndex].odometryTurnPositions[deltaIndex]); + lastPositionMeters[moduleIndex] = measure; } - - // The twist represents the motion of the robot since the last - // sample in x, y, and theta based only on the modules, without - // the gyro. The gyro is always disconnected in simulation. - var twist = kinematics.toTwist2d(wheelDeltas); + Logger.recordOutput("Odometry/WheelDeltas", wheelDeltas); + Twist2d twist = kinematics.toTwist2d(wheelDeltas); if (gyroInputs.connected) { // If the gyro is connected, replace the theta component of the twist - // with the change in angle since the last sample. Rotation2d gyroRotation = gyroInputs.odometryYawPositions[deltaIndex]; twist = new Twist2d(twist.dx, twist.dy, gyroRotation.minus(lastGyroRotation).getRadians()); lastGyroRotation = gyroRotation; @@ -131,18 +142,18 @@ public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.maxLinearSpeed); - + SwerveDriveKinematics.desaturateWheelSpeeds( + setpointStates, DriveConstants.drivetrainConfig.maxLinearVelocity()); // Send setpoints to modules SwerveModuleState[] optimizedSetpointStates = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { + for (int i = 0; i < modules.length; i++) { // The module returns the optimized state, useful for logging - optimizedSetpointStates[i] = modules[i].runSetpoint(setpointStates[i]); + optimizedSetpointStates[i] = + SwerveModuleState.optimize(setpointStates[i], modules[i].getAngle()); + modules[i].runSetpoint(optimizedSetpointStates[i]); } - // Log setpoint states - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveStates/SetpointsOptimized", optimizedSetpointStates); + Logger.recordOutput("SwerveStates/Setpoints", optimizedSetpointStates); } /** Stops the drive. */ @@ -182,7 +193,7 @@ public double getCharacterizationVelocity() { /** Returns the module states (turn angles and drive velocities) for all of the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { - return Arrays.stream(modules).map(Module::getState).toArray(SwerveModuleState[]::new); + return Arrays.stream(modules).map(ModuleIO::getState).toArray(SwerveModuleState[]::new); } /** Returns the current odometry pose. */ diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index fc3e33fe..0f93748b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -1,30 +1,144 @@ package frc.robot.subsystems.drive; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; import frc.robot.Constants; +/** All Constants Measured in Meters and Radians (m/s, m/s^2, rad/s, rad/s^2) */ public final class DriveConstants { - public static final double maxLinearSpeed = Units.feetToMeters(12.16); - public static final double trackwidthX = Units.inchesToMeters(26.0); - public static final double trackwidthY = Units.inchesToMeters(26.0); - public static final double driveBaseRadius = Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); - public static final double maxAngularSpeed = maxLinearSpeed / driveBaseRadius; + public static final double wheelRadius = Units.inchesToMeters(2.0); public static final Translation2d[] moduleTranslations = new Translation2d[] { - new Translation2d(trackwidthX / 2.0, trackwidthY / 2.0), - new Translation2d(trackwidthX / 2.0, -trackwidthY / 2.0), - new Translation2d(-trackwidthX / 2.0, trackwidthY / 2.0), - new Translation2d(-trackwidthX / 2.0, -trackwidthY / 2.0) + new Translation2d( + getDrivetrainConfig().trackwidthX / 2.0, getDrivetrainConfig().trackwidthY / 2.0), + new Translation2d( + getDrivetrainConfig().trackwidthX / 2.0, -getDrivetrainConfig().trackwidthY / 2.0), + new Translation2d( + -getDrivetrainConfig().trackwidthX / 2.0, getDrivetrainConfig().trackwidthY / 2.0), + new Translation2d( + -getDrivetrainConfig().trackwidthX / 2.0, -getDrivetrainConfig().trackwidthY / 2.0) }; + public static final double odometryFrequency = + switch (Constants.getRobot()) { + case SIMBOT -> 50.0; + case RAINBOWT -> 100.0; + case COMPBOT -> 250.0; + }; + + public static DrivetrainConfig drivetrainConfig = getDrivetrainConfig(); + public static ModuleConfig[] moduleConfigs = getModuleConfig(); + public static ModuleConstants moduleConstants = getModuleConstants(); + public static TrajectoryConstants trajectoryConstants = getTrajectoryConstants(); + + private static DrivetrainConfig getDrivetrainConfig() { + // Generated by choreo + return switch (Constants.getRobot()) { + default -> + new DrivetrainConfig( + Units.inchesToMeters(26.0), + Units.inchesToMeters(26.0), + Units.feetToMeters(12.16), + Units.feetToMeters(21.32), + 7.93, + 29.89); + }; + } + + /** + * @return configs in order: fl, fr, bl, br + */ + private static ModuleConfig[] getModuleConfig() { + return switch (Constants.getRobot()) { + case COMPBOT, RAINBOWT -> + new ModuleConfig[] { + new ModuleConfig(15, 11, 0, new Rotation2d(-0.036), true), + new ModuleConfig(12, 9, 1, new Rotation2d(1.0185), true), + new ModuleConfig(14, 10, 2, new Rotation2d(1.0705), true), + new ModuleConfig(13, 8, 3, new Rotation2d(0.7465), true) + }; + case SIMBOT -> { + ModuleConfig[] configs = new ModuleConfig[4]; + for (int i = 0; i < configs.length; i++) + configs[i] = + new ModuleConfig(0, 0, 0, new Rotation2d(2 * Math.PI * Math.random()), false); + yield configs; + } + }; + } + + private static ModuleConstants getModuleConstants() { + return switch (Constants.getRobot()) { + case COMPBOT, RAINBOWT -> + new ModuleConstants( + 0.1, + 0.13, + 0.05, + 0.0, + 7.0, + 0.0, + Mk4iReductions.L2.reduction, + Mk4iReductions.TURN.reduction); + case SIMBOT -> + new ModuleConstants( + 0.014, + 0.134, + 0.1, + 0.0, + 10.0, + 0.0, + Mk4iReductions.L2.reduction, + Mk4iReductions.TURN.reduction); + }; + } - // Replace with robots - public static TrajectoryConstants trajectoryConstants() { + private static TrajectoryConstants getTrajectoryConstants() { return switch (Constants.getRobot()) { - case COMPBOT, KITBOT -> new TrajectoryConstants(6.0, 0.0, 8.0, 0.0); + case COMPBOT, RAINBOWT -> new TrajectoryConstants(6.0, 0.0, 8.0, 0.0); case SIMBOT -> new TrajectoryConstants(2.5, 0.0, 7.5, 0.0); }; } + public record DrivetrainConfig( + double trackwidthX, + double trackwidthY, + double maxLinearVelocity, + double maxLinearAcceleration, + double maxAngularVelocity, + double maxAngularAcceleration) { + public double driveBaseRadius() { + return Math.hypot(trackwidthX / 2.0, trackwidthY / 2.0); + } + } + + public record ModuleConfig( + int driveID, + int turnID, + int absoluteEncoderChannel, + Rotation2d absoluteEncoderOffset, + boolean turnMotorInverted) {} + + public record ModuleConstants( + double ffKs, + double ffKv, + double driveKp, + double drivekD, + double turnKp, + double turnkD, + double driveReduction, + double turnReduction) {} + + private enum Mk4iReductions { + L2((50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0)), + L3((50.0 / 14.0) * (16.0 / 28.0) * (45.0 / 15.0)), + TURN((150.0 / 7.0)); + + final double reduction; + + Mk4iReductions(double reduction) { + this.reduction = reduction; + } + } + public record TrajectoryConstants(double drivekp, double drivekd, double turnkp, double turnkd) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIO.java b/src/main/java/frc/robot/subsystems/drive/GyroIO.java index 38ccec1f..bcf122ed 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIO.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIO.java @@ -18,12 +18,12 @@ public interface GyroIO { @AutoLog - public static class GyroIOInputs { + class GyroIOInputs { public boolean connected = false; public Rotation2d yawPosition = new Rotation2d(); public Rotation2d[] odometryYawPositions = new Rotation2d[] {}; public double yawVelocityRadPerSec = 0.0; } - public default void updateInputs(GyroIOInputs inputs) {} + default void updateInputs(GyroIOInputs inputs) {} } diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index 59c7c8c0..bfd52494 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -32,7 +32,7 @@ public class GyroIOPigeon2 implements GyroIO { public GyroIOPigeon2(boolean phoenixDrive) { pigeon.getConfigurator().apply(new Pigeon2Configuration()); pigeon.getConfigurator().setYaw(0.0); - yaw.setUpdateFrequency(Module.ODOMETRY_FREQUENCY); + yaw.setUpdateFrequency(DriveConstants.odometryFrequency); yawVelocity.setUpdateFrequency(100.0); pigeon.optimizeBusUtilization(); if (phoenixDrive) { diff --git a/src/main/java/frc/robot/subsystems/drive/Module.java b/src/main/java/frc/robot/subsystems/drive/Module.java deleted file mode 100644 index a163f4f7..00000000 --- a/src/main/java/frc/robot/subsystems/drive/Module.java +++ /dev/null @@ -1,202 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.drive; - -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.controller.SimpleMotorFeedforward; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.math.util.Units; -import frc.robot.Constants; -import org.littletonrobotics.junction.Logger; - -public class Module { - private static final double WHEEL_RADIUS = Units.inchesToMeters(2.0); - public static final double ODOMETRY_FREQUENCY = 250.0; - - private final ModuleIO io; - private final ModuleIOInputsAutoLogged inputs = new ModuleIOInputsAutoLogged(); - private final int index; - - private final SimpleMotorFeedforward driveFeedforward; - private final PIDController driveFeedback; - private final PIDController turnFeedback; - private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop - private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop - private Rotation2d turnRelativeOffset = null; // Relative + Offset = Absolute - private double lastPositionMeters = 0.0; // Used for delta calculation - private SwerveModulePosition[] positionDeltas = new SwerveModulePosition[] {}; - - public Module(ModuleIO io, int index) { - this.io = io; - this.index = index; - - // Switch constants based on mode (the physics simulator is treated as a - // separate robot with different tuning) - switch (Constants.getMode()) { - case REAL: - case REPLAY: - driveFeedforward = new SimpleMotorFeedforward(0.1, 0.13); - driveFeedback = new PIDController(0.05, 0.0, 0.0); - turnFeedback = new PIDController(7.0, 0.0, 0.0); - break; - case SIM: - driveFeedforward = new SimpleMotorFeedforward(0.01356, 0.13396); - driveFeedback = new PIDController(0.1, 0.0, 0.0); - turnFeedback = new PIDController(10.0, 0.0, 0.0); - break; - default: - driveFeedforward = new SimpleMotorFeedforward(0.0, 0.0); - driveFeedback = new PIDController(0.0, 0.0, 0.0); - turnFeedback = new PIDController(0.0, 0.0, 0.0); - break; - } - - turnFeedback.enableContinuousInput(-Math.PI, Math.PI); - setBrakeMode(true); - } - - /** - * Update inputs without running the rest of the periodic logic. This is useful since these - * updates need to be properly thread-locked. - */ - public void updateInputs() { - io.updateInputs(inputs); - } - - public void periodic() { - Logger.processInputs("Drive/Module" + index, inputs); - - // On first cycle, reset relative turn encoder - // Wait until absolute angle is nonzero in case it wasn't initialized yet - if (turnRelativeOffset == null && inputs.turnAbsolutePosition.getRadians() != 0.0) { - turnRelativeOffset = inputs.turnAbsolutePosition.minus(inputs.turnPosition); - } - - // Run closed loop turn control - if (angleSetpoint != null) { - io.setTurnVoltage( - turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians())); - - // Run closed loop drive control - // Only allowed if closed loop turn control is running - if (speedSetpoint != null) { - // Scale velocity based on turn error - // - // When the error is 90°, the velocity setpoint should be 0. As the wheel turns - // towards the setpoint, its velocity should increase. This is achieved by - // taking the component of the velocity in the direction of the setpoint. - double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); - - // Run drive controller - double velocityRadPerSec = adjustSpeedSetpoint / WHEEL_RADIUS; - io.setDriveVoltage( - driveFeedforward.calculate(velocityRadPerSec) - + driveFeedback.calculate(inputs.driveVelocityRadPerSec, velocityRadPerSec)); - } - } - - // Calculate position deltas for odometry - int deltaCount = - Math.min(inputs.odometryDrivePositionsRad.length, inputs.odometryTurnPositions.length); - positionDeltas = new SwerveModulePosition[deltaCount]; - for (int i = 0; i < deltaCount; i++) { - double positionMeters = inputs.odometryDrivePositionsRad[i] * WHEEL_RADIUS; - Rotation2d angle = - inputs.odometryTurnPositions[i].plus( - turnRelativeOffset != null ? turnRelativeOffset : new Rotation2d()); - positionDeltas[i] = new SwerveModulePosition(positionMeters - lastPositionMeters, angle); - lastPositionMeters = positionMeters; - } - } - - /** Runs the module with the specified setpoint state. Returns the optimized state. */ - public SwerveModuleState runSetpoint(SwerveModuleState state) { - // Optimize state based on current angle - // Controllers run in "periodic" when the setpoint is not null - var optimizedState = SwerveModuleState.optimize(state, getAngle()); - - // Update setpoints, controllers run in "periodic" - angleSetpoint = optimizedState.angle; - speedSetpoint = optimizedState.speedMetersPerSecond; - - return optimizedState; - } - - /** Runs the module with the specified voltage while controlling to zero degrees. */ - public void runCharacterization(double volts) { - // Closed loop turn control - angleSetpoint = new Rotation2d(); - - // Open loop drive control - io.setDriveVoltage(volts); - speedSetpoint = null; - } - - /** Disables all outputs to motors. */ - public void stop() { - io.setTurnVoltage(0.0); - io.setDriveVoltage(0.0); - - // Disable closed loop control for turn and drive - angleSetpoint = null; - speedSetpoint = null; - } - - /** Sets whether brake mode is enabled. */ - public void setBrakeMode(boolean enabled) { - io.setDriveBrakeMode(enabled); - io.setTurnBrakeMode(enabled); - } - - /** Returns the current turn angle of the module. */ - public Rotation2d getAngle() { - if (turnRelativeOffset == null) { - return new Rotation2d(); - } else { - return inputs.turnPosition.plus(turnRelativeOffset); - } - } - - /** Returns the current drive position of the module in meters. */ - public double getPositionMeters() { - return inputs.drivePositionRad * WHEEL_RADIUS; - } - - /** Returns the current drive velocity of the module in meters per second. */ - public double getVelocityMetersPerSec() { - return inputs.driveVelocityRadPerSec * WHEEL_RADIUS; - } - - /** Returns the module position (turn angle and drive position). */ - public SwerveModulePosition getPosition() { - return new SwerveModulePosition(getPositionMeters(), getAngle()); - } - - /** Returns the module state (turn angle and drive velocity). */ - public SwerveModuleState getState() { - return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); - } - - /** Returns the module position deltas received this cycle. */ - public SwerveModulePosition[] getPositionDeltas() { - return positionDeltas; - } - - /** Returns the drive velocity in radians/sec. */ - public double getCharacterizationVelocity() { - return inputs.driveVelocityRadPerSec; - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java index 8620ae3d..da0e9a6d 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIO.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIO.java @@ -14,11 +14,13 @@ package frc.robot.subsystems.drive; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModulePosition; +import edu.wpi.first.math.kinematics.SwerveModuleState; import org.littletonrobotics.junction.AutoLog; public interface ModuleIO { @AutoLog - public static class ModuleIOInputs { + class ModuleIOInputs { public double drivePositionRad = 0.0; public double driveVelocityRadPerSec = 0.0; public double driveAppliedVolts = 0.0; @@ -30,22 +32,64 @@ public static class ModuleIOInputs { public double turnAppliedVolts = 0.0; public double[] turnCurrentAmps = new double[] {}; - public double[] odometryDrivePositionsRad = new double[] {}; + public double[] odometryDrivePositionsMeters = new double[] {}; public Rotation2d[] odometryTurnPositions = new Rotation2d[] {}; + public double[] odometryTimestamps = new double[] {}; } /** Updates the set of loggable inputs. */ - public default void updateInputs(ModuleIOInputs inputs) {} + default void updateInputs(ModuleIOInputs inputs) {} - /** Run the drive motor at the specified voltage. */ - public default void setDriveVoltage(double volts) {} + default void periodic() {} - /** Run the turn motor at the specified voltage. */ - public default void setTurnVoltage(double volts) {} + /** Set setpoint for module */ + default void runSetpoint(SwerveModuleState state) {} + + /** Runs the module with the specified voltage while controlling to zero degrees. */ + default void runCharacterization(double volts) {} /** Enable or disable brake mode on the drive motor. */ - public default void setDriveBrakeMode(boolean enable) {} + default void setDriveBrakeMode(boolean enable) {} /** Enable or disable brake mode on the turn motor. */ - public default void setTurnBrakeMode(boolean enable) {} + default void setTurnBrakeMode(boolean enable) {} + + /** Enable or disable brake mode on drive & turn motor */ + default void setBrakeMode(boolean enable) { + setDriveBrakeMode(enable); + setTurnBrakeMode(enable); + } + + /** Disable output to all motors */ + default void stop() {} + + /** Get current heading of module */ + default Rotation2d getAngle() { + return new Rotation2d(); + } + + /** Returns the current drive position of the module in meters. */ + default double getPositionMeters() { + return 0.0; + } + + /** Returns the current drive velocity of the module in meters per second. */ + default double getVelocityMetersPerSec() { + return 0.0; + } + + /** Returns the module position (turn angle and drive position). */ + default SwerveModulePosition getPosition() { + return new SwerveModulePosition(getPositionMeters(), getAngle()); + } + + /** Returns the module state (turn angle and drive velocity). */ + default SwerveModuleState getState() { + return new SwerveModuleState(getVelocityMetersPerSec(), getAngle()); + } + + /** Returns the drive velocity in radians/sec. */ + default double getCharacterizationVelocity() { + return 0.0; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java new file mode 100644 index 00000000..32e2ad71 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOKrakenFOC.java @@ -0,0 +1,308 @@ +// Copyright 2021-2024 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package frc.robot.subsystems.drive; + +import static frc.robot.subsystems.drive.DriveConstants.*; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusCode; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.NeutralOut; +import com.ctre.phoenix6.controls.PositionTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VelocityTorqueCurrentFOC; +import com.ctre.phoenix6.controls.VoltageOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.RobotController; +import java.util.Queue; +import java.util.function.Supplier; + +public class ModuleIOKrakenFOC implements ModuleIO { + // Reseed relative encoder to absolute encoder value + private static final int reseedRelativeEncoderCounts = 100; + private int currentReseedCount = reseedRelativeEncoderCounts; + + private final StatusSignal drivePosition; + private final StatusSignal driveVelocity; + private final StatusSignal driveAppliedVolts; + private final StatusSignal driveCurrent; + + private final StatusSignal turnPosition; + private final Supplier turnAbsolutePosition; + private final StatusSignal turnVelocity; + private final StatusSignal turnAppliedVolts; + private final StatusSignal turnCurrent; + private final boolean isTurnMotorInverted = true; + + // queues + private final Queue timestampsQueue; + private final Queue drivePositionQueue; + private final Queue turnPositionQueue; + + private final TalonFX driveTalon; + private final TalonFX turnTalon; + private final AnalogInput turnAbsoluteEncoder; + private final Rotation2d absoluteEncoderOffset; + + // Setpoint control + private final VelocityTorqueCurrentFOC driveVelocityControl = + new VelocityTorqueCurrentFOC(0, 0, 0, 1, false, false, false); + private final PositionTorqueCurrentFOC turnPositionControl = + new PositionTorqueCurrentFOC(0.0, 0.0, 0.0, 1, false, false, false); + private final VoltageOut driveVoltageControl = new VoltageOut(0.0).withEnableFOC(true); + private final NeutralOut driveBrake = new NeutralOut(); + private final NeutralOut turnBrake = new NeutralOut(); + private Mode currentMode = Mode.SETPOINT; + private double driveSpeedSetpoint = 0.0; + private Rotation2d turnAngleSetpoint = new Rotation2d(); + private double driveVoltage = 0.0; + + public ModuleIOKrakenFOC(ModuleConfig config) { + // init controllers and encoders from config constants + driveTalon = new TalonFX(config.driveID()); + turnTalon = new TalonFX(config.turnID()); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + + var driveConfig = new TalonFXConfiguration(); + driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; + driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; + driveConfig.Voltage.PeakForwardVoltage = 12.0; + driveConfig.Voltage.PeakReverseVoltage = -12.0; + // TUNE PID CONSTANTS + driveConfig.Slot0.kP = moduleConstants.driveKp(); + driveConfig.Slot0.kI = 0.0; + driveConfig.Slot0.kD = moduleConstants.drivekD(); + driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = 40; + driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -40; + + var turnConfig = new TalonFXConfiguration(); + turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; + turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; + turnConfig.Voltage.PeakForwardVoltage = 12.0; + turnConfig.Voltage.PeakReverseVoltage = -12.0; + // TUNE PID CONSTANTS + turnConfig.Slot0.kP = moduleConstants.turnKp(); + turnConfig.Slot0.kI = 0.0; + turnConfig.Slot0.kD = moduleConstants.turnkD(); + turnConfig.TorqueCurrent.PeakForwardTorqueCurrent = 30; + turnConfig.TorqueCurrent.PeakReverseTorqueCurrent = -30; + turnConfig.ClosedLoopGeneral.ContinuousWrap = true; + // Map rotation of motor shaft to 1 rotation of turn shaft + turnConfig.Feedback.SensorToMechanismRatio = moduleConstants.turnReduction(); + + // Apply configs + for (int i = 0; i < 4; i++) { + boolean error = driveTalon.getConfigurator().apply(driveConfig, 0.1) == StatusCode.OK; + setDriveBrakeMode(true); + error = error && (turnTalon.getConfigurator().apply(turnConfig, 0.1) == StatusCode.OK); + setTurnBrakeMode(true); + if (!error) break; + } + + // Get signals and set update rate + // 50hz signals + drivePosition = driveTalon.getPosition(); + driveVelocity = driveTalon.getVelocity(); + driveAppliedVolts = driveTalon.getMotorVoltage(); + driveCurrent = driveTalon.getStatorCurrent(); + + turnPosition = turnTalon.getPosition(); + turnAbsolutePosition = + () -> + Rotation2d.fromRadians( + turnAbsoluteEncoder.getVoltage() + / RobotController.getVoltage5V() + * 2.0 + * Math.PI) + .minus(absoluteEncoderOffset); + turnVelocity = turnTalon.getVelocity(); + turnAppliedVolts = turnTalon.getMotorVoltage(); + turnCurrent = turnTalon.getStatorCurrent(); + + // 250hz signals + timestampsQueue = PhoenixOdometryThread.getInstance().getTimestampQueue(); + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + turnPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + + BaseStatusSignal.setUpdateFrequencyForAll( + 50.0, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnVelocity, + turnAppliedVolts, + turnCurrent); + BaseStatusSignal.setUpdateFrequencyForAll(odometryFrequency, drivePosition, turnPosition); + driveTalon.optimizeBusUtilization(); + turnTalon.optimizeBusUtilization(); + } + + @Override + public void updateInputs(ModuleIOInputs inputs) { + BaseStatusSignal.refreshAll( + drivePosition, + driveVelocity, + driveAppliedVolts, + driveCurrent, + turnPosition, + turnVelocity, + turnAppliedVolts, + turnCurrent); + + inputs.drivePositionRad = + Units.rotationsToRadians(drivePosition.getValueAsDouble()) + / moduleConstants.driveReduction(); + inputs.driveVelocityRadPerSec = + Units.rotationsToRadians(driveVelocity.getValueAsDouble()) + / moduleConstants.driveReduction(); + inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); + inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; + + inputs.turnAbsolutePosition = turnAbsolutePosition.get(); + inputs.turnPosition = + Rotation2d.fromRotations(turnPosition.getValueAsDouble() / moduleConstants.turnReduction()); + inputs.turnVelocityRadPerSec = + Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / moduleConstants.turnReduction(); + inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); + inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; + + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream() + .mapToDouble( + signal -> + Units.rotationsToRadians(signal) + * wheelRadius + / moduleConstants.driveReduction()) + .toArray(); + inputs.odometryTurnPositions = + turnPositionQueue.stream() + .map((signal) -> Rotation2d.fromRotations(signal / moduleConstants.turnReduction())) + .toArray(Rotation2d[]::new); + inputs.odometryTimestamps = timestampsQueue.stream().mapToDouble(d -> d).toArray(); + drivePositionQueue.clear(); + turnPositionQueue.clear(); + timestampsQueue.clear(); + } + + @Override + public void periodic() { + // Only reseed if turn abs encoder is online + if (++currentReseedCount >= reseedRelativeEncoderCounts + && turnAbsoluteEncoder.getVoltage() != 0.0) { + // heading of wheel --> turn motor shaft rotations + // timeout of 20 ms + boolean passed = + turnTalon.setPosition( + Units.radiansToRotations(turnAbsolutePosition.get().getRadians()) + * moduleConstants.turnReduction(), + 0.02) + == StatusCode.OK; + // redo next cycle if did not pass + currentReseedCount = passed ? 0 : currentReseedCount; + } + + // Control motors + if (currentMode == Mode.NEUTRAL) { + driveTalon.setControl(driveBrake); + turnTalon.setControl(turnBrake); + } else { + if (currentMode == Mode.SETPOINT) { + driveTalon.setControl(driveVelocityControl.withVelocity(driveSpeedSetpoint)); + } else if (currentMode == Mode.CHARACTERIZATION) { + driveTalon.setControl(driveVoltageControl.withOutput(driveVoltage)); + } + turnTalon.setControl(turnPositionControl.withPosition(turnAngleSetpoint.getRotations())); + } + } + + @Override + public void runSetpoint(SwerveModuleState state) { + currentMode = Mode.SETPOINT; + driveSpeedSetpoint = state.speedMetersPerSecond / wheelRadius; + turnAngleSetpoint = state.angle; + } + + @Override + public void runCharacterization(double volts) { + currentMode = Mode.CHARACTERIZATION; + driveVoltage = volts; + } + + @Override + public void setDriveBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + driveTalon.getConfigurator().apply(config); + } + + @Override + public void setTurnBrakeMode(boolean enable) { + var config = new MotorOutputConfigs(); + config.Inverted = + isTurnMotorInverted + ? InvertedValue.Clockwise_Positive + : InvertedValue.CounterClockwise_Positive; + config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; + turnTalon.getConfigurator().apply(config); + } + + @Override + public void stop() { + currentMode = Mode.NEUTRAL; + } + + @Override + public Rotation2d getAngle() { + return Rotation2d.fromRadians( + Units.rotationsToRadians(turnPosition.getValueAsDouble()) + / moduleConstants.turnReduction()); + } + + @Override + public double getPositionMeters() { + return Units.rotationsToRadians(drivePosition.getValueAsDouble()) + / moduleConstants.driveReduction() + * wheelRadius; + } + + @Override + public double getVelocityMetersPerSec() { + return Units.rotationsToRadians(driveVelocity.getValueAsDouble()) + / moduleConstants.driveReduction() + * wheelRadius; + } + + @Override + public double getCharacterizationVelocity() { + return Units.rotationsToRadians(driveVelocity.getValueAsDouble()) + / moduleConstants.driveReduction(); + } + + private enum Mode { + SETPOINT, + CHARACTERIZATION, + NEUTRAL; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java index b4f95abd..5470a318 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSim.java @@ -13,8 +13,12 @@ package frc.robot.subsystems.drive; -import edu.wpi.first.math.MathUtil; +import static frc.robot.subsystems.drive.DriveConstants.*; + +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.math.system.plant.DCMotor; import edu.wpi.first.wpilibj.simulation.DCMotorSim; @@ -28,12 +32,29 @@ public class ModuleIOSim implements ModuleIO { private static final double LOOP_PERIOD_SECS = 0.02; - private final DCMotorSim driveSim = new DCMotorSim(DCMotor.getNEO(1), 6.75, 0.025); - private final DCMotorSim turnSim = new DCMotorSim(DCMotor.getNEO(1), 150.0 / 7.0, 0.004); + private final DCMotorSim driveSim = + new DCMotorSim(DCMotor.getNEO(1), moduleConstants.driveReduction(), 0.025); + private final DCMotorSim turnSim = + new DCMotorSim(DCMotor.getNEO(1), moduleConstants.turnReduction(), 0.004); - private final Rotation2d turnAbsoluteInitPosition = new Rotation2d(Math.random() * 2.0 * Math.PI); private double driveAppliedVolts = 0.0; private double turnAppliedVolts = 0.0; + private final Rotation2d turnAbsoluteInitPosition; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + + public ModuleIOSim(ModuleConfig config) { + turnAbsoluteInitPosition = config.absoluteEncoderOffset(); + + driveFeedforward = new SimpleMotorFeedforward(moduleConstants.ffKs(), moduleConstants.ffKv()); + driveFeedback = new PIDController(moduleConstants.driveKp(), 0.0, moduleConstants.drivekD()); + turnFeedback = new PIDController(moduleConstants.turnKp(), 0.0, moduleConstants.turnkD()); + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); + } @Override public void updateInputs(ModuleIOInputs inputs) { @@ -52,19 +73,74 @@ public void updateInputs(ModuleIOInputs inputs) { inputs.turnAppliedVolts = turnAppliedVolts; inputs.turnCurrentAmps = new double[] {Math.abs(turnSim.getCurrentDrawAmps())}; - inputs.odometryDrivePositionsRad = new double[] {inputs.drivePositionRad}; + inputs.odometryDrivePositionsMeters = + new double[] {driveSim.getAngularPositionRad() * wheelRadius}; inputs.odometryTurnPositions = new Rotation2d[] {inputs.turnPosition}; } @Override - public void setDriveVoltage(double volts) { - driveAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - driveSim.setInputVoltage(driveAppliedVolts); + public void periodic() { + // Run closed loop turn control + if (angleSetpoint != null) { + turnAppliedVolts = + turnFeedback.calculate(getAngle().getRadians(), angleSetpoint.getRadians()); + turnSim.setInputVoltage(turnAppliedVolts); + // Run closed loop drive control + if (speedSetpoint != null) { + // Scale velocity based on turn error + double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); + double velocityRadPerSec = adjustSpeedSetpoint / wheelRadius; + driveAppliedVolts = + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate( + driveSim.getAngularVelocityRadPerSec(), velocityRadPerSec); + driveSim.setInputVoltage(driveAppliedVolts); + } + } + } + + @Override + public void runSetpoint(SwerveModuleState state) { + angleSetpoint = state.angle; + speedSetpoint = state.speedMetersPerSecond; + } + + @Override + public void runCharacterization(double volts) { + // Lock wheels to forward and no speed setpoint + angleSetpoint = new Rotation2d(); + speedSetpoint = null; + + driveAppliedVolts = volts; + driveSim.setInputVoltage(volts); + } + + @Override + public void stop() { + driveSim.setInputVoltage(0.0); + turnSim.setInputVoltage(0.0); + + speedSetpoint = null; + angleSetpoint = null; + } + + @Override + public Rotation2d getAngle() { + return Rotation2d.fromRadians(turnSim.getAngularPositionRad()); + } + + @Override + public double getPositionMeters() { + return driveSim.getAngularPositionRad() * wheelRadius; + } + + @Override + public double getVelocityMetersPerSec() { + return driveSim.getAngularVelocityRadPerSec() * wheelRadius; } @Override - public void setTurnVoltage(double volts) { - turnAppliedVolts = MathUtil.clamp(volts, -12.0, 12.0); - turnSim.setInputVoltage(turnAppliedVolts); + public double getCharacterizationVelocity() { + return driveSim.getAngularVelocityRadPerSec(); } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java index 59352842..c0d4f29b 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOSparkMax.java @@ -13,156 +13,181 @@ package frc.robot.subsystems.drive; +import static frc.robot.subsystems.drive.DriveConstants.*; + import com.revrobotics.CANSparkBase.IdleMode; -import com.revrobotics.CANSparkLowLevel.MotorType; +import com.revrobotics.CANSparkLowLevel; import com.revrobotics.CANSparkLowLevel.PeriodicFrame; import com.revrobotics.CANSparkMax; +import com.revrobotics.REVLibError; import com.revrobotics.RelativeEncoder; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.SimpleMotorFeedforward; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; +import edu.wpi.first.math.kinematics.SwerveModuleState; import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.RobotController; import java.util.Queue; +import java.util.function.Supplier; -/** - * Module IO implementation for SparkMax drive motor controller, SparkMax turn motor controller (NEO - * or NEO 550), and analog absolute encoder connected to the RIO - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware - * configurations (e.g. If using a CANcoder, copy from "ModuleIOTalonFX") - * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" - */ public class ModuleIOSparkMax implements ModuleIO { - // Gear ratios for SDS MK4i L2, adjust as necessary - private static final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private static final double TURN_GEAR_RATIO = 150.0 / 7.0; + // Every 100 times through periodic (2 secs) we reset relative encoder to absolute encoder + private static final int reSeedIterations = 100; private final CANSparkMax driveSparkMax; private final CANSparkMax turnSparkMax; - private final RelativeEncoder driveEncoder; private final RelativeEncoder turnRelativeEncoder; private final AnalogInput turnAbsoluteEncoder; + + // Queues private final Queue drivePositionQueue; private final Queue turnPositionQueue; + private final Queue timestampQueue; - private final boolean isTurnMotorInverted = true; private final Rotation2d absoluteEncoderOffset; - - public ModuleIOSparkMax(int index) { - switch (index) { - case 0: - driveSparkMax = new CANSparkMax(15, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(11, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(0); - absoluteEncoderOffset = new Rotation2d(-0.036); - break; - case 1: - driveSparkMax = new CANSparkMax(12, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(9, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(1); - absoluteEncoderOffset = new Rotation2d(1.0185); - break; - case 2: - driveSparkMax = new CANSparkMax(14, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(10, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(2); - absoluteEncoderOffset = new Rotation2d(1.0705); - break; - case 3: - driveSparkMax = new CANSparkMax(13, MotorType.kBrushless); - turnSparkMax = new CANSparkMax(8, MotorType.kBrushless); - turnAbsoluteEncoder = new AnalogInput(3); - absoluteEncoderOffset = new Rotation2d(0.7465); - break; - default: - throw new RuntimeException("Invalid module index"); - } + private int currentIteration = reSeedIterations; + private Supplier absoluteEncoderValue; + + private final SimpleMotorFeedforward driveFeedforward; + private final PIDController driveFeedback; + private final PIDController turnFeedback; + private Rotation2d angleSetpoint = null; // Setpoint for closed loop control, null for open loop + private Double speedSetpoint = null; // Setpoint for closed loop control, null for open loop + + public ModuleIOSparkMax(ModuleConfig config) { + // Init motor & encoder objects + driveSparkMax = new CANSparkMax(config.driveID(), CANSparkLowLevel.MotorType.kBrushless); + turnSparkMax = new CANSparkMax(config.turnID(), CANSparkLowLevel.MotorType.kBrushless); + turnAbsoluteEncoder = new AnalogInput(config.absoluteEncoderChannel()); + absoluteEncoderOffset = config.absoluteEncoderOffset(); + driveEncoder = driveSparkMax.getEncoder(); + turnRelativeEncoder = turnSparkMax.getEncoder(); driveSparkMax.restoreFactoryDefaults(); turnSparkMax.restoreFactoryDefaults(); - driveSparkMax.setCANTimeout(250); turnSparkMax.setCANTimeout(250); - driveEncoder = driveSparkMax.getEncoder(); - turnRelativeEncoder = turnSparkMax.getEncoder(); - - turnSparkMax.setInverted(isTurnMotorInverted); - driveSparkMax.setSmartCurrentLimit(40); - turnSparkMax.setSmartCurrentLimit(30); - driveSparkMax.enableVoltageCompensation(12.0); - turnSparkMax.enableVoltageCompensation(12.0); - - driveEncoder.setPosition(0.0); - driveEncoder.setMeasurementPeriod(10); - driveEncoder.setAverageDepth(2); - - turnRelativeEncoder.setPosition(0.0); - turnRelativeEncoder.setMeasurementPeriod(10); - turnRelativeEncoder.setAverageDepth(2); + for (int i = 0; i < 4; i++) { + turnSparkMax.setInverted(config.turnMotorInverted()); + driveSparkMax.setSmartCurrentLimit(40); + turnSparkMax.setSmartCurrentLimit(30); + driveSparkMax.enableVoltageCompensation(12.0); + turnSparkMax.enableVoltageCompensation(12.0); + + driveEncoder.setPosition(0.0); + driveEncoder.setMeasurementPeriod(10); + driveEncoder.setAverageDepth(2); + driveEncoder.setPositionConversionFactor(2 * Math.PI / moduleConstants.driveReduction()); + driveEncoder.setVelocityConversionFactor( + 2 * Math.PI / (60.0 * moduleConstants.driveReduction())); + + turnRelativeEncoder.setPosition(0.0); + turnRelativeEncoder.setMeasurementPeriod(10); + turnRelativeEncoder.setAverageDepth(2); + turnRelativeEncoder.setPositionConversionFactor( + 2 * Math.PI / moduleConstants.turnReduction()); + turnRelativeEncoder.setVelocityConversionFactor( + 2 * Math.PI / (60.0 * moduleConstants.turnReduction())); + + driveSparkMax.setCANTimeout(0); + turnSparkMax.setCANTimeout(0); + + driveSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + turnSparkMax.setPeriodicFramePeriod( + PeriodicFrame.kStatus2, (int) (1000.0 / odometryFrequency)); + + if (driveSparkMax.burnFlash().equals(REVLibError.kOk) + && turnSparkMax.burnFlash().equals(REVLibError.kOk)) break; + } - driveSparkMax.setCANTimeout(0); - turnSparkMax.setCANTimeout(0); + absoluteEncoderValue = + () -> + Rotation2d.fromRadians( + turnAbsoluteEncoder.getVoltage() + / RobotController.getVoltage5V() + * 2.0 + * Math.PI) + .minus(absoluteEncoderOffset); - driveSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); - turnSparkMax.setPeriodicFramePeriod( - PeriodicFrame.kStatus2, (int) (1000.0 / Module.ODOMETRY_FREQUENCY)); drivePositionQueue = SparkMaxOdometryThread.getInstance().registerSignal(driveEncoder::getPosition); turnPositionQueue = - SparkMaxOdometryThread.getInstance().registerSignal(turnRelativeEncoder::getPosition); - - driveSparkMax.burnFlash(); - turnSparkMax.burnFlash(); + SparkMaxOdometryThread.getInstance() + .registerSignal(() -> absoluteEncoderValue.get().getRadians()); + timestampQueue = SparkMaxOdometryThread.getInstance().getTimestampQueue(); + + driveFeedforward = new SimpleMotorFeedforward(moduleConstants.ffKs(), moduleConstants.ffKv()); + driveFeedback = new PIDController(moduleConstants.driveKp(), 0.0, moduleConstants.drivekD()); + turnFeedback = new PIDController(moduleConstants.turnKp(), 0.0, moduleConstants.turnkD()); + turnFeedback.enableContinuousInput(-Math.PI, Math.PI); } @Override public void updateInputs(ModuleIOInputs inputs) { - inputs.drivePositionRad = - Units.rotationsToRadians(driveEncoder.getPosition()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(driveEncoder.getVelocity()) / DRIVE_GEAR_RATIO; + inputs.drivePositionRad = driveEncoder.getPosition(); + inputs.driveVelocityRadPerSec = driveEncoder.getVelocity(); inputs.driveAppliedVolts = driveSparkMax.getAppliedOutput() * driveSparkMax.getBusVoltage(); inputs.driveCurrentAmps = new double[] {driveSparkMax.getOutputCurrent()}; - inputs.turnAbsolutePosition = - new Rotation2d( - turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V() * 2.0 * Math.PI) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnRelativeEncoder.getPosition() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsPerMinuteToRadiansPerSecond(turnRelativeEncoder.getVelocity()) - / TURN_GEAR_RATIO; + inputs.turnAbsolutePosition = absoluteEncoderValue.get(); + inputs.turnPosition = Rotation2d.fromRadians(turnRelativeEncoder.getPosition()); + inputs.turnVelocityRadPerSec = turnRelativeEncoder.getVelocity(); inputs.turnAppliedVolts = turnSparkMax.getAppliedOutput() * turnSparkMax.getBusVoltage(); inputs.turnCurrentAmps = new double[] {turnSparkMax.getOutputCurrent()}; - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); + inputs.odometryDrivePositionsMeters = + drivePositionQueue.stream().mapToDouble(rads -> rads * wheelRadius).toArray(); inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); + turnPositionQueue.stream().map(Rotation2d::fromRadians).toArray(Rotation2d[]::new); + inputs.odometryTimestamps = timestampQueue.stream().mapToDouble(d -> d).toArray(); drivePositionQueue.clear(); turnPositionQueue.clear(); } @Override - public void setDriveVoltage(double volts) { - driveSparkMax.setVoltage(volts); + public void periodic() { + // Reset to absolute encoder if abs encoder is non zero + if (++currentIteration >= reSeedIterations && absoluteEncoderValue.get().getRadians() != 0.0) { + REVLibError error = + turnRelativeEncoder.setPosition( + (turnAbsoluteEncoder.getVoltage() / RobotController.getVoltage5V()) + * moduleConstants.turnReduction()); + // Reset current iteration to 0 if reset went through + if (error == REVLibError.kOk) currentIteration = 0; + } + + // Run closed loop turn control + if (angleSetpoint != null) { + turnSparkMax.setVoltage( + turnFeedback.calculate( + absoluteEncoderValue.get().getRadians(), angleSetpoint.getRadians())); + // Run closed loop drive control + if (speedSetpoint != null) { + // Scale velocity based on turn error + double adjustSpeedSetpoint = speedSetpoint * Math.cos(turnFeedback.getPositionError()); + double velocityRadPerSec = adjustSpeedSetpoint / wheelRadius; + driveSparkMax.setVoltage( + driveFeedforward.calculate(velocityRadPerSec) + + driveFeedback.calculate(driveEncoder.getVelocity(), velocityRadPerSec)); + } + } + } + + @Override + public void runSetpoint(SwerveModuleState state) { + angleSetpoint = state.angle; + speedSetpoint = state.speedMetersPerSecond; } @Override - public void setTurnVoltage(double volts) { - turnSparkMax.setVoltage(volts); + public void runCharacterization(double volts) { + angleSetpoint = new Rotation2d(); + speedSetpoint = null; + + driveSparkMax.setVoltage(volts); } @Override @@ -174,4 +199,33 @@ public void setDriveBrakeMode(boolean enable) { public void setTurnBrakeMode(boolean enable) { turnSparkMax.setIdleMode(enable ? IdleMode.kBrake : IdleMode.kCoast); } + + @Override + public void stop() { + driveSparkMax.setVoltage(0.0); + turnSparkMax.setVoltage(0.0); + + speedSetpoint = null; + angleSetpoint = null; + } + + @Override + public Rotation2d getAngle() { + return absoluteEncoderValue.get(); + } + + @Override + public double getPositionMeters() { + return driveEncoder.getPosition() * wheelRadius; + } + + @Override + public double getVelocityMetersPerSec() { + return driveEncoder.getVelocity() * wheelRadius; + } + + @Override + public double getCharacterizationVelocity() { + return driveEncoder.getVelocity(); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java deleted file mode 100644 index b793f09b..00000000 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ /dev/null @@ -1,211 +0,0 @@ -// Copyright 2021-2024 FRC 6328 -// http://github.com/Mechanical-Advantage -// -// This program is free software; you can redistribute it and/or -// modify it under the terms of the GNU General Public License -// version 3 as published by the Free Software Foundation or -// available in the root directory of this project. -// -// This program is distributed in the hope that it will be useful, -// but WITHOUT ANY WARRANTY; without even the implied warranty of -// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -// GNU General Public License for more details. - -package frc.robot.subsystems.drive; - -import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.StatusSignal; -import com.ctre.phoenix6.configs.CANcoderConfiguration; -import com.ctre.phoenix6.configs.MotorOutputConfigs; -import com.ctre.phoenix6.configs.TalonFXConfiguration; -import com.ctre.phoenix6.controls.VoltageOut; -import com.ctre.phoenix6.hardware.CANcoder; -import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.InvertedValue; -import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; -import java.util.Queue; - -/** - * Module IO implementation for Talon FX drive motor controller, Talon FX turn motor controller, and - * CANcoder - * - *

NOTE: This implementation should be used as a starting point and adapted to different hardware - * configurations (e.g. If using an analog encoder, copy from "ModuleIOSparkMax") - * - *

To calibrate the absolute encoder offsets, point the modules straight (such that forward - * motion on the drive motor will propel the robot forward) and copy the reported values from the - * absolute encoders using AdvantageScope. These values are logged under - * "/Drive/ModuleX/TurnAbsolutePositionRad" - */ -public class ModuleIOTalonFX implements ModuleIO { - private final TalonFX driveTalon; - private final TalonFX turnTalon; - private final CANcoder cancoder; - - private final StatusSignal drivePosition; - private final Queue drivePositionQueue; - private final StatusSignal driveVelocity; - private final StatusSignal driveAppliedVolts; - private final StatusSignal driveCurrent; - - private final StatusSignal turnAbsolutePosition; - private final StatusSignal turnPosition; - private final Queue turnPositionQueue; - private final StatusSignal turnVelocity; - private final StatusSignal turnAppliedVolts; - private final StatusSignal turnCurrent; - - // Gear ratios for SDS MK4i L2, adjust as necessary - private final double DRIVE_GEAR_RATIO = (50.0 / 14.0) * (17.0 / 27.0) * (45.0 / 15.0); - private final double TURN_GEAR_RATIO = 150.0 / 7.0; - - private final boolean isTurnMotorInverted = true; - private final Rotation2d absoluteEncoderOffset; - - public ModuleIOTalonFX(int index) { - switch (index) { - case 0: - driveTalon = new TalonFX(0); - turnTalon = new TalonFX(1); - cancoder = new CANcoder(2); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 1: - driveTalon = new TalonFX(3); - turnTalon = new TalonFX(4); - cancoder = new CANcoder(5); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 2: - driveTalon = new TalonFX(6); - turnTalon = new TalonFX(7); - cancoder = new CANcoder(8); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - case 3: - driveTalon = new TalonFX(9); - turnTalon = new TalonFX(10); - cancoder = new CANcoder(11); - absoluteEncoderOffset = new Rotation2d(0.0); // MUST BE CALIBRATED - break; - default: - throw new RuntimeException("Invalid module index"); - } - - var driveConfig = new TalonFXConfiguration(); - driveConfig.CurrentLimits.StatorCurrentLimit = 40.0; - driveConfig.CurrentLimits.StatorCurrentLimitEnable = true; - driveTalon.getConfigurator().apply(driveConfig); - setDriveBrakeMode(true); - - var turnConfig = new TalonFXConfiguration(); - turnConfig.CurrentLimits.StatorCurrentLimit = 30.0; - turnConfig.CurrentLimits.StatorCurrentLimitEnable = true; - turnTalon.getConfigurator().apply(turnConfig); - setTurnBrakeMode(true); - - cancoder.getConfigurator().apply(new CANcoderConfiguration()); - - drivePosition = driveTalon.getPosition(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); - driveVelocity = driveTalon.getVelocity(); - driveAppliedVolts = driveTalon.getMotorVoltage(); - driveCurrent = driveTalon.getStatorCurrent(); - - turnAbsolutePosition = cancoder.getAbsolutePosition(); - turnPosition = turnTalon.getPosition(); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); - turnVelocity = turnTalon.getVelocity(); - turnAppliedVolts = turnTalon.getMotorVoltage(); - turnCurrent = turnTalon.getStatorCurrent(); - - BaseStatusSignal.setUpdateFrequencyForAll( - Module.ODOMETRY_FREQUENCY, drivePosition, turnPosition); - BaseStatusSignal.setUpdateFrequencyForAll( - 50.0, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - driveTalon.optimizeBusUtilization(); - turnTalon.optimizeBusUtilization(); - } - - @Override - public void updateInputs(ModuleIOInputs inputs) { - BaseStatusSignal.refreshAll( - drivePosition, - driveVelocity, - driveAppliedVolts, - driveCurrent, - turnAbsolutePosition, - turnPosition, - turnVelocity, - turnAppliedVolts, - turnCurrent); - - inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); - inputs.driveCurrentAmps = new double[] {driveCurrent.getValueAsDouble()}; - - inputs.turnAbsolutePosition = - Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; - inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); - inputs.turnCurrentAmps = new double[] {turnCurrent.getValueAsDouble()}; - - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); - drivePositionQueue.clear(); - turnPositionQueue.clear(); - } - - @Override - public void setDriveVoltage(double volts) { - driveTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setTurnVoltage(double volts) { - turnTalon.setControl(new VoltageOut(volts)); - } - - @Override - public void setDriveBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - driveTalon.getConfigurator().apply(config); - } - - @Override - public void setTurnBrakeMode(boolean enable) { - var config = new MotorOutputConfigs(); - config.Inverted = - isTurnMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; - config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; - turnTalon.getConfigurator().apply(config); - } -} diff --git a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java index 2a2b1cf7..cc083d55 100644 --- a/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java @@ -17,10 +17,8 @@ import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.hardware.ParentDevice; -import java.util.ArrayDeque; -import java.util.ArrayList; -import java.util.List; -import java.util.Queue; +import edu.wpi.first.wpilibj.Timer; +import java.util.*; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -37,6 +35,7 @@ public class PhoenixOdometryThread extends Thread { new ReentrantLock(); // Prevents conflicts when registering signals private BaseStatusSignal[] signals = new BaseStatusSignal[0]; private final List> queues = new ArrayList<>(); + private final Queue timestampQueue; private boolean isCANFD = false; private static PhoenixOdometryThread instance = null; @@ -51,6 +50,7 @@ public static PhoenixOdometryThread getInstance() { private PhoenixOdometryThread() { setName("PhoenixOdometryThread"); setDaemon(true); + timestampQueue = new ArrayDeque<>(100); start(); } @@ -79,13 +79,13 @@ public void run() { signalsLock.lock(); try { if (isCANFD) { - BaseStatusSignal.waitForAll(2.0 / Module.ODOMETRY_FREQUENCY, signals); + BaseStatusSignal.waitForAll(2.0 / DriveConstants.odometryFrequency, signals); } else { // "waitForAll" does not support blocking on multiple // signals with a bus that is not CAN FD, regardless // of Pro licensing. No reasoning for this behavior // is provided by the documentation. - Thread.sleep((long) (1000.0 / Module.ODOMETRY_FREQUENCY)); + Thread.sleep((long) (1000.0 / DriveConstants.odometryFrequency)); if (signals.length > 0) BaseStatusSignal.refreshAll(signals); } } catch (InterruptedException e) { @@ -93,16 +93,25 @@ public void run() { } finally { signalsLock.unlock(); } + double fpgaTimestamp = Timer.getFPGATimestamp(); // Save new data to queues Drive.odometryLock.lock(); try { for (int i = 0; i < signals.length; i++) { + // Get time of signal - latency + double signalTimeSeconds = + signals[i].getTimestamp().getTime() - signals[i].getTimestamp().getLatency(); queues.get(i).offer(signals[i].getValueAsDouble()); } + timestampQueue.offer(fpgaTimestamp); } finally { Drive.odometryLock.unlock(); } } } + + public Queue getTimestampQueue() { + return timestampQueue; + } } diff --git a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java index 93fe1df1..a3ea866a 100644 --- a/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java +++ b/src/main/java/frc/robot/subsystems/drive/SparkMaxOdometryThread.java @@ -14,6 +14,7 @@ package frc.robot.subsystems.drive; import edu.wpi.first.wpilibj.Notifier; +import edu.wpi.first.wpilibj.Timer; import java.util.ArrayDeque; import java.util.ArrayList; import java.util.List; @@ -29,6 +30,7 @@ public class SparkMaxOdometryThread { private List signals = new ArrayList<>(); private List> queues = new ArrayList<>(); + private Queue timestampQueue; private final Notifier notifier; private static SparkMaxOdometryThread instance = null; @@ -43,7 +45,8 @@ public static SparkMaxOdometryThread getInstance() { private SparkMaxOdometryThread() { notifier = new Notifier(this::periodic); notifier.setName("SparkMaxOdometryThread"); - notifier.startPeriodic(1.0 / Module.ODOMETRY_FREQUENCY); + timestampQueue = new ArrayDeque<>(100); + notifier.startPeriodic(1.0 / DriveConstants.odometryFrequency); } public Queue registerSignal(DoubleSupplier signal) { @@ -64,8 +67,13 @@ private void periodic() { for (int i = 0; i < signals.size(); i++) { queues.get(i).offer(signals.get(i).getAsDouble()); } + timestampQueue.offer(Timer.getFPGATimestamp()); } finally { Drive.odometryLock.unlock(); } } + + public Queue getTimestampQueue() { + return timestampQueue; + } }