From 91a9eb2f610562aff67e54ba47f17fcffa90d84a Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Mon, 11 Mar 2024 15:06:22 -0400 Subject: [PATCH 01/30] removed extra datalogging, double checked odometry calcs --- .../robot/subsystems/drive/module/Module.java | 6 -- .../drive/module/ModuleIOTalonFX.java | 8 +-- .../subsystems/vision/VisionSubsystem.java | 65 ++----------------- 3 files changed, 10 insertions(+), 69 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/module/Module.java b/src/main/java/frc/robot/subsystems/drive/module/Module.java index e2c7ba34..05dc2b9a 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/Module.java +++ b/src/main/java/frc/robot/subsystems/drive/module/Module.java @@ -43,12 +43,6 @@ public Module(ModuleIO io) { Timer.delay(0.5); setBrakeMode(true); - - DataLogUtil.getTable("Swerve").addDoubleArray("Module" + m_index + "/DriveCurrentDraw", - () -> m_inputs.driveCurrentAmps, false); - DataLogUtil.getTable("Swerve").addDoubleArray("Module" + m_index + "/TurnCurrentDraw", - () -> m_inputs.turnCurrentAmps, false); - } /** diff --git a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java index 9e64bf48..3ebf2f6c 100644 --- a/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/module/ModuleIOTalonFX.java @@ -14,7 +14,6 @@ package frc.robot.subsystems.drive.module; import com.ctre.phoenix6.BaseStatusSignal; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.StatusSignal; import com.ctre.phoenix6.configs.CANcoderConfiguration; import com.ctre.phoenix6.configs.MotorOutputConfigs; @@ -65,7 +64,7 @@ public class ModuleIOTalonFX implements ModuleIO { private final StatusSignal m_turnAbsolutePosition; private final StatusSignal m_turnPosition; private final Queue m_turnPositionQueue; - private final Queue timestampQueue; + private final Queue m_timestampQueue; private final StatusSignal m_turnVelocity; private final StatusSignal m_turnAppliedVolts; private final StatusSignal m_turnCurrent; @@ -173,7 +172,7 @@ public ModuleIOTalonFX(ModuleConstants moduleConstants) { m_turnClosedLoopError = m_turnTalon.getClosedLoopError(); m_turnClosedLoopReference = m_turnTalon.getClosedLoopReference(); - timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); + m_timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); // setup refresh rates on all inputs BaseStatusSignal.setUpdateFrequencyForAll( @@ -230,7 +229,7 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) { inputs.setTurnCurrentAmps(new double[] {m_turnCurrent.getValueAsDouble()}); inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + m_timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); inputs.setOdometryDrivePositionsRad(m_drivePositionQueue.stream() .mapToDouble(Units::rotationsToRadians) .toArray()); @@ -238,6 +237,7 @@ public void updateInputs(ModuleIOInputsAutoLogged inputs) { .map(Rotation2d::fromRotations) .toArray(Rotation2d[]::new)); + m_timestampQueue.clear(); m_drivePositionQueue.clear(); m_turnPositionQueue.clear(); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 82ee25bf..3c0aaede 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -32,7 +32,6 @@ public class VisionSubsystem { private AprilTagFieldLayout m_aprilTagFieldLayout; private final String m_name; - private Pose3d m_lastEstimatedPose = new Pose3d(); private final double xyStdDevCoefficient = Units.inchesToMeters(8.0); private final double thetaStdDevCoefficient = Units.degreesToRadians(24.0); @@ -69,18 +68,7 @@ public void updateInputs() { Logger.processInputs("Vision/" + m_name, inputs); } - public double getTagDistance(){ - var latestResult = m_camera.getLatestResult(); - if (latestResult.hasTargets()){ - var bestTarget = latestResult.getBestTarget(); - var camToTarget = bestTarget.getBestCameraToTarget(); - return camToTarget.getX(); - } - return -1; - } - public Optional getPose(Pose2d prevEstimatedRobotPose) { - m_photonPoseEstimator.setReferencePose(prevEstimatedRobotPose); PhotonPipelineResult camResult = m_camera.getLatestResult(); @@ -91,7 +79,6 @@ public Optional getPose(Pose2d prevEstima return Optional.empty(); } else { EstimatedRobotPose estPose = opPose.get(); - m_lastEstimatedPose = estPose.estimatedPose; // find average distance to tags int numTags = 0; @@ -107,20 +94,15 @@ public Optional getPose(Pose2d prevEstima avgDist += tagPose.get().toPose2d().getTranslation().getDistance(estPose.estimatedPose.toPose2d().getTranslation()); } - - avgDist /= numTags; - - double xyStdDev; - double thetaStdDev; - - if (estPose.strategy != PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR) { - xyStdDev = xyStdDevCoefficient * Math.pow(avgDist, 2.0); - thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDist, 2.0); + if (numTags == 0.0) { + avgDist = 0.0; } else { - xyStdDev = xyStdDevMultiTagCoefficient * Math.pow(avgDist, 2.0); - thetaStdDev = thetaStdDevMultiTagCoefficient * Math.pow(avgDist, 2.0); + avgDist /= numTags; } + double xyStdDev = xyStdDevCoefficient * Math.pow(avgDist, 2.0); + double thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDist, 2.0); + Logger.recordOutput("Vision/" + m_name + "/Estimated Pose", estPose.estimatedPose); return Optional.of(new PoseEstimator.TimestampedVisionUpdate(estPose.timestampSeconds, @@ -147,40 +129,5 @@ public int[] getTargetIDs () { return results; } - - public record VisionUpdate(Pose2d pose, Matrix stdDevs, double timestamp) { - public Pose2d apply(Pose2d lastPose, Matrix q) { - // Apply vision updates - // Calculate Kalman gains based on std devs - // (https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/java/edu/wpi/first/math/estimator/) - Matrix visionK = new Matrix<>(Nat.N3(), Nat.N3()); - var r = new double[3]; - for (int i = 0; i < 3; ++i) { - r[i] = this.stdDevs().get(i, 0) * this.stdDevs().get(i, 0); - } - for (int row = 0; row < 3; ++row) { - if (q.get(row, 0) == 0.0) { - visionK.set(row, row, 0.0); - } else { - visionK.set( - row, row, q.get(row, 0) / (q.get(row, 0) + Math.sqrt(q.get(row, 0) * r[row]))); - } - } - - // Calculate twist between current and vision pose - var visionTwist = lastPose.log(this.pose()); - - // Multiply by Kalman gain matrix - var twistMatrix = - visionK.times(VecBuilder.fill(visionTwist.dx, visionTwist.dy, visionTwist.dtheta)); - - // Apply twist - lastPose = - lastPose.exp( - new Twist2d(twistMatrix.get(0, 0), twistMatrix.get(1, 0), twistMatrix.get(2, 0))); - - return lastPose; - } - } } From 8765be3adaa9dd8e4f61bdba8ca81ad8b666d044 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Mon, 11 Mar 2024 23:11:06 -0400 Subject: [PATCH 02/30] created and tuned a lot of auto stuff --- .pathplanner/settings.json | 4 +- Crescendo2024.chor | 5180 +++++++++++------ src/main/deploy/choreo/FrontWing1.1.traj | 157 + src/main/deploy/choreo/FrontWing1.2.traj | 85 + src/main/deploy/choreo/FrontWing1.traj | 229 + src/main/deploy/choreo/FrontWing2.1.traj | 103 + src/main/deploy/choreo/FrontWing2.traj | 103 + .../deploy/choreo/FrontWing3Contested5.1.traj | 166 + .../deploy/choreo/FrontWing3Contested5.2.traj | 679 +++ .../deploy/choreo/FrontWing3Contested5.traj | 832 +++ src/main/deploy/choreo/NewPath.1.traj | 670 +++ src/main/deploy/choreo/NewPath.traj | 670 +++ src/main/deploy/choreo/Test1.1.traj | 1200 ++-- src/main/deploy/choreo/Test1.2.traj | 690 ++- src/main/deploy/choreo/Test1.traj | 1884 +++--- src/main/deploy/choreo/Test2.1.traj | 1444 ++--- src/main/deploy/choreo/Test2.2.traj | 517 +- src/main/deploy/choreo/Test2.traj | 1955 ++++--- .../deploy/pathplanner/autos/FrontWing1.auto | 69 + .../deploy/pathplanner/autos/FrontWing2.auto | 50 + .../deploy/pathplanner/autos/FrontWing3.auto | 50 + .../autos/FrontWing3Contested5.auto | 88 + .../pathplanner/autos/PathPlannerTest.auto | 19 + .../pathplanner/paths/AmpLineupPath.path | 52 + .../pathplanner/paths/OnePieceAmpStart.path | 84 + src/main/java/frc/robot/Constants.java | 32 +- src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 19 +- .../frc/robot/commands/DriveCommands.java | 2 +- .../frc/robot/commands/auto/AutoFactory.java | 14 + .../frc/robot/subsystems/arm/ArmIOKraken.java | 6 +- .../subsystems/drive/DriveSubsystem.java | 21 +- .../subsystems/shooter/ShooterSubsystem.java | 4 +- .../subsystems/vision/VisionSubsystem.java | 19 +- src/main/java/lib/utils/AimbotUtils.java | 19 +- src/main/java/lib/utils/FieldConstants.java | 2 +- 36 files changed, 11543 insertions(+), 5577 deletions(-) create mode 100644 src/main/deploy/choreo/FrontWing1.1.traj create mode 100644 src/main/deploy/choreo/FrontWing1.2.traj create mode 100644 src/main/deploy/choreo/FrontWing1.traj create mode 100644 src/main/deploy/choreo/FrontWing2.1.traj create mode 100644 src/main/deploy/choreo/FrontWing2.traj create mode 100644 src/main/deploy/choreo/FrontWing3Contested5.1.traj create mode 100644 src/main/deploy/choreo/FrontWing3Contested5.2.traj create mode 100644 src/main/deploy/choreo/FrontWing3Contested5.traj create mode 100644 src/main/deploy/choreo/NewPath.1.traj create mode 100644 src/main/deploy/choreo/NewPath.traj create mode 100644 src/main/deploy/pathplanner/autos/FrontWing1.auto create mode 100644 src/main/deploy/pathplanner/autos/FrontWing2.auto create mode 100644 src/main/deploy/pathplanner/autos/FrontWing3.auto create mode 100644 src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto create mode 100644 src/main/deploy/pathplanner/autos/PathPlannerTest.auto create mode 100644 src/main/deploy/pathplanner/paths/AmpLineupPath.path create mode 100644 src/main/deploy/pathplanner/paths/OnePieceAmpStart.path create mode 100644 src/main/java/frc/robot/commands/auto/AutoFactory.java diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 2aaab015..3294b25d 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -4,8 +4,8 @@ "holonomicMode": true, "pathFolders": [], "autoFolders": [], - "defaultMaxVel": 3.0, - "defaultMaxAccel": 3.0, + "defaultMaxVel": 1.5, + "defaultMaxAccel": 1.5, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 720.0, "maxModuleSpeed": 4.5 diff --git a/Crescendo2024.chor b/Crescendo2024.chor index a180f910..558b1600 100644 --- a/Crescendo2024.chor +++ b/Crescendo2024.chor @@ -3,8 +3,8 @@ "robotConfiguration": { "mass": 74.08797700309194, "rotationalInertia": 6, - "motorMaxTorque": 0.7759834368530021, - "motorMaxVelocity": 5800, + "motorMaxTorque": 1.7, + "motorMaxVelocity": 1750, "gearing": 5.731, "wheelbase": 0.4724397448825378, "trackWidth": 0.4724397448825378, @@ -31,7 +31,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 28 + "controlIntervalCount": 26 }, { "x": 2.1113932132720947, @@ -40,7 +40,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 25 + "controlIntervalCount": 23 }, { "x": 2.59393310546875, @@ -49,7 +49,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 25 + "controlIntervalCount": 23 }, { "x": 1.941084861755371, @@ -58,7 +58,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 23 + "controlIntervalCount": 21 }, { "x": 2.5655484199523926, @@ -75,1063 +75,991 @@ "x": 1.4869295358657837, "y": 5.564249038696289, "heading": 3.141, - "angularVelocity": 8.029589971503427e-22, - "velocityX": 1.7351458241819956e-21, - "velocityY": 6.201477276822387e-22, + "angularVelocity": 3.7405663636389993e-19, + "velocityX": -1.1249906625590385e-18, + "velocityY": 2.3803295269896554e-19, "timestamp": 0 }, { - "x": 1.506153170031305, - "y": 5.579314508687984, - "heading": 3.1569331073832556, - "angularVelocity": 0.2189381700102546, - "velocityX": 0.2641535755648618, - "velocityY": 0.20701589156378014, - "timestamp": 0.07277446131256805 - }, - { - "x": 1.5445753142190985, - "y": 5.609481391551554, - "heading": 3.1887853931444132, - "angularVelocity": 0.4376849403852184, - "velocityX": 0.5279619181620503, - "velocityY": 0.41452567727023953, - "timestamp": 0.1455489226251361 - }, - { - "x": 1.6021601427736685, - "y": 5.654796415146542, - "heading": 3.2365776272564357, - "angularVelocity": 0.6567171127073494, - "velocityX": 0.7912779774108067, - "velocityY": 0.6226775544288833, - "timestamp": 0.21832338393770417 - }, - { - "x": 1.6788531458253628, - "y": 5.715327275578862, - "heading": 3.3003664786278266, - "angularVelocity": 0.8765279772723583, - "velocityX": 1.0538450119513225, - "velocityY": 0.8317596494783837, - "timestamp": 0.2910978452502722 - }, - { - "x": 1.7745670450726352, - "y": 5.791182757948707, - "heading": 3.380219753358853, - "angularVelocity": 1.097270571169936, - "velocityX": 1.3152127480020601, - "velocityY": 1.0423365697486087, - "timestamp": 0.36387230656284025 - }, - { - "x": 1.8891522077377947, - "y": 5.8825573533612445, - "heading": 3.4761247222373646, - "angularVelocity": 1.317838251891654, - "velocityX": 1.5745243674565124, - "velocityY": 1.255586008669736, - "timestamp": 0.4366467678754083 - }, - { - "x": 2.0223089309237166, - "y": 5.989857408352276, - "heading": 3.5877133538349852, - "angularVelocity": 1.5333487817703626, - "velocityX": 1.829717744168665, - "velocityY": 1.4744190895508729, - "timestamp": 0.5094212291879764 - }, - { - "x": 2.1730917573654285, - "y": 6.114310741146903, - "heading": 3.713064984762661, - "angularVelocity": 1.7224673143135665, - "velocityX": 2.0719195130019052, - "velocityY": 1.7101237240368758, - "timestamp": 0.5821956905005444 - }, - { - "x": 2.304244374220996, - "y": 6.249503988496409, - "heading": 3.8044927424626787, - "angularVelocity": 1.2563165161379026, - "velocityX": 1.8021791503514413, - "velocityY": 1.8577017941616667, - "timestamp": 0.6549701518131125 - }, - { - "x": 2.414916720860036, - "y": 6.372175349075486, - "heading": 3.8745670686179303, - "angularVelocity": 0.9628972154706921, - "velocityX": 1.5207580330096784, - "velocityY": 1.6856374937933305, - "timestamp": 0.7277446131256805 - }, - { - "x": 2.505647288657887, - "y": 6.481209512574969, - "heading": 3.9250064522902046, - "angularVelocity": 0.6930918176863726, - "velocityX": 1.2467363709936818, - "velocityY": 1.4982476205653745, - "timestamp": 0.8005190744382485 - }, - { - "x": 2.576640808380743, - "y": 6.576219510667492, - "heading": 3.9564510372491593, - "angularVelocity": 0.432082689336569, - "velocityX": 0.975527931672858, - "velocityY": 1.305540382971075, - "timestamp": 0.8732935357508166 - }, - { - "x": 2.628005259573955, - "y": 6.657012046349283, - "heading": 3.969203722939461, - "angularVelocity": 0.17523572775796842, - "velocityX": 0.7058032483758355, - "velocityY": 1.1101770349736413, - "timestamp": 0.9460679970633846 - }, - { - "x": 2.6598078437532253, - "y": 6.723475709345355, - "heading": 3.963391945850161, - "angularVelocity": -0.07986011829532402, - "velocityX": 0.4370019867639682, - "velocityY": 0.9132827890076579, - "timestamp": 1.0188424583759528 - }, - { - "x": 2.6720941279928496, - "y": 6.775543377099927, - "heading": 3.9390315825966313, - "angularVelocity": -0.3347378024400806, - "velocityX": 0.16882686615644266, - "velocityY": 0.7154662063514367, - "timestamp": 1.0916169196885208 - }, - { - "x": 2.6648950576782227, + "x": 1.5092825889004946, + "y": 5.579306288055266, + "heading": 3.4062283948642453, + "angularVelocity": 3.352121601432042, + "velocityX": 0.282511802606821, + "velocityY": 0.19030289294706518, + "timestamp": 0.07912254577845217 + }, + { + "x": 1.5673643497428493, + "y": 5.60987650843196, + "heading": 3.626081158643754, + "angularVelocity": 2.77863612218835, + "velocityX": 0.7340734587204384, + "velocityY": 0.38636547997701703, + "timestamp": 0.15824509155690433 + }, + { + "x": 1.639918967505268, + "y": 5.662681063987355, + "heading": 3.7707265189248944, + "angularVelocity": 1.828118128126927, + "velocityX": 0.9169904361466855, + "velocityY": 0.6673768523986875, + "timestamp": 0.23736763733535649 + }, + { + "x": 1.719103738696596, + "y": 5.732106202864848, + "heading": 3.861411095653706, + "angularVelocity": 1.1461281463659418, + "velocityX": 1.000786443513203, + "velocityY": 0.8774381333973873, + "timestamp": 0.31649018311380867 + }, + { + "x": 1.8011465793982657, + "y": 5.8124573886259565, + "heading": 3.9165727578512475, + "angularVelocity": 0.6971674338183805, + "velocityX": 1.0369085055907419, + "velocityY": 1.0155283171258929, + "timestamp": 0.39561272889226085 + }, + { + "x": 1.8844011608490543, + "y": 5.899562457377884, + "heading": 3.9495869202931786, + "angularVelocity": 0.41725354154267424, + "velocityX": 1.0522232396807132, + "velocityY": 1.1008880957373124, + "timestamp": 0.474735274670713 + }, + { + "x": 1.968168332271066, + "y": 5.990724119399961, + "heading": 3.96923386729424, + "angularVelocity": 0.24831034956931133, + "velocityX": 1.0587016709063517, + "velocityY": 1.1521578473642025, + "timestamp": 0.5538578204491652 + }, + { + "x": 2.05213613980173, + "y": 6.084290626509761, + "heading": 3.9810005346780404, + "angularVelocity": 0.1487144690307097, + "velocityX": 1.0612374349756033, + "velocityY": 1.1825517769839073, + "timestamp": 0.6329803662276173 + }, + { + "x": 2.1361397889017213, + "y": 6.179275610733387, + "heading": 3.988261371091814, + "angularVelocity": 0.0917669716303672, + "velocityX": 1.0616904230458737, + "velocityY": 1.2004793739775566, + "timestamp": 0.7121029120060695 + }, + { + "x": 2.2200564987148903, + "y": 6.2750981229467735, + "heading": 3.9931304198236632, + "angularVelocity": 0.061538069635463585, + "velocityX": 1.0605916302053886, + "velocityY": 1.2110645742073904, + "timestamp": 0.7912254577845216 + }, + { + "x": 2.3037490732526433, + "y": 6.3714204686821265, + "heading": 3.997036412999259, + "angularVelocity": 0.049366373859271756, + "velocityX": 1.0577588690345867, + "velocityY": 1.217381781484405, + "timestamp": 0.8703480035629737 + }, + { + "x": 2.3870186605553965, + "y": 6.4680502574631, + "heading": 4.001128510533661, + "angularVelocity": 0.05171847662561717, + "velocityX": 1.0524128929813872, + "velocityY": 1.221267438127472, + "timestamp": 0.9494705493414258 + }, + { + "x": 2.4696215654523987, + "y": 6.564870888609008, + "heading": 4.006432047635611, + "angularVelocity": 0.06702940419537926, + "velocityX": 1.0439869456209716, + "velocityY": 1.2236794227654333, + "timestamp": 1.028593095119878 + }, + { + "x": 2.5522006651677756, + "y": 6.661687676467445, + "heading": 4.0117852212202445, + "angularVelocity": 0.06765674097018534, + "velocityX": 1.043686080913052, + "velocityY": 1.223630848905317, + "timestamp": 1.1077156408983302 + }, + { + "x": 2.626171983754375, + "y": 6.756004010665758, + "heading": 4.047305777483365, + "angularVelocity": 0.44893090728627943, + "velocityX": 0.9348955832857486, + "velocityY": 1.1920285586159498, + "timestamp": 1.1868381866767823 + }, + { + "x": 2.664895057678222, "y": 6.813176155090332, "heading": 3.896073037200959, - "angularVelocity": -0.5902969890929726, - "velocityX": -0.09892303130498589, - "velocityY": 0.5171151707845827, - "timestamp": 1.1643913810010889 - }, - { - "x": 2.659637231596543, - "y": 6.825031823463049, - "heading": 3.877816032306183, - "angularVelocity": -0.6840416135896586, - "velocityX": -0.19699681615987222, - "velocityY": 0.44420049020077496, - "timestamp": 1.191081284849143 - }, - { - "x": 2.6517927610129908, - "y": 6.834893701518374, - "heading": 3.857106562184998, - "angularVelocity": -0.775928989444263, - "velocityX": -0.29391153404714393, - "velocityY": 0.36949844823228756, - "timestamp": 1.2177711886971971 - }, - { - "x": 2.641397527843252, - "y": 6.842708311525597, - "heading": 3.834005474054525, - "angularVelocity": -0.865536581247652, - "velocityX": -0.38948185159896215, - "velocityY": 0.2927927373478789, - "timestamp": 1.2444610925452513 - }, - { - "x": 2.628493660297315, - "y": 6.848415370045661, - "heading": 3.808587288042731, - "angularVelocity": -0.9523521012476956, - "velocityX": -0.4834737367133058, - "velocityY": 0.2138283656829084, - "timestamp": 1.2711509963933054 - }, - { - "x": 2.6131312860336138, - "y": 6.85194659507741, - "heading": 3.7809426101756807, - "angularVelocity": -1.0357728534516963, - "velocityX": -0.5755874712460319, - "velocityY": 0.13230564830254915, - "timestamp": 1.2978409002413596 - }, - { - "x": 2.5953708985643273, - "y": 6.853224371337149, - "heading": 3.7511803744930092, - "angularVelocity": -1.1151121357389708, - "velocityX": -0.6654346741148351, - "velocityY": 0.0478748918322195, - "timestamp": 1.3245308040894137 - }, - { - "x": 2.5752865454465885, - "y": 6.852160328589509, - "heading": 3.7194297145938124, - "angularVelocity": -1.1896131241218921, - "velocityX": -0.752507511157751, - "velocityY": -0.03986686327897337, - "timestamp": 1.3512207079374678 - }, - { - "x": 2.5529701073140734, - "y": 6.848653945190506, - "heading": 3.6858413880737997, - "angularVelocity": -1.2584656247257915, - "velocityX": -0.8361378242335784, - "velocityY": -0.1313748981249787, - "timestamp": 1.377910611785522 - }, - { - "x": 2.5285370169793717, - "y": 6.842591376045072, - "heading": 3.650589102058915, - "angularVelocity": -1.3208097794422922, - "velocityX": -0.9154431755842751, - "velocityY": -0.2271484071260898, - "timestamp": 1.4046005156335761 - }, - { - "x": 2.502133887928051, - "y": 6.833844846711091, - "heading": 3.613872024847483, - "angularVelocity": -1.375691625584805, - "velocityX": -0.9892553079859069, - "velocityY": -0.32770928601974636, - "timestamp": 1.4312904194816303 - }, - { - "x": 2.4739487064187284, - "y": 6.822273243464229, - "heading": 3.5759212160148097, - "angularVelocity": -1.4219162814796094, - "velocityX": -1.0560240932219476, - "velocityY": -0.43355732237695227, - "timestamp": 1.4579803233296844 - }, - { - "x": 2.4442244144939402, - "y": 6.807725217915028, - "heading": 3.5370136666138436, - "angularVelocity": -1.4577628163243674, - "velocityX": -1.113690483637882, - "velocityY": -0.54507598199017, - "timestamp": 1.4846702271777386 - }, - { - "x": 2.4132762941421024, - "y": 6.79004764808573, - "heading": 3.497494895227568, - "angularVelocity": -1.4806636850869153, - "velocityX": -1.1595440930782523, - "velocityY": -0.6623317165148433, - "timestamp": 1.5113601310257927 - }, - { - "x": 2.3815108616317744, - "y": 6.769104478274178, - "heading": 3.45780088591702, - "angularVelocity": -1.4872293859328474, - "velocityX": -1.1901666147307672, - "velocityY": -0.7846850978100616, - "timestamp": 1.5380500348738468 - }, - { - "x": 2.3494374003667597, - "y": 6.744810785756249, - "heading": 3.4184626098399313, - "angularVelocity": -1.4739010039542162, - "velocityX": -1.2017076362510986, - "velocityY": -0.910220308631834, - "timestamp": 1.564739938721901 - }, - { - "x": 2.317656105280672, - "y": 6.717179467626764, - "heading": 3.3800987305060795, - "angularVelocity": -1.4373929390026206, - "velocityX": -1.190760943427104, - "velocityY": -1.0352722994728694, - "timestamp": 1.5914298425699551 - }, - { - "x": 2.2868057964945567, - "y": 6.686361396326711, - "heading": 3.343389515038899, - "angularVelocity": -1.3753970668521929, - "velocityX": -1.1558793527974576, - "velocityY": -1.1546714995865741, - "timestamp": 1.6181197464180093 - }, - { - "x": 2.25748075399746, - "y": 6.652639268574606, - "heading": 3.308925014353095, - "angularVelocity": -1.291293549876024, - "velocityX": -1.0987316651286732, - "velocityY": -1.263478802474685, - "timestamp": 1.6448096502660634 - }, - { - "x": 2.2301787041350396, - "y": 6.616374519308364, - "heading": 3.2770382136187797, - "angularVelocity": -1.194713960599006, - "velocityX": -1.022935489683713, - "velocityY": -1.3587440956212415, - "timestamp": 1.6714995541141175 - }, - { - "x": 2.2052911587380817, - "y": 6.577965167741144, - "heading": 3.2479010535236887, - "angularVelocity": -1.0916922091952401, - "velocityX": -0.9324704029899484, - "velocityY": -1.4390966631383846, - "timestamp": 1.6981894579621717 - }, - { - "x": 2.183095873519221, - "y": 6.537812097974725, - "heading": 3.2217255041499464, - "angularVelocity": -0.9807285002883435, - "velocityX": -0.8315985454731583, - "velocityY": -1.504429165238335, - "timestamp": 1.7248793618102258 - }, - { - "x": 2.163764936777705, - "y": 6.4962772278935255, - "heading": 3.198795874429834, - "angularVelocity": -0.8591124887766933, - "velocityX": -0.72427899521733, - "velocityY": -1.5562015628702863, - "timestamp": 1.75156926565828 - }, - { - "x": 2.1473967460074728, - "y": 6.453660043976164, - "heading": 3.1793849811459, - "angularVelocity": -0.7272747550699427, - "velocityX": -0.61327275150245, - "velocityY": -1.596752995439064, - "timestamp": 1.7782591695063341 - }, - { - "x": 2.134047988560311, - "y": 6.410199882298709, - "heading": 3.163697789085614, - "angularVelocity": -0.5877575336948715, - "velocityX": -0.5001425828716374, - "velocityY": -1.628337139199698, - "timestamp": 1.8049490733543883 - }, - { - "x": 2.1237534679344243, - "y": 6.366088523528368, - "heading": 3.151864532890396, - "angularVelocity": -0.443360765276075, - "velocityX": -0.38570841935190137, - "velocityY": -1.6527357693556206, - "timestamp": 1.8316389772024424 - }, - { - "x": 2.116536197487218, - "y": 6.321482513089734, - "heading": 3.143955647543306, - "angularVelocity": -0.29632498461271933, - "velocityX": -0.27041200628876155, - "velocityY": -1.671269057115243, - "timestamp": 1.8583288810504965 - }, - { - "x": 2.1124122616124033, - "y": 6.276512434450225, - "heading": 3.14, - "angularVelocity": -0.1482076355848078, - "velocityX": -0.15451295359818015, - "velocityY": -1.6849097282449244, - "timestamp": 1.8850187848985507 - }, - { - "x": 2.1113932132720947, + "angularVelocity": -1.9113735382816162, + "velocityX": 0.4894063195625013, + "velocityY": 0.7225771600506798, + "timestamp": 1.2659607324552344 + }, + { + "x": 2.67401621750823, + "y": 6.8287147997218245, + "heading": 3.8216991351700837, + "angularVelocity": -2.7569472556880648, + "velocityX": 0.338109953564034, + "velocityY": 0.5759980652369127, + "timestamp": 1.2929376366660208 + }, + { + "x": 2.6786727556459358, + "y": 6.8400421309827735, + "heading": 3.72670670460024, + "angularVelocity": -3.521250245306635, + "velocityX": 0.17261202773019518, + "velocityY": 0.41988996114756666, + "timestamp": 1.3199145408768072 + }, + { + "x": 2.678518823316929, + "y": 6.846619952079936, + "heading": 3.614568928899329, + "angularVelocity": -4.15680668266135, + "velocityX": -0.00570607834774554, + "velocityY": 0.24383157703223057, + "timestamp": 1.3468914450875935 + }, + { + "x": 2.6741686792707893, + "y": 6.846169187557711, + "heading": 3.4948207380964975, + "angularVelocity": -4.438915224191635, + "velocityX": -0.16125438308832984, + "velocityY": -0.0167092754120811, + "timestamp": 1.3738683492983799 + }, + { + "x": 2.663571596898345, + "y": 6.842490941859769, + "heading": 3.391106479438399, + "angularVelocity": -3.844557472114035, + "velocityX": -0.3928205508549501, + "velocityY": -0.1363479541318252, + "timestamp": 1.4008452535091662 + }, + { + "x": 2.648321534336774, + "y": 6.83512618854535, + "heading": 3.3066791856345255, + "angularVelocity": -3.129613878011262, + "velocityX": -0.5653006898944218, + "velocityY": -0.2730021672197161, + "timestamp": 1.4278221577199526 + }, + { + "x": 2.630755187717048, + "y": 6.821913643765559, + "heading": 3.2403542901418882, + "angularVelocity": -2.4585806797690686, + "velocityX": -0.6511624344467275, + "velocityY": -0.4897724615306165, + "timestamp": 1.454799061930739 + }, + { + "x": 2.610177790375068, + "y": 6.803317677931451, + "heading": 3.192043949934185, + "angularVelocity": -1.7908037123240415, + "velocityX": -0.7627783077407181, + "velocityY": -0.6893291271973806, + "timestamp": 1.4817759661415253 + }, + { + "x": 2.585880089062531, + "y": 6.779956801667491, + "heading": 3.1617447456020207, + "angularVelocity": -1.1231534980960616, + "velocityX": -0.9006853092812849, + "velocityY": -0.8659583798581432, + "timestamp": 1.5087528703523116 + }, + { + "x": 2.557481302008236, + "y": 6.752192795634918, + "heading": 3.1494506971418432, + "angularVelocity": -0.4557249550980476, + "velocityX": -1.0527074134380248, + "velocityY": -1.029176877214801, + "timestamp": 1.535729774563098 + }, + { + "x": 2.5262145192081267, + "y": 6.721488912807054, + "heading": 3.1494498734965517, + "angularVelocity": -0.00003053149779678828, + "velocityX": -1.1590204181995478, + "velocityY": -1.1381544223143767, + "timestamp": 1.5627066787738844 + }, + { + "x": 2.494951526671565, + "y": 6.690781099940809, + "heading": 3.1494491983381225, + "angularVelocity": -0.000025027276073123234, + "velocityX": -1.1588799178840683, + "velocityY": -1.1383001039076166, + "timestamp": 1.5896835829846707 + }, + { + "x": 2.46368853251831, + "y": 6.660073288720487, + "heading": 3.149448523179691, + "angularVelocity": -0.000025027276154336604, + "velocityX": -1.1588799778128431, + "velocityY": -1.138300042895271, + "timestamp": 1.616660487195457 + }, + { + "x": 2.4324255384011386, + "y": 6.629365477463429, + "heading": 3.1494478480212726, + "angularVelocity": -0.000025027275679943262, + "velocityX": -1.1588799764752862, + "velocityY": -1.138300044257102, + "timestamp": 1.6436373914062434 + }, + { + "x": 2.401162544284488, + "y": 6.5986576662058365, + "heading": 3.1494471728628666, + "angularVelocity": -0.000025027275214919653, + "velocityX": -1.1588799764559639, + "velocityY": -1.1383000442768603, + "timestamp": 1.6706142956170298 + }, + { + "x": 2.3698995501679407, + "y": 6.567949854948137, + "heading": 3.1494464977044734, + "angularVelocity": -0.000025027274749328804, + "velocityX": -1.1588799764521394, + "velocityY": -1.1383000442808404, + "timestamp": 1.6975911998278161 + }, + { + "x": 2.3386365560514903, + "y": 6.537242043690337, + "heading": 3.1494458225460926, + "angularVelocity": -0.000025027274284044715, + "velocityX": -1.1588799764485511, + "velocityY": -1.1383000442845796, + "timestamp": 1.7245681040386025 + }, + { + "x": 2.307373561935169, + "y": 6.5065342324324025, + "heading": 3.1494451473877243, + "angularVelocity": -0.00002502727381983999, + "velocityX": -1.1588799764437645, + "velocityY": -1.1383000442895392, + "timestamp": 1.7515450082493889 + }, + { + "x": 2.2761105678205396, + "y": 6.475826421172744, + "heading": 3.1494444722293684, + "angularVelocity": -0.00002502727335405626, + "velocityX": -1.158879976381056, + "velocityY": -1.1383000443534677, + "timestamp": 1.7785219124601752 + }, + { + "x": 2.244847573837531, + "y": 6.445118609779082, + "heading": 3.1494437970710263, + "angularVelocity": -0.000025027272855365495, + "velocityX": -1.1588799715020328, + "velocityY": -1.138300049320807, + "timestamp": 1.8054988166709616 + }, + { + "x": 2.2135845753549397, + "y": 6.414410802966355, + "heading": 3.149443121912666, + "angularVelocity": -0.000025027273518628322, + "velocityX": -1.1588801382959264, + "velocityY": -1.1382998795112838, + "timestamp": 1.832475720881748 + }, + { + "x": 2.182332647777799, + "y": 6.3836917291800725, + "heading": 3.14944244669543, + "angularVelocity": -0.000025029455959603075, + "velocityX": -1.1584697537175366, + "velocityY": -1.1387175320883667, + "timestamp": 1.8594526250925343 + }, + { + "x": 2.153094421241092, + "y": 6.351050761951557, + "heading": 3.149440476891046, + "angularVelocity": -0.00007301817765136917, + "velocityX": -1.0838243820804343, + "velocityY": -1.2099597112211005, + "timestamp": 1.8864295293033206 + }, + { + "x": 2.1314747036692956, + "y": 6.314801767366594, + "heading": 3.1444623769659996, + "angularVelocity": -0.18453191983123554, + "velocityX": -0.801415811201077, + "velocityY": -1.3437047595123932, + "timestamp": 1.913406433514107 + }, + { + "x": 2.1175303161219627, + "y": 6.274679449481932, + "heading": 3.139999999999996, + "angularVelocity": -0.16541471664709217, + "velocityX": -0.5169009549193061, + "velocityY": -1.487283995641412, + "timestamp": 1.9403833377248934 + }, + { + "x": 2.111393213272095, "y": 6.231289386749268, + "heading": 3.139999999999996, + "angularVelocity": 1.2396255317011e-17, + "velocityX": -0.22749470442979314, + "velocityY": -1.6084151981870152, + "timestamp": 1.9673602419356797 + }, + { + "x": 2.1135515859889713, + "y": 6.18871804241084, + "heading": 3.139999999999996, + "angularVelocity": 4.342266970142451e-18, + "velocityX": 0.08225287430098827, + "velocityY": -1.6223404824025023, + "timestamp": 1.9936009385096722 + }, + { + "x": 2.1237593069102005, + "y": 6.1473322849843255, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422681619637e-18, + "velocityX": 0.3890034280317459, + "velocityY": -1.5771592537498764, + "timestamp": 2.0198416350836648 + }, + { + "x": 2.141644452417822, + "y": 6.1086399046863455, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422691347276405e-18, + "velocityX": 0.6815804396497286, + "velocityY": -1.4745180330437861, + "timestamp": 2.0460823316576575 + }, + { + "x": 2.1665552441015765, + "y": 6.0740504370872355, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422696264537505e-18, + "velocityX": 0.9493189943912999, + "velocityY": -1.3181611814907124, + "timestamp": 2.0723230282316503 + }, + { + "x": 2.1935012145011026, + "y": 6.041021760141006, + "heading": 3.139999999999997, + "angularVelocity": 4.34227316559682e-18, + "velocityX": 1.0268770999862573, + "velocityY": -1.2586814093559167, + "timestamp": 2.098563724805643 + }, + { + "x": 2.220442534619127, + "y": 6.00798928987496, + "heading": 3.139999999999997, + "angularVelocity": 4.342268965346379e-18, + "velocityX": 1.0266998835971468, + "velocityY": -1.2588259680112164, + "timestamp": 2.1248044213796358 + }, + { + "x": 2.247383856273639, + "y": 5.974956820862076, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269673161453e-18, + "velocityX": 1.0266999421507437, + "velocityY": -1.2588259202547785, + "timestamp": 2.1510451179536285 + }, + { + "x": 2.274325177898134, + "y": 5.941924351824711, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269691700593e-18, + "velocityX": 1.0266999410068467, + "velocityY": -1.2588259211877415, + "timestamp": 2.1772858145276213 + }, + { + "x": 2.3012664995222347, + "y": 5.908891882787024, + "heading": 3.1399999999999975, + "angularVelocity": 4.3422697108078704e-18, + "velocityX": 1.0266999409917994, + "velocityY": -1.258825921200014, + "timestamp": 2.203526511101614 + }, + { + "x": 2.328207821146328, + "y": 5.87585941374933, + "heading": 3.139999999999998, + "angularVelocity": 4.3422696965810506e-18, + "velocityX": 1.0266999409915183, + "velocityY": -1.2588259212002433, + "timestamp": 2.2297672076756068 + }, + { + "x": 2.355149142770421, + "y": 5.8428269447116365, + "heading": 3.139999999999998, + "angularVelocity": 4.342269684490725e-18, + "velocityX": 1.0266999409915205, + "velocityY": -1.2588259212002417, + "timestamp": 2.2560079042495995 + }, + { + "x": 2.3820904643945138, + "y": 5.809794475673942, + "heading": 3.139999999999998, + "angularVelocity": 4.342269693460261e-18, + "velocityX": 1.026699940991488, + "velocityY": -1.258825921200268, + "timestamp": 2.2822486008235923 + }, + { + "x": 2.4090317860186072, + "y": 5.7767620066362495, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269692405428e-18, + "velocityX": 1.026699940991538, + "velocityY": -1.258825921200227, + "timestamp": 2.308489297397585 + }, + { + "x": 2.4359731076427, + "y": 5.743729537598556, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269716594298e-18, + "velocityX": 1.0266999409914905, + "velocityY": -1.258825921200266, + "timestamp": 2.3347299939715778 + }, + { + "x": 2.4629144292667973, + "y": 5.710697068560865, + "heading": 3.139999999999999, + "angularVelocity": 4.342269681682682e-18, + "velocityX": 1.026699940991683, + "velocityY": -1.258825921200109, + "timestamp": 2.3609706905455705 + }, + { + "x": 2.4898557508911936, + "y": 5.677664599523419, + "heading": 3.139999999999999, + "angularVelocity": 4.3422696993715505e-18, + "velocityX": 1.0266999410030797, + "velocityY": -1.2588259211908142, + "timestamp": 2.3872113871195633 + }, + { + "x": 2.5167970725019893, + "y": 5.644632130474532, + "heading": 3.139999999999999, + "angularVelocity": 4.342269692409483e-18, + "velocityX": 1.026699940484769, + "velocityY": -1.2588259216268345, + "timestamp": 2.413452083693556 + }, + { + "x": 2.5425090771755396, + "y": 5.613106913889164, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374488911e-18, + "velocityX": 0.9798522154718704, + "velocityY": -1.2013864226688296, + "timestamp": 2.4396927802675488 + }, + { + "x": 2.563078686478754, + "y": 5.587886733807481, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374369648e-18, + "velocityX": 0.7838819844288143, + "velocityY": -0.9611093977847911, + "timestamp": 2.4659334768415415 + }, + { + "x": 2.578505895291019, + "y": 5.568971596499493, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374398685e-18, + "velocityX": 0.5879115582455986, + "velocityY": -0.7208321339584491, + "timestamp": 2.4921741734155343 + }, + { + "x": 2.5887907019054897, + "y": 5.5563615040551815, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374398075e-18, + "velocityX": 0.39194106701667647, + "velocityY": -0.48055479048560223, + "timestamp": 2.518414869989527 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374359544e-18, + "velocityX": 0.19597054326510124, + "velocityY": -0.2402774071896897, + "timestamp": 2.5446555665635198 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, "heading": 3.14, - "angularVelocity": 1.1480104978612554e-22, - "velocityX": -0.038181042019109025, - "velocityY": -1.6943878089038094, - "timestamp": 1.9117086887466048 - }, - { - "x": 2.1139722457961088, - "y": 6.184041017313733, - "heading": 3.14, - "angularVelocity": 1.326030822415817e-23, - "velocityX": 0.09287464315336291, - "velocityY": -1.7014812376514836, - "timestamp": 1.9394776516890055 - }, - { - "x": 2.1201947185383916, - "y": 6.136700429098736, - "heading": 3.14, - "angularVelocity": 1.3260179507784046e-23, - "velocityX": 0.224080126981686, - "velocityY": -1.7048021675563763, - "timestamp": 1.9672466146314063 - }, - { - "x": 2.13006161842343, - "y": 6.089396035622716, - "heading": 3.14, - "angularVelocity": 1.326007041519161e-23, - "velocityX": 0.35532115136976367, - "velocityY": -1.7034987433322955, - "timestamp": 1.995015577573807 - }, - { - "x": 2.1435678181376194, - "y": 6.042288248235824, - "heading": 3.14, - "angularVelocity": 1.3260437690465338e-23, - "velocityX": 0.4863775338749469, - "velocityY": -1.6964186773774967, - "timestamp": 2.022784540516208 - }, - { - "x": 2.1606964661173613, - "y": 5.995581737791835, - "heading": 3.14, - "angularVelocity": 1.325999769616951e-23, - "velocityX": 0.6168270675167464, - "velocityY": -1.6819681217793025, - "timestamp": 2.050553503458609 - }, - { - "x": 2.181407907708738, - "y": 5.949543614413858, - "heading": 3.14, - "angularVelocity": 1.3260185229673763e-23, - "velocityX": 0.7458485804578618, - "velocityY": -1.6578985493073877, - "timestamp": 2.07832246640101 - }, - { - "x": 2.2056170475643233, - "y": 5.904529822011414, - "heading": 3.14, - "angularVelocity": 1.3260157963898354e-23, - "velocityX": 0.871805688451557, - "velocityY": -1.6210109284892367, - "timestamp": 2.1060914293434108 - }, - { - "x": 2.2331463169007804, - "y": 5.861019319967013, - "heading": 3.14, - "angularVelocity": 1.326020172336188e-23, - "velocityX": 0.9913682910506778, - "velocityY": -1.566875296519094, - "timestamp": 2.1338603922858117 - }, - { - "x": 2.2636325105277852, - "y": 5.819639483049468, - "heading": 3.14, - "angularVelocity": 1.32601511090322e-23, - "velocityX": 1.0978513562152263, - "velocityY": -1.4901470034504147, - "timestamp": 2.1616293552282126 - }, - { - "x": 2.2963834267173127, - "y": 5.781115195207252, - "heading": 3.14, - "angularVelocity": 1.3260107576996925e-23, - "velocityX": 1.1794072489296905, - "velocityY": -1.387314604514559, - "timestamp": 2.1893983181706136 - }, - { - "x": 2.3303148904385163, - "y": 5.746039013141612, - "heading": 3.14, - "angularVelocity": 1.3260240855176863e-23, - "velocityX": 1.221920450957617, - "velocityY": -1.2631433928013844, - "timestamp": 2.2171672811130145 - }, - { - "x": 2.3642062447518297, - "y": 5.714607213055508, - "heading": 3.14, - "angularVelocity": 1.326023555417506e-23, - "velocityX": 1.2204760539171806, - "velocityY": -1.1319039948052778, - "timestamp": 2.2449362440554155 - }, - { - "x": 2.397064742551735, - "y": 5.68667060145179, - "heading": 3.14, - "angularVelocity": 1.3260077501265654e-23, - "velocityX": 1.1832814163085987, - "velocityY": -1.0060372676381828, - "timestamp": 2.2727052069978164 - }, - { - "x": 2.4282178000518853, - "y": 5.661954954768778, - "heading": 3.14, - "angularVelocity": 1.3260249833501151e-23, - "velocityX": 1.1218660763374204, - "velocityY": -0.8900457224231765, - "timestamp": 2.3004741699402174 - }, - { - "x": 2.457231034537111, - "y": 5.640189613922927, - "heading": 3.14, - "angularVelocity": 1.3260141947431602e-23, - "velocityX": 1.0448079946451803, - "velocityY": -0.7838009972139435, - "timestamp": 2.3282431328826183 - }, - { - "x": 2.483818861573163, - "y": 5.621144668272307, - "heading": 3.14, - "angularVelocity": 1.3260165607649488e-23, - "velocityX": 0.9574656097601072, - "velocityY": -0.6858356824532662, - "timestamp": 2.3560120958250192 - }, - { - "x": 2.507786212179532, - "y": 5.604632755958765, - "heading": 3.14, - "angularVelocity": 1.3260126001429812e-23, - "velocityX": 0.8630985123961222, - "velocityY": -0.5946175356923091, - "timestamp": 2.38378105876742 - }, - { - "x": 2.5289943781295285, - "y": 5.590502031840209, - "heading": 3.14, - "angularVelocity": 1.3260018717887446e-23, - "velocityX": 0.763736333761809, - "velocityY": -0.5088675492802951, - "timestamp": 2.411550021709821 - }, - { - "x": 2.547341071809568, - "y": 5.578628548447832, - "heading": 3.14, - "angularVelocity": 1.3260377381509874e-23, - "velocityX": 0.6606906321310845, - "velocityY": -0.4275810881740915, - "timestamp": 2.439318984652222 - }, - { - "x": 2.562748457639991, - "y": 5.568909973472095, - "heading": 3.14, - "angularVelocity": 1.3260175762620676e-23, - "velocityX": 0.5548419601546171, - "velocityY": -0.3499797596293578, - "timestamp": 2.467087947594623 - }, - { - "x": 2.575155705150767, - "y": 5.561260778200535, - "heading": 3.14, - "angularVelocity": 1.3260285574981282e-23, - "velocityX": 0.4468026961075851, - "velocityY": -0.2754584421256751, - "timestamp": 2.494856910537024 - }, - { - "x": 2.5845141850851863, - "y": 5.555608625671519, - "heading": 3.14, - "angularVelocity": 1.326006595026007e-23, - "velocityX": 0.33701222310069334, - "velocityY": -0.2035420818825477, - "timestamp": 2.522625873479425 - }, - { - "x": 2.5907842648688533, - "y": 5.55189166019714, - "heading": 3.14, - "angularVelocity": 1.3260302513246253e-23, - "velocityX": 0.22579452450827125, - "velocityY": -0.1338532332694335, - "timestamp": 2.550394836421826 - }, - { - "x": 2.59393310546875, - "y": 5.550056457519531, - "heading": 3.14, - "angularVelocity": 1.3260089047889746e-23, - "velocityX": 0.11339424545409957, - "velocityY": -0.06608826845335893, - "timestamp": 2.578163799364227 - }, - { - "x": 2.59393310546875, - "y": 5.550056457519531, - "heading": 3.14, - "angularVelocity": 4.554749859713047e-24, - "velocityX": -4.596278509340535e-25, - "velocityY": -1.5195442763313795e-23, - "timestamp": 2.6059327623066277 - }, - { - "x": 2.5899783566862378, - "y": 5.5489813066194555, - "heading": 3.1367490851985087, - "angularVelocity": -0.1085265960704612, - "velocityX": -0.13202296888338985, - "velocityY": -0.03589219483504766, - "timestamp": 2.6358877679845065 - }, - { - "x": 2.5820812048170954, - "y": 5.546779098674632, - "heading": 3.130275233897137, - "angularVelocity": -0.21611918124764368, - "velocityX": -0.26363379643671836, - "velocityY": -0.07351719337009978, - "timestamp": 2.6658427736623853 - }, - { - "x": 2.5702568313825034, - "y": 5.5433888083370855, - "heading": 3.1206149262957497, - "angularVelocity": -0.32249393324338926, - "velocityX": -0.3947378131636905, - "velocityY": -0.1131794256360211, - "timestamp": 2.695797779340264 - }, - { - "x": 2.554524436678559, - "y": 5.5387376635070815, - "heading": 3.107814789143658, - "angularVelocity": -0.42731212571741595, - "velocityX": -0.5252008586852795, - "velocityY": -0.15527103817038457, - "timestamp": 2.725752785018143 - }, - { - "x": 2.5349090755324424, - "y": 5.532737501107672, - "heading": 3.0919335422287366, - "angularVelocity": -0.5301700519005096, - "velocityX": -0.6548274888361012, - "velocityY": -0.20030583415450579, - "timestamp": 2.7557077906960217 - }, - { - "x": 2.511444606178007, - "y": 5.52527960958652, - "heading": 3.0730442108839475, - "angularVelocity": -0.6305901440285391, - "velocityX": -0.7833238159511905, - "velocityY": -0.24896979160511046, - "timestamp": 2.7856627963739005 - }, - { - "x": 2.4841786133457258, - "y": 5.5162272720643095, - "heading": 3.0512365155339536, - "angularVelocity": -0.7280150631418102, - "velocityX": -0.9102316028741883, - "velocityY": -0.3021978236143635, - "timestamp": 2.8156178020517793 - }, - { - "x": 2.4531809940870857, - "y": 5.505404760057258, - "heading": 3.026619340133933, - "angularVelocity": -0.8218050653951327, - "velocityX": -1.03480598842053, - "velocityY": -0.3612922702613114, - "timestamp": 2.845572807729658 - }, - { - "x": 2.4185596883958564, - "y": 5.492580836854246, - "heading": 2.999323661656212, - "angularVelocity": -0.9112226107130541, - "velocityX": -1.1557769697502154, - "velocityY": -0.4281061850200961, - "timestamp": 2.875527813407537 - }, - { - "x": 2.380491125275952, - "y": 5.477444069032361, - "heading": 2.9695096535475827, - "angularVelocity": -0.9952930214480606, - "velocityX": -1.27085815069692, - "velocityY": -0.5053168069690293, - "timestamp": 2.9054828190854156 - }, - { - "x": 2.3392828051052046, - "y": 5.459567907623698, - "heading": 2.9373981254397274, - "angularVelocity": -1.0719920554570026, - "velocityX": -1.3756739228789079, - "velocityY": -0.5967670846367005, - "timestamp": 2.9354378247632944 - }, - { - "x": 2.2955095509891357, - "y": 5.438376261665861, - "heading": 2.9034010779477137, - "angularVelocity": -1.1349371072601777, - "velocityX": -1.4613001441823985, - "velocityY": -0.7074492385586875, - "timestamp": 2.965392830441173 - }, - { - "x": 2.2502877957375587, - "y": 5.413212847808716, - "heading": 2.8684201412995205, - "angularVelocity": -1.1677826746007212, - "velocityX": -1.5096560400578645, - "velocityY": -0.8400403634617765, - "timestamp": 2.995347836119052 - }, - { - "x": 2.205434968136323, - "y": 5.383827174903616, - "heading": 2.8338778225381454, - "angularVelocity": -1.15314011730883, - "velocityX": -1.4973399799539564, - "velocityY": -0.9809937351072376, - "timestamp": 3.0253028417969308 - }, - { - "x": 2.1627117665288242, - "y": 5.35082401983714, - "heading": 2.8010931886625263, - "angularVelocity": -1.0944626159703914, - "velocityX": -1.426245819043514, - "velocityY": -1.1017575967561364, - "timestamp": 3.0552578474748096 - }, - { - "x": 2.1231203758060415, - "y": 5.315102619215835, - "heading": 2.770927863402835, - "angularVelocity": -1.0070211831729985, - "velocityX": -1.3216953169206167, - "velocityY": -1.1925018811692434, - "timestamp": 3.0852128531526883 - }, - { - "x": 2.0871120973907478, - "y": 5.277368588566571, - "heading": 2.743899688375806, - "angularVelocity": -0.9022924354505801, - "velocityX": -1.2020788379247493, - "velocityY": -1.2596903187079023, - "timestamp": 3.115167858830567 - }, - { - "x": 2.0548974336097707, - "y": 5.238102066889186, - "heading": 2.7202319811811844, - "angularVelocity": -0.7901085864957776, - "velocityX": -1.0754350751055555, - "velocityY": -1.310850082942305, - "timestamp": 3.145122864508446 - }, - { - "x": 2.026587153392112, - "y": 5.197634903381892, - "heading": 2.6999923980627787, - "angularVelocity": -0.6756661419481035, - "velocityX": -0.9450934685873016, - "velocityY": -1.350931591950202, - "timestamp": 3.1750778701863247 - }, - { - "x": 2.002245678733676, - "y": 5.1562068665429, - "heading": 2.6831832138201195, - "angularVelocity": -0.5611477568529534, - "velocityX": -0.812601236674494, - "velocityY": -1.3830088127670148, - "timestamp": 3.2050328758642035 - }, - { - "x": 1.9819131544340292, - "y": 5.113998389763002, - "heading": 2.66978325115483, - "angularVelocity": -0.44733634202529504, - "velocityX": -0.6787688347748394, - "velocityY": -1.4090625531434064, - "timestamp": 3.2349878815420823 - }, - { - "x": 1.9656157831702816, - "y": 5.071149791653941, - "heading": 2.659765787935313, - "angularVelocity": -0.33441700286222414, - "velocityX": -0.5440616983685925, - "velocityY": -1.4304319808793762, - "timestamp": 3.264942887219961 - }, - { - "x": 1.9533711923999129, - "y": 5.027773084636261, - "heading": 2.6531059130951715, - "angularVelocity": -0.22232927984587103, - "velocityX": -0.4087660974610013, - "velocityY": -1.4480620529380386, - "timestamp": 3.29489789289784 - }, - { - "x": 1.9451914498185667, - "y": 4.983959581247479, + "angularVelocity": 3.0510058962998796e-17, + "velocityX": 1.3525026943280928e-16, + "velocityY": 8.359694800915289e-17, + "timestamp": 2.5708962631375125 + }, + { + "x": 2.5906368423225947, + "y": 5.5485199070890205, + "heading": 3.101591851410996, + "angularVelocity": -1.2805791583146104, + "velocityX": -0.10990183178303778, + "velocityY": -0.05123065102809662, + "timestamp": 2.60088905845952 + }, + { + "x": 2.584621501308477, + "y": 5.544975862182894, + "heading": 3.0243371709692783, + "angularVelocity": -2.5757746022741834, + "velocityX": -0.20055953270102006, + "velocityY": -0.11816320779855842, + "timestamp": 2.6308818537815273 + }, + { + "x": 2.5759613994637824, + "y": 5.536844199254011, + "heading": 2.9135350884263085, + "angularVelocity": -3.6942899570776517, + "velocityX": -0.28873940397378167, + "velocityY": -0.2711205421688905, + "timestamp": 2.6608746491035347 + }, + { + "x": 2.56275074361348, + "y": 5.521892690270415, + "heading": 2.8248688434153846, + "angularVelocity": -2.956251461684603, + "velocityX": -0.44046097433337356, + "velocityY": -0.49850335131840534, + "timestamp": 2.690867444425542 + }, + { + "x": 2.543962903792877, + "y": 5.501410245111836, + "heading": 2.7589030524712514, + "angularVelocity": -2.199387894182165, + "velocityX": -0.6264117638620101, + "velocityY": -0.6829121773583486, + "timestamp": 2.7208602397475494 + }, + { + "x": 2.518778715441927, + "y": 5.477002144640871, + "heading": 2.7154691851313926, + "angularVelocity": -1.448143358205586, + "velocityX": -0.839674597873837, + "velocityY": -0.8137987876364632, + "timestamp": 2.750853035069557 + }, + { + "x": 2.486733800013198, + "y": 5.449761748396881, + "heading": 2.6943182531197865, + "angularVelocity": -0.705200425130341, + "velocityX": -1.0684204351295292, + "velocityY": -0.9082313252703665, + "timestamp": 2.780845830391564 + }, + { + "x": 2.4478196867061426, + "y": 5.4204460978412365, + "heading": 2.694317441639593, + "angularVelocity": -0.000027055837428819432, + "velocityX": -1.2974487002285997, + "velocityY": -0.9774230858209427, + "timestamp": 2.8108386257135716 + }, + { + "x": 2.408901839144878, + "y": 5.391135398464328, + "heading": 2.6943166423252785, + "angularVelocity": -0.000026650210696695328, + "velocityX": -1.2975732052797133, + "velocityY": -0.9772580068715555, + "timestamp": 2.840831421035579 + }, + { + "x": 2.3699840051756724, + "y": 5.361824681040097, + "heading": 2.6943158430110494, + "angularVelocity": -0.00002665020784043696, + "velocityX": -1.2975727521022344, + "velocityY": -0.9772586085935405, + "timestamp": 2.8708242163575863 + }, + { + "x": 2.3310661711945366, + "y": 5.332513963631788, + "heading": 2.6943150436964616, + "angularVelocity": -0.000026650219800690006, + "velocityX": -1.297572752499997, + "velocityY": -0.9772586080626477, + "timestamp": 2.9008170116795937 + }, + { + "x": 2.292148337213513, + "y": 5.303203246223414, + "heading": 2.694314244381513, + "angularVelocity": -0.000026650231834296552, + "velocityX": -1.2975727524962546, + "velocityY": -0.9772586080648237, + "timestamp": 2.930809807001601 + }, + { + "x": 2.25323050323266, + "y": 5.273892528814898, + "heading": 2.6943134450662036, + "angularVelocity": -0.00002665024386857193, + "velocityX": -1.297572752490578, + "velocityY": -0.9772586080695672, + "timestamp": 2.9608026023236085 + }, + { + "x": 2.214312669251978, + "y": 5.2445818114062375, + "heading": 2.694312645750533, + "angularVelocity": -0.00002665025590264624, + "velocityX": -1.2975727524848706, + "velocityY": -0.9772586080743522, + "timestamp": 2.990795397645616 + }, + { + "x": 2.1753948352714723, + "y": 5.215271093997427, + "heading": 2.694311846434502, + "angularVelocity": -0.000026650267936849635, + "velocityX": -1.2975727524790048, + "velocityY": -0.9772586080793467, + "timestamp": 3.0207881929676232 + }, + { + "x": 2.136477001291495, + "y": 5.185960376588, + "heading": 2.6943110471181098, + "angularVelocity": -0.000026650279973527825, + "velocityX": -1.2975727524613627, + "velocityY": -0.9772586080999772, + "timestamp": 3.0507809882896306 + }, + { + "x": 2.097559167357152, + "y": 5.1566496591180675, + "heading": 2.694310247801348, + "angularVelocity": -0.000026650292283470146, + "velocityX": -1.2975727509398636, + "velocityY": -0.9772586101172457, + "timestamp": 3.080773783611638 + }, + { + "x": 2.058641302557044, + "y": 5.127338982627939, + "heading": 2.694309448489961, + "angularVelocity": -0.000026650113093683626, + "velocityX": -1.2975737800459084, + "velocityY": -0.9772572437956306, + "timestamp": 3.1107665789336454 + }, + { + "x": 2.0197569370912083, + "y": 5.0979839038638834, + "heading": 2.6943086027536207, + "angularVelocity": -0.000028197983267341566, + "velocityX": -1.2964568673313885, + "velocityY": -0.9787376751348899, + "timestamp": 3.1407593742556528 + }, + { + "x": 1.9877778714785685, + "y": 5.067888184962114, + "heading": 2.678621118987198, + "angularVelocity": -0.5230417371285907, + "velocityX": -1.0662249139900737, + "velocityY": -1.0034316101155079, + "timestamp": 3.17075216957766 + }, + { + "x": 1.9640654183560038, + "y": 5.031205594348394, + "heading": 2.6596531558314034, + "angularVelocity": -0.6324173173088112, + "velocityX": -0.7906049725553251, + "velocityY": -1.223046742388833, + "timestamp": 3.2007449648996675 + }, + { + "x": 1.9481708008996033, + "y": 4.987987936601904, "heading": 2.649783348569472, - "angularVelocity": -0.11091850762536054, - "velocityX": -0.27306763581709725, - "velocityY": -1.4626438018383596, - "timestamp": 3.3248528985757186 + "angularVelocity": -0.32907260414160233, + "velocityX": -0.5299478519964009, + "velocityY": -1.4409346405525878, + "timestamp": 3.230737760221675 }, { - "x": 1.941084861755371, + "x": 1.9410848617553718, "y": 4.939785003662109, "heading": 2.649783348569472, - "angularVelocity": 6.132292113202646e-25, - "velocityX": -0.13709188064779135, - "velocityY": -1.4746976869375825, - "timestamp": 3.3548079042535974 - }, - { - "x": 1.9419022015392158, - "y": 4.889062683793002, - "heading": 2.649783348569472, - "angularVelocity": 4.614715107153485e-26, - "velocityX": 0.023936675164732843, - "velocityY": -1.4854577230996109, - "timestamp": 3.3889538237774923 - }, - { - "x": 1.948223657679148, - "y": 4.838070468392074, - "heading": 2.649783348569472, - "angularVelocity": 4.653945144562686e-26, - "velocityX": 0.18513064600613907, - "velocityY": -1.4933619042018718, - "timestamp": 3.423099743301387 + "angularVelocity": 6.681257540282107e-18, + "velocityX": -0.23625470944103766, + "velocityY": -1.6071503980304298, + "timestamp": 3.2607305555436823 }, { - "x": 1.9600538072506943, - "y": 4.786928471515363, + "x": 1.9456173734407256, + "y": 4.888006870907468, "heading": 2.649783348569472, - "angularVelocity": 4.629261284078221e-26, - "velocityX": 0.3464586614300294, - "velocityY": -1.4977484159101515, - "timestamp": 3.457245662825282 + "angularVelocity": -1.484655388526889e-19, + "velocityX": 0.14165581617647935, + "velocityY": -1.6182360167593688, + "timestamp": 3.292727206352592 }, { - "x": 1.977394685635232, - "y": 4.735788039854744, + "x": 1.9619962278910381, + "y": 4.838678856610239, "heading": 2.649783348569472, - "angularVelocity": 4.640038416252255e-26, - "velocityX": 0.5078462851879747, - "velocityY": -1.4977025768725385, - "timestamp": 3.491391582349177 + "angularVelocity": -1.4846556369937314e-19, + "velocityX": 0.5118927774045035, + "velocityY": -1.5416618005383362, + "timestamp": 3.324723857161502 }, { - "x": 2.0002427480647933, - "y": 4.684845229382865, + "x": 1.9893341653194017, + "y": 4.794473003544163, "heading": 2.649783348569472, - "angularVelocity": 4.6490332666751425e-26, - "velocityX": 0.6691300966012094, - "velocityY": -1.4919150276866968, - "timestamp": 3.525537501873072 + "angularVelocity": -1.4846558519027202e-19, + "velocityX": 0.8543999680324345, + "velocityY": -1.381577507287736, + "timestamp": 3.3567205079704117 }, { - "x": 2.0285822715254445, - "y": 4.634362832128866, + "x": 2.026135553103086, + "y": 4.757769076744815, "heading": 2.649783348569472, - "angularVelocity": 4.622795940866032e-26, - "velocityX": 0.8299534426308102, - "velocityY": -1.478431331119124, - "timestamp": 3.559683421396967 + "angularVelocity": -1.4846559448429508e-19, + "velocityX": 1.1501637469359431, + "velocityY": -1.1471177723711978, + "timestamp": 3.3887171587793214 }, { - "x": 2.0623699922411927, - "y": 4.5847082412928, + "x": 2.0637808893572873, + "y": 4.721931260837698, "heading": 2.649783348569472, - "angularVelocity": 4.634016030607817e-26, - "velocityX": 0.9895097624213414, - "velocityY": -1.454188129311252, - "timestamp": 3.5938293409208617 + "angularVelocity": -1.4846636373679086e-19, + "velocityX": 1.1765398972283652, + "velocityY": -1.120048973905845, + "timestamp": 3.4207138095882312 }, { - "x": 2.101495965027088, - "y": 4.536421557771227, + "x": 2.1014301321789435, + "y": 4.6860975489840255, "heading": 2.649783348569472, - "angularVelocity": 4.645177261974479e-26, - "velocityX": 1.1458462191511727, - "velocityY": -1.4141274915083855, - "timestamp": 3.6279752604447566 + "angularVelocity": -1.484648310808098e-19, + "velocityX": 1.1766619902345172, + "velocityY": -1.1199207088167735, + "timestamp": 3.452710460397141 }, { - "x": 2.145673252803998, - "y": 4.4903376692314145, + "x": 2.139079370280558, + "y": 4.650263832171171, "heading": 2.649783348569472, - "angularVelocity": 4.6233288536561975e-26, - "velocityX": 1.2937794147261192, - "velocityY": -1.3496162698902718, - "timestamp": 3.6621211799686515 + "angularVelocity": -1.48465589810743e-19, + "velocityX": 1.1766618427177973, + "velocityY": -1.1199208638074638, + "timestamp": 3.484707111206051 }, { - "x": 2.1941116699379357, - "y": 4.447748182132556, + "x": 2.17672860838618, + "y": 4.614430115362526, "heading": 2.649783348569472, - "angularVelocity": 4.6479494614855375e-26, - "velocityX": 1.4185711736373225, - "velocityY": -1.2472789631292271, - "timestamp": 3.6962670994925464 + "angularVelocity": -1.4846559463647802e-19, + "velocityX": 1.1766618428430395, + "velocityY": -1.1199208636758753, + "timestamp": 3.5167037620149606 }, { - "x": 2.2448201323304646, - "y": 4.410180142680831, + "x": 2.2143778464918284, + "y": 4.57859639855391, "heading": 2.649783348569472, - "angularVelocity": 4.6329904581670297e-26, - "velocityX": 1.485051891985029, - "velocityY": -1.1002204648621148, - "timestamp": 3.7304130190164413 + "angularVelocity": -1.4846559409367017e-19, + "velocityX": 1.176661842843871, + "velocityY": -1.1199208636750018, + "timestamp": 3.5487004128238704 }, { - "x": 2.294852293597042, - "y": 4.3780811675391975, + "x": 2.2520270845974713, + "y": 4.5427626817452875, "heading": 2.649783348569472, - "angularVelocity": 4.6446166642422895e-26, - "velocityX": 1.4652456856979683, - "velocityY": -0.9400530309096446, - "timestamp": 3.764558938540336 + "angularVelocity": -1.4846559515966518e-19, + "velocityX": 1.1766618428436975, + "velocityY": -1.1199208636751843, + "timestamp": 3.58069706363278 }, { - "x": 2.342149827765447, - "y": 4.350766530180341, + "x": 2.289676322703114, + "y": 4.506928964936664, "heading": 2.649783348569472, - "angularVelocity": 4.633506801188826e-26, - "velocityX": 1.3851591882100787, - "velocityY": -0.7999385501902068, - "timestamp": 3.798704858064231 + "angularVelocity": -1.4846559610313848e-19, + "velocityX": 1.1766618428436995, + "velocityY": -1.1199208636751825, + "timestamp": 3.61269371444169 }, { - "x": 2.3856944951859207, - "y": 4.3274872473595005, + "x": 2.3273255608087573, + "y": 4.471095248128042, "heading": 2.649783348569472, - "angularVelocity": 4.6381778689211406e-26, - "velocityX": 1.2752524467821442, - "velocityY": -0.6817588498253565, - "timestamp": 3.832850777588126 + "angularVelocity": -1.484655947429445e-19, + "velocityX": 1.1766618428436957, + "velocityY": -1.1199208636751863, + "timestamp": 3.6446903652505998 }, { - "x": 2.4249725857215654, - "y": 4.307695719032298, + "x": 2.3649747989143997, + "y": 4.435261531319418, "heading": 2.649783348569472, - "angularVelocity": 4.6432377599415885e-26, - "velocityX": 1.150301151156826, - "velocityY": -0.5796162060697082, - "timestamp": 3.866996697112021 + "angularVelocity": -1.4846559574023035e-19, + "velocityX": 1.1766618428436864, + "velocityY": -1.1199208636751963, + "timestamp": 3.6766870160595095 }, { - "x": 2.4596964858202144, - "y": 4.291007007021927, + "x": 2.4026240370200447, + "y": 4.399427814510799, "heading": 2.649783348569472, - "angularVelocity": 4.6331179794053255e-26, - "velocityX": 1.0169267831358226, - "velocityY": -0.4887468910800007, - "timestamp": 3.9011426166359158 + "angularVelocity": -1.484655924580371e-19, + "velocityX": 1.1766618428437539, + "velocityY": -1.1199208636751252, + "timestamp": 3.7086836668684193 }, { - "x": 2.4896890256889828, - "y": 4.277143601542489, + "x": 2.4402732751256884, + "y": 4.363594097702177, "heading": 2.649783348569472, - "angularVelocity": 4.6467164999430504e-26, - "velocityX": 0.8783638070657234, - "velocityY": -0.4060047488173824, - "timestamp": 3.9352885361598107 + "angularVelocity": -1.4846559654067578e-19, + "velocityX": 1.1766618428437225, + "velocityY": -1.1199208636751585, + "timestamp": 3.740680317677329 }, { - "x": 2.514832571254234, - "y": 4.265898475268687, + "x": 2.4779225132411047, + "y": 4.327760380902407, "heading": 2.649783348569472, - "angularVelocity": 4.632856845272505e-26, - "velocityX": 0.7363557905551701, - "velocityY": -0.3293256245723021, - "timestamp": 3.9694344556837056 + "angularVelocity": -1.4846559492892923e-19, + "velocityX": 1.1766618431491565, + "velocityY": -1.1199208633985085, + "timestamp": 3.772676968486239 }, { - "x": 2.5350443768209585, - "y": 4.257112226026017, + "x": 2.5129728702731393, + "y": 4.294400221963306, "heading": 2.649783348569472, - "angularVelocity": 4.629729705599566e-26, - "velocityX": 0.5919244773180152, - "velocityY": -0.2573147645510306, - "timestamp": 4.0035803752076005 + "angularVelocity": -1.484655203554499e-19, + "velocityX": 1.0954383082580312, + "velocityY": -1.0426140891540008, + "timestamp": 3.8046736192951487 }, { - "x": 2.550263563230007, - "y": 4.2506587632470065, + "x": 2.53926064365096, + "y": 4.2693800974206075, "heading": 2.649783348569472, - "angularVelocity": 4.62810585566047e-26, - "velocityX": 0.445710252388941, - "velocityY": -0.18899660249284694, - "timestamp": 4.037726294731495 + "angularVelocity": -1.484655203554444e-19, + "velocityX": 0.8215789063304317, + "velocityY": -0.7819607337071065, + "timestamp": 3.8366702701040585 }, { - "x": 2.5604437164785643, - "y": 4.246436059535304, + "x": 2.5567858275270727, + "y": 4.252700012844841, "heading": 2.649783348569472, - "angularVelocity": 4.6519462435401186e-26, - "velocityX": 0.29813674343821717, - "velocityY": -0.12366642253542871, - "timestamp": 4.07187221425539 + "angularVelocity": -1.4846552035544855e-19, + "velocityX": 0.5477193216495216, + "velocityY": -0.5213072041629537, + "timestamp": 3.8686669209129683 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 4.6699948971014915e-26, - "velocityX": 0.14949673474910496, - "velocityY": -0.06080051354533503, - "timestamp": 4.106018133779285 + "angularVelocity": -1.4846552035527083e-19, + "velocityX": 0.2738596760533411, + "velocityY": -0.260653616588761, + "timestamp": 3.900663571721878 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 1.535197914331468e-26, - "velocityX": 1.1963425727073795e-24, - "velocityY": -7.168329076335369e-24, - "timestamp": 4.14016405330318 + "angularVelocity": -4.948850678520552e-20, + "velocityX": 6.0427487543509175e-18, + "velocityY": -9.335003878092332e-18, + "timestamp": 3.932660222530788 } ], "constraints": [ @@ -1197,7 +1125,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": false, - "controlIntervalCount": 25 + "controlIntervalCount": 24 }, { "x": 2.6290202140808105, @@ -1206,7 +1134,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 + "controlIntervalCount": 36 }, { "x": 7.999455451965332, @@ -1215,7 +1143,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 32 + "controlIntervalCount": 47 }, { "x": 2.4533867835998535, @@ -1229,976 +1157,1219 @@ ], "trajectory": [ { - "x": 0.3137660622596754, + "x": 0.3137660622596741, "y": 6.865104675292969, - "heading": 3.135568639860517, - "angularVelocity": -8.5693301650697e-28, - "velocityX": -3.4362695197914885e-17, - "velocityY": -4.743570524232907e-17, + "heading": 3.135568639860519, + "angularVelocity": 1.3323616061473399e-26, + "velocityX": -9.030177506128314e-25, + "velocityY": 7.578270673418084e-24, "timestamp": 0 }, { - "x": 0.3176438425782737, - "y": 6.865425616765028, - "heading": 3.1355686398605154, - "angularVelocity": -5.1868461530169816e-17, - "velocityX": 0.13515003367410133, - "velocityY": 0.011185587421808195, - "timestamp": 0.028692411042588198 - }, - { - "x": 0.3253999903453479, - "y": 6.866060324505518, - "heading": 3.1355686398605136, - "angularVelocity": -5.186846153227154e-17, - "velocityX": 0.2703205302461453, - "velocityY": 0.022121101623313478, - "timestamp": 0.057384822085176396 - }, - { - "x": 0.3370351106624937, - "y": 6.867001226604551, - "heading": 3.135568639860512, - "angularVelocity": -5.186846153153239e-17, - "velocityX": 0.40551211607398097, - "velocityY": 0.032792716430754286, - "timestamp": 0.0860772331277646 - }, - { - "x": 0.352549826865152, - "y": 6.86824032070545, - "heading": 3.13556863986051, - "angularVelocity": -5.186846153220684e-17, - "velocityX": 0.5407254266512643, - "velocityY": 0.04318542973123231, - "timestamp": 0.11476964417035279 - }, - { - "x": 0.3719447806630506, - "y": 6.869769136338039, - "heading": 3.1355686398605083, - "angularVelocity": -5.1868461532349994e-17, - "velocityX": 0.6759611023663795, - "velocityY": 0.053282926635904025, - "timestamp": 0.14346205521294098 - }, - { - "x": 0.3952206321171861, - "y": 6.871578692739439, - "heading": 3.135568639860506, - "angularVelocity": -5.1868461531715824e-17, - "velocityX": 0.8112197828054859, - "velocityY": 0.06306742220841621, - "timestamp": 0.17215446625552916 - }, - { - "x": 0.42237805939934603, - "y": 6.8736594514681, - "heading": 3.1355686398605043, - "angularVelocity": -5.1868461532243226e-17, - "velocityX": 0.9465020991734979, - "velocityY": 0.07251947999676765, - "timestamp": 0.20084687729811734 - }, - { - "x": 0.45341775826418784, - "y": 6.876001262985059, - "heading": 3.1355686398605025, - "angularVelocity": -5.1868461531961763e-17, - "velocityX": 1.081808664276005, - "velocityY": 0.08161780177636822, - "timestamp": 0.22953928834070553 - }, - { - "x": 0.4883404411428946, - "y": 6.878593306214626, - "heading": 3.1355686398605007, - "angularVelocity": -5.186846153181617e-17, - "velocityX": 1.2171400593303083, - "velocityY": 0.09033898286621334, - "timestamp": 0.2582316993832937 - }, - { - "x": 0.5271468357396778, - "y": 6.881424019896891, - "heading": 3.135568639860499, - "angularVelocity": -5.186846153207198e-17, - "velocityX": 1.3524968166384297, - "velocityY": 0.09865722605409555, - "timestamp": 0.2869241104258819 - }, - { - "x": 0.5698376829754908, - "y": 6.884481024296066, - "heading": 3.135568639860497, - "angularVelocity": -5.18684615321628e-17, - "velocityX": 1.4878793968358233, - "velocityY": 0.10654400547371551, - "timestamp": 0.3156165214684701 - }, - { - "x": 0.6164137340738928, - "y": 6.887751031517755, - "heading": 3.1355686398604954, - "angularVelocity": -5.186846153204876e-17, - "velocityX": 1.6232881589932495, - "velocityY": 0.11396766959864212, - "timestamp": 0.34430893251105826 - }, - { - "x": 0.666875746517318, - "y": 6.891219742296381, - "heading": 3.1355686398604936, - "angularVelocity": -5.186846153160267e-17, - "velocityX": 1.758723321247663, - "velocityY": 0.12089296969422078, - "timestamp": 0.37300134355364645 - }, - { - "x": 0.7212244785112947, - "y": 6.894871726616115, - "heading": 3.135568639860492, - "angularVelocity": -5.186846153215831e-17, - "velocityX": 1.894184908800638, - "velocityY": 0.1272804963763128, - "timestamp": 0.40169375459623463 - }, - { - "x": 0.7794606814696723, - "y": 6.898690284891088, - "heading": 3.13556863986049, - "angularVelocity": -5.186846153213201e-17, - "velocityX": 2.02967268494572, - "velocityY": 0.1330860020548787, - "timestamp": 0.4303861656388228 - }, - { - "x": 0.8415850898614354, - "y": 6.9026572856068995, - "heading": 3.1355686398604883, - "angularVelocity": -5.186846153171342e-17, - "velocityX": 2.165186059113343, - "velocityY": 0.13825958055332968, - "timestamp": 0.459078576681411 - }, - { - "x": 0.9075984075182041, - "y": 6.906752974251311, - "heading": 3.1355686398604865, - "angularVelocity": -5.1868461532457525e-17, - "velocityX": 2.3007239635171715, - "velocityY": 0.14274466646709222, - "timestamp": 0.4877709877239992 - }, - { - "x": 0.9775012891595924, - "y": 6.910955746947304, - "heading": 3.1355686398604847, - "angularVelocity": -5.186846153166985e-17, - "velocityX": 2.43628468648487, - "velocityY": 0.14647680495568935, - "timestamp": 0.5164633987665874 - }, - { - "x": 1.051294315403438, - "y": 6.915241880318482, - "heading": 3.1355686398604825, - "angularVelocity": -5.1868461532007456e-17, - "velocityX": 2.571865645390099, - "velocityY": 0.1493821263335398, - "timestamp": 0.5451558098091757 - }, - { - "x": 1.1289779588157673, - "y": 6.919585206580955, - "heading": 3.1355686398604807, - "angularVelocity": -5.186846153206435e-17, - "velocityX": 2.7074630743656276, - "velocityY": 0.15137543708073178, - "timestamp": 0.5738482208517639 - }, - { - "x": 1.210552537505391, - "y": 6.923956719399065, - "heading": 3.135568639860479, - "angularVelocity": -5.1868461531968826e-17, - "velocityX": 2.843071590203452, - "velocityY": 0.1523578067950584, - "timestamp": 0.6025406318943521 - }, - { - "x": 1.29601815119489, - "y": 6.9283240912670765, - "heading": 3.135568639860477, - "angularVelocity": -5.186846153216223e-17, - "velocityX": 2.978683581614699, - "velocityY": 0.15221348465724693, - "timestamp": 0.6312330429369404 - }, - { - "x": 1.3853745923008893, - "y": 6.932651076487761, - "heading": 3.1355686398604754, - "angularVelocity": -5.1868461531841376e-17, - "velocityX": 3.1142883382426785, - "velocityY": 0.15080591220658612, - "timestamp": 0.6599254539795286 - }, - { - "x": 1.478621220826743, - "y": 6.936896764298285, - "heading": 3.1355686398604736, - "angularVelocity": -5.186846153215644e-17, - "velocityX": 3.2498707894378844, - "velocityY": 0.1479725006107481, - "timestamp": 0.6886178650221169 - }, - { - "x": 1.5757567859462194, - "y": 6.941014632922591, - "heading": 3.135568639860472, - "angularVelocity": -5.186846153173054e-17, - "velocityX": 3.3854096463101833, - "velocityY": 0.14351769247253987, - "timestamp": 0.7173102760647051 - }, - { - "x": 1.676779167520274, - "y": 6.94495133505747, - "heading": 3.13556863986047, - "angularVelocity": -5.186846153388186e-17, - "velocityX": 3.52087461120313, - "velocityY": 0.13720360164351214, - "timestamp": 0.7460026871072933 - }, - { - "x": 1.7816849946975712, - "y": 6.948645114898683, - "heading": 3.1355686398604683, - "angularVelocity": -1.6596882408000968e-16, - "velocityX": 3.6562220937649346, - "velocityY": 0.12873717150261788, - "timestamp": 0.7746950981498816 - }, - { - "x": 1.8121461784699802, - "y": 6.949692765946351, - "heading": 3.1362037838100756, - "angularVelocity": 0.07689012502825784, - "velocityX": 3.687611651202223, - "velocityY": 0.12682797354934053, - "timestamp": 0.7829555080448543 - }, - { - "x": 1.8428693964358074, - "y": 6.950724930078729, - "heading": 3.1374589452138397, - "angularVelocity": 0.15194904607940982, - "velocityX": 3.7193333450103565, - "velocityY": 0.12495313737441759, - "timestamp": 0.7912159179398269 - }, - { - "x": 1.8738574968140793, - "y": 6.95174197296817, - "heading": 3.139318096432083, - "angularVelocity": 0.22506767120320992, - "velocityX": 3.751399842413374, - "velocityY": 0.1231225692636009, - "timestamp": 0.7994763278347996 - }, - { - "x": 1.9051134303883683, - "y": 6.952744343319344, - "heading": 3.1417642609142864, - "angularVelocity": 0.2961311258529596, - "velocityX": 3.783823559810237, - "velocityY": 0.12134632105632295, - "timestamp": 0.8077367377297723 - }, - { - "x": 1.9366402470506743, - "y": 6.953732574884012, - "heading": 3.1447794649491048, - "angularVelocity": 0.36501869436959006, - "velocityX": 3.8166164952046153, - "velocityY": 0.11963468849991928, - "timestamp": 0.815997147624745 - }, - { - "x": 1.96844109067084, - "y": 6.9547072895390265, - "heading": 3.148344689757031, - "angularVelocity": 0.43160386146187707, - "velocityX": 3.849790025494764, - "velocityY": 0.117998339961998, - "timestamp": 0.8242575575197176 - }, - { - "x": 2.0005191919654135, - "y": 6.955669201722795, - "heading": 3.152439824908413, - "angularVelocity": 0.495754472653526, - "velocityX": 3.883354664287875, - "velocityY": 0.11644848088619933, - "timestamp": 0.8325179674146903 - }, - { - "x": 2.032877859008433, - "y": 6.9566191245667985, - "heading": 3.1570436241813726, - "angularVelocity": 0.5573330296555058, - "velocityX": 3.91731977643278, - "velocityY": 0.11499705899340439, - "timestamp": 0.840778377309663 - }, - { - "x": 2.065520465002766, - "y": 6.957557978090485, - "heading": 3.1621336650538163, - "angularVelocity": 0.6161971303075195, - "velocityX": 3.951693246384564, - "velocityY": 0.11365701407324179, - "timestamp": 0.8490387872046357 - }, - { - "x": 2.0984504329177724, - "y": 6.958486799841183, - "heading": 3.1676863130152344, - "angularVelocity": 0.6722000520579139, - "velocityX": 3.9864810988430395, - "velocityY": 0.11244257397721802, - "timestamp": 0.8572991970996083 - }, - { - "x": 2.1316712166001497, - "y": 6.959406758344203, - "heading": 3.1736766917418935, - "angularVelocity": 0.7251914617828771, - "velocityX": 4.021687071799445, - "velocityY": 0.1113695948160438, - "timestamp": 0.865559606994581 - }, - { - "x": 2.165186277981829, - "y": 6.960319169668765, - "heading": 3.1800786598480077, - "angularVelocity": 0.7750182118701426, - "velocityX": 4.057312144046869, - "velocityY": 0.11045593816320934, - "timestamp": 0.8738200168895537 - }, - { - "x": 2.1989990600414004, - "y": 6.961225517299853, - "heading": 3.1868647943382302, - "angularVelocity": 0.8215251514763765, - "velocityX": 4.093354021105941, - "velocityY": 0.10972187126423899, - "timestamp": 0.8820804267845264 - }, - { - "x": 2.2331129552203137, - "y": 6.962127475323903, - "heading": 3.19400637995892, - "angularVelocity": 0.8645558406293681, - "velocityX": 4.1298065849823535, - "velocityY": 0.10919046821108658, - "timestamp": 0.890340836679499 - }, - { - "x": 2.2675312690432237, - "y": 6.963026934685406, - "heading": 3.201473402262516, - "angularVelocity": 0.9039529997346885, - "velocityX": 4.1666593135838506, - "velocityY": 0.10888798170273117, - "timestamp": 0.8986012465744717 - }, - { - "x": 2.302257178726397, - "y": 6.963926031963223, - "heading": 3.2092345401896574, - "angularVelocity": 0.9395584511933502, - "velocityX": 4.203896673978105, - "velocityY": 0.10884414808067534, - "timestamp": 0.9068616564694444 - }, - { - "x": 2.3372936865473584, - "y": 6.964827179782931, - "heading": 3.2172571510612844, - "angularVelocity": 0.9712122005605541, - "velocityX": 4.241497488191654, - "velocityY": 0.10909238538572823, - "timestamp": 0.9151220663644171 - }, - { - "x": 2.372643567633051, - "y": 6.965733097685614, - "heading": 3.225507236598066, - "angularVelocity": 0.9987501397242283, - "velocityX": 4.279434257518503, - "velocityY": 0.10966984861524744, - "timestamp": 0.9233824762593897 - }, - { - "x": 2.4083093114869696, - "y": 6.9666468421059164, - "heading": 3.233949372180996, - "angularVelocity": 1.0219995969049733, - "velocityX": 4.31767240456481, - "velocityY": 0.11061732189065114, - "timestamp": 0.9316428861543624 - }, - { - "x": 2.44429305579275, - "y": 6.967571834188082, - "heading": 3.2425465717794997, - "angularVelocity": 1.0407715486052915, - "velocityX": 4.356169338240552, - "velocityY": 0.11197895672538041, - "timestamp": 0.9399032960493351 - }, - { - "x": 2.480596509367599, - "y": 6.968511884597095, - "heading": 3.251260045781759, - "angularVelocity": 1.054847654420144, - "velocityX": 4.394873140247142, - "velocityY": 0.11380190825438415, - "timestamp": 0.9481637059443078 - }, - { - "x": 2.517220857768171, - "y": 6.969471215284249, - "heading": 3.260048785175124, - "angularVelocity": 1.0639592350877263, - "velocityX": 4.433720464992984, - "velocityY": 0.11613596653794503, - "timestamp": 0.9564241158392804 - }, - { - "x": 2.55416663843719, - "y": 6.970454479070239, - "heading": 3.2688688682505744, - "angularVelocity": 1.0677536814268536, - "velocityX": 4.4726328522150816, - "velocityY": 0.11903329235341958, - "timestamp": 0.9646845257342531 - }, - { - "x": 2.5914335596417093, - "y": 6.971466777824258, - "heading": 3.2776723283732965, - "angularVelocity": 1.065741317277861, - "velocityX": 4.511509922431134, - "velocityY": 0.12254824722845813, - "timestamp": 0.9729449356292258 - }, - { - "x": 2.6290202140808088, - "y": 6.9725136756896955, - "heading": 3.286405336491996, - "angularVelocity": 1.0572124422077387, - "velocityX": 4.550216625687468, - "velocityY": 0.12673679378462502, - "timestamp": 0.9812053455241985 - }, - { - "x": 2.9717276780335036, - "y": 6.984363087388876, - "heading": 3.35108767309623, - "angularVelocity": 0.9203746590757935, - "velocityX": 4.876435853393211, - "velocityY": 0.16860705449782717, - "timestamp": 1.0514836131475132 - }, - { - "x": 3.3366815448761877, - "y": 6.999743371642888, - "heading": 3.3973428408952557, - "angularVelocity": 0.6581717131524953, - "velocityX": 5.192983253355129, - "velocityY": 0.21884836912099448, - "timestamp": 1.1217618807708278 - }, - { - "x": 3.71402485435163, - "y": 7.027526643127482, - "heading": 3.3973428368677423, - "angularVelocity": -5.730809398962461e-8, - "velocityX": 5.369274488921224, - "velocityY": 0.3953323327989187, - "timestamp": 1.1920401483941425 - }, - { - "x": 4.090360227255461, - "y": 7.066661937541599, - "heading": 3.3973428243317834, - "angularVelocity": -1.783760359331945e-7, - "velocityX": 5.354932408421835, - "velocityY": 0.5568619679682045, - "timestamp": 1.2623184160174572 - }, - { - "x": 4.466694898854889, - "y": 7.105803975313799, - "heading": 3.3973428117958613, - "angularVelocity": -1.7837551225317533e-7, - "velocityX": 5.354922429456401, - "velocityY": 0.5569579202208789, - "timestamp": 1.3325966836407719 - }, - { - "x": 4.8430291421579215, - "y": 7.144950130722809, - "heading": 3.397342799228156, - "angularVelocity": -1.7882775854425254e-7, - "velocityX": 5.354916335162799, - "velocityY": 0.5570165106920909, - "timestamp": 1.4028749512640866 - }, - { - "x": 5.214361577079249, - "y": 7.183676617694927, - "heading": 3.378511132398759, - "angularVelocity": -0.26795860891639617, - "velocityX": 5.283744854264688, - "velocityY": 0.55104498562348, - "timestamp": 1.4731532188874013 - }, - { - "x": 5.562524060208366, - "y": 7.219962031584869, - "heading": 3.355414523374288, - "angularVelocity": -0.32864511043819594, - "velocityX": 4.9540561385951625, - "velocityY": 0.5163105909843465, - "timestamp": 1.543431486510716 - }, - { - "x": 5.887470499962918, - "y": 7.253817931255832, - "heading": 3.331666055733143, - "angularVelocity": -0.3379205043646441, - "velocityX": 4.6237115788942855, - "velocityY": 0.48174066914158636, - "timestamp": 1.6137097541340306 - }, - { - "x": 6.189199807150463, - "y": 7.285249171486667, - "heading": 3.308363686841363, - "angularVelocity": -0.33157289841972754, - "velocityX": 4.29335152091094, - "velocityY": 0.4472398266744939, - "timestamp": 1.6839880217573453 - }, - { - "x": 6.467713626267079, - "y": 7.31425833816948, - "heading": 3.2860397702782755, - "angularVelocity": -0.3176503536305448, - "velocityX": 3.963014862708697, - "velocityY": 0.41277577925367004, - "timestamp": 1.75426628938066 - }, - { - "x": 6.723013594809074, - "y": 7.34084703024564, - "heading": 3.265008761402615, - "angularVelocity": -0.29925337642620564, - "velocityX": 3.6327015046867763, - "velocityY": 0.37833448340917136, - "timestamp": 1.8245445570039747 - }, - { - "x": 6.955101065031071, - "y": 7.365016333172441, - "heading": 3.245478308115181, - "angularVelocity": -0.27790174612893576, - "velocityX": 3.302407388098493, - "velocityY": 0.34390863269912847, - "timestamp": 1.8948228246272893 - }, - { - "x": 7.163977127241494, - "y": 7.386767031992689, - "heading": 3.2275957745381954, - "angularVelocity": -0.2544532496565816, - "velocityX": 2.972128785672115, - "velocityY": 0.3094939524808096, - "timestamp": 1.965101092250604 - }, - { - "x": 7.349642665971283, - "y": 7.4060997209757735, - "heading": 3.2114711639593976, - "angularVelocity": -0.22943949992088322, - "velocityX": 2.6418627693690517, - "velocityY": 0.2750877282106925, - "timestamp": 2.035379359873919 - }, - { - "x": 7.512098408217057, - "y": 7.423014865697383, - "heading": 3.1971897253730983, - "angularVelocity": -0.2032127294720464, - "velocityX": 2.3116070976097918, - "velocityY": 0.24068812868678965, - "timestamp": 2.1056576274972336 - }, - { - "x": 7.651344959888608, - "y": 7.43751284079522, - "heading": 3.184819461924971, - "angularVelocity": -0.17601833207433357, - "velocityX": 1.981360047431725, - "velocityY": 0.20629385993890986, - "timestamp": 2.1759358951205483 - }, - { - "x": 7.767382832665261, - "y": 7.449593954241994, - "heading": 3.174415882660127, - "angularVelocity": -0.14803408815663477, - "velocityX": 1.6511202780154728, - "velocityY": 0.1719039733808395, - "timestamp": 2.246214162743863 - }, - { - "x": 7.860212463836391, - "y": 7.459258463658005, - "heading": 3.1660251568616267, - "angularVelocity": -0.11939289459260129, - "velocityX": 1.3208867308552614, - "velocityY": 0.13751775254062693, - "timestamp": 2.3164924303671777 - }, - { - "x": 7.929834231127341, - "y": 7.466506587678373, - "heading": 3.159686290431417, - "angularVelocity": -0.09019668020541635, - "velocityX": 0.9906585584055106, - "velocityY": 0.10313464269233455, - "timestamp": 2.3867706979904924 - }, - { - "x": 7.976248463934981, - "y": 7.471338514116222, - "heading": 3.1554326760982865, - "angularVelocity": -0.06052531567696676, - "velocityX": 0.6604350729932147, - "velocityY": 0.06875420526505516, - "timestamp": 2.457048965613807 + "x": 0.3285749308509426, + "y": 6.865947459797136, + "heading": 3.135568639860519, + "angularVelocity": 3.3337145202478786e-22, + "velocityX": 0.41797046933441173, + "velocityY": 0.023787032249159984, + "timestamp": 0.03543041836149472 + }, + { + "x": 0.35819266531481686, + "y": 6.867633028650745, + "heading": 3.135568639860519, + "angularVelocity": 3.3337147019213317e-22, + "velocityX": 0.8359408619363751, + "velocityY": 0.04757406013141577, + "timestamp": 0.07086083672298944 + }, + { + "x": 0.4026192574953566, + "y": 6.87016138138964, + "heading": 3.135568639860519, + "angularVelocity": 3.3337143389947597e-22, + "velocityX": 1.2539110243423486, + "velocityY": 0.07136107491303612, + "timestamp": 0.10629125508448417 + }, + { + "x": 0.4600803298886451, + "y": 6.873431536777097, + "heading": 3.135568639860519, + "angularVelocity": 3.331660178480041e-22, + "velocityX": 1.621800561512317, + "velocityY": 0.09229796143220789, + "timestamp": 0.14172167344597889 + }, + { + "x": 0.5175414022820589, + "y": 6.8767016921645565, + "heading": 3.135568639860519, + "angularVelocity": 3.3316629368551667e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233462, + "timestamp": 0.1771520918074736 + }, + { + "x": 0.5750024746754727, + "y": 6.879971847552017, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614681891394e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233254, + "timestamp": 0.2125825101689683 + }, + { + "x": 0.6324635470688867, + "y": 6.883242002939478, + "heading": 3.135568639860519, + "angularVelocity": 3.331683104806914e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.24801292853046303 + }, + { + "x": 0.6899246194623005, + "y": 6.886512158326939, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614878052174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.28344334689195777 + }, + { + "x": 0.7473856918557144, + "y": 6.8897823137144, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614758461554e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.3188737652534525 + }, + { + "x": 0.8048467642491284, + "y": 6.89305246910186, + "heading": 3.135568639860519, + "angularVelocity": 3.331638534198991e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233299, + "timestamp": 0.35430418361494725 + }, + { + "x": 0.8623078366425422, + "y": 6.89632262448932, + "heading": 3.135568639860519, + "angularVelocity": 3.3317076614902997e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.389734601976442 + }, + { + "x": 0.9197689090359561, + "y": 6.899592779876781, + "heading": 3.135568639860519, + "angularVelocity": 3.331661479116866e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233396, + "timestamp": 0.42516502033793674 + }, + { + "x": 0.97722998142937, + "y": 6.902862935264242, + "heading": 3.135568639860519, + "angularVelocity": 3.33166145502926e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.4605954386994315 + }, + { + "x": 1.0346910538227838, + "y": 6.906133090651703, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600659908063e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233301, + "timestamp": 0.4960258570609262 + }, + { + "x": 1.0921521262161977, + "y": 6.9094032460391634, + "heading": 3.135568639860519, + "angularVelocity": 3.331661460674642e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5314562754224209 + }, + { + "x": 1.1496131986096116, + "y": 6.912673401426624, + "heading": 3.135568639860519, + "angularVelocity": 3.331661475383932e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5668866937839157 + }, + { + "x": 1.2070742710030253, + "y": 6.915943556814084, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614753921174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.6023171121454104 + }, + { + "x": 1.2645353433964392, + "y": 6.919213712201545, + "heading": 3.135568639860519, + "angularVelocity": 3.331662731932369e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.6377475305069051 + }, + { + "x": 1.3219964157898532, + "y": 6.922483867589006, + "heading": 3.135568639860519, + "angularVelocity": 3.3316627143346654e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233393, + "timestamp": 0.6731779488683999 + }, + { + "x": 1.379457488183267, + "y": 6.925754022976466, + "heading": 3.135568639860519, + "angularVelocity": 3.331661483573468e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233157, + "timestamp": 0.7086083672298946 + }, + { + "x": 1.436918560576681, + "y": 6.929024178363926, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614890219236e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233392, + "timestamp": 0.7440387855913894 + }, + { + "x": 1.494379632970095, + "y": 6.932294333751387, + "heading": 3.135568639860519, + "angularVelocity": 3.331662743669949e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233207, + "timestamp": 0.7794692039528841 + }, + { + "x": 1.5518407053635086, + "y": 6.935564489138847, + "heading": 3.135568639860519, + "angularVelocity": 3.331638909213181e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.8148996223143788 + }, + { + "x": 1.6093017777569225, + "y": 6.938834644526308, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600948450887e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233622, + "timestamp": 0.8503300406758736 + }, + { + "x": 1.666762850150336, + "y": 6.942104799913777, + "heading": 3.135568639860519, + "angularVelocity": 3.331671581625241e-22, + "velocityX": 1.621800561515844, + "velocityY": 0.09229796143255317, + "timestamp": 0.8857604590373683 + }, + { + "x": 1.7242239225437261, + "y": 6.945374955301653, + "heading": 3.135568639860519, + "angularVelocity": 3.3334679678699274e-22, + "velocityX": 1.6218005615151887, + "velocityY": 0.09229796144406777, + "timestamp": 0.9211908773988631 + }, + { + "x": 1.7816849946975708, + "y": 6.948645114898682, + "heading": 3.135568639860519, + "angularVelocity": 1.7645788213167404e-22, + "velocityX": 1.6218005547541783, + "velocityY": 0.09229808024460076, + "timestamp": 0.9566212957603578 + }, + { + "x": 1.8178857407766151, + "y": 6.94853231479887, + "heading": 3.1384797599068888, + "angularVelocity": 0.12817174361754557, + "velocityX": 1.5938582646199322, + "velocityY": -0.004966399613447973, + "timestamp": 0.9793339464913666 + }, + { + "x": 1.8540603970824046, + "y": 6.948528771707492, + "heading": 3.1414999940123267, + "angularVelocity": 0.13297585302602363, + "velocityX": 1.592709575567121, + "velocityY": -0.00015599638369279018, + "timestamp": 1.0020465972223755 + }, + { + "x": 1.8901908310793394, + "y": 6.948640868874459, + "heading": 3.1447030512216227, + "angularVelocity": 0.14102524831780386, + "velocityX": 1.5907625413183106, + "velocityY": 0.0049354506567203535, + "timestamp": 1.0247592479533845 + }, + { + "x": 1.926257669084107, + "y": 6.9488776015696425, + "heading": 3.148167194864803, + "angularVelocity": 0.15252044704983372, + "velocityX": 1.5879625162168578, + "velocityY": 0.010422944375284186, + "timestamp": 1.0474718986843934 + }, + { + "x": 1.9622398024006038, + "y": 6.94925072993569, + "heading": 3.151976954699645, + "angularVelocity": 0.16773734954856945, + "velocityX": 1.584233110553301, + "velocityY": 0.01642821749276163, + "timestamp": 1.0701845494154023 + }, + { + "x": 1.9981171224268564, + "y": 6.949774504499785, + "heading": 3.156211459419019, + "angularVelocity": 0.1864381559653407, + "velocityX": 1.5796183567984214, + "velocityY": 0.023060917472754097, + "timestamp": 1.0928972001464112 + }, + { + "x": 2.033847488660219, + "y": 6.950468453992633, + "heading": 3.161038496208879, + "angularVelocity": 0.2125263513725333, + "velocityX": 1.5731482272380108, + "velocityY": 0.03055343478252191, + "timestamp": 1.1156098508774202 + }, + { + "x": 2.0694053227389695, + "y": 6.951354952814641, + "heading": 3.166554769842392, + "angularVelocity": 0.24287229609803831, + "velocityX": 1.565551925218666, + "velocityY": 0.039031059496573456, + "timestamp": 1.138322501608429 + }, + { + "x": 2.104784911670157, + "y": 6.952454847185295, + "heading": 3.1727732170956275, + "angularVelocity": 0.27378782542301205, + "velocityX": 1.5577040896809682, + "velocityY": 0.04842650836667849, + "timestamp": 1.161035152339438 + }, + { + "x": 2.1400047240253706, + "y": 6.9537191803371154, + "heading": 3.1796054213345153, + "angularVelocity": 0.3008105183231903, + "velocityX": 1.550669394441415, + "velocityY": 0.05566647269821489, + "timestamp": 1.183747803070447 + }, + { + "x": 2.175095855891208, + "y": 6.955084615506324, + "heading": 3.18691368664007, + "angularVelocity": 0.3217706903570311, + "velocityX": 1.5450038078527182, + "velocityY": 0.060117825320333544, + "timestamp": 1.2064604538014558 + }, + { + "x": 2.2101380445057046, + "y": 6.956471002562802, + "heading": 3.194369840668752, + "angularVelocity": 0.3282819833311086, + "velocityX": 1.5428489184071512, + "velocityY": 0.06104030185188484, + "timestamp": 1.2291731045324648 + }, + { + "x": 2.2451610724837057, + "y": 6.957849512193169, + "heading": 3.2018529792356185, + "angularVelocity": 0.32947006738627316, + "velocityX": 1.5420053076493287, + "velocityY": 0.060693471963871995, + "timestamp": 1.2518857552634737 + }, + { + "x": 2.2801629678458126, + "y": 6.959221595789265, + "heading": 3.2093707720743723, + "angularVelocity": 0.3309958369804056, + "velocityX": 1.5410748739388525, + "velocityY": 0.060410544429463274, + "timestamp": 1.2745984059944826 + }, + { + "x": 2.315144049729391, + "y": 6.960586594052822, + "heading": 3.2169217131634897, + "angularVelocity": 0.3324552989672944, + "velocityX": 1.540158491312511, + "velocityY": 0.06009858909567994, + "timestamp": 1.2973110567254915 + }, + { + "x": 2.3501040543802802, + "y": 6.961944386116353, + "heading": 3.2245066187504112, + "angularVelocity": 0.3339506989629348, + "velocityX": 1.539230496031878, + "velocityY": 0.05978131216881497, + "timestamp": 1.3200237074565004 + }, + { + "x": 2.3850426980471595, + "y": 6.963294864074251, + "heading": 3.232126373384074, + "angularVelocity": 0.3354850441678861, + "velocityX": 1.5382900076554706, + "velocityY": 0.05945928433854099, + "timestamp": 1.3427363581875094 + }, + { + "x": 2.419959710539657, + "y": 6.964637896582436, + "heading": 3.239781798872735, + "angularVelocity": 0.3370555722150803, + "velocityX": 1.5373376232491214, + "velocityY": 0.05913147364836824, + "timestamp": 1.3654490089185183 + }, + { + "x": 2.454854803951486, + "y": 6.965973356645481, + "heading": 3.247473776388813, + "angularVelocity": 0.3386648968090985, + "velocityX": 1.5363725628109135, + "velocityY": 0.05879807156202029, + "timestamp": 1.3881616596495272 + }, + { + "x": 2.48972762858419, + "y": 6.967301155248667, + "heading": 3.2552034126870217, + "angularVelocity": 0.3403229499609042, + "velocityX": 1.5353921057348447, + "velocityY": 0.05846075030657316, + "timestamp": 1.4108743103805361 + }, + { + "x": 2.5245780070203394, + "y": 6.968621054178632, + "heading": 3.262971149366171, + "angularVelocity": 0.34200044596925644, + "velocityX": 1.5344038372663136, + "velocityY": 0.05811294091547401, + "timestamp": 1.433586961111545 + }, + { + "x": 2.559405532428149, + "y": 6.969932985582836, + "heading": 3.2707782920481354, + "angularVelocity": 0.3437354263236719, + "velocityX": 1.5333976566751537, + "velocityY": 0.05776214409057404, + "timestamp": 1.456299611842554 + }, + { + "x": 2.594207801112396, + "y": 6.97123839692077, + "heading": 3.2786335848605472, + "angularVelocity": 0.34585539598366793, + "velocityX": 1.5322856454060756, + "velocityY": 0.05747507648462563, + "timestamp": 1.4790122625735629 + }, + { + "x": 2.6290202140808105, + "y": 6.972513675689697, + "heading": 3.286405336491994, + "angularVelocity": 0.3421772175995684, + "velocityX": 1.5327322812606092, + "velocityY": 0.05614838990089804, + "timestamp": 1.5017249133045718 + }, + { + "x": 2.7840510289336455, + "y": 6.98699854094784, + "heading": 3.286405287600397, + "angularVelocity": -5.100684883763116e-7, + "velocityX": 1.6173808639626774, + "velocityY": 0.1511154018498607, + "timestamp": 1.5975779175703702 + }, + { + "x": 2.93908160001489, + "y": 7.001486015045406, + "heading": 3.2864052387079807, + "angularVelocity": -5.100770354356606e-7, + "velocityX": 1.6173783207811374, + "velocityY": 0.15114261893547631, + "timestamp": 1.6934309218361685 + }, + { + "x": 3.094112171101734, + "y": 7.015973489083046, + "heading": 3.286405189815564, + "angularVelocity": -5.100770390190521e-7, + "velocityX": 1.617378320839561, + "velocityY": 0.15114261831028078, + "timestamp": 1.7892839261019668 + }, + { + "x": 3.2491427421885786, + "y": 7.030460963120681, + "heading": 3.286405140923147, + "angularVelocity": -5.100770430673946e-7, + "velocityX": 1.617378320839565, + "velocityY": 0.15114261831023715, + "timestamp": 1.8851369303677652 + }, + { + "x": 3.404173313275423, + "y": 7.0449484371583155, + "heading": 3.28640509203073, + "angularVelocity": -5.100770460466358e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023496, + "timestamp": 1.9809899346335635 + }, + { + "x": 3.5592038843622675, + "y": 7.05943591119595, + "heading": 3.286405043138312, + "angularVelocity": -5.100770488976204e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023276, + "timestamp": 2.076842938899362 + }, + { + "x": 3.714234455449112, + "y": 7.073923385233584, + "heading": 3.2864049942458946, + "angularVelocity": -5.100770524050277e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831023054, + "timestamp": 2.1726959431651602 + }, + { + "x": 3.8692650265359565, + "y": 7.088410859271219, + "heading": 3.2864049453534765, + "angularVelocity": -5.100770565197187e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831022827, + "timestamp": 2.2685489474309586 + }, + { + "x": 4.0242955976228005, + "y": 7.102898333308853, + "heading": 3.2864048964610575, + "angularVelocity": -5.100770606946053e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831022607, + "timestamp": 2.364401951696757 + }, + { + "x": 4.179326168709644, + "y": 7.117385807346487, + "heading": 3.286404847568639, + "angularVelocity": -5.100770634020749e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.151142618310224, + "timestamp": 2.4602549559625553 + }, + { + "x": 4.334356739796489, + "y": 7.13187328138412, + "heading": 3.2864047986762195, + "angularVelocity": -5.100770666030285e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831022163, + "timestamp": 2.5561079602283536 + }, + { + "x": 4.489387310883332, + "y": 7.146360755421753, + "heading": 3.2864047497838, + "angularVelocity": -5.100770706092546e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831021944, + "timestamp": 2.651960964494152 + }, + { + "x": 4.644417881970177, + "y": 7.160848229459387, + "heading": 3.28640470089138, + "angularVelocity": -5.100770740673254e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021727, + "timestamp": 2.7478139687599503 + }, + { + "x": 4.79944845305702, + "y": 7.175335703497019, + "heading": 3.28640465199896, + "angularVelocity": -5.100770777885376e-7, + "velocityX": 1.6173783208395638, + "velocityY": 0.15114261831021514, + "timestamp": 2.8436669730257487 + }, + { + "x": 4.954479024143865, + "y": 7.1898231775346515, + "heading": 3.286404603106539, + "angularVelocity": -5.100770810299081e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021283, + "timestamp": 2.939519977291547 + }, + { + "x": 5.109509595230708, + "y": 7.2043106515722855, + "heading": 3.2864045542141187, + "angularVelocity": -5.100770846564483e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021056, + "timestamp": 3.0353729815573454 + }, + { + "x": 5.264540166317553, + "y": 7.218798125609918, + "heading": 3.286404505321697, + "angularVelocity": -5.100770889061609e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.1511426183102084, + "timestamp": 3.1312259858231437 + }, + { + "x": 5.419570737404396, + "y": 7.233285599647549, + "heading": 3.2864044564292754, + "angularVelocity": -5.100770922378371e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020614, + "timestamp": 3.227078990088942 + }, + { + "x": 5.574601308491241, + "y": 7.247773073685181, + "heading": 3.2864044075368533, + "angularVelocity": -5.100770952675197e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020392, + "timestamp": 3.3229319943547404 + }, + { + "x": 5.7296318795780845, + "y": 7.262260547722812, + "heading": 3.286404358644431, + "angularVelocity": -5.100770991840634e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.1511426183102018, + "timestamp": 3.4187849986205388 + }, + { + "x": 5.884662450664929, + "y": 7.276748021760444, + "heading": 3.286404309752008, + "angularVelocity": -5.100771023487481e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.15114261831019962, + "timestamp": 3.514638002886337 + }, + { + "x": 6.039693021751773, + "y": 7.291235495798076, + "heading": 3.286404260859585, + "angularVelocity": -5.100771061559551e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.15114261831019746, + "timestamp": 3.6104910071521354 + }, + { + "x": 6.194723592838616, + "y": 7.305722969835706, + "heading": 3.2864042119671617, + "angularVelocity": -5.100771091660107e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.1511426183101951, + "timestamp": 3.706344011417934 + }, + { + "x": 6.349754163925461, + "y": 7.320210443873337, + "heading": 3.2864041630747383, + "angularVelocity": -5.100771129976334e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831019293, + "timestamp": 3.802197015683732 + }, + { + "x": 6.504784735012304, + "y": 7.334697917910969, + "heading": 3.286404114182314, + "angularVelocity": -5.100771163404491e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.1511426183101907, + "timestamp": 3.8980500199495305 + }, + { + "x": 6.659815306099149, + "y": 7.349185391948599, + "heading": 3.28640406528989, + "angularVelocity": -5.100771202032421e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.1511426183101885, + "timestamp": 3.993903024215329 + }, + { + "x": 6.814845877185992, + "y": 7.363672865986229, + "heading": 3.2864040163974653, + "angularVelocity": -5.100771233944824e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831018627, + "timestamp": 4.089756028481127 + }, + { + "x": 6.969876448272837, + "y": 7.378160340023859, + "heading": 3.2864039675050396, + "angularVelocity": -5.100771270486632e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.15114261831018408, + "timestamp": 4.1856090327469255 + }, + { + "x": 7.12490701935968, + "y": 7.392647814061489, + "heading": 3.286403918612615, + "angularVelocity": -5.100771304657537e-7, + "velocityX": 1.617378320839562, + "velocityY": 0.15114261831018191, + "timestamp": 4.281462037012724 + }, + { + "x": 7.279937590446525, + "y": 7.407135288099118, + "heading": 3.286403869720189, + "angularVelocity": -5.10077134427039e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.1511426183101796, + "timestamp": 4.377315041278522 + }, + { + "x": 7.4349681615333685, + "y": 7.421622762136748, + "heading": 3.286403820827763, + "angularVelocity": -5.100771387503622e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.15114261831017736, + "timestamp": 4.473168045544321 + }, + { + "x": 7.589998732620213, + "y": 7.436110236174376, + "heading": 3.2864037719353365, + "angularVelocity": -5.100771410554518e-7, + "velocityX": 1.6173783208395616, + "velocityY": 0.15114261831017525, + "timestamp": 4.569021049810119 + }, + { + "x": 7.745029303707056, + "y": 7.450597710212007, + "heading": 3.28640372304291, + "angularVelocity": -5.100771448287922e-7, + "velocityX": 1.6173783208395598, + "velocityY": 0.1511426183101905, + "timestamp": 4.664874054075917 + }, + { + "x": 7.90005987484092, + "y": 7.465085183746354, + "heading": 3.2864036741504377, + "angularVelocity": -5.1007762076643e-7, + "velocityX": 1.6173783213301043, + "velocityY": 0.15114261305961757, + "timestamp": 4.760727058341716 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -0.03044253542482025, - "velocityX": 0.33021570985126986, - "velocityY": 0.03437608724320182, - "timestamp": 2.5273272332371217 + "angularVelocity": -1.3886935284235506, + "velocityX": 1.0369583915052876, + "velocityY": 0.09044288486721058, + "timestamp": 4.856580062607514 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -3.79407884851814e-18, - "velocityX": 2.546987187300363e-16, - "velocityY": -1.0256209789219734e-16, - "timestamp": 2.5976055008604364 + "angularVelocity": 5.239369635456734e-24, + "velocityX": 2.4359888438060422e-22, + "velocityY": -8.774946070326848e-22, + "timestamp": 4.952433066873312 + }, + { + "x": 7.928671024084623, + "y": 7.451618987983874, + "heading": 3.153293227446569, + "angularVelocity": 2.2670564289793535e-36, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 5.031658209240836 + }, + { + "x": 7.805841482889084, + "y": 7.413208234129291, + "heading": 3.153293227446569, + "angularVelocity": 3.3814188338755363e-34, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 5.1108833516083605 + }, + { + "x": 7.683011941693537, + "y": 7.374797480274705, + "heading": 3.153293227446569, + "angularVelocity": 6.0567834914455396e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.190108493975885 }, { - "x": 7.9777911181971035, - "y": 7.466979623755239, + "x": 7.56018240049799, + "y": 7.336386726420119, "heading": 3.153293227446569, - "angularVelocity": 4.0010921973360205e-29, - "velocityX": -0.31261406582002, - "velocityY": -0.09775939742848679, - "timestamp": 2.6669060734735086 + "angularVelocity": -6.892446902508999e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.269333636343409 }, { - "x": 7.934462450883723, - "y": 7.453430059384792, + "x": 7.437352859302442, + "y": 7.297975972565533, "heading": 3.153293227446569, - "angularVelocity": -2.051733815514594e-34, - "velocityX": -0.6252281284210741, - "velocityY": -0.1955187938503514, - "timestamp": 2.736206646086581 + "angularVelocity": -4.625140327997389e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.348558778710933 }, { - "x": 7.869469450281316, - "y": 7.433105712944096, + "x": 7.314523318106895, + "y": 7.2595652187109465, "heading": 3.153293227446569, - "angularVelocity": -1.98522229630168e-34, - "velocityX": -0.9378421873262779, - "velocityY": -0.29327818911646475, - "timestamp": 2.805507218699653 + "angularVelocity": 2.407409636834091e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.427783921078457 }, { - "x": 7.782812116686986, - "y": 7.40600658452606, + "x": 7.191693776911348, + "y": 7.22115446485636, "heading": 3.153293227446569, - "angularVelocity": -1.9190015159804115e-34, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190667, - "timestamp": 2.8748077913127252 + "angularVelocity": -8.071167864078362e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.507009063445981 }, { - "x": 7.6744904504495075, - "y": 7.372132674239751, + "x": 7.0688642357158, + "y": 7.182743711001775, "heading": 3.153293227446569, - "angularVelocity": -8.200637720207044e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.4887969753935167, - "timestamp": 2.9441083639257974 + "angularVelocity": 6.706721989227974e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.586234205813505 }, { - "x": 7.544504451984089, - "y": 7.331483982215012, + "x": 6.946034694520253, + "y": 7.1443329571471885, "heading": 3.153293227446569, - "angularVelocity": 6.610929004011549e-29, - "velocityX": -1.8756843351233508, - "velocityY": -0.5865563658715173, - "timestamp": 3.0134089365388697 + "angularVelocity": -3.932169261853096e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.665459348181029 }, { - "x": 7.392854121793352, - "y": 7.28406050860902, + "x": 6.823205153324706, + "y": 7.105922203292602, "heading": 3.153293227446569, - "angularVelocity": -6.483737543755527e-29, - "velocityX": -2.188298371464413, - "velocityY": -0.6843157540814643, - "timestamp": 3.082709509151942 + "angularVelocity": -5.711604766985455e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.744684490548553 }, { - "x": 7.219539460498179, - "y": 7.229862253615935, + "x": 6.700375612129158, + "y": 7.067511449438016, "heading": 3.153293227446569, - "angularVelocity": 8.073376116568391e-29, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 3.152010081765014 + "angularVelocity": 9.147104299173202e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.823909632916077 }, { - "x": 7.024560468885024, - "y": 7.168889217481695, + "x": 6.577546070933611, + "y": 7.0291006955834305, "heading": 3.153293227446569, - "angularVelocity": -3.526812750348743e-30, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 3.2213106543780863 + "angularVelocity": 4.987399043745874e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.903134775283601 }, { - "x": 6.807917147982327, - "y": 7.101141400527908, + "x": 6.4547165297380635, + "y": 6.990689941728844, "heading": 3.153293227446569, - "angularVelocity": -9.438045628849687e-29, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 3.2906112269911585 + "angularVelocity": 1.0726945561042014e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.9823599176511255 }, { - "x": 6.569609499192503, - "y": 7.026618803193132, + "x": 6.331886988542516, + "y": 6.952279187874258, "heading": 3.153293227446569, - "angularVelocity": 9.790681244582267e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 3.3599117996042307 + "angularVelocity": 1.8749531736864053e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.06158506001865 }, { - "x": 6.309637524541267, - "y": 6.94532142611084, + "x": 6.209057447346969, + "y": 6.913868434019672, "heading": 3.153293227446569, - "angularVelocity": -1.3895984503554753e-34, - "velocityX": -3.7513683487544087, - "velocityY": -1.1731126312072742, - "timestamp": 3.429212372217303 + "angularVelocity": -8.352063684746622e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.140810202386174 }, { - "x": 6.028001227211883, - "y": 6.85724927027649, + "x": 6.0862279061514215, + "y": 6.875457680165086, "heading": 3.153293227446569, - "angularVelocity": -1.323503512813964e-34, - "velocityX": -4.063982254545725, - "velocityY": -1.2708719785922367, - "timestamp": 3.498512944830375 + "angularVelocity": 6.429027209138877e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.220035344753698 }, { - "x": 5.724700612934229, - "y": 6.762402337481905, + "x": 5.963398364955874, + "y": 6.8370469263105, "heading": 3.153293227446569, - "angularVelocity": -2.1989550257903952e-29, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 3.5678135174434473 + "angularVelocity": -7.349253697576567e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.299260487121222 }, { - "x": 5.399735695078014, - "y": 6.660780631908008, + "x": 5.840568823760327, + "y": 6.798636172455914, "heading": 3.153293227446569, - "angularVelocity": 1.8024841235265752e-29, - "velocityX": -4.689209707841794, - "velocityY": -1.4663905613202457, - "timestamp": 3.6371140900565195 + "angularVelocity": -5.316547406258321e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.378485629488746 }, { - "x": 5.053106540487171, - "y": 6.552384174457959, + "x": 5.7177392825647795, + "y": 6.760225418601328, "heading": 3.153293227446569, - "angularVelocity": 1.896211807861914e-29, - "velocityX": -5.001822373477154, - "velocityY": -1.564149520888688, - "timestamp": 3.7064146626695917 + "angularVelocity": 3.261106516242251e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.45771077185627 }, { - "x": 4.7281416226309565, - "y": 6.4507624688840615, + "x": 5.594909741369232, + "y": 6.721814664746741, "heading": 3.153293227446569, - "angularVelocity": -1.4997927313419928e-29, - "velocityX": -4.689209707841795, - "velocityY": -1.4663905613202457, - "timestamp": 3.775715235282664 + "angularVelocity": -2.0479976334874182e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.536935914223794 }, { - "x": 4.424841008353304, - "y": 6.355915536089477, + "x": 5.472080200173685, + "y": 6.683403910892156, "heading": 3.153293227446569, - "angularVelocity": -4.4185947102012545e-35, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 3.845015807895736 + "angularVelocity": -7.212902306224552e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.616161056591318 }, { - "x": 4.14320471102392, - "y": 6.267843380255127, + "x": 5.3492506589781375, + "y": 6.64499315703757, "heading": 3.153293227446569, - "angularVelocity": -9.264733448000688e-35, - "velocityX": -4.063982254545726, - "velocityY": -1.2708719785922367, - "timestamp": 3.9143163805088084 + "angularVelocity": 2.340395125909778e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.695386198958842 }, { - "x": 3.883232736372683, - "y": 6.186546003172835, + "x": 5.22642111778259, + "y": 6.606582403182983, "heading": 3.153293227446569, - "angularVelocity": 3.5265482976182584e-30, - "velocityX": -3.751368348754409, - "velocityY": -1.1731126312072742, - "timestamp": 3.9836169531218806 + "angularVelocity": -2.132648663400479e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.774611341326366 }, { - "x": 3.644925087582859, - "y": 6.112023405838059, + "x": 5.103591576587043, + "y": 6.568171649328397, "heading": 3.153293227446569, - "angularVelocity": 2.156768838744686e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 4.052917525734953 + "angularVelocity": -2.6423732915608732e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.8538364836938905 }, { - "x": 3.4282817666801613, - "y": 6.044275588884272, + "x": 4.980762035391495, + "y": 6.529760895473811, "heading": 3.153293227446569, - "angularVelocity": -2.105657187574439e-28, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 4.122218098348026 + "angularVelocity": -1.5836433593674017e-28, + "velocityX": -1.5503858689927383, + "velocityY": -0.4848303544397451, + "timestamp": 6.933061626061415 }, { - "x": 3.2333027750670063, - "y": 5.983302552750032, + "x": 4.857932494195948, + "y": 6.491350141619225, "heading": 3.153293227446569, - "angularVelocity": 1.8547117767579628e-28, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 4.1915186709610985 + "angularVelocity": 3.462450644614712e-28, + "velocityX": -1.5503858689927386, + "velocityY": -0.4848303544397447, + "timestamp": 7.012286768428939 }, { - "x": 3.059988113771833, - "y": 5.929104297756947, + "x": 4.735102953000401, + "y": 6.452939387764639, "heading": 3.153293227446569, - "angularVelocity": -5.955568710068408e-35, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 4.260819243574171 + "angularVelocity": 9.425461275139336e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.091511910796463 }, { - "x": 2.908337783581096, - "y": 5.881680824150955, + "x": 4.612273411804853, + "y": 6.414528633910053, "heading": 3.153293227446569, - "angularVelocity": -2.0810713408746708e-29, - "velocityX": -2.1882983714644135, - "velocityY": -0.6843157540814642, - "timestamp": 4.330119816187244 + "angularVelocity": 2.8373243283513945e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.170737053163987 }, { - "x": 2.7783517851156776, - "y": 5.841032132126216, + "x": 4.489443870609306, + "y": 6.3761178800554665, "heading": 3.153293227446569, - "angularVelocity": 2.081061415014159e-29, - "velocityX": -1.875684335123351, - "velocityY": -0.5865563658715173, - "timestamp": 4.3994203888003165 + "angularVelocity": 4.424546575494848e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.249962195531511 }, { - "x": 2.6700301188781994, - "y": 5.807158221839908, + "x": 4.366614329413759, + "y": 6.337707126200881, "heading": 3.153293227446569, - "angularVelocity": 7.326893682940581e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.48879697539351663, - "timestamp": 4.468720961413389 + "angularVelocity": 2.6048393166300173e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.329187337899035 }, { - "x": 2.583372785283869, - "y": 5.780059093421871, + "x": 4.243784788218211, + "y": 6.299296372346295, "heading": 3.153293227446569, - "angularVelocity": -7.326900961882318e-29, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190656, - "timestamp": 4.538021534026462 + "angularVelocity": -8.141761105066643e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.408412480266559 }, { - "x": 2.5183797846814615, - "y": 5.759734746981175, + "x": 4.120955247022664, + "y": 6.2608856184917085, "heading": 3.153293227446569, - "angularVelocity": -2.64698564160025e-35, - "velocityX": -0.9378421873262781, - "velocityY": -0.2932781891164647, - "timestamp": 4.6073221066395345 + "angularVelocity": -1.5769317536066289e-37, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.487637622634083 }, { - "x": 2.4750511173680816, - "y": 5.746185182610728, + "x": 3.998125705827117, + "y": 6.222474864637123, "heading": 3.153293227446569, - "angularVelocity": -4.5099737006589516e-29, - "velocityX": -0.6252281284210743, - "velocityY": -0.1955187938503513, - "timestamp": 4.676622679252607 + "angularVelocity": -1.9751877406999063e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.566862765001607 + }, + { + "x": 3.8752961646315702, + "y": 6.184064110782536, + "heading": 3.153293227446569, + "angularVelocity": 6.296656188647329e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.6460879073691315 + }, + { + "x": 3.7524666234360233, + "y": 6.145653356927951, + "heading": 3.153293227446569, + "angularVelocity": 3.349857109653225e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.725313049736656 + }, + { + "x": 3.6296370822404764, + "y": 6.107242603073365, + "heading": 3.153293227446569, + "angularVelocity": 1.1073851910178502e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.80453819210418 + }, + { + "x": 3.5068075410449295, + "y": 6.068831849218778, + "heading": 3.153293227446569, + "angularVelocity": -1.0776722091283702e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.883763334471704 + }, + { + "x": 3.3839779998493826, + "y": 6.030421095364192, + "heading": 3.153293227446569, + "angularVelocity": -2.2657744543933122e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.962988476839228 + }, + { + "x": 3.2611484586538357, + "y": 5.992010341509607, + "heading": 3.153293227446569, + "angularVelocity": 6.322058172960319e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.042213619206752 + }, + { + "x": 3.138318917458289, + "y": 5.95359958765502, + "heading": 3.153293227446569, + "angularVelocity": -1.267303885279781e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.121438761574275 + }, + { + "x": 3.015489376262742, + "y": 5.915188833800434, + "heading": 3.153293227446569, + "angularVelocity": 4.539047579816208e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.200663903941798 + }, + { + "x": 2.892659835067195, + "y": 5.876778079945848, + "heading": 3.153293227446569, + "angularVelocity": -2.4352191603126546e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.279889046309322 + }, + { + "x": 2.769830293871648, + "y": 5.838367326091261, + "heading": 3.153293227446569, + "angularVelocity": 1.9777124768596482e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.359114188676845 + }, + { + "x": 2.6470007526761012, + "y": 5.799956572236676, + "heading": 3.153293227446569, + "angularVelocity": 1.6826010379363495e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.438339331044368 + }, + { + "x": 2.5241712114805623, + "y": 5.761545818382093, + "heading": 3.153293227446569, + "angularVelocity": -1.9705474475500694e-33, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 8.517564473411891 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": 1.9207053764808128e-28, - "velocityX": -0.31261406582002027, - "velocityY": -0.09775939742848665, - "timestamp": 4.74592325186568 + "angularVelocity": 2.748235734113482e-34, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 8.596789615779414 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": -4.899028012023128e-29, - "velocityX": 1.275108562409782e-22, - "velocityY": 3.987692290669026e-23, - "timestamp": 4.815223824478752 + "angularVelocity": -7.888608661057978e-31, + "velocityX": -1.038819230645035e-25, + "velocityY": -4.640517458430809e-27, + "timestamp": 8.676014758146938 } ], "constraints": [ @@ -2233,6 +2404,1329 @@ 4 ], "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 3 + ], + "type": "WptZeroVelocity" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "FrontWing2": { + "waypoints": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.8084842834156656e-35, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.347146156455349, + "y": 5.593360447165824, + "heading": 3.140264869371664, + "angularVelocity": -0.007271662750997594, + "velocityX": 1.1945189788836181, + "velocityY": -0.007419158267666702, + "timestamp": 0.10109525888490682 + }, + { + "x": 1.5113646664969056, + "y": 5.59234048587755, + "heading": 3.1402648691641017, + "angularVelocity": -2.053134778232203e-9, + "velocityX": 1.6243937831794193, + "velocityY": -0.010089110998122406, + "timestamp": 0.20219051776981364 + }, + { + "x": 1.6755831765384634, + "y": 5.591320524589276, + "heading": 3.14026486895654, + "angularVelocity": -2.053134775032819e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122467, + "timestamp": 0.30328577665472045 + }, + { + "x": 1.8398016865800209, + "y": 5.5903005633010014, + "heading": 3.140264868748978, + "angularVelocity": -2.0531347455807303e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122468, + "timestamp": 0.4043810355396273 + }, + { + "x": 2.0040201966215783, + "y": 5.589280602012727, + "heading": 3.1402648685414154, + "angularVelocity": -2.0531349299385344e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.01008911099812247, + "timestamp": 0.5054762944245341 + }, + { + "x": 2.168238706663136, + "y": 5.588260640724453, + "heading": 3.1402648683338534, + "angularVelocity": -2.053134785130435e-9, + "velocityX": 1.6243937831794293, + "velocityY": -0.010089110998122468, + "timestamp": 0.6065715533094409 + }, + { + "x": 2.3324572167046935, + "y": 5.587240679436179, + "heading": 3.1402648681262915, + "angularVelocity": -2.0531346533039746e-9, + "velocityX": 1.6243937831794288, + "velocityY": -0.010089110998122467, + "timestamp": 0.7076668121943477 + }, + { + "x": 2.49667572674625, + "y": 5.586220718147905, + "heading": 3.140264867918729, + "angularVelocity": -2.0531352108819935e-9, + "velocityX": 1.624393783179419, + "velocityY": -0.010089110998122406, + "timestamp": 0.8087620710792545 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": -0.0072716627525318345, + "velocityX": 1.1945189788836168, + "velocityY": -0.007419158267749976, + "timestamp": 0.9098573299641612 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": 0, + "velocityX": -2.8085786967275447e-37, + "velocityY": 1.7440158343416895e-35, + "timestamp": 1.010952588849068 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "FrontWing1": { + "waypoints": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.6112520136935844e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.2701999983580183, + "y": 5.571793741558495, + "heading": 2.9123192627847865, + "angularVelocity": -2.75479375784366, + "velocityX": 0.5278042458729727, + "velocityY": -0.26883784352486495, + "timestamp": 0.08301192659671743 + }, + { + "x": 1.3598509429602765, + "y": 5.50482600027521, + "heading": 2.837562269615421, + "angularVelocity": -0.9005572600735416, + "velocityX": 1.0799766765779808, + "velocityY": -0.806724335029887, + "timestamp": 0.16602385319343485 + }, + { + "x": 1.459326562321469, + "y": 5.413786422039427, + "heading": 2.837562034760696, + "angularVelocity": -0.000002829168462845694, + "velocityX": 1.1983292454403298, + "velocityY": -1.0967047985533822, + "timestamp": 0.2490357797901523 + }, + { + "x": 1.558802177818681, + "y": 5.322746839581507, + "heading": 2.837561799906192, + "angularVelocity": -0.000002829165803381977, + "velocityX": 1.198329198893034, + "velocityY": -1.0967048494152307, + "timestamp": 0.3320477063868697 + }, + { + "x": 1.6582777933158976, + "y": 5.231707257123597, + "heading": 2.8375615650516535, + "angularVelocity": -0.0000028291662173205513, + "velocityX": 1.1983291988930937, + "velocityY": -1.0967048494150837, + "timestamp": 0.4150596329835871 + }, + { + "x": 1.7577534088131206, + "y": 5.140667674665702, + "heading": 2.837561330197081, + "angularVelocity": -0.0000028291666305712727, + "velocityX": 1.19832919889317, + "velocityY": -1.0967048494149187, + "timestamp": 0.4980715595803045 + }, + { + "x": 1.8572290243103502, + "y": 5.04962809220782, + "heading": 2.837561095342474, + "angularVelocity": -0.000002829167043337366, + "velocityX": 1.1983291988932463, + "velocityY": -1.0967048494147538, + "timestamp": 0.581083486177022 + }, + { + "x": 1.9567046398075858, + "y": 4.958588509749952, + "heading": 2.8375608604878324, + "angularVelocity": -0.000002829167457373439, + "velocityX": 1.198329198893323, + "velocityY": -1.0967048494145886, + "timestamp": 0.6640954127737394 + }, + { + "x": 2.056180255304828, + "y": 4.867548927292098, + "heading": 2.8375606256331567, + "angularVelocity": -0.00000282916787026248, + "velocityX": 1.1983291988933988, + "velocityY": -1.096704849414424, + "timestamp": 0.7471073393704568 + }, + { + "x": 2.1556558708020765, + "y": 4.776509344834258, + "heading": 2.8375603907784472, + "angularVelocity": -0.0000028291682836294088, + "velocityX": 1.1983291988934754, + "velocityY": -1.0967048494142588, + "timestamp": 0.8301192659671742 + }, + { + "x": 2.2551314862993315, + "y": 4.685469762376432, + "heading": 2.8375601559237027, + "angularVelocity": -0.000002829168697323748, + "velocityX": 1.1983291988935516, + "velocityY": -1.096704849414094, + "timestamp": 0.9131311925638916 + }, + { + "x": 2.354607101796593, + "y": 4.594430179918619, + "heading": 2.8375599210689244, + "angularVelocity": -0.000002829169110584532, + "velocityX": 1.1983291988936342, + "velocityY": -1.096704849413922, + "timestamp": 0.996143119160609 + }, + { + "x": 2.45408271880603, + "y": 4.503390599113157, + "heading": 2.837559686214013, + "angularVelocity": -0.000002829170713915412, + "velocityX": 1.1983292171099962, + "velocityY": -1.0967048295089454, + "timestamp": 1.0791550457573265 + }, + { + "x": 2.5484359060007082, + "y": 4.424785420118879, + "heading": 2.7982046114916295, + "angularVelocity": -0.47408940300319863, + "velocityX": 1.13662206218943, + "velocityY": -0.9469142834878584, + "timestamp": 1.162166972354044 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -2.3995421391526537, + "velocityX": 0.5485627867734783, + "velocityY": -0.4243619716588108, + "timestamp": 1.2451788989507615 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": 0, + "velocityX": -4.934633813021939e-37, + "velocityY": -2.767548277117811e-37, + "timestamp": 1.328190825547479 + }, + { + "x": 2.534366599021267, + "y": 4.442118048365455, + "heading": 2.6991662321397754, + "angularVelocity": 1.173988745719896, + "velocityX": -0.6987106220563037, + "velocityY": 0.6161074104533542, + "timestamp": 1.4135001923828505 + }, + { + "x": 2.4322324251030567, + "y": 4.53578051923557, + "heading": 2.6991662757513564, + "angularVelocity": 5.112167893680353e-7, + "velocityX": -1.1972210990067154, + "velocityY": 1.097915438182342, + "timestamp": 1.498809559218222 + }, + { + "x": 2.330098253355979, + "y": 4.629442992473233, + "heading": 2.6991663193628423, + "angularVelocity": 5.112156854714172e-7, + "velocityX": -1.1972210735566082, + "velocityY": 1.0979154659348471, + "timestamp": 1.5841189260535935 + }, + { + "x": 2.227964081608902, + "y": 4.723105465710898, + "heading": 2.6991663629743283, + "angularVelocity": 5.112156767559466e-7, + "velocityX": -1.1972210735566007, + "velocityY": 1.0979154659348562, + "timestamp": 1.669428292888965 + }, + { + "x": 2.1258299098618236, + "y": 4.816767938948561, + "heading": 2.6991664065858134, + "angularVelocity": 5.112156675644069e-7, + "velocityX": -1.1972210735566096, + "velocityY": 1.0979154659348476, + "timestamp": 1.7547376597243365 + }, + { + "x": 2.023695735807304, + "y": 4.910430409670039, + "heading": 2.6991664501973878, + "angularVelocity": 5.112167177081662e-7, + "velocityX": -1.1972211006045375, + "velocityY": 1.0979154364400177, + "timestamp": 1.840047026559708 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 1.1341858822337967, + "velocityX": -0.702995210451579, + "velocityY": 0.6160140441054905, + "timestamp": 1.9253563933950795 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": -1.527841446263418e-36, + "velocityX": 0, + "velocityY": 0, + "timestamp": 2.010665760230451 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "FrontWing3Contested5": { + "waypoints": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 38 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 36 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": 0, + "velocityX": -1.009927502555409e-33, + "velocityY": 9.510006388327946e-34, + "timestamp": 0 + }, + { + "x": 1.2629372157532939, + "y": 5.6118681655215035, + "heading": 3.3611684682063676, + "angularVelocity": 2.615104090958444, + "velocityX": 0.4341464636351002, + "velocityY": 0.21092108774289103, + "timestamp": 0.08419109165389853 + }, + { + "x": 1.3467908645165432, + "y": 5.672396843669079, + "heading": 3.470572560445472, + "angularVelocity": 1.2994734964223353, + "velocityX": 0.9959919406671149, + "velocityY": 0.718943975645346, + "timestamp": 0.16838218330779706 + }, + { + "x": 1.4464012333437837, + "y": 5.766107211983687, + "heading": 3.4705728197139094, + "angularVelocity": 0.000003079523404990731, + "velocityX": 1.1831461841203954, + "velocityY": 1.1130675048120657, + "timestamp": 0.25257327496169557 + }, + { + "x": 1.5460115900456228, + "y": 5.859817593187631, + "heading": 3.4705730789812166, + "angularVelocity": 0.000003079509987095674, + "velocityX": 1.1831460400980154, + "velocityY": 1.1130676579082395, + "timestamp": 0.3367643666155941 + }, + { + "x": 1.645621946747464, + "y": 5.953527974391563, + "heading": 3.470573338248568, + "angularVelocity": 0.000003079510511134596, + "velocityX": 1.1831460400980387, + "velocityY": 1.1130676579081111, + "timestamp": 0.42095545826949265 + }, + { + "x": 1.7452323034493133, + "y": 6.047238355595477, + "heading": 3.470573597515964, + "angularVelocity": 0.000003079511034719833, + "velocityX": 1.183146040098137, + "velocityY": 1.1130676579079029, + "timestamp": 0.5051465499233911 + }, + { + "x": 1.844842660151171, + "y": 6.140948736799373, + "heading": 3.470573856783404, + "angularVelocity": 0.0000030795115586857372, + "velocityX": 1.1831460400982357, + "velocityY": 1.1130676579076944, + "timestamp": 0.5893376415772896 + }, + { + "x": 1.944453016853037, + "y": 6.234659118003253, + "heading": 3.470574116050888, + "angularVelocity": 0.0000030795120819840736, + "velocityX": 1.1831460400983345, + "velocityY": 1.1130676579074863, + "timestamp": 0.6735287332311881 + }, + { + "x": 2.044063373554911, + "y": 6.328369499207114, + "heading": 3.470574375318416, + "angularVelocity": 0.0000030795126059487745, + "velocityX": 1.1831460400984328, + "velocityY": 1.1130676579072778, + "timestamp": 0.7577198248850866 + }, + { + "x": 2.1436737302567934, + "y": 6.4220798804109585, + "heading": 3.470574634585988, + "angularVelocity": 0.000003079513129853114, + "velocityX": 1.183146040098531, + "velocityY": 1.1130676579070695, + "timestamp": 0.8419109165389851 + }, + { + "x": 2.243284086958684, + "y": 6.515790261614786, + "heading": 3.470574893853604, + "angularVelocity": 0.0000030795136536386766, + "velocityX": 1.1831460400986302, + "velocityY": 1.1130676579068612, + "timestamp": 0.9261020081928836 + }, + { + "x": 2.342894443660583, + "y": 6.6095006428185945, + "heading": 3.4705751531212647, + "angularVelocity": 0.0000030795141771132826, + "velocityX": 1.1831460400987281, + "velocityY": 1.1130676579066532, + "timestamp": 1.010293099846782 + }, + { + "x": 2.442504800362493, + "y": 6.703211024022384, + "heading": 3.470575412388969, + "angularVelocity": 0.0000030795147010529834, + "velocityX": 1.1831460400988572, + "velocityY": 1.1130676579064127, + "timestamp": 1.0944841915006807 + }, + { + "x": 2.5421151619283058, + "y": 6.79692140005584, + "heading": 3.470575671657143, + "angularVelocity": 0.0000030795202773893193, + "velocityX": 1.1831460978710306, + "velocityY": 1.1130675964945436, + "timestamp": 1.1786752831545793 + }, + { + "x": 2.632005363421341, + "y": 6.868564036243049, + "heading": 3.5437695268680747, + "angularVelocity": 0.8693776713553536, + "velocityX": 1.0676925518743172, + "velocityY": 0.8509526932103911, + "timestamp": 1.2628663748084779 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 2.207199307384385, + "velocityX": 0.46866491549483613, + "velocityY": 0.37605700085290744, + "timestamp": 1.3470574664623765 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 0, + "velocityX": -1.5194658088032264e-34, + "velocityY": -2.7343286363371283e-33, + "timestamp": 1.431248558116275 + }, + { + "x": 2.7208277247721226, + "y": 6.912165859423971, + "heading": 3.486294740771461, + "angularVelocity": -2.694751338131124, + "velocityX": 0.5467552516805291, + "velocityY": 0.13225779416916014, + "timestamp": 1.5215356661845725 + }, + { + "x": 2.839209601133149, + "y": 6.942185820162622, + "heading": 3.3899477762404344, + "angularVelocity": -1.0671176272269702, + "velocityX": 1.311171427392232, + "velocityY": 0.33249443227201864, + "timestamp": 1.61182277425287 + }, + { + "x": 2.985101375777118, + "y": 6.957221507964457, + "heading": 3.3899472751996993, + "angularVelocity": -0.0000055494161427999685, + "velocityX": 1.615864964171956, + "velocityY": 0.16653194596133747, + "timestamp": 1.7021098823211673 + }, + { + "x": 3.130993151670898, + "y": 6.97225718364292, + "heading": 3.3899467741601925, + "angularVelocity": -0.000005549402536979108, + "velocityX": 1.6158649780145908, + "velocityY": 0.1665318116855625, + "timestamp": 1.7923969903894648 + }, + { + "x": 3.2768849275645753, + "y": 6.98729285932146, + "heading": 3.3899462731201484, + "angularVelocity": -0.00000554940849560343, + "velocityX": 1.6158649780134444, + "velocityY": 0.1665318116864077, + "timestamp": 1.8826840984577622 + }, + { + "x": 3.4227767034581484, + "y": 7.002328535000082, + "heading": 3.3899457720795656, + "angularVelocity": -0.0000055494144557147385, + "velocityX": 1.615864978012292, + "velocityY": 0.1665318116873102, + "timestamp": 1.9729712065260596 + }, + { + "x": 3.5686684793516172, + "y": 7.017364210678785, + "heading": 3.3899452710384446, + "angularVelocity": -0.0000055494204159770735, + "velocityX": 1.6158649780111394, + "velocityY": 0.1665318116882126, + "timestamp": 2.063258314594357 + }, + { + "x": 3.7145602552449817, + "y": 7.03239988635757, + "heading": 3.3899447699967857, + "angularVelocity": -0.000005549426375721003, + "velocityX": 1.6158649780099874, + "velocityY": 0.1665318116891151, + "timestamp": 2.1535454226626545 + }, + { + "x": 3.8604520311382426, + "y": 7.047435562036436, + "heading": 3.3899442689545887, + "angularVelocity": -0.000005549432334941507, + "velocityX": 1.615864978008835, + "velocityY": 0.16653181169001743, + "timestamp": 2.243832530730952 + }, + { + "x": 4.006343807031399, + "y": 7.062471237715384, + "heading": 3.389943767911854, + "angularVelocity": -0.000005549438294605419, + "velocityX": 1.6158649780076826, + "velocityY": 0.16653181169091993, + "timestamp": 2.3341196387992493 + }, + { + "x": 4.152235582924452, + "y": 7.077506913394413, + "heading": 3.3899432668685807, + "angularVelocity": -0.000005549444254929096, + "velocityX": 1.6158649780065304, + "velocityY": 0.16653181169182238, + "timestamp": 2.4244067468675468 + }, + { + "x": 4.298127358817401, + "y": 7.092542589073523, + "heading": 3.3899427658247694, + "angularVelocity": -0.000005549450214439604, + "velocityX": 1.615864978005378, + "velocityY": 0.1665318116927247, + "timestamp": 2.514693854935844 + }, + { + "x": 4.444019134710246, + "y": 7.107578264752715, + "heading": 3.38994226478042, + "angularVelocity": -0.000005549456174760312, + "velocityX": 1.6158649780042256, + "velocityY": 0.16653181169362713, + "timestamp": 2.6049809630041416 + }, + { + "x": 4.5899109106029865, + "y": 7.122613940431989, + "heading": 3.389941763735532, + "angularVelocity": -0.000005549462134716656, + "velocityX": 1.6158649780030732, + "velocityY": 0.16653181169452963, + "timestamp": 2.695268071072439 + }, + { + "x": 4.735802686495623, + "y": 7.137649616111343, + "heading": 3.389941262690107, + "angularVelocity": -0.0000055494680950242725, + "velocityX": 1.6158649780019205, + "velocityY": 0.1665318116954319, + "timestamp": 2.7855551791407365 + }, + { + "x": 4.881694462388156, + "y": 7.15268529179078, + "heading": 3.389940761644143, + "angularVelocity": -0.000005549474055578583, + "velocityX": 1.6158649780007681, + "velocityY": 0.16653181169633444, + "timestamp": 2.875842287209034 + }, + { + "x": 5.027586238280584, + "y": 7.167720967470297, + "heading": 3.3899402605976414, + "angularVelocity": -0.000005549480015574405, + "velocityX": 1.6158649779996155, + "velocityY": 0.1665318116972368, + "timestamp": 2.9661293952773313 + }, + { + "x": 5.173478014172908, + "y": 7.182756643149896, + "heading": 3.389939759550601, + "angularVelocity": -0.000005549485976020103, + "velocityX": 1.6158649779984633, + "velocityY": 0.16653181169813924, + "timestamp": 3.0564165033456288 + }, + { + "x": 5.319369790065129, + "y": 7.197792318829577, + "heading": 3.389939258503023, + "angularVelocity": -0.000005549491936617687, + "velocityX": 1.6158649779973107, + "velocityY": 0.16653181169904166, + "timestamp": 3.146703611413926 + }, + { + "x": 5.4652615659572445, + "y": 7.212827994509339, + "heading": 3.3899387574549067, + "angularVelocity": -0.00000554949789662324, + "velocityX": 1.6158649779961582, + "velocityY": 0.1665318116999441, + "timestamp": 3.2369907194822236 + }, + { + "x": 5.611153341849257, + "y": 7.2278636701891825, + "heading": 3.389938256406252, + "angularVelocity": -0.000005549503857497513, + "velocityX": 1.6158649779950054, + "velocityY": 0.16653181170084652, + "timestamp": 3.327277827550521 + }, + { + "x": 5.757045117741165, + "y": 7.242899345869108, + "heading": 3.3899377553570593, + "angularVelocity": -0.000005549509818264508, + "velocityX": 1.615864977993853, + "velocityY": 0.16653181170174894, + "timestamp": 3.4175649356188185 + }, + { + "x": 5.902936893632969, + "y": 7.257935021549114, + "heading": 3.3899372543073283, + "angularVelocity": -0.000005549515778536107, + "velocityX": 1.6158649779927006, + "velocityY": 0.1665318117026513, + "timestamp": 3.507852043687116 + }, + { + "x": 6.048828669524668, + "y": 7.272970697229202, + "heading": 3.3899367532570595, + "angularVelocity": -0.000005549521739714091, + "velocityX": 1.6158649779915477, + "velocityY": 0.16653181170355377, + "timestamp": 3.5981391517554133 + }, + { + "x": 6.194720445416264, + "y": 7.288006372909372, + "heading": 3.3899362522062524, + "angularVelocity": -0.000005549527700309559, + "velocityX": 1.6158649779903953, + "velocityY": 0.16653181170445627, + "timestamp": 3.6884262598237108 + }, + { + "x": 6.340612221307755, + "y": 7.303042048589623, + "heading": 3.389935751154907, + "angularVelocity": -0.000005549533660762894, + "velocityX": 1.6158649779892427, + "velocityY": 0.16653181170535863, + "timestamp": 3.778713367892008 + }, + { + "x": 6.486503997199144, + "y": 7.318077724269955, + "heading": 3.3899352501030235, + "angularVelocity": -0.000005549539621393899, + "velocityX": 1.6158649779880898, + "velocityY": 0.16653181170626094, + "timestamp": 3.8690004759603056 + }, + { + "x": 6.632395773090427, + "y": 7.333113399950369, + "heading": 3.3899347490506018, + "angularVelocity": -0.00000554954558299688, + "velocityX": 1.6158649779869374, + "velocityY": 0.1665318117071635, + "timestamp": 3.959287584028603 + }, + { + "x": 6.778287548981607, + "y": 7.3481490756308645, + "heading": 3.3899342479976413, + "angularVelocity": -0.000005549551543569729, + "velocityX": 1.615864977985785, + "velocityY": 0.16653181170806597, + "timestamp": 4.0495746920969005 + }, + { + "x": 6.924179324872682, + "y": 7.363184751311442, + "heading": 3.3899337469441435, + "angularVelocity": -0.000005549557505439446, + "velocityX": 1.615864977984632, + "velocityY": 0.16653181170896839, + "timestamp": 4.139861800165198 + }, + { + "x": 7.070071100763654, + "y": 7.378220426992101, + "heading": 3.389933245890107, + "angularVelocity": -0.00000554956346606953, + "velocityX": 1.6158649779834795, + "velocityY": 0.1665318117098707, + "timestamp": 4.230148908233495 + }, + { + "x": 7.215962876654522, + "y": 7.393256102672841, + "heading": 3.3899327448355323, + "angularVelocity": -0.000005549569427174084, + "velocityX": 1.6158649779823266, + "velocityY": 0.16653181171077316, + "timestamp": 4.320436016301793 + }, + { + "x": 7.361854652545285, + "y": 7.408291778353663, + "heading": 3.3899322437804194, + "angularVelocity": -0.000005549575389475234, + "velocityX": 1.615864977981174, + "velocityY": 0.16653181171167572, + "timestamp": 4.41072312437009 + }, + { + "x": 7.5077464284359445, + "y": 7.423327454034565, + "heading": 3.389931742724768, + "angularVelocity": -0.000005549581350385169, + "velocityX": 1.6158649779800212, + "velocityY": 0.1665318117125781, + "timestamp": 4.501010232438388 + }, + { + "x": 7.6536382043265, + "y": 7.43836312971555, + "heading": 3.3899312416685787, + "angularVelocity": -0.000005549587311867374, + "velocityX": 1.6158649779788683, + "velocityY": 0.166531811713483, + "timestamp": 4.591297340506685 + }, + { + "x": 7.799529980161501, + "y": 7.453398805927109, + "heading": 3.3899307406089574, + "angularVelocity": -0.000005549625325450788, + "velocityX": 1.6158649773635676, + "velocityY": 0.16653181759000288, + "timestamp": 4.6815844485749825 + }, + { + "x": 7.908273189210625, + "y": 7.465055953097054, + "heading": 3.2483837637195303, + "angularVelocity": -1.5677429471143751, + "velocityX": 1.204415684317477, + "velocityY": 0.12911197865732152, + "timestamp": 4.77187155664328 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "angularVelocity": -1.3108321012839574, + "velocityX": 0.3122914163117384, + "velocityY": -0.13619538226979142, + "timestamp": 4.862158664711577 + }, + { + "x": 7.897313987574637, + "y": 7.4320035366195905, + "heading": 3.223159902402127, + "angularVelocity": 0.9773255855261127, + "velocityX": -0.4109132292719665, + "velocityY": -0.21782107097140077, + "timestamp": 4.957446642075316 + }, + { + "x": 7.764258802337476, + "y": 7.410354345348057, + "heading": 3.302887545584371, + "angularVelocity": 0.8367020204228218, + "velocityX": -1.396348090475833, + "velocityY": -0.22719751085588358, + "timestamp": 5.052734619439054 + }, + { + "x": 7.611104941895525, + "y": 7.387923016344696, + "heading": 3.302889098211225, + "angularVelocity": 0.000016294047758499195, + "velocityX": -1.607273705236954, + "velocityY": -0.23540565792192894, + "timestamp": 5.148022596802792 + }, + { + "x": 7.457951081309142, + "y": 7.365491688274562, + "heading": 3.302890650804134, + "angularVelocity": 0.000016293691520657764, + "velocityX": -1.6072737067526932, + "velocityY": -0.2354056481281831, + "timestamp": 5.2433105741665305 + }, + { + "x": 7.30479722071746, + "y": 7.343060360198914, + "heading": 3.3028922033698884, + "angularVelocity": 0.000016293406553711894, + "velocityX": -1.6072737068083087, + "velocityY": -0.23540564818605267, + "timestamp": 5.338598551530269 + }, + { + "x": 7.151643360120445, + "y": 7.320629032117711, + "heading": 3.302893755908325, + "angularVelocity": 0.00001629311986139254, + "velocityX": -1.6072737068642717, + "velocityY": -0.23540564824432697, + "timestamp": 5.433886528894007 + }, + { + "x": 6.998489499518071, + "y": 7.298197704030925, + "heading": 3.302895308419307, + "angularVelocity": 0.00001629283173826444, + "velocityX": -1.6072737069205232, + "velocityY": -0.2354056483029315, + "timestamp": 5.529174506257745 + }, + { + "x": 6.845335638910315, + "y": 7.275766375938532, + "heading": 3.3028968609027314, + "angularVelocity": 0.000016292542536041963, + "velocityX": -1.6072737069769925, + "velocityY": -0.23540564836178338, + "timestamp": 5.624462483621484 + }, + { + "x": 6.692181778297163, + "y": 7.253335047840514, + "heading": 3.302898413358527, + "angularVelocity": 0.000016292252586671315, + "velocityX": -1.6072737070336132, + "velocityY": -0.23540564842080344, + "timestamp": 5.719750460985222 + }, + { + "x": 6.539027917678609, + "y": 7.230903719736863, + "heading": 3.3028999657866516, + "angularVelocity": 0.000016291962193735624, + "velocityX": -1.607273707090324, + "velocityY": -0.23540564847991918, + "timestamp": 5.81503843834896 + }, + { + "x": 6.385874057054646, + "y": 7.208472391627575, + "heading": 3.302901518187087, + "angularVelocity": 0.000016291671606816483, + "velocityX": -1.607273707147075, + "velocityY": -0.2354056485390708, + "timestamp": 5.9103264157126985 + }, + { + "x": 6.232720196425276, + "y": 7.186041063512652, + "heading": 3.3029030705598315, + "angularVelocity": 0.00001629138100746966, + "velocityX": -1.60727370720383, + "velocityY": -0.23540564859821628, + "timestamp": 6.005614393076437 + }, + { + "x": 6.079566335790499, + "y": 7.1636097353920976, + "heading": 3.3029046229048977, + "angularVelocity": 0.000016291090533089062, + "velocityX": -1.6072737072605614, + "velocityY": -0.23540564865732222, + "timestamp": 6.100902370440175 + }, + { + "x": 5.926412475150321, + "y": 7.141178407265915, + "heading": 3.302906175222303, + "angularVelocity": 0.000016290800243766995, + "velocityX": -1.6072737073172572, + "velocityY": -0.23540564871637523, + "timestamp": 6.196190347803913 + }, + { + "x": 5.773258614504743, + "y": 7.118747079134112, + "heading": 3.3029077275120664, + "angularVelocity": 0.00001629051016274816, + "velocityX": -1.6072737073739125, + "velocityY": -0.23540564877536937, + "timestamp": 6.291478325167652 + }, + { + "x": 5.62010475385377, + "y": 7.096315750996691, + "heading": 3.302909279774206, + "angularVelocity": 0.000016290220263808567, + "velocityX": -1.6072737074305328, + "velocityY": -0.23540564883431206, + "timestamp": 6.38676630253139 + }, + { + "x": 5.466950893197405, + "y": 7.073884422853658, + "heading": 3.302910832008734, + "angularVelocity": 0.00001628993049423592, + "velocityX": -1.6072737074871284, + "velocityY": -0.23540564889321514, + "timestamp": 6.482054279895128 + }, + { + "x": 5.313797032535647, + "y": 7.051453094705014, + "heading": 3.3029123842156554, + "angularVelocity": 0.000016289640769266148, + "velocityX": -1.6072737075437156, + "velocityY": -0.23540564895209914, + "timestamp": 6.5773422572588665 + }, + { + "x": 5.160643171868496, + "y": 7.02902176655076, + "heading": 3.3029139363949653, + "angularVelocity": 0.000016289351008016845, + "velocityX": -1.6072737076003119, + "velocityY": -0.23540564901098462, + "timestamp": 6.672630234622605 + }, + { + "x": 5.0074893111959495, + "y": 7.006590438390892, + "heading": 3.302915488546652, + "angularVelocity": 0.000016289061112899286, + "velocityX": -1.6072737076569361, + "velocityY": -0.23540564906989445, + "timestamp": 6.767918211986343 + }, + { + "x": 4.854335450518003, + "y": 6.984159110225406, + "heading": 3.3029170406706947, + "angularVelocity": 0.000016288771004663956, + "velocityX": -1.607273707713604, + "velocityY": -0.23540564912884793, + "timestamp": 6.863206189350081 + }, + { + "x": 4.701181589834651, + "y": 6.961727782054296, + "heading": 3.3029185927670666, + "angularVelocity": 0.00001628848060846621, + "velocityX": -1.6072737077703305, + "velocityY": -0.235405649187863, + "timestamp": 6.95849416671382 + }, + { + "x": 4.548027729145889, + "y": 6.939296453877556, + "heading": 3.3029201448357353, + "angularVelocity": 0.000016288189883493963, + "velocityX": -1.607273707827124, + "velocityY": -0.23540564924694996, + "timestamp": 7.053782144077558 + }, + { + "x": 4.3948738684517075, + "y": 6.916865125695178, + "heading": 3.3029216968766675, + "angularVelocity": 0.00001628789879930085, + "velocityX": -1.6072737078839905, + "velocityY": -0.23540564930611646, + "timestamp": 7.149070121441296 + }, + { + "x": 4.2417200077521, + "y": 6.894433797507156, + "heading": 3.302923248889828, + "angularVelocity": 0.00001628760735122893, + "velocityX": -1.6072737079409307, + "velocityY": -0.23540564936536426, + "timestamp": 7.2443580988050345 + }, + { + "x": 4.088566147047059, + "y": 6.872002469313479, + "heading": 3.3029248008751826, + "angularVelocity": 0.000016287315540072175, + "velocityX": -1.6072737079979444, + "velocityY": -0.23540564942469375, + "timestamp": 7.339646076168773 + }, + { + "x": 3.9354122863365792, + "y": 6.849571141114141, + "heading": 3.3029263528326944, + "angularVelocity": 0.000016287023346622306, + "velocityX": -1.6072737080550357, + "velocityY": -0.23540564948410944, + "timestamp": 7.434934053532511 + }, + { + "x": 3.7822584256206495, + "y": 6.827139812909132, + "heading": 3.3029279047623197, + "angularVelocity": 0.00001628673068839883, + "velocityX": -1.6072737081122201, + "velocityY": -0.23540564954363288, + "timestamp": 7.530222030896249 + }, + { + "x": 3.629104564899259, + "y": 6.804708484698436, + "heading": 3.3029294566639957, + "angularVelocity": 0.000016286437378238427, + "velocityX": -1.607273708169536, + "velocityY": -0.2354056496033085, + "timestamp": 7.625510008259988 + }, + { + "x": 3.4759507041723876, + "y": 6.7822771564820306, + "heading": 3.302931008537628, + "angularVelocity": 0.00001628614306837342, + "velocityX": -1.6072737082270525, + "velocityY": -0.2354056496632214, + "timestamp": 7.720797985623726 + }, + { + "x": 3.3227968434400057, + "y": 6.759845828259881, + "heading": 3.3029325603830677, + "angularVelocity": 0.000016285847206848686, + "velocityX": -1.6072737082848803, + "velocityY": -0.23540564972350414, + "timestamp": 7.816085962987464 + }, + { + "x": 3.1696429827020713, + "y": 6.737414500031936, + "heading": 3.3029341122001052, + "angularVelocity": 0.00001628554913981109, + "velocityX": -1.6072737083431483, + "velocityY": -0.2354056497843278, + "timestamp": 7.9113739403512024 + }, + { + "x": 3.016489122487879, + "y": 6.714983168191867, + "heading": 3.3029356639933174, + "angularVelocity": 0.000016285299102484297, + "velocityX": -1.6072737028467396, + "velocityY": -0.23540568769178183, + "timestamp": 8.006661917714942 + }, + { + "x": 2.8768170839321456, + "y": 6.685183755290752, + "heading": 3.3517957161483998, + "angularVelocity": 0.512761982223329, + "velocityX": -1.465788679956645, + "velocityY": -0.31273003925103915, + "timestamp": 8.10194989507868 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 2.3762078534459548, + "velocityX": -0.6771226429407677, + "velocityY": -0.18518169587388544, + "timestamp": 8.19723787244242 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 4.7042853211430965e-33, + "velocityX": -2.4138813228334137e-34, + "velocityY": 0, + "timestamp": 8.29252584980616 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, diff --git a/src/main/deploy/choreo/FrontWing1.1.traj b/src/main/deploy/choreo/FrontWing1.1.traj new file mode 100644 index 00000000..090e2a15 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing1.1.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.6112520136935844e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.2701999983580183, + "y": 5.571793741558495, + "heading": 2.9123192627847865, + "angularVelocity": -2.75479375784366, + "velocityX": 0.5278042458729727, + "velocityY": -0.26883784352486495, + "timestamp": 0.08301192659671743 + }, + { + "x": 1.3598509429602765, + "y": 5.50482600027521, + "heading": 2.837562269615421, + "angularVelocity": -0.9005572600735416, + "velocityX": 1.0799766765779808, + "velocityY": -0.806724335029887, + "timestamp": 0.16602385319343485 + }, + { + "x": 1.459326562321469, + "y": 5.413786422039427, + "heading": 2.837562034760696, + "angularVelocity": -0.000002829168462845694, + "velocityX": 1.1983292454403298, + "velocityY": -1.0967047985533822, + "timestamp": 0.2490357797901523 + }, + { + "x": 1.558802177818681, + "y": 5.322746839581507, + "heading": 2.837561799906192, + "angularVelocity": -0.000002829165803381977, + "velocityX": 1.198329198893034, + "velocityY": -1.0967048494152307, + "timestamp": 0.3320477063868697 + }, + { + "x": 1.6582777933158976, + "y": 5.231707257123597, + "heading": 2.8375615650516535, + "angularVelocity": -0.0000028291662173205513, + "velocityX": 1.1983291988930937, + "velocityY": -1.0967048494150837, + "timestamp": 0.4150596329835871 + }, + { + "x": 1.7577534088131206, + "y": 5.140667674665702, + "heading": 2.837561330197081, + "angularVelocity": -0.0000028291666305712727, + "velocityX": 1.19832919889317, + "velocityY": -1.0967048494149187, + "timestamp": 0.4980715595803045 + }, + { + "x": 1.8572290243103502, + "y": 5.04962809220782, + "heading": 2.837561095342474, + "angularVelocity": -0.000002829167043337366, + "velocityX": 1.1983291988932463, + "velocityY": -1.0967048494147538, + "timestamp": 0.581083486177022 + }, + { + "x": 1.9567046398075858, + "y": 4.958588509749952, + "heading": 2.8375608604878324, + "angularVelocity": -0.000002829167457373439, + "velocityX": 1.198329198893323, + "velocityY": -1.0967048494145886, + "timestamp": 0.6640954127737394 + }, + { + "x": 2.056180255304828, + "y": 4.867548927292098, + "heading": 2.8375606256331567, + "angularVelocity": -0.00000282916787026248, + "velocityX": 1.1983291988933988, + "velocityY": -1.096704849414424, + "timestamp": 0.7471073393704568 + }, + { + "x": 2.1556558708020765, + "y": 4.776509344834258, + "heading": 2.8375603907784472, + "angularVelocity": -0.0000028291682836294088, + "velocityX": 1.1983291988934754, + "velocityY": -1.0967048494142588, + "timestamp": 0.8301192659671742 + }, + { + "x": 2.2551314862993315, + "y": 4.685469762376432, + "heading": 2.8375601559237027, + "angularVelocity": -0.000002829168697323748, + "velocityX": 1.1983291988935516, + "velocityY": -1.096704849414094, + "timestamp": 0.9131311925638916 + }, + { + "x": 2.354607101796593, + "y": 4.594430179918619, + "heading": 2.8375599210689244, + "angularVelocity": -0.000002829169110584532, + "velocityX": 1.1983291988936342, + "velocityY": -1.096704849413922, + "timestamp": 0.996143119160609 + }, + { + "x": 2.45408271880603, + "y": 4.503390599113157, + "heading": 2.837559686214013, + "angularVelocity": -0.000002829170713915412, + "velocityX": 1.1983292171099962, + "velocityY": -1.0967048295089454, + "timestamp": 1.0791550457573265 + }, + { + "x": 2.5484359060007082, + "y": 4.424785420118879, + "heading": 2.7982046114916295, + "angularVelocity": -0.47408940300319863, + "velocityX": 1.13662206218943, + "velocityY": -0.9469142834878584, + "timestamp": 1.162166972354044 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -2.3995421391526537, + "velocityX": 0.5485627867734783, + "velocityY": -0.4243619716588108, + "timestamp": 1.2451788989507615 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": 0, + "velocityX": -4.934633813021939e-37, + "velocityY": -2.767548277117811e-37, + "timestamp": 1.328190825547479 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing1.2.traj b/src/main/deploy/choreo/FrontWing1.2.traj new file mode 100644 index 00000000..d3d054b7 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing1.2.traj @@ -0,0 +1,85 @@ +{ + "samples": [ + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": 0, + "velocityX": -4.934633813021939e-37, + "velocityY": -2.767548277117811e-37, + "timestamp": 0 + }, + { + "x": 2.534366599021267, + "y": 4.442118048365455, + "heading": 2.6991662321397754, + "angularVelocity": 1.173988745719896, + "velocityX": -0.6987106220563037, + "velocityY": 0.6161074104533542, + "timestamp": 0.08530936683537149 + }, + { + "x": 2.4322324251030567, + "y": 4.53578051923557, + "heading": 2.6991662757513564, + "angularVelocity": 5.112167893680353e-7, + "velocityX": -1.1972210990067154, + "velocityY": 1.097915438182342, + "timestamp": 0.17061873367074298 + }, + { + "x": 2.330098253355979, + "y": 4.629442992473233, + "heading": 2.6991663193628423, + "angularVelocity": 5.112156854714172e-7, + "velocityX": -1.1972210735566082, + "velocityY": 1.0979154659348471, + "timestamp": 0.2559281005061145 + }, + { + "x": 2.227964081608902, + "y": 4.723105465710898, + "heading": 2.6991663629743283, + "angularVelocity": 5.112156767559466e-7, + "velocityX": -1.1972210735566007, + "velocityY": 1.0979154659348562, + "timestamp": 0.34123746734148597 + }, + { + "x": 2.1258299098618236, + "y": 4.816767938948561, + "heading": 2.6991664065858134, + "angularVelocity": 5.112156675644069e-7, + "velocityX": -1.1972210735566096, + "velocityY": 1.0979154659348476, + "timestamp": 0.42654683417685746 + }, + { + "x": 2.023695735807304, + "y": 4.910430409670039, + "heading": 2.6991664501973878, + "angularVelocity": 5.112167177081662e-7, + "velocityX": -1.1972211006045375, + "velocityY": 1.0979154364400177, + "timestamp": 0.511856201012229 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 1.1341858822337967, + "velocityX": -0.702995210451579, + "velocityY": 0.6160140441054905, + "timestamp": 0.5971655678476004 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": -1.527841446263418e-36, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0.6824749346829719 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing1.traj b/src/main/deploy/choreo/FrontWing1.traj new file mode 100644 index 00000000..e9811392 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing1.traj @@ -0,0 +1,229 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.6112520136935844e-31, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.2701999983580183, + "y": 5.571793741558495, + "heading": 2.9123192627847865, + "angularVelocity": -2.75479375784366, + "velocityX": 0.5278042458729727, + "velocityY": -0.26883784352486495, + "timestamp": 0.08301192659671743 + }, + { + "x": 1.3598509429602765, + "y": 5.50482600027521, + "heading": 2.837562269615421, + "angularVelocity": -0.9005572600735416, + "velocityX": 1.0799766765779808, + "velocityY": -0.806724335029887, + "timestamp": 0.16602385319343485 + }, + { + "x": 1.459326562321469, + "y": 5.413786422039427, + "heading": 2.837562034760696, + "angularVelocity": -0.000002829168462845694, + "velocityX": 1.1983292454403298, + "velocityY": -1.0967047985533822, + "timestamp": 0.2490357797901523 + }, + { + "x": 1.558802177818681, + "y": 5.322746839581507, + "heading": 2.837561799906192, + "angularVelocity": -0.000002829165803381977, + "velocityX": 1.198329198893034, + "velocityY": -1.0967048494152307, + "timestamp": 0.3320477063868697 + }, + { + "x": 1.6582777933158976, + "y": 5.231707257123597, + "heading": 2.8375615650516535, + "angularVelocity": -0.0000028291662173205513, + "velocityX": 1.1983291988930937, + "velocityY": -1.0967048494150837, + "timestamp": 0.4150596329835871 + }, + { + "x": 1.7577534088131206, + "y": 5.140667674665702, + "heading": 2.837561330197081, + "angularVelocity": -0.0000028291666305712727, + "velocityX": 1.19832919889317, + "velocityY": -1.0967048494149187, + "timestamp": 0.4980715595803045 + }, + { + "x": 1.8572290243103502, + "y": 5.04962809220782, + "heading": 2.837561095342474, + "angularVelocity": -0.000002829167043337366, + "velocityX": 1.1983291988932463, + "velocityY": -1.0967048494147538, + "timestamp": 0.581083486177022 + }, + { + "x": 1.9567046398075858, + "y": 4.958588509749952, + "heading": 2.8375608604878324, + "angularVelocity": -0.000002829167457373439, + "velocityX": 1.198329198893323, + "velocityY": -1.0967048494145886, + "timestamp": 0.6640954127737394 + }, + { + "x": 2.056180255304828, + "y": 4.867548927292098, + "heading": 2.8375606256331567, + "angularVelocity": -0.00000282916787026248, + "velocityX": 1.1983291988933988, + "velocityY": -1.096704849414424, + "timestamp": 0.7471073393704568 + }, + { + "x": 2.1556558708020765, + "y": 4.776509344834258, + "heading": 2.8375603907784472, + "angularVelocity": -0.0000028291682836294088, + "velocityX": 1.1983291988934754, + "velocityY": -1.0967048494142588, + "timestamp": 0.8301192659671742 + }, + { + "x": 2.2551314862993315, + "y": 4.685469762376432, + "heading": 2.8375601559237027, + "angularVelocity": -0.000002829168697323748, + "velocityX": 1.1983291988935516, + "velocityY": -1.096704849414094, + "timestamp": 0.9131311925638916 + }, + { + "x": 2.354607101796593, + "y": 4.594430179918619, + "heading": 2.8375599210689244, + "angularVelocity": -0.000002829169110584532, + "velocityX": 1.1983291988936342, + "velocityY": -1.096704849413922, + "timestamp": 0.996143119160609 + }, + { + "x": 2.45408271880603, + "y": 4.503390599113157, + "heading": 2.837559686214013, + "angularVelocity": -0.000002829170713915412, + "velocityX": 1.1983292171099962, + "velocityY": -1.0967048295089454, + "timestamp": 1.0791550457573265 + }, + { + "x": 2.5484359060007082, + "y": 4.424785420118879, + "heading": 2.7982046114916295, + "angularVelocity": -0.47408940300319863, + "velocityX": 1.13662206218943, + "velocityY": -0.9469142834878584, + "timestamp": 1.162166972354044 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -2.3995421391526537, + "velocityX": 0.5485627867734783, + "velocityY": -0.4243619716588108, + "timestamp": 1.2451788989507615 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": 0, + "velocityX": -4.934633813021939e-37, + "velocityY": -2.767548277117811e-37, + "timestamp": 1.328190825547479 + }, + { + "x": 2.534366599021267, + "y": 4.442118048365455, + "heading": 2.6991662321397754, + "angularVelocity": 1.173988745719896, + "velocityX": -0.6987106220563037, + "velocityY": 0.6161074104533542, + "timestamp": 1.4135001923828505 + }, + { + "x": 2.4322324251030567, + "y": 4.53578051923557, + "heading": 2.6991662757513564, + "angularVelocity": 5.112167893680353e-7, + "velocityX": -1.1972210990067154, + "velocityY": 1.097915438182342, + "timestamp": 1.498809559218222 + }, + { + "x": 2.330098253355979, + "y": 4.629442992473233, + "heading": 2.6991663193628423, + "angularVelocity": 5.112156854714172e-7, + "velocityX": -1.1972210735566082, + "velocityY": 1.0979154659348471, + "timestamp": 1.5841189260535935 + }, + { + "x": 2.227964081608902, + "y": 4.723105465710898, + "heading": 2.6991663629743283, + "angularVelocity": 5.112156767559466e-7, + "velocityX": -1.1972210735566007, + "velocityY": 1.0979154659348562, + "timestamp": 1.669428292888965 + }, + { + "x": 2.1258299098618236, + "y": 4.816767938948561, + "heading": 2.6991664065858134, + "angularVelocity": 5.112156675644069e-7, + "velocityX": -1.1972210735566096, + "velocityY": 1.0979154659348476, + "timestamp": 1.7547376597243365 + }, + { + "x": 2.023695735807304, + "y": 4.910430409670039, + "heading": 2.6991664501973878, + "angularVelocity": 5.112167177081662e-7, + "velocityX": -1.1972211006045375, + "velocityY": 1.0979154364400177, + "timestamp": 1.840047026559708 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 1.1341858822337967, + "velocityX": -0.702995210451579, + "velocityY": 0.6160140441054905, + "timestamp": 1.9253563933950795 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": -1.527841446263418e-36, + "velocityX": 0, + "velocityY": 0, + "timestamp": 2.010665760230451 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing2.1.traj b/src/main/deploy/choreo/FrontWing2.1.traj new file mode 100644 index 00000000..73d19d55 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing2.1.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.8084842834156656e-35, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.347146156455349, + "y": 5.593360447165824, + "heading": 3.140264869371664, + "angularVelocity": -0.007271662750997594, + "velocityX": 1.1945189788836181, + "velocityY": -0.007419158267666702, + "timestamp": 0.10109525888490682 + }, + { + "x": 1.5113646664969056, + "y": 5.59234048587755, + "heading": 3.1402648691641017, + "angularVelocity": -2.053134778232203e-9, + "velocityX": 1.6243937831794193, + "velocityY": -0.010089110998122406, + "timestamp": 0.20219051776981364 + }, + { + "x": 1.6755831765384634, + "y": 5.591320524589276, + "heading": 3.14026486895654, + "angularVelocity": -2.053134775032819e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122467, + "timestamp": 0.30328577665472045 + }, + { + "x": 1.8398016865800209, + "y": 5.5903005633010014, + "heading": 3.140264868748978, + "angularVelocity": -2.0531347455807303e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122468, + "timestamp": 0.4043810355396273 + }, + { + "x": 2.0040201966215783, + "y": 5.589280602012727, + "heading": 3.1402648685414154, + "angularVelocity": -2.0531349299385344e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.01008911099812247, + "timestamp": 0.5054762944245341 + }, + { + "x": 2.168238706663136, + "y": 5.588260640724453, + "heading": 3.1402648683338534, + "angularVelocity": -2.053134785130435e-9, + "velocityX": 1.6243937831794293, + "velocityY": -0.010089110998122468, + "timestamp": 0.6065715533094409 + }, + { + "x": 2.3324572167046935, + "y": 5.587240679436179, + "heading": 3.1402648681262915, + "angularVelocity": -2.0531346533039746e-9, + "velocityX": 1.6243937831794288, + "velocityY": -0.010089110998122467, + "timestamp": 0.7076668121943477 + }, + { + "x": 2.49667572674625, + "y": 5.586220718147905, + "heading": 3.140264867918729, + "angularVelocity": -2.0531352108819935e-9, + "velocityX": 1.624393783179419, + "velocityY": -0.010089110998122406, + "timestamp": 0.8087620710792545 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": -0.0072716627525318345, + "velocityX": 1.1945189788836168, + "velocityY": -0.007419158267749976, + "timestamp": 0.9098573299641612 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": 0, + "velocityX": -2.8085786967275447e-37, + "velocityY": 1.7440158343416895e-35, + "timestamp": 1.010952588849068 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing2.traj b/src/main/deploy/choreo/FrontWing2.traj new file mode 100644 index 00000000..73d19d55 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing2.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -1.8084842834156656e-35, + "velocityX": 0, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 1.347146156455349, + "y": 5.593360447165824, + "heading": 3.140264869371664, + "angularVelocity": -0.007271662750997594, + "velocityX": 1.1945189788836181, + "velocityY": -0.007419158267666702, + "timestamp": 0.10109525888490682 + }, + { + "x": 1.5113646664969056, + "y": 5.59234048587755, + "heading": 3.1402648691641017, + "angularVelocity": -2.053134778232203e-9, + "velocityX": 1.6243937831794193, + "velocityY": -0.010089110998122406, + "timestamp": 0.20219051776981364 + }, + { + "x": 1.6755831765384634, + "y": 5.591320524589276, + "heading": 3.14026486895654, + "angularVelocity": -2.053134775032819e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122467, + "timestamp": 0.30328577665472045 + }, + { + "x": 1.8398016865800209, + "y": 5.5903005633010014, + "heading": 3.140264868748978, + "angularVelocity": -2.0531347455807303e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.010089110998122468, + "timestamp": 0.4043810355396273 + }, + { + "x": 2.0040201966215783, + "y": 5.589280602012727, + "heading": 3.1402648685414154, + "angularVelocity": -2.0531349299385344e-9, + "velocityX": 1.624393783179429, + "velocityY": -0.01008911099812247, + "timestamp": 0.5054762944245341 + }, + { + "x": 2.168238706663136, + "y": 5.588260640724453, + "heading": 3.1402648683338534, + "angularVelocity": -2.053134785130435e-9, + "velocityX": 1.6243937831794293, + "velocityY": -0.010089110998122468, + "timestamp": 0.6065715533094409 + }, + { + "x": 2.3324572167046935, + "y": 5.587240679436179, + "heading": 3.1402648681262915, + "angularVelocity": -2.0531346533039746e-9, + "velocityX": 1.6243937831794288, + "velocityY": -0.010089110998122467, + "timestamp": 0.7076668121943477 + }, + { + "x": 2.49667572674625, + "y": 5.586220718147905, + "heading": 3.140264867918729, + "angularVelocity": -2.0531352108819935e-9, + "velocityX": 1.624393783179419, + "velocityY": -0.010089110998122406, + "timestamp": 0.8087620710792545 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": -0.0072716627525318345, + "velocityX": 1.1945189788836168, + "velocityY": -0.007419158267749976, + "timestamp": 0.9098573299641612 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": 0, + "velocityX": -2.8085786967275447e-37, + "velocityY": 1.7440158343416895e-35, + "timestamp": 1.010952588849068 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3Contested5.1.traj b/src/main/deploy/choreo/FrontWing3Contested5.1.traj new file mode 100644 index 00000000..0cd984e3 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing3Contested5.1.traj @@ -0,0 +1,166 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": 0, + "velocityX": -1.009927502555409e-33, + "velocityY": 9.510006388327946e-34, + "timestamp": 0 + }, + { + "x": 1.2629372157532939, + "y": 5.6118681655215035, + "heading": 3.3611684682063676, + "angularVelocity": 2.615104090958444, + "velocityX": 0.4341464636351002, + "velocityY": 0.21092108774289103, + "timestamp": 0.08419109165389853 + }, + { + "x": 1.3467908645165432, + "y": 5.672396843669079, + "heading": 3.470572560445472, + "angularVelocity": 1.2994734964223353, + "velocityX": 0.9959919406671149, + "velocityY": 0.718943975645346, + "timestamp": 0.16838218330779706 + }, + { + "x": 1.4464012333437837, + "y": 5.766107211983687, + "heading": 3.4705728197139094, + "angularVelocity": 0.000003079523404990731, + "velocityX": 1.1831461841203954, + "velocityY": 1.1130675048120657, + "timestamp": 0.25257327496169557 + }, + { + "x": 1.5460115900456228, + "y": 5.859817593187631, + "heading": 3.4705730789812166, + "angularVelocity": 0.000003079509987095674, + "velocityX": 1.1831460400980154, + "velocityY": 1.1130676579082395, + "timestamp": 0.3367643666155941 + }, + { + "x": 1.645621946747464, + "y": 5.953527974391563, + "heading": 3.470573338248568, + "angularVelocity": 0.000003079510511134596, + "velocityX": 1.1831460400980387, + "velocityY": 1.1130676579081111, + "timestamp": 0.42095545826949265 + }, + { + "x": 1.7452323034493133, + "y": 6.047238355595477, + "heading": 3.470573597515964, + "angularVelocity": 0.000003079511034719833, + "velocityX": 1.183146040098137, + "velocityY": 1.1130676579079029, + "timestamp": 0.5051465499233911 + }, + { + "x": 1.844842660151171, + "y": 6.140948736799373, + "heading": 3.470573856783404, + "angularVelocity": 0.0000030795115586857372, + "velocityX": 1.1831460400982357, + "velocityY": 1.1130676579076944, + "timestamp": 0.5893376415772896 + }, + { + "x": 1.944453016853037, + "y": 6.234659118003253, + "heading": 3.470574116050888, + "angularVelocity": 0.0000030795120819840736, + "velocityX": 1.1831460400983345, + "velocityY": 1.1130676579074863, + "timestamp": 0.6735287332311881 + }, + { + "x": 2.044063373554911, + "y": 6.328369499207114, + "heading": 3.470574375318416, + "angularVelocity": 0.0000030795126059487745, + "velocityX": 1.1831460400984328, + "velocityY": 1.1130676579072778, + "timestamp": 0.7577198248850866 + }, + { + "x": 2.1436737302567934, + "y": 6.4220798804109585, + "heading": 3.470574634585988, + "angularVelocity": 0.000003079513129853114, + "velocityX": 1.183146040098531, + "velocityY": 1.1130676579070695, + "timestamp": 0.8419109165389851 + }, + { + "x": 2.243284086958684, + "y": 6.515790261614786, + "heading": 3.470574893853604, + "angularVelocity": 0.0000030795136536386766, + "velocityX": 1.1831460400986302, + "velocityY": 1.1130676579068612, + "timestamp": 0.9261020081928836 + }, + { + "x": 2.342894443660583, + "y": 6.6095006428185945, + "heading": 3.4705751531212647, + "angularVelocity": 0.0000030795141771132826, + "velocityX": 1.1831460400987281, + "velocityY": 1.1130676579066532, + "timestamp": 1.010293099846782 + }, + { + "x": 2.442504800362493, + "y": 6.703211024022384, + "heading": 3.470575412388969, + "angularVelocity": 0.0000030795147010529834, + "velocityX": 1.1831460400988572, + "velocityY": 1.1130676579064127, + "timestamp": 1.0944841915006807 + }, + { + "x": 2.5421151619283058, + "y": 6.79692140005584, + "heading": 3.470575671657143, + "angularVelocity": 0.0000030795202773893193, + "velocityX": 1.1831460978710306, + "velocityY": 1.1130675964945436, + "timestamp": 1.1786752831545793 + }, + { + "x": 2.632005363421341, + "y": 6.868564036243049, + "heading": 3.5437695268680747, + "angularVelocity": 0.8693776713553536, + "velocityX": 1.0676925518743172, + "velocityY": 0.8509526932103911, + "timestamp": 1.2628663748084779 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 2.207199307384385, + "velocityX": 0.46866491549483613, + "velocityY": 0.37605700085290744, + "timestamp": 1.3470574664623765 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 0, + "velocityX": -1.5194658088032264e-34, + "velocityY": -2.7343286363371283e-33, + "timestamp": 1.431248558116275 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3Contested5.2.traj b/src/main/deploy/choreo/FrontWing3Contested5.2.traj new file mode 100644 index 00000000..17798e6e --- /dev/null +++ b/src/main/deploy/choreo/FrontWing3Contested5.2.traj @@ -0,0 +1,679 @@ +{ + "samples": [ + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 0, + "velocityX": -1.5194658088032264e-34, + "velocityY": -2.7343286363371283e-33, + "timestamp": 0 + }, + { + "x": 2.7208277247721226, + "y": 6.912165859423971, + "heading": 3.486294740771461, + "angularVelocity": -2.694751338131124, + "velocityX": 0.5467552516805291, + "velocityY": 0.13225779416916014, + "timestamp": 0.09028710806829743 + }, + { + "x": 2.839209601133149, + "y": 6.942185820162622, + "heading": 3.3899477762404344, + "angularVelocity": -1.0671176272269702, + "velocityX": 1.311171427392232, + "velocityY": 0.33249443227201864, + "timestamp": 0.18057421613659486 + }, + { + "x": 2.985101375777118, + "y": 6.957221507964457, + "heading": 3.3899472751996993, + "angularVelocity": -0.0000055494161427999685, + "velocityX": 1.615864964171956, + "velocityY": 0.16653194596133747, + "timestamp": 0.2708613242048923 + }, + { + "x": 3.130993151670898, + "y": 6.97225718364292, + "heading": 3.3899467741601925, + "angularVelocity": -0.000005549402536979108, + "velocityX": 1.6158649780145908, + "velocityY": 0.1665318116855625, + "timestamp": 0.3611484322731897 + }, + { + "x": 3.2768849275645753, + "y": 6.98729285932146, + "heading": 3.3899462731201484, + "angularVelocity": -0.00000554940849560343, + "velocityX": 1.6158649780134444, + "velocityY": 0.1665318116864077, + "timestamp": 0.45143554034148714 + }, + { + "x": 3.4227767034581484, + "y": 7.002328535000082, + "heading": 3.3899457720795656, + "angularVelocity": -0.0000055494144557147385, + "velocityX": 1.615864978012292, + "velocityY": 0.1665318116873102, + "timestamp": 0.5417226484097846 + }, + { + "x": 3.5686684793516172, + "y": 7.017364210678785, + "heading": 3.3899452710384446, + "angularVelocity": -0.0000055494204159770735, + "velocityX": 1.6158649780111394, + "velocityY": 0.1665318116882126, + "timestamp": 0.632009756478082 + }, + { + "x": 3.7145602552449817, + "y": 7.03239988635757, + "heading": 3.3899447699967857, + "angularVelocity": -0.000005549426375721003, + "velocityX": 1.6158649780099874, + "velocityY": 0.1665318116891151, + "timestamp": 0.7222968645463794 + }, + { + "x": 3.8604520311382426, + "y": 7.047435562036436, + "heading": 3.3899442689545887, + "angularVelocity": -0.000005549432334941507, + "velocityX": 1.615864978008835, + "velocityY": 0.16653181169001743, + "timestamp": 0.8125839726146769 + }, + { + "x": 4.006343807031399, + "y": 7.062471237715384, + "heading": 3.389943767911854, + "angularVelocity": -0.000005549438294605419, + "velocityX": 1.6158649780076826, + "velocityY": 0.16653181169091993, + "timestamp": 0.9028710806829743 + }, + { + "x": 4.152235582924452, + "y": 7.077506913394413, + "heading": 3.3899432668685807, + "angularVelocity": -0.000005549444254929096, + "velocityX": 1.6158649780065304, + "velocityY": 0.16653181169182238, + "timestamp": 0.9931581887512717 + }, + { + "x": 4.298127358817401, + "y": 7.092542589073523, + "heading": 3.3899427658247694, + "angularVelocity": -0.000005549450214439604, + "velocityX": 1.615864978005378, + "velocityY": 0.1665318116927247, + "timestamp": 1.0834452968195691 + }, + { + "x": 4.444019134710246, + "y": 7.107578264752715, + "heading": 3.38994226478042, + "angularVelocity": -0.000005549456174760312, + "velocityX": 1.6158649780042256, + "velocityY": 0.16653181169362713, + "timestamp": 1.1737324048878666 + }, + { + "x": 4.5899109106029865, + "y": 7.122613940431989, + "heading": 3.389941763735532, + "angularVelocity": -0.000005549462134716656, + "velocityX": 1.6158649780030732, + "velocityY": 0.16653181169452963, + "timestamp": 1.264019512956164 + }, + { + "x": 4.735802686495623, + "y": 7.137649616111343, + "heading": 3.389941262690107, + "angularVelocity": -0.0000055494680950242725, + "velocityX": 1.6158649780019205, + "velocityY": 0.1665318116954319, + "timestamp": 1.3543066210244614 + }, + { + "x": 4.881694462388156, + "y": 7.15268529179078, + "heading": 3.389940761644143, + "angularVelocity": -0.000005549474055578583, + "velocityX": 1.6158649780007681, + "velocityY": 0.16653181169633444, + "timestamp": 1.4445937290927588 + }, + { + "x": 5.027586238280584, + "y": 7.167720967470297, + "heading": 3.3899402605976414, + "angularVelocity": -0.000005549480015574405, + "velocityX": 1.6158649779996155, + "velocityY": 0.1665318116972368, + "timestamp": 1.5348808371610563 + }, + { + "x": 5.173478014172908, + "y": 7.182756643149896, + "heading": 3.389939759550601, + "angularVelocity": -0.000005549485976020103, + "velocityX": 1.6158649779984633, + "velocityY": 0.16653181169813924, + "timestamp": 1.6251679452293537 + }, + { + "x": 5.319369790065129, + "y": 7.197792318829577, + "heading": 3.389939258503023, + "angularVelocity": -0.000005549491936617687, + "velocityX": 1.6158649779973107, + "velocityY": 0.16653181169904166, + "timestamp": 1.7154550532976511 + }, + { + "x": 5.4652615659572445, + "y": 7.212827994509339, + "heading": 3.3899387574549067, + "angularVelocity": -0.00000554949789662324, + "velocityX": 1.6158649779961582, + "velocityY": 0.1665318116999441, + "timestamp": 1.8057421613659486 + }, + { + "x": 5.611153341849257, + "y": 7.2278636701891825, + "heading": 3.389938256406252, + "angularVelocity": -0.000005549503857497513, + "velocityX": 1.6158649779950054, + "velocityY": 0.16653181170084652, + "timestamp": 1.896029269434246 + }, + { + "x": 5.757045117741165, + "y": 7.242899345869108, + "heading": 3.3899377553570593, + "angularVelocity": -0.000005549509818264508, + "velocityX": 1.615864977993853, + "velocityY": 0.16653181170174894, + "timestamp": 1.9863163775025434 + }, + { + "x": 5.902936893632969, + "y": 7.257935021549114, + "heading": 3.3899372543073283, + "angularVelocity": -0.000005549515778536107, + "velocityX": 1.6158649779927006, + "velocityY": 0.1665318117026513, + "timestamp": 2.076603485570841 + }, + { + "x": 6.048828669524668, + "y": 7.272970697229202, + "heading": 3.3899367532570595, + "angularVelocity": -0.000005549521739714091, + "velocityX": 1.6158649779915477, + "velocityY": 0.16653181170355377, + "timestamp": 2.1668905936391383 + }, + { + "x": 6.194720445416264, + "y": 7.288006372909372, + "heading": 3.3899362522062524, + "angularVelocity": -0.000005549527700309559, + "velocityX": 1.6158649779903953, + "velocityY": 0.16653181170445627, + "timestamp": 2.2571777017074357 + }, + { + "x": 6.340612221307755, + "y": 7.303042048589623, + "heading": 3.389935751154907, + "angularVelocity": -0.000005549533660762894, + "velocityX": 1.6158649779892427, + "velocityY": 0.16653181170535863, + "timestamp": 2.347464809775733 + }, + { + "x": 6.486503997199144, + "y": 7.318077724269955, + "heading": 3.3899352501030235, + "angularVelocity": -0.000005549539621393899, + "velocityX": 1.6158649779880898, + "velocityY": 0.16653181170626094, + "timestamp": 2.4377519178440306 + }, + { + "x": 6.632395773090427, + "y": 7.333113399950369, + "heading": 3.3899347490506018, + "angularVelocity": -0.00000554954558299688, + "velocityX": 1.6158649779869374, + "velocityY": 0.1665318117071635, + "timestamp": 2.528039025912328 + }, + { + "x": 6.778287548981607, + "y": 7.3481490756308645, + "heading": 3.3899342479976413, + "angularVelocity": -0.000005549551543569729, + "velocityX": 1.615864977985785, + "velocityY": 0.16653181170806597, + "timestamp": 2.6183261339806254 + }, + { + "x": 6.924179324872682, + "y": 7.363184751311442, + "heading": 3.3899337469441435, + "angularVelocity": -0.000005549557505439446, + "velocityX": 1.615864977984632, + "velocityY": 0.16653181170896839, + "timestamp": 2.708613242048923 + }, + { + "x": 7.070071100763654, + "y": 7.378220426992101, + "heading": 3.389933245890107, + "angularVelocity": -0.00000554956346606953, + "velocityX": 1.6158649779834795, + "velocityY": 0.1665318117098707, + "timestamp": 2.7989003501172203 + }, + { + "x": 7.215962876654522, + "y": 7.393256102672841, + "heading": 3.3899327448355323, + "angularVelocity": -0.000005549569427174084, + "velocityX": 1.6158649779823266, + "velocityY": 0.16653181171077316, + "timestamp": 2.8891874581855177 + }, + { + "x": 7.361854652545285, + "y": 7.408291778353663, + "heading": 3.3899322437804194, + "angularVelocity": -0.000005549575389475234, + "velocityX": 1.615864977981174, + "velocityY": 0.16653181171167572, + "timestamp": 2.979474566253815 + }, + { + "x": 7.5077464284359445, + "y": 7.423327454034565, + "heading": 3.389931742724768, + "angularVelocity": -0.000005549581350385169, + "velocityX": 1.6158649779800212, + "velocityY": 0.1665318117125781, + "timestamp": 3.0697616743221126 + }, + { + "x": 7.6536382043265, + "y": 7.43836312971555, + "heading": 3.3899312416685787, + "angularVelocity": -0.000005549587311867374, + "velocityX": 1.6158649779788683, + "velocityY": 0.166531811713483, + "timestamp": 3.16004878239041 + }, + { + "x": 7.799529980161501, + "y": 7.453398805927109, + "heading": 3.3899307406089574, + "angularVelocity": -0.000005549625325450788, + "velocityX": 1.6158649773635676, + "velocityY": 0.16653181759000288, + "timestamp": 3.2503358904587074 + }, + { + "x": 7.908273189210625, + "y": 7.465055953097054, + "heading": 3.2483837637195303, + "angularVelocity": -1.5677429471143751, + "velocityX": 1.204415684317477, + "velocityY": 0.12911197865732152, + "timestamp": 3.340622998527005 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "angularVelocity": -1.3108321012839574, + "velocityX": 0.3122914163117384, + "velocityY": -0.13619538226979142, + "timestamp": 3.4309101065953023 + }, + { + "x": 7.897313987574637, + "y": 7.4320035366195905, + "heading": 3.223159902402127, + "angularVelocity": 0.9773255855261127, + "velocityX": -0.4109132292719665, + "velocityY": -0.21782107097140077, + "timestamp": 3.5261980839590406 + }, + { + "x": 7.764258802337476, + "y": 7.410354345348057, + "heading": 3.302887545584371, + "angularVelocity": 0.8367020204228218, + "velocityX": -1.396348090475833, + "velocityY": -0.22719751085588358, + "timestamp": 3.621486061322779 + }, + { + "x": 7.611104941895525, + "y": 7.387923016344696, + "heading": 3.302889098211225, + "angularVelocity": 0.000016294047758499195, + "velocityX": -1.607273705236954, + "velocityY": -0.23540565792192894, + "timestamp": 3.716774038686517 + }, + { + "x": 7.457951081309142, + "y": 7.365491688274562, + "heading": 3.302890650804134, + "angularVelocity": 0.000016293691520657764, + "velocityX": -1.6072737067526932, + "velocityY": -0.2354056481281831, + "timestamp": 3.8120620160502554 + }, + { + "x": 7.30479722071746, + "y": 7.343060360198914, + "heading": 3.3028922033698884, + "angularVelocity": 0.000016293406553711894, + "velocityX": -1.6072737068083087, + "velocityY": -0.23540564818605267, + "timestamp": 3.9073499934139937 + }, + { + "x": 7.151643360120445, + "y": 7.320629032117711, + "heading": 3.302893755908325, + "angularVelocity": 0.00001629311986139254, + "velocityX": -1.6072737068642717, + "velocityY": -0.23540564824432697, + "timestamp": 4.002637970777732 + }, + { + "x": 6.998489499518071, + "y": 7.298197704030925, + "heading": 3.302895308419307, + "angularVelocity": 0.00001629283173826444, + "velocityX": -1.6072737069205232, + "velocityY": -0.2354056483029315, + "timestamp": 4.09792594814147 + }, + { + "x": 6.845335638910315, + "y": 7.275766375938532, + "heading": 3.3028968609027314, + "angularVelocity": 0.000016292542536041963, + "velocityX": -1.6072737069769925, + "velocityY": -0.23540564836178338, + "timestamp": 4.1932139255052086 + }, + { + "x": 6.692181778297163, + "y": 7.253335047840514, + "heading": 3.302898413358527, + "angularVelocity": 0.000016292252586671315, + "velocityX": -1.6072737070336132, + "velocityY": -0.23540564842080344, + "timestamp": 4.288501902868947 + }, + { + "x": 6.539027917678609, + "y": 7.230903719736863, + "heading": 3.3028999657866516, + "angularVelocity": 0.000016291962193735624, + "velocityX": -1.607273707090324, + "velocityY": -0.23540564847991918, + "timestamp": 4.383789880232685 + }, + { + "x": 6.385874057054646, + "y": 7.208472391627575, + "heading": 3.302901518187087, + "angularVelocity": 0.000016291671606816483, + "velocityX": -1.607273707147075, + "velocityY": -0.2354056485390708, + "timestamp": 4.479077857596423 + }, + { + "x": 6.232720196425276, + "y": 7.186041063512652, + "heading": 3.3029030705598315, + "angularVelocity": 0.00001629138100746966, + "velocityX": -1.60727370720383, + "velocityY": -0.23540564859821628, + "timestamp": 4.574365834960162 + }, + { + "x": 6.079566335790499, + "y": 7.1636097353920976, + "heading": 3.3029046229048977, + "angularVelocity": 0.000016291090533089062, + "velocityX": -1.6072737072605614, + "velocityY": -0.23540564865732222, + "timestamp": 4.6696538123239 + }, + { + "x": 5.926412475150321, + "y": 7.141178407265915, + "heading": 3.302906175222303, + "angularVelocity": 0.000016290800243766995, + "velocityX": -1.6072737073172572, + "velocityY": -0.23540564871637523, + "timestamp": 4.764941789687638 + }, + { + "x": 5.773258614504743, + "y": 7.118747079134112, + "heading": 3.3029077275120664, + "angularVelocity": 0.00001629051016274816, + "velocityX": -1.6072737073739125, + "velocityY": -0.23540564877536937, + "timestamp": 4.8602297670513765 + }, + { + "x": 5.62010475385377, + "y": 7.096315750996691, + "heading": 3.302909279774206, + "angularVelocity": 0.000016290220263808567, + "velocityX": -1.6072737074305328, + "velocityY": -0.23540564883431206, + "timestamp": 4.955517744415115 + }, + { + "x": 5.466950893197405, + "y": 7.073884422853658, + "heading": 3.302910832008734, + "angularVelocity": 0.00001628993049423592, + "velocityX": -1.6072737074871284, + "velocityY": -0.23540564889321514, + "timestamp": 5.050805721778853 + }, + { + "x": 5.313797032535647, + "y": 7.051453094705014, + "heading": 3.3029123842156554, + "angularVelocity": 0.000016289640769266148, + "velocityX": -1.6072737075437156, + "velocityY": -0.23540564895209914, + "timestamp": 5.146093699142591 + }, + { + "x": 5.160643171868496, + "y": 7.02902176655076, + "heading": 3.3029139363949653, + "angularVelocity": 0.000016289351008016845, + "velocityX": -1.6072737076003119, + "velocityY": -0.23540564901098462, + "timestamp": 5.24138167650633 + }, + { + "x": 5.0074893111959495, + "y": 7.006590438390892, + "heading": 3.302915488546652, + "angularVelocity": 0.000016289061112899286, + "velocityX": -1.6072737076569361, + "velocityY": -0.23540564906989445, + "timestamp": 5.336669653870068 + }, + { + "x": 4.854335450518003, + "y": 6.984159110225406, + "heading": 3.3029170406706947, + "angularVelocity": 0.000016288771004663956, + "velocityX": -1.607273707713604, + "velocityY": -0.23540564912884793, + "timestamp": 5.431957631233806 + }, + { + "x": 4.701181589834651, + "y": 6.961727782054296, + "heading": 3.3029185927670666, + "angularVelocity": 0.00001628848060846621, + "velocityX": -1.6072737077703305, + "velocityY": -0.235405649187863, + "timestamp": 5.5272456085975445 + }, + { + "x": 4.548027729145889, + "y": 6.939296453877556, + "heading": 3.3029201448357353, + "angularVelocity": 0.000016288189883493963, + "velocityX": -1.607273707827124, + "velocityY": -0.23540564924694996, + "timestamp": 5.622533585961283 + }, + { + "x": 4.3948738684517075, + "y": 6.916865125695178, + "heading": 3.3029216968766675, + "angularVelocity": 0.00001628789879930085, + "velocityX": -1.6072737078839905, + "velocityY": -0.23540564930611646, + "timestamp": 5.717821563325021 + }, + { + "x": 4.2417200077521, + "y": 6.894433797507156, + "heading": 3.302923248889828, + "angularVelocity": 0.00001628760735122893, + "velocityX": -1.6072737079409307, + "velocityY": -0.23540564936536426, + "timestamp": 5.813109540688759 + }, + { + "x": 4.088566147047059, + "y": 6.872002469313479, + "heading": 3.3029248008751826, + "angularVelocity": 0.000016287315540072175, + "velocityX": -1.6072737079979444, + "velocityY": -0.23540564942469375, + "timestamp": 5.908397518052498 + }, + { + "x": 3.9354122863365792, + "y": 6.849571141114141, + "heading": 3.3029263528326944, + "angularVelocity": 0.000016287023346622306, + "velocityX": -1.6072737080550357, + "velocityY": -0.23540564948410944, + "timestamp": 6.003685495416236 + }, + { + "x": 3.7822584256206495, + "y": 6.827139812909132, + "heading": 3.3029279047623197, + "angularVelocity": 0.00001628673068839883, + "velocityX": -1.6072737081122201, + "velocityY": -0.23540564954363288, + "timestamp": 6.098973472779974 + }, + { + "x": 3.629104564899259, + "y": 6.804708484698436, + "heading": 3.3029294566639957, + "angularVelocity": 0.000016286437378238427, + "velocityX": -1.607273708169536, + "velocityY": -0.2354056496033085, + "timestamp": 6.1942614501437125 + }, + { + "x": 3.4759507041723876, + "y": 6.7822771564820306, + "heading": 3.302931008537628, + "angularVelocity": 0.00001628614306837342, + "velocityX": -1.6072737082270525, + "velocityY": -0.2354056496632214, + "timestamp": 6.289549427507451 + }, + { + "x": 3.3227968434400057, + "y": 6.759845828259881, + "heading": 3.3029325603830677, + "angularVelocity": 0.000016285847206848686, + "velocityX": -1.6072737082848803, + "velocityY": -0.23540564972350414, + "timestamp": 6.384837404871189 + }, + { + "x": 3.1696429827020713, + "y": 6.737414500031936, + "heading": 3.3029341122001052, + "angularVelocity": 0.00001628554913981109, + "velocityX": -1.6072737083431483, + "velocityY": -0.2354056497843278, + "timestamp": 6.480125382234927 + }, + { + "x": 3.016489122487879, + "y": 6.714983168191867, + "heading": 3.3029356639933174, + "angularVelocity": 0.000016285299102484297, + "velocityX": -1.6072737028467396, + "velocityY": -0.23540568769178183, + "timestamp": 6.575413359598667 + }, + { + "x": 2.8768170839321456, + "y": 6.685183755290752, + "heading": 3.3517957161483998, + "angularVelocity": 0.512761982223329, + "velocityX": -1.465788679956645, + "velocityY": -0.31273003925103915, + "timestamp": 6.670701336962406 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 2.3762078534459548, + "velocityX": -0.6771226429407677, + "velocityY": -0.18518169587388544, + "timestamp": 6.765989314326145 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 4.7042853211430965e-33, + "velocityX": -2.4138813228334137e-34, + "velocityY": 0, + "timestamp": 6.861277291689884 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3Contested5.traj b/src/main/deploy/choreo/FrontWing3Contested5.traj new file mode 100644 index 00000000..c412d812 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing3Contested5.traj @@ -0,0 +1,832 @@ +{ + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": 0, + "velocityX": -1.009927502555409e-33, + "velocityY": 9.510006388327946e-34, + "timestamp": 0 + }, + { + "x": 1.2629372157532939, + "y": 5.6118681655215035, + "heading": 3.3611684682063676, + "angularVelocity": 2.615104090958444, + "velocityX": 0.4341464636351002, + "velocityY": 0.21092108774289103, + "timestamp": 0.08419109165389853 + }, + { + "x": 1.3467908645165432, + "y": 5.672396843669079, + "heading": 3.470572560445472, + "angularVelocity": 1.2994734964223353, + "velocityX": 0.9959919406671149, + "velocityY": 0.718943975645346, + "timestamp": 0.16838218330779706 + }, + { + "x": 1.4464012333437837, + "y": 5.766107211983687, + "heading": 3.4705728197139094, + "angularVelocity": 0.000003079523404990731, + "velocityX": 1.1831461841203954, + "velocityY": 1.1130675048120657, + "timestamp": 0.25257327496169557 + }, + { + "x": 1.5460115900456228, + "y": 5.859817593187631, + "heading": 3.4705730789812166, + "angularVelocity": 0.000003079509987095674, + "velocityX": 1.1831460400980154, + "velocityY": 1.1130676579082395, + "timestamp": 0.3367643666155941 + }, + { + "x": 1.645621946747464, + "y": 5.953527974391563, + "heading": 3.470573338248568, + "angularVelocity": 0.000003079510511134596, + "velocityX": 1.1831460400980387, + "velocityY": 1.1130676579081111, + "timestamp": 0.42095545826949265 + }, + { + "x": 1.7452323034493133, + "y": 6.047238355595477, + "heading": 3.470573597515964, + "angularVelocity": 0.000003079511034719833, + "velocityX": 1.183146040098137, + "velocityY": 1.1130676579079029, + "timestamp": 0.5051465499233911 + }, + { + "x": 1.844842660151171, + "y": 6.140948736799373, + "heading": 3.470573856783404, + "angularVelocity": 0.0000030795115586857372, + "velocityX": 1.1831460400982357, + "velocityY": 1.1130676579076944, + "timestamp": 0.5893376415772896 + }, + { + "x": 1.944453016853037, + "y": 6.234659118003253, + "heading": 3.470574116050888, + "angularVelocity": 0.0000030795120819840736, + "velocityX": 1.1831460400983345, + "velocityY": 1.1130676579074863, + "timestamp": 0.6735287332311881 + }, + { + "x": 2.044063373554911, + "y": 6.328369499207114, + "heading": 3.470574375318416, + "angularVelocity": 0.0000030795126059487745, + "velocityX": 1.1831460400984328, + "velocityY": 1.1130676579072778, + "timestamp": 0.7577198248850866 + }, + { + "x": 2.1436737302567934, + "y": 6.4220798804109585, + "heading": 3.470574634585988, + "angularVelocity": 0.000003079513129853114, + "velocityX": 1.183146040098531, + "velocityY": 1.1130676579070695, + "timestamp": 0.8419109165389851 + }, + { + "x": 2.243284086958684, + "y": 6.515790261614786, + "heading": 3.470574893853604, + "angularVelocity": 0.0000030795136536386766, + "velocityX": 1.1831460400986302, + "velocityY": 1.1130676579068612, + "timestamp": 0.9261020081928836 + }, + { + "x": 2.342894443660583, + "y": 6.6095006428185945, + "heading": 3.4705751531212647, + "angularVelocity": 0.0000030795141771132826, + "velocityX": 1.1831460400987281, + "velocityY": 1.1130676579066532, + "timestamp": 1.010293099846782 + }, + { + "x": 2.442504800362493, + "y": 6.703211024022384, + "heading": 3.470575412388969, + "angularVelocity": 0.0000030795147010529834, + "velocityX": 1.1831460400988572, + "velocityY": 1.1130676579064127, + "timestamp": 1.0944841915006807 + }, + { + "x": 2.5421151619283058, + "y": 6.79692140005584, + "heading": 3.470575671657143, + "angularVelocity": 0.0000030795202773893193, + "velocityX": 1.1831460978710306, + "velocityY": 1.1130675964945436, + "timestamp": 1.1786752831545793 + }, + { + "x": 2.632005363421341, + "y": 6.868564036243049, + "heading": 3.5437695268680747, + "angularVelocity": 0.8693776713553536, + "velocityX": 1.0676925518743172, + "velocityY": 0.8509526932103911, + "timestamp": 1.2628663748084779 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 2.207199307384385, + "velocityX": 0.46866491549483613, + "velocityY": 0.37605700085290744, + "timestamp": 1.3470574664623765 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 0, + "velocityX": -1.5194658088032264e-34, + "velocityY": -2.7343286363371283e-33, + "timestamp": 1.431248558116275 + }, + { + "x": 2.7208277247721226, + "y": 6.912165859423971, + "heading": 3.486294740771461, + "angularVelocity": -2.694751338131124, + "velocityX": 0.5467552516805291, + "velocityY": 0.13225779416916014, + "timestamp": 1.5215356661845725 + }, + { + "x": 2.839209601133149, + "y": 6.942185820162622, + "heading": 3.3899477762404344, + "angularVelocity": -1.0671176272269702, + "velocityX": 1.311171427392232, + "velocityY": 0.33249443227201864, + "timestamp": 1.61182277425287 + }, + { + "x": 2.985101375777118, + "y": 6.957221507964457, + "heading": 3.3899472751996993, + "angularVelocity": -0.0000055494161427999685, + "velocityX": 1.615864964171956, + "velocityY": 0.16653194596133747, + "timestamp": 1.7021098823211673 + }, + { + "x": 3.130993151670898, + "y": 6.97225718364292, + "heading": 3.3899467741601925, + "angularVelocity": -0.000005549402536979108, + "velocityX": 1.6158649780145908, + "velocityY": 0.1665318116855625, + "timestamp": 1.7923969903894648 + }, + { + "x": 3.2768849275645753, + "y": 6.98729285932146, + "heading": 3.3899462731201484, + "angularVelocity": -0.00000554940849560343, + "velocityX": 1.6158649780134444, + "velocityY": 0.1665318116864077, + "timestamp": 1.8826840984577622 + }, + { + "x": 3.4227767034581484, + "y": 7.002328535000082, + "heading": 3.3899457720795656, + "angularVelocity": -0.0000055494144557147385, + "velocityX": 1.615864978012292, + "velocityY": 0.1665318116873102, + "timestamp": 1.9729712065260596 + }, + { + "x": 3.5686684793516172, + "y": 7.017364210678785, + "heading": 3.3899452710384446, + "angularVelocity": -0.0000055494204159770735, + "velocityX": 1.6158649780111394, + "velocityY": 0.1665318116882126, + "timestamp": 2.063258314594357 + }, + { + "x": 3.7145602552449817, + "y": 7.03239988635757, + "heading": 3.3899447699967857, + "angularVelocity": -0.000005549426375721003, + "velocityX": 1.6158649780099874, + "velocityY": 0.1665318116891151, + "timestamp": 2.1535454226626545 + }, + { + "x": 3.8604520311382426, + "y": 7.047435562036436, + "heading": 3.3899442689545887, + "angularVelocity": -0.000005549432334941507, + "velocityX": 1.615864978008835, + "velocityY": 0.16653181169001743, + "timestamp": 2.243832530730952 + }, + { + "x": 4.006343807031399, + "y": 7.062471237715384, + "heading": 3.389943767911854, + "angularVelocity": -0.000005549438294605419, + "velocityX": 1.6158649780076826, + "velocityY": 0.16653181169091993, + "timestamp": 2.3341196387992493 + }, + { + "x": 4.152235582924452, + "y": 7.077506913394413, + "heading": 3.3899432668685807, + "angularVelocity": -0.000005549444254929096, + "velocityX": 1.6158649780065304, + "velocityY": 0.16653181169182238, + "timestamp": 2.4244067468675468 + }, + { + "x": 4.298127358817401, + "y": 7.092542589073523, + "heading": 3.3899427658247694, + "angularVelocity": -0.000005549450214439604, + "velocityX": 1.615864978005378, + "velocityY": 0.1665318116927247, + "timestamp": 2.514693854935844 + }, + { + "x": 4.444019134710246, + "y": 7.107578264752715, + "heading": 3.38994226478042, + "angularVelocity": -0.000005549456174760312, + "velocityX": 1.6158649780042256, + "velocityY": 0.16653181169362713, + "timestamp": 2.6049809630041416 + }, + { + "x": 4.5899109106029865, + "y": 7.122613940431989, + "heading": 3.389941763735532, + "angularVelocity": -0.000005549462134716656, + "velocityX": 1.6158649780030732, + "velocityY": 0.16653181169452963, + "timestamp": 2.695268071072439 + }, + { + "x": 4.735802686495623, + "y": 7.137649616111343, + "heading": 3.389941262690107, + "angularVelocity": -0.0000055494680950242725, + "velocityX": 1.6158649780019205, + "velocityY": 0.1665318116954319, + "timestamp": 2.7855551791407365 + }, + { + "x": 4.881694462388156, + "y": 7.15268529179078, + "heading": 3.389940761644143, + "angularVelocity": -0.000005549474055578583, + "velocityX": 1.6158649780007681, + "velocityY": 0.16653181169633444, + "timestamp": 2.875842287209034 + }, + { + "x": 5.027586238280584, + "y": 7.167720967470297, + "heading": 3.3899402605976414, + "angularVelocity": -0.000005549480015574405, + "velocityX": 1.6158649779996155, + "velocityY": 0.1665318116972368, + "timestamp": 2.9661293952773313 + }, + { + "x": 5.173478014172908, + "y": 7.182756643149896, + "heading": 3.389939759550601, + "angularVelocity": -0.000005549485976020103, + "velocityX": 1.6158649779984633, + "velocityY": 0.16653181169813924, + "timestamp": 3.0564165033456288 + }, + { + "x": 5.319369790065129, + "y": 7.197792318829577, + "heading": 3.389939258503023, + "angularVelocity": -0.000005549491936617687, + "velocityX": 1.6158649779973107, + "velocityY": 0.16653181169904166, + "timestamp": 3.146703611413926 + }, + { + "x": 5.4652615659572445, + "y": 7.212827994509339, + "heading": 3.3899387574549067, + "angularVelocity": -0.00000554949789662324, + "velocityX": 1.6158649779961582, + "velocityY": 0.1665318116999441, + "timestamp": 3.2369907194822236 + }, + { + "x": 5.611153341849257, + "y": 7.2278636701891825, + "heading": 3.389938256406252, + "angularVelocity": -0.000005549503857497513, + "velocityX": 1.6158649779950054, + "velocityY": 0.16653181170084652, + "timestamp": 3.327277827550521 + }, + { + "x": 5.757045117741165, + "y": 7.242899345869108, + "heading": 3.3899377553570593, + "angularVelocity": -0.000005549509818264508, + "velocityX": 1.615864977993853, + "velocityY": 0.16653181170174894, + "timestamp": 3.4175649356188185 + }, + { + "x": 5.902936893632969, + "y": 7.257935021549114, + "heading": 3.3899372543073283, + "angularVelocity": -0.000005549515778536107, + "velocityX": 1.6158649779927006, + "velocityY": 0.1665318117026513, + "timestamp": 3.507852043687116 + }, + { + "x": 6.048828669524668, + "y": 7.272970697229202, + "heading": 3.3899367532570595, + "angularVelocity": -0.000005549521739714091, + "velocityX": 1.6158649779915477, + "velocityY": 0.16653181170355377, + "timestamp": 3.5981391517554133 + }, + { + "x": 6.194720445416264, + "y": 7.288006372909372, + "heading": 3.3899362522062524, + "angularVelocity": -0.000005549527700309559, + "velocityX": 1.6158649779903953, + "velocityY": 0.16653181170445627, + "timestamp": 3.6884262598237108 + }, + { + "x": 6.340612221307755, + "y": 7.303042048589623, + "heading": 3.389935751154907, + "angularVelocity": -0.000005549533660762894, + "velocityX": 1.6158649779892427, + "velocityY": 0.16653181170535863, + "timestamp": 3.778713367892008 + }, + { + "x": 6.486503997199144, + "y": 7.318077724269955, + "heading": 3.3899352501030235, + "angularVelocity": -0.000005549539621393899, + "velocityX": 1.6158649779880898, + "velocityY": 0.16653181170626094, + "timestamp": 3.8690004759603056 + }, + { + "x": 6.632395773090427, + "y": 7.333113399950369, + "heading": 3.3899347490506018, + "angularVelocity": -0.00000554954558299688, + "velocityX": 1.6158649779869374, + "velocityY": 0.1665318117071635, + "timestamp": 3.959287584028603 + }, + { + "x": 6.778287548981607, + "y": 7.3481490756308645, + "heading": 3.3899342479976413, + "angularVelocity": -0.000005549551543569729, + "velocityX": 1.615864977985785, + "velocityY": 0.16653181170806597, + "timestamp": 4.0495746920969005 + }, + { + "x": 6.924179324872682, + "y": 7.363184751311442, + "heading": 3.3899337469441435, + "angularVelocity": -0.000005549557505439446, + "velocityX": 1.615864977984632, + "velocityY": 0.16653181170896839, + "timestamp": 4.139861800165198 + }, + { + "x": 7.070071100763654, + "y": 7.378220426992101, + "heading": 3.389933245890107, + "angularVelocity": -0.00000554956346606953, + "velocityX": 1.6158649779834795, + "velocityY": 0.1665318117098707, + "timestamp": 4.230148908233495 + }, + { + "x": 7.215962876654522, + "y": 7.393256102672841, + "heading": 3.3899327448355323, + "angularVelocity": -0.000005549569427174084, + "velocityX": 1.6158649779823266, + "velocityY": 0.16653181171077316, + "timestamp": 4.320436016301793 + }, + { + "x": 7.361854652545285, + "y": 7.408291778353663, + "heading": 3.3899322437804194, + "angularVelocity": -0.000005549575389475234, + "velocityX": 1.615864977981174, + "velocityY": 0.16653181171167572, + "timestamp": 4.41072312437009 + }, + { + "x": 7.5077464284359445, + "y": 7.423327454034565, + "heading": 3.389931742724768, + "angularVelocity": -0.000005549581350385169, + "velocityX": 1.6158649779800212, + "velocityY": 0.1665318117125781, + "timestamp": 4.501010232438388 + }, + { + "x": 7.6536382043265, + "y": 7.43836312971555, + "heading": 3.3899312416685787, + "angularVelocity": -0.000005549587311867374, + "velocityX": 1.6158649779788683, + "velocityY": 0.166531811713483, + "timestamp": 4.591297340506685 + }, + { + "x": 7.799529980161501, + "y": 7.453398805927109, + "heading": 3.3899307406089574, + "angularVelocity": -0.000005549625325450788, + "velocityX": 1.6158649773635676, + "velocityY": 0.16653181759000288, + "timestamp": 4.6815844485749825 + }, + { + "x": 7.908273189210625, + "y": 7.465055953097054, + "heading": 3.2483837637195303, + "angularVelocity": -1.5677429471143751, + "velocityX": 1.204415684317477, + "velocityY": 0.12911197865732152, + "timestamp": 4.77187155664328 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "angularVelocity": -1.3108321012839574, + "velocityX": 0.3122914163117384, + "velocityY": -0.13619538226979142, + "timestamp": 4.862158664711577 + }, + { + "x": 7.897313987574637, + "y": 7.4320035366195905, + "heading": 3.223159902402127, + "angularVelocity": 0.9773255855261127, + "velocityX": -0.4109132292719665, + "velocityY": -0.21782107097140077, + "timestamp": 4.957446642075316 + }, + { + "x": 7.764258802337476, + "y": 7.410354345348057, + "heading": 3.302887545584371, + "angularVelocity": 0.8367020204228218, + "velocityX": -1.396348090475833, + "velocityY": -0.22719751085588358, + "timestamp": 5.052734619439054 + }, + { + "x": 7.611104941895525, + "y": 7.387923016344696, + "heading": 3.302889098211225, + "angularVelocity": 0.000016294047758499195, + "velocityX": -1.607273705236954, + "velocityY": -0.23540565792192894, + "timestamp": 5.148022596802792 + }, + { + "x": 7.457951081309142, + "y": 7.365491688274562, + "heading": 3.302890650804134, + "angularVelocity": 0.000016293691520657764, + "velocityX": -1.6072737067526932, + "velocityY": -0.2354056481281831, + "timestamp": 5.2433105741665305 + }, + { + "x": 7.30479722071746, + "y": 7.343060360198914, + "heading": 3.3028922033698884, + "angularVelocity": 0.000016293406553711894, + "velocityX": -1.6072737068083087, + "velocityY": -0.23540564818605267, + "timestamp": 5.338598551530269 + }, + { + "x": 7.151643360120445, + "y": 7.320629032117711, + "heading": 3.302893755908325, + "angularVelocity": 0.00001629311986139254, + "velocityX": -1.6072737068642717, + "velocityY": -0.23540564824432697, + "timestamp": 5.433886528894007 + }, + { + "x": 6.998489499518071, + "y": 7.298197704030925, + "heading": 3.302895308419307, + "angularVelocity": 0.00001629283173826444, + "velocityX": -1.6072737069205232, + "velocityY": -0.2354056483029315, + "timestamp": 5.529174506257745 + }, + { + "x": 6.845335638910315, + "y": 7.275766375938532, + "heading": 3.3028968609027314, + "angularVelocity": 0.000016292542536041963, + "velocityX": -1.6072737069769925, + "velocityY": -0.23540564836178338, + "timestamp": 5.624462483621484 + }, + { + "x": 6.692181778297163, + "y": 7.253335047840514, + "heading": 3.302898413358527, + "angularVelocity": 0.000016292252586671315, + "velocityX": -1.6072737070336132, + "velocityY": -0.23540564842080344, + "timestamp": 5.719750460985222 + }, + { + "x": 6.539027917678609, + "y": 7.230903719736863, + "heading": 3.3028999657866516, + "angularVelocity": 0.000016291962193735624, + "velocityX": -1.607273707090324, + "velocityY": -0.23540564847991918, + "timestamp": 5.81503843834896 + }, + { + "x": 6.385874057054646, + "y": 7.208472391627575, + "heading": 3.302901518187087, + "angularVelocity": 0.000016291671606816483, + "velocityX": -1.607273707147075, + "velocityY": -0.2354056485390708, + "timestamp": 5.9103264157126985 + }, + { + "x": 6.232720196425276, + "y": 7.186041063512652, + "heading": 3.3029030705598315, + "angularVelocity": 0.00001629138100746966, + "velocityX": -1.60727370720383, + "velocityY": -0.23540564859821628, + "timestamp": 6.005614393076437 + }, + { + "x": 6.079566335790499, + "y": 7.1636097353920976, + "heading": 3.3029046229048977, + "angularVelocity": 0.000016291090533089062, + "velocityX": -1.6072737072605614, + "velocityY": -0.23540564865732222, + "timestamp": 6.100902370440175 + }, + { + "x": 5.926412475150321, + "y": 7.141178407265915, + "heading": 3.302906175222303, + "angularVelocity": 0.000016290800243766995, + "velocityX": -1.6072737073172572, + "velocityY": -0.23540564871637523, + "timestamp": 6.196190347803913 + }, + { + "x": 5.773258614504743, + "y": 7.118747079134112, + "heading": 3.3029077275120664, + "angularVelocity": 0.00001629051016274816, + "velocityX": -1.6072737073739125, + "velocityY": -0.23540564877536937, + "timestamp": 6.291478325167652 + }, + { + "x": 5.62010475385377, + "y": 7.096315750996691, + "heading": 3.302909279774206, + "angularVelocity": 0.000016290220263808567, + "velocityX": -1.6072737074305328, + "velocityY": -0.23540564883431206, + "timestamp": 6.38676630253139 + }, + { + "x": 5.466950893197405, + "y": 7.073884422853658, + "heading": 3.302910832008734, + "angularVelocity": 0.00001628993049423592, + "velocityX": -1.6072737074871284, + "velocityY": -0.23540564889321514, + "timestamp": 6.482054279895128 + }, + { + "x": 5.313797032535647, + "y": 7.051453094705014, + "heading": 3.3029123842156554, + "angularVelocity": 0.000016289640769266148, + "velocityX": -1.6072737075437156, + "velocityY": -0.23540564895209914, + "timestamp": 6.5773422572588665 + }, + { + "x": 5.160643171868496, + "y": 7.02902176655076, + "heading": 3.3029139363949653, + "angularVelocity": 0.000016289351008016845, + "velocityX": -1.6072737076003119, + "velocityY": -0.23540564901098462, + "timestamp": 6.672630234622605 + }, + { + "x": 5.0074893111959495, + "y": 7.006590438390892, + "heading": 3.302915488546652, + "angularVelocity": 0.000016289061112899286, + "velocityX": -1.6072737076569361, + "velocityY": -0.23540564906989445, + "timestamp": 6.767918211986343 + }, + { + "x": 4.854335450518003, + "y": 6.984159110225406, + "heading": 3.3029170406706947, + "angularVelocity": 0.000016288771004663956, + "velocityX": -1.607273707713604, + "velocityY": -0.23540564912884793, + "timestamp": 6.863206189350081 + }, + { + "x": 4.701181589834651, + "y": 6.961727782054296, + "heading": 3.3029185927670666, + "angularVelocity": 0.00001628848060846621, + "velocityX": -1.6072737077703305, + "velocityY": -0.235405649187863, + "timestamp": 6.95849416671382 + }, + { + "x": 4.548027729145889, + "y": 6.939296453877556, + "heading": 3.3029201448357353, + "angularVelocity": 0.000016288189883493963, + "velocityX": -1.607273707827124, + "velocityY": -0.23540564924694996, + "timestamp": 7.053782144077558 + }, + { + "x": 4.3948738684517075, + "y": 6.916865125695178, + "heading": 3.3029216968766675, + "angularVelocity": 0.00001628789879930085, + "velocityX": -1.6072737078839905, + "velocityY": -0.23540564930611646, + "timestamp": 7.149070121441296 + }, + { + "x": 4.2417200077521, + "y": 6.894433797507156, + "heading": 3.302923248889828, + "angularVelocity": 0.00001628760735122893, + "velocityX": -1.6072737079409307, + "velocityY": -0.23540564936536426, + "timestamp": 7.2443580988050345 + }, + { + "x": 4.088566147047059, + "y": 6.872002469313479, + "heading": 3.3029248008751826, + "angularVelocity": 0.000016287315540072175, + "velocityX": -1.6072737079979444, + "velocityY": -0.23540564942469375, + "timestamp": 7.339646076168773 + }, + { + "x": 3.9354122863365792, + "y": 6.849571141114141, + "heading": 3.3029263528326944, + "angularVelocity": 0.000016287023346622306, + "velocityX": -1.6072737080550357, + "velocityY": -0.23540564948410944, + "timestamp": 7.434934053532511 + }, + { + "x": 3.7822584256206495, + "y": 6.827139812909132, + "heading": 3.3029279047623197, + "angularVelocity": 0.00001628673068839883, + "velocityX": -1.6072737081122201, + "velocityY": -0.23540564954363288, + "timestamp": 7.530222030896249 + }, + { + "x": 3.629104564899259, + "y": 6.804708484698436, + "heading": 3.3029294566639957, + "angularVelocity": 0.000016286437378238427, + "velocityX": -1.607273708169536, + "velocityY": -0.2354056496033085, + "timestamp": 7.625510008259988 + }, + { + "x": 3.4759507041723876, + "y": 6.7822771564820306, + "heading": 3.302931008537628, + "angularVelocity": 0.00001628614306837342, + "velocityX": -1.6072737082270525, + "velocityY": -0.2354056496632214, + "timestamp": 7.720797985623726 + }, + { + "x": 3.3227968434400057, + "y": 6.759845828259881, + "heading": 3.3029325603830677, + "angularVelocity": 0.000016285847206848686, + "velocityX": -1.6072737082848803, + "velocityY": -0.23540564972350414, + "timestamp": 7.816085962987464 + }, + { + "x": 3.1696429827020713, + "y": 6.737414500031936, + "heading": 3.3029341122001052, + "angularVelocity": 0.00001628554913981109, + "velocityX": -1.6072737083431483, + "velocityY": -0.2354056497843278, + "timestamp": 7.9113739403512024 + }, + { + "x": 3.016489122487879, + "y": 6.714983168191867, + "heading": 3.3029356639933174, + "angularVelocity": 0.000016285299102484297, + "velocityX": -1.6072737028467396, + "velocityY": -0.23540568769178183, + "timestamp": 8.006661917714942 + }, + { + "x": 2.8768170839321456, + "y": 6.685183755290752, + "heading": 3.3517957161483998, + "angularVelocity": 0.512761982223329, + "velocityX": -1.465788679956645, + "velocityY": -0.31273003925103915, + "timestamp": 8.10194989507868 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 2.3762078534459548, + "velocityX": -0.6771226429407677, + "velocityY": -0.18518169587388544, + "timestamp": 8.19723787244242 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 4.7042853211430965e-33, + "velocityX": -2.4138813228334137e-34, + "velocityY": 0, + "timestamp": 8.29252584980616 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/NewPath.1.traj b/src/main/deploy/choreo/NewPath.1.traj new file mode 100644 index 00000000..38751a08 --- /dev/null +++ b/src/main/deploy/choreo/NewPath.1.traj @@ -0,0 +1,670 @@ +{ + "samples": [ + { + "x": 1.373390793800354, + "y": 5.677787780761719, + "heading": 0, + "angularVelocity": 1.7597793138716488e-8, + "velocityX": -0.24611796880717063, + "velocityY": -3.3913013287386686, + "timestamp": 0 + }, + { + "x": 1.352399870888487, + "y": 5.388550285801571, + "heading": 3.752867032965106e-9, + "angularVelocity": 4.895503936124812e-8, + "velocityX": -0.27382037475691573, + "velocityY": -3.773017489334137, + "timestamp": 0.07665946308963235 + }, + { + "x": 1.3314089479766167, + "y": 5.099312790841379, + "heading": 7.505725037785982e-9, + "angularVelocity": 4.8954921591779524e-8, + "velocityX": -0.27382037475695803, + "velocityY": -3.7730174893347197, + "timestamp": 0.1533189261792647 + }, + { + "x": 1.3104180250647461, + "y": 4.8100752958811865, + "heading": 1.1258583081678353e-8, + "angularVelocity": 4.895492210145562e-8, + "velocityX": -0.27382037475695814, + "velocityY": -3.77301748933472, + "timestamp": 0.22997838926889708 + }, + { + "x": 1.2894271021528758, + "y": 4.520837800920995, + "heading": 1.5011441030111597e-8, + "angularVelocity": 4.895492085621956e-8, + "velocityX": -0.2738203747569581, + "velocityY": -3.77301748933472, + "timestamp": 0.3066378523585294 + }, + { + "x": 1.2684361792410237, + "y": 4.231600305960802, + "heading": 1.876429908089443e-8, + "angularVelocity": 4.895492219133962e-8, + "velocityX": -0.2738203747567213, + "velocityY": -3.773017489334737, + "timestamp": 0.38329731544816176 + }, + { + "x": 1.2474452563299916, + "y": 3.9423628110005486, + "heading": 2.251715713543734e-8, + "angularVelocity": 4.8954922240388724e-8, + "velocityX": -0.27382037474602466, + "velocityY": -3.7730174893355133, + "timestamp": 0.4599567785377941 + }, + { + "x": 1.2264544553180678, + "y": 3.6531253071936995, + "heading": 2.6270015297030064e-8, + "angularVelocity": 4.8954923636821905e-8, + "velocityX": -0.27381878460824305, + "velocityY": -3.773017604736736, + "timestamp": 0.5366162416274265 + }, + { + "x": 1.221016329808459, + "y": 3.3631781154569036, + "heading": 3.0068452251973244e-8, + "angularVelocity": 4.954948550189996e-8, + "velocityX": -0.07093873724696509, + "velocityY": -3.782275273670803, + "timestamp": 0.6132757047170588 + }, + { + "x": 1.2644718237504313, + "y": 3.076455070529721, + "heading": 0.0000024276011016837486, + "angularVelocity": 0.00003127510359229755, + "velocityX": 0.5668640529240702, + "velocityY": -3.740217233088862, + "timestamp": 0.6899351678066912 + }, + { + "x": 1.3174690376414906, + "y": 2.8354946463151376, + "heading": 0.053981349204806524, + "angularVelocity": 0.7041390511774281, + "velocityX": 0.6913329647129592, + "velocityY": -3.1432573944960427, + "timestamp": 0.7665946308963235 + }, + { + "x": 1.3769500323372545, + "y": 2.642804109504741, + "heading": 0.127260696547334, + "angularVelocity": 0.9559073908050616, + "velocityX": 0.7759119657049679, + "velocityY": -2.5135910042194918, + "timestamp": 0.8432540939859559 + }, + { + "x": 1.4418762753700956, + "y": 2.498725528039788, + "heading": 0.2108122800980793, + "angularVelocity": 1.0899056709157284, + "velocityX": 0.8469436181274491, + "velocityY": -1.879462438922806, + "timestamp": 0.9199135570755882 + }, + { + "x": 1.5117489112481097, + "y": 2.403367219851462, + "heading": 0.3004574317472213, + "angularVelocity": 1.1693944626813566, + "velocityX": 0.9114678483505286, + "velocityY": -1.2439208982827146, + "timestamp": 0.9965730201652205 + }, + { + "x": 1.5862760543823242, + "y": 2.3567769527435303, + "heading": 0.393778947091327, + "angularVelocity": 1.2173515386481575, + "velocityX": 0.9721845174818851, + "velocityY": -0.607756240784745, + "timestamp": 1.073232483254853 + }, + { + "x": 1.6648735689075516, + "y": 2.3587409751636828, + "heading": 0.4887400714151268, + "angularVelocity": 1.2445985149202732, + "velocityX": 1.0301304934108237, + "velocityY": 0.025741264173079863, + "timestamp": 1.1495310833137506 + }, + { + "x": 1.747708145966923, + "y": 2.4090584282818597, + "heading": 0.5850785373582581, + "angularVelocity": 1.2626505056287294, + "velocityX": 1.0856631313736873, + "velocityY": 0.6594806861375633, + "timestamp": 1.2258296833726483 + }, + { + "x": 1.8345057249855814, + "y": 2.507753992923127, + "heading": 0.6817510390114973, + "angularVelocity": 1.2670285113830946, + "velocityX": 1.1376038217169926, + "velocityY": 1.2935435848767918, + "timestamp": 1.302128283431546 + }, + { + "x": 1.9248152988345435, + "y": 2.6548610401322725, + "heading": 0.7770428443944881, + "angularVelocity": 1.248932553276624, + "velocityX": 1.1836334320583706, + "velocityY": 1.9280438578897694, + "timestamp": 1.3784268834904436 + }, + { + "x": 2.017756729148212, + "y": 2.8504188772247816, + "heading": 0.8676164076432323, + "angularVelocity": 1.187093382825206, + "velocityX": 1.2181275965997216, + "velocityY": 2.5630593083169373, + "timestamp": 1.4547254835493413 + }, + { + "x": 2.110868096637194, + "y": 3.094357922878678, + "heading": 0.9442027964608976, + "angularVelocity": 1.003771874694233, + "velocityX": 1.2203548612570354, + "velocityY": 3.197162798080057, + "timestamp": 1.531024083608239 + }, + { + "x": 2.1793890973429906, + "y": 3.3747396477963205, + "heading": 0.9442028150467319, + "angularVelocity": 2.435933862401011e-7, + "velocityX": 0.898063668965131, + "velocityY": 3.6747951430459493, + "timestamp": 1.6073226836671366 + }, + { + "x": 2.2112713834031283, + "y": 3.6616064511690984, + "heading": 0.9442028192329045, + "angularVelocity": 5.486565302861873e-8, + "velocityX": 0.41786200579730515, + "velocityY": 3.7597911777062087, + "timestamp": 1.6836212837260343 + }, + { + "x": 2.243152894403425, + "y": 3.9484733406807293, + "heading": 0.9442028234190755, + "angularVelocity": 5.486563331370058e-8, + "velocityX": 0.41785184755271976, + "velocityY": 3.7597923066765064, + "timestamp": 1.759919883784932 + }, + { + "x": 2.275034438637313, + "y": 4.235340226498878, + "heading": 0.9442028276052465, + "angularVelocity": 5.4865633459900915e-8, + "velocityX": 0.41785228312547945, + "velocityY": 3.7597922582682526, + "timestamp": 1.8362184838438296 + }, + { + "x": 2.3114043136255, + "y": 4.521672682225187, + "heading": 0.9442028318036487, + "angularVelocity": 5.5025940885755316e-8, + "velocityX": 0.4766781429818065, + "velocityY": 3.752787803515112, + "timestamp": 1.9125170839027272 + }, + { + "x": 2.395240306854248, + "y": 4.7978620529174805, + "heading": 0.944202836290881, + "angularVelocity": 5.881146304501388e-8, + "velocityX": 1.098788092625971, + "velocityY": 3.6198484700779985, + "timestamp": 1.988815683961625 + }, + { + "x": 2.5138560537660655, + "y": 5.041648695718218, + "heading": 0.9442028403664345, + "angularVelocity": 5.686795868350027e-8, + "velocityX": 1.6550967466461282, + "velocityY": 3.401660318130064, + "timestamp": 2.0604826406831447 + }, + { + "x": 2.669379769810612, + "y": 5.263716246351053, + "heading": 0.9442028441855257, + "angularVelocity": 5.328942931675909e-8, + "velocityX": 2.170089580458573, + "velocityY": 3.098604444663902, + "timestamp": 2.1321495974046645 + }, + { + "x": 2.829793194306078, + "y": 5.482277808796993, + "heading": 0.9442028479893113, + "angularVelocity": 5.307586429308322e-8, + "velocityX": 2.2383177943328927, + "velocityY": 3.049683877260457, + "timestamp": 2.2038165541261843 + }, + { + "x": 2.9902066606790894, + "y": 5.70083934050687, + "heading": 0.9442028517930969, + "angularVelocity": 5.307586336486897e-8, + "velocityX": 2.2383183786683687, + "velocityY": 3.049683448386907, + "timestamp": 2.275483510847704 + }, + { + "x": 3.1506201270524525, + "y": 5.919400872216489, + "heading": 0.9442028555968824, + "angularVelocity": 5.307586343565819e-8, + "velocityX": 2.2383183786732843, + "velocityY": 3.0496834483832997, + "timestamp": 2.347150467569224 + }, + { + "x": 3.3110335934258157, + "y": 6.137962403926107, + "heading": 0.9442028594006682, + "angularVelocity": 5.3075864810896055e-8, + "velocityX": 2.2383183786732843, + "velocityY": 3.0496834483832993, + "timestamp": 2.4188174242907436 + }, + { + "x": 3.4714470597991793, + "y": 6.3565239356357255, + "heading": 0.9442028632044539, + "angularVelocity": 5.307586438143788e-8, + "velocityX": 2.2383183786732848, + "velocityY": 3.0496834483832993, + "timestamp": 2.4904843810122634 + }, + { + "x": 3.631860526174069, + "y": 6.575085467344224, + "heading": 0.9442028670082396, + "angularVelocity": 5.307586459415007e-8, + "velocityX": 2.2383183786945806, + "velocityY": 3.0496834483676687, + "timestamp": 2.5621513377337832 + }, + { + "x": 3.7922741739851236, + "y": 6.793646865887341, + "heading": 0.9442028708120256, + "angularVelocity": 5.307586821274769e-8, + "velocityX": 2.2383209103517525, + "velocityY": 3.0496815902535848, + "timestamp": 2.633818294455303 + }, + { + "x": 3.9681792086390733, + "y": 6.999945295226872, + "heading": 0.9442028746807025, + "angularVelocity": 5.3981320792667595e-8, + "velocityX": 2.4544789216804146, + "velocityY": 2.878571084595562, + "timestamp": 2.705485251176823 + }, + { + "x": 4.17437520362896, + "y": 7.175970389393745, + "heading": 0.9442028789510033, + "angularVelocity": 5.958535155683375e-8, + "velocityX": 2.8771417738737903, + "velocityY": 2.4561541639177027, + "timestamp": 2.7771522078983426 + }, + { + "x": 4.405717344151184, + "y": 7.3173296257195375, + "heading": 0.9442028842473172, + "angularVelocity": 7.390175547435562e-8, + "velocityX": 3.228016803073774, + "velocityY": 1.9724464773225439, + "timestamp": 2.8488191646198624 + }, + { + "x": 4.656433083655096, + "y": 7.420495670437542, + "heading": 0.9442028919014063, + "angularVelocity": 1.0680080940487753e-7, + "velocityX": 3.498344997097253, + "velocityY": 1.4395203792296967, + "timestamp": 2.920486121341382 + }, + { + "x": 4.920266408469536, + "y": 7.482894237840949, + "heading": 0.9442029059804885, + "angularVelocity": 1.964515145326873e-7, + "velocityX": 3.6813803304029413, + "velocityY": 0.8706741608391041, + "timestamp": 2.992153078062902 + }, + { + "x": 5.190633947862971, + "y": 7.502968306176006, + "heading": 0.9442029492980424, + "angularVelocity": 6.044285388451107e-7, + "velocityX": 3.772555048542354, + "velocityY": 0.2801021454427761, + "timestamp": 3.0638200347844218 + }, + { + "x": 5.460788726806641, + "y": 7.480216979980469, + "heading": 0.9442042125234751, + "angularVelocity": 0.00001762633003205625, + "velocityX": 3.7695863100958116, + "velocityY": -0.31745908067434947, + "timestamp": 3.1354869915059416 + }, + { + "x": 5.6861880959283475, + "y": 7.430105467011311, + "heading": 0.94453500375596, + "angularVelocity": 0.00541646085327845, + "velocityX": 3.6907473331858487, + "velocityY": -0.8205388221514872, + "timestamp": 3.196558462612879 + }, + { + "x": 5.902931349588688, + "y": 7.350124119825956, + "heading": 0.944535163065715, + "angularVelocity": 0.0000026085789673009567, + "velocityX": 3.5490098687948497, + "velocityY": -1.3096351821180778, + "timestamp": 3.2576299337198167 + }, + { + "x": 6.106969137969694, + "y": 7.241757655990003, + "heading": 0.9445351882967145, + "angularVelocity": 4.1313888378756724e-7, + "velocityX": 3.340967307365667, + "velocityY": -1.7744203942000976, + "timestamp": 3.3187014048267542 + }, + { + "x": 6.294604237914379, + "y": 7.106969701935095, + "heading": 0.9445352004387211, + "angularVelocity": 1.988163445738462e-7, + "velocityX": 3.0723854615542603, + "velocityY": -2.207052681257527, + "timestamp": 3.379772875933692 + }, + { + "x": 6.462436717197839, + "y": 6.94820261304716, + "heading": 0.9445352081247251, + "angularVelocity": 1.2585260943351558e-7, + "velocityX": 2.7481322496649945, + "velocityY": -2.599693212071642, + "timestamp": 3.4408443470406294 + }, + { + "x": 6.607425485391203, + "y": 6.7683332434118935, + "heading": 0.944535213822349, + "angularVelocity": 9.329435992491954e-8, + "velocityX": 2.3740834397043513, + "velocityY": -2.945227393005009, + "timestamp": 3.501915818147567 + }, + { + "x": 6.726943413314801, + "y": 6.570620826362678, + "heading": 0.9445352185350564, + "angularVelocity": 7.716708691487089e-8, + "velocityX": 1.9570173398037076, + "velocityY": -3.237394047754587, + "timestamp": 3.5629872892545045 + }, + { + "x": 6.818825069558712, + "y": 6.358647957429321, + "heading": 0.9445352227854475, + "angularVelocity": 6.959699896596626e-8, + "velocityX": 1.5044939081788649, + "velocityY": -3.470898360417538, + "timestamp": 3.624058760361442 + }, + { + "x": 6.885266799619373, + "y": 6.137378364635986, + "heading": 0.944535226910566, + "angularVelocity": 6.754575173659361e-8, + "velocityX": 1.0879340034945393, + "velocityY": -3.623125311749637, + "timestamp": 3.6851302314683796 + }, + { + "x": 6.951706477654193, + "y": 5.916108155680916, + "heading": 0.9445352310356804, + "angularVelocity": 6.754568510636454e-8, + "velocityX": 1.087900403094797, + "velocityY": -3.6231354009406083, + "timestamp": 3.746201702575317 + }, + { + "x": 7.007755279541016, + "y": 5.691980361938477, + "heading": 0.9445352351916124, + "angularVelocity": 6.805030176915139e-8, + "velocityX": 0.917757520343414, + "velocityY": -3.669926230366784, + "timestamp": 3.8072731736822547 + }, + { + "x": 7.033287944264436, + "y": 5.499297549749691, + "heading": 0.9445352391429122, + "angularVelocity": 7.690359033060341e-8, + "velocityX": 0.49693866281360616, + "velocityY": -3.750158476346985, + "timestamp": 3.858653085599064 + }, + { + "x": 7.048377702483028, + "y": 5.30551704932481, + "heading": 0.9445352430586353, + "angularVelocity": 7.621117031703296e-8, + "velocityX": 0.29368984211229937, + "velocityY": -3.7715226281165486, + "timestamp": 3.9100329975158736 + }, + { + "x": 7.063467124411225, + "y": 5.111736522713165, + "heading": 0.9445352469743582, + "angularVelocity": 7.621116311616718e-8, + "velocityX": 0.2936832969396488, + "velocityY": -3.7715231377858505, + "timestamp": 3.961412909432683 + }, + { + "x": 7.0785565463316305, + "y": 4.917955996100912, + "heading": 0.9445352508900811, + "angularVelocity": 7.621116383171076e-8, + "velocityX": 0.29368329678799265, + "velocityY": -3.7715231377976592, + "timestamp": 4.0127928213494926 + }, + { + "x": 7.093645968252035, + "y": 4.72417546948866, + "heading": 0.9445352548058039, + "angularVelocity": 7.621116339702062e-8, + "velocityX": 0.29368329678798916, + "velocityY": -3.77152313779766, + "timestamp": 4.064172733266302 + }, + { + "x": 7.1087353901724395, + "y": 4.530394942876407, + "heading": 0.9445352587215268, + "angularVelocity": 7.621116320689e-8, + "velocityX": 0.2936832967879892, + "velocityY": -3.77152313779766, + "timestamp": 4.1155526451831115 + }, + { + "x": 7.1238248120928445, + "y": 4.336614416264154, + "heading": 0.9445352626372497, + "angularVelocity": 7.621116316535163e-8, + "velocityX": 0.29368329678798916, + "velocityY": -3.77152313779766, + "timestamp": 4.166932557099921 + }, + { + "x": 7.138914234013249, + "y": 4.142833889651901, + "heading": 0.9445352665529726, + "angularVelocity": 7.621116395669404e-8, + "velocityX": 0.2936832967879891, + "velocityY": -3.7715231377976606, + "timestamp": 4.21831246901673 + }, + { + "x": 7.1540036559336535, + "y": 3.9490533630396487, + "heading": 0.9445352704686956, + "angularVelocity": 7.621116465419702e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.7715231377976597, + "timestamp": 4.26969238093354 + }, + { + "x": 7.1690930778540585, + "y": 3.755272836427396, + "heading": 0.9445352743844184, + "angularVelocity": 7.621116300190572e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.7715231377976597, + "timestamp": 4.321072292850349 + }, + { + "x": 7.184182499774463, + "y": 3.561492309815143, + "heading": 0.9445352783001413, + "angularVelocity": 7.621116375934217e-8, + "velocityX": 0.29368329678798927, + "velocityY": -3.7715231377976597, + "timestamp": 4.372452204767159 + }, + { + "x": 7.199271921694868, + "y": 3.367711783202891, + "heading": 0.9445352822158641, + "angularVelocity": 7.621116358329164e-8, + "velocityX": 0.2936832967879893, + "velocityY": -3.7715231377976597, + "timestamp": 4.423832116683968 + }, + { + "x": 7.2143613436152725, + "y": 3.173931256590638, + "heading": 0.944535286131587, + "angularVelocity": 7.621116395825444e-8, + "velocityX": 0.2936832967879889, + "velocityY": -3.77152313779766, + "timestamp": 4.475212028600778 + }, + { + "x": 7.2294507655356774, + "y": 2.980150729978386, + "heading": 0.94453529004731, + "angularVelocity": 7.621116318952045e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.77152313779766, + "timestamp": 4.526591940517587 + }, + { + "x": 7.244540187456082, + "y": 2.786370203366133, + "heading": 0.9445352939630328, + "angularVelocity": 7.621116338095618e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.77152313779766, + "timestamp": 4.577971852434397 + }, + { + "x": 7.2596296093764865, + "y": 2.5925896767538807, + "heading": 0.9445352978787557, + "angularVelocity": 7.621116372521823e-8, + "velocityX": 0.2936832967879889, + "velocityY": -3.77152313779766, + "timestamp": 4.629351764351206 + }, + { + "x": 7.2747190312968915, + "y": 2.398809150141628, + "heading": 0.9445353017944785, + "angularVelocity": 7.621116368417063e-8, + "velocityX": 0.2936832967879889, + "velocityY": -3.77152313779766, + "timestamp": 4.680731676268016 + }, + { + "x": 7.289808453217296, + "y": 2.2050286235293757, + "heading": 0.9445353057102015, + "angularVelocity": 7.621116284744578e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.77152313779766, + "timestamp": 4.732111588184825 + }, + { + "x": 7.3048978751377005, + "y": 2.011248096917123, + "heading": 0.9445353096259244, + "angularVelocity": 7.621116384489279e-8, + "velocityX": 0.29368329678798877, + "velocityY": -3.77152313779766, + "timestamp": 4.7834915001016345 + }, + { + "x": 7.3199872970581055, + "y": 1.8174675703048704, + "heading": 0.9445353135416472, + "angularVelocity": 7.621116322392135e-8, + "velocityX": 0.2936832967879892, + "velocityY": -3.77152313779766, + "timestamp": 4.834871412018444 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/NewPath.traj b/src/main/deploy/choreo/NewPath.traj new file mode 100644 index 00000000..38751a08 --- /dev/null +++ b/src/main/deploy/choreo/NewPath.traj @@ -0,0 +1,670 @@ +{ + "samples": [ + { + "x": 1.373390793800354, + "y": 5.677787780761719, + "heading": 0, + "angularVelocity": 1.7597793138716488e-8, + "velocityX": -0.24611796880717063, + "velocityY": -3.3913013287386686, + "timestamp": 0 + }, + { + "x": 1.352399870888487, + "y": 5.388550285801571, + "heading": 3.752867032965106e-9, + "angularVelocity": 4.895503936124812e-8, + "velocityX": -0.27382037475691573, + "velocityY": -3.773017489334137, + "timestamp": 0.07665946308963235 + }, + { + "x": 1.3314089479766167, + "y": 5.099312790841379, + "heading": 7.505725037785982e-9, + "angularVelocity": 4.8954921591779524e-8, + "velocityX": -0.27382037475695803, + "velocityY": -3.7730174893347197, + "timestamp": 0.1533189261792647 + }, + { + "x": 1.3104180250647461, + "y": 4.8100752958811865, + "heading": 1.1258583081678353e-8, + "angularVelocity": 4.895492210145562e-8, + "velocityX": -0.27382037475695814, + "velocityY": -3.77301748933472, + "timestamp": 0.22997838926889708 + }, + { + "x": 1.2894271021528758, + "y": 4.520837800920995, + "heading": 1.5011441030111597e-8, + "angularVelocity": 4.895492085621956e-8, + "velocityX": -0.2738203747569581, + "velocityY": -3.77301748933472, + "timestamp": 0.3066378523585294 + }, + { + "x": 1.2684361792410237, + "y": 4.231600305960802, + "heading": 1.876429908089443e-8, + "angularVelocity": 4.895492219133962e-8, + "velocityX": -0.2738203747567213, + "velocityY": -3.773017489334737, + "timestamp": 0.38329731544816176 + }, + { + "x": 1.2474452563299916, + "y": 3.9423628110005486, + "heading": 2.251715713543734e-8, + "angularVelocity": 4.8954922240388724e-8, + "velocityX": -0.27382037474602466, + "velocityY": -3.7730174893355133, + "timestamp": 0.4599567785377941 + }, + { + "x": 1.2264544553180678, + "y": 3.6531253071936995, + "heading": 2.6270015297030064e-8, + "angularVelocity": 4.8954923636821905e-8, + "velocityX": -0.27381878460824305, + "velocityY": -3.773017604736736, + "timestamp": 0.5366162416274265 + }, + { + "x": 1.221016329808459, + "y": 3.3631781154569036, + "heading": 3.0068452251973244e-8, + "angularVelocity": 4.954948550189996e-8, + "velocityX": -0.07093873724696509, + "velocityY": -3.782275273670803, + "timestamp": 0.6132757047170588 + }, + { + "x": 1.2644718237504313, + "y": 3.076455070529721, + "heading": 0.0000024276011016837486, + "angularVelocity": 0.00003127510359229755, + "velocityX": 0.5668640529240702, + "velocityY": -3.740217233088862, + "timestamp": 0.6899351678066912 + }, + { + "x": 1.3174690376414906, + "y": 2.8354946463151376, + "heading": 0.053981349204806524, + "angularVelocity": 0.7041390511774281, + "velocityX": 0.6913329647129592, + "velocityY": -3.1432573944960427, + "timestamp": 0.7665946308963235 + }, + { + "x": 1.3769500323372545, + "y": 2.642804109504741, + "heading": 0.127260696547334, + "angularVelocity": 0.9559073908050616, + "velocityX": 0.7759119657049679, + "velocityY": -2.5135910042194918, + "timestamp": 0.8432540939859559 + }, + { + "x": 1.4418762753700956, + "y": 2.498725528039788, + "heading": 0.2108122800980793, + "angularVelocity": 1.0899056709157284, + "velocityX": 0.8469436181274491, + "velocityY": -1.879462438922806, + "timestamp": 0.9199135570755882 + }, + { + "x": 1.5117489112481097, + "y": 2.403367219851462, + "heading": 0.3004574317472213, + "angularVelocity": 1.1693944626813566, + "velocityX": 0.9114678483505286, + "velocityY": -1.2439208982827146, + "timestamp": 0.9965730201652205 + }, + { + "x": 1.5862760543823242, + "y": 2.3567769527435303, + "heading": 0.393778947091327, + "angularVelocity": 1.2173515386481575, + "velocityX": 0.9721845174818851, + "velocityY": -0.607756240784745, + "timestamp": 1.073232483254853 + }, + { + "x": 1.6648735689075516, + "y": 2.3587409751636828, + "heading": 0.4887400714151268, + "angularVelocity": 1.2445985149202732, + "velocityX": 1.0301304934108237, + "velocityY": 0.025741264173079863, + "timestamp": 1.1495310833137506 + }, + { + "x": 1.747708145966923, + "y": 2.4090584282818597, + "heading": 0.5850785373582581, + "angularVelocity": 1.2626505056287294, + "velocityX": 1.0856631313736873, + "velocityY": 0.6594806861375633, + "timestamp": 1.2258296833726483 + }, + { + "x": 1.8345057249855814, + "y": 2.507753992923127, + "heading": 0.6817510390114973, + "angularVelocity": 1.2670285113830946, + "velocityX": 1.1376038217169926, + "velocityY": 1.2935435848767918, + "timestamp": 1.302128283431546 + }, + { + "x": 1.9248152988345435, + "y": 2.6548610401322725, + "heading": 0.7770428443944881, + "angularVelocity": 1.248932553276624, + "velocityX": 1.1836334320583706, + "velocityY": 1.9280438578897694, + "timestamp": 1.3784268834904436 + }, + { + "x": 2.017756729148212, + "y": 2.8504188772247816, + "heading": 0.8676164076432323, + "angularVelocity": 1.187093382825206, + "velocityX": 1.2181275965997216, + "velocityY": 2.5630593083169373, + "timestamp": 1.4547254835493413 + }, + { + "x": 2.110868096637194, + "y": 3.094357922878678, + "heading": 0.9442027964608976, + "angularVelocity": 1.003771874694233, + "velocityX": 1.2203548612570354, + "velocityY": 3.197162798080057, + "timestamp": 1.531024083608239 + }, + { + "x": 2.1793890973429906, + "y": 3.3747396477963205, + "heading": 0.9442028150467319, + "angularVelocity": 2.435933862401011e-7, + "velocityX": 0.898063668965131, + "velocityY": 3.6747951430459493, + "timestamp": 1.6073226836671366 + }, + { + "x": 2.2112713834031283, + "y": 3.6616064511690984, + "heading": 0.9442028192329045, + "angularVelocity": 5.486565302861873e-8, + "velocityX": 0.41786200579730515, + "velocityY": 3.7597911777062087, + "timestamp": 1.6836212837260343 + }, + { + "x": 2.243152894403425, + "y": 3.9484733406807293, + "heading": 0.9442028234190755, + "angularVelocity": 5.486563331370058e-8, + "velocityX": 0.41785184755271976, + "velocityY": 3.7597923066765064, + "timestamp": 1.759919883784932 + }, + { + "x": 2.275034438637313, + "y": 4.235340226498878, + "heading": 0.9442028276052465, + "angularVelocity": 5.4865633459900915e-8, + "velocityX": 0.41785228312547945, + "velocityY": 3.7597922582682526, + "timestamp": 1.8362184838438296 + }, + { + "x": 2.3114043136255, + "y": 4.521672682225187, + "heading": 0.9442028318036487, + "angularVelocity": 5.5025940885755316e-8, + "velocityX": 0.4766781429818065, + "velocityY": 3.752787803515112, + "timestamp": 1.9125170839027272 + }, + { + "x": 2.395240306854248, + "y": 4.7978620529174805, + "heading": 0.944202836290881, + "angularVelocity": 5.881146304501388e-8, + "velocityX": 1.098788092625971, + "velocityY": 3.6198484700779985, + "timestamp": 1.988815683961625 + }, + { + "x": 2.5138560537660655, + "y": 5.041648695718218, + "heading": 0.9442028403664345, + "angularVelocity": 5.686795868350027e-8, + "velocityX": 1.6550967466461282, + "velocityY": 3.401660318130064, + "timestamp": 2.0604826406831447 + }, + { + "x": 2.669379769810612, + "y": 5.263716246351053, + "heading": 0.9442028441855257, + "angularVelocity": 5.328942931675909e-8, + "velocityX": 2.170089580458573, + "velocityY": 3.098604444663902, + "timestamp": 2.1321495974046645 + }, + { + "x": 2.829793194306078, + "y": 5.482277808796993, + "heading": 0.9442028479893113, + "angularVelocity": 5.307586429308322e-8, + "velocityX": 2.2383177943328927, + "velocityY": 3.049683877260457, + "timestamp": 2.2038165541261843 + }, + { + "x": 2.9902066606790894, + "y": 5.70083934050687, + "heading": 0.9442028517930969, + "angularVelocity": 5.307586336486897e-8, + "velocityX": 2.2383183786683687, + "velocityY": 3.049683448386907, + "timestamp": 2.275483510847704 + }, + { + "x": 3.1506201270524525, + "y": 5.919400872216489, + "heading": 0.9442028555968824, + "angularVelocity": 5.307586343565819e-8, + "velocityX": 2.2383183786732843, + "velocityY": 3.0496834483832997, + "timestamp": 2.347150467569224 + }, + { + "x": 3.3110335934258157, + "y": 6.137962403926107, + "heading": 0.9442028594006682, + "angularVelocity": 5.3075864810896055e-8, + "velocityX": 2.2383183786732843, + "velocityY": 3.0496834483832993, + "timestamp": 2.4188174242907436 + }, + { + "x": 3.4714470597991793, + "y": 6.3565239356357255, + "heading": 0.9442028632044539, + "angularVelocity": 5.307586438143788e-8, + "velocityX": 2.2383183786732848, + "velocityY": 3.0496834483832993, + "timestamp": 2.4904843810122634 + }, + { + "x": 3.631860526174069, + "y": 6.575085467344224, + "heading": 0.9442028670082396, + "angularVelocity": 5.307586459415007e-8, + "velocityX": 2.2383183786945806, + "velocityY": 3.0496834483676687, + "timestamp": 2.5621513377337832 + }, + { + "x": 3.7922741739851236, + "y": 6.793646865887341, + "heading": 0.9442028708120256, + "angularVelocity": 5.307586821274769e-8, + "velocityX": 2.2383209103517525, + "velocityY": 3.0496815902535848, + "timestamp": 2.633818294455303 + }, + { + "x": 3.9681792086390733, + "y": 6.999945295226872, + "heading": 0.9442028746807025, + "angularVelocity": 5.3981320792667595e-8, + "velocityX": 2.4544789216804146, + "velocityY": 2.878571084595562, + "timestamp": 2.705485251176823 + }, + { + "x": 4.17437520362896, + "y": 7.175970389393745, + "heading": 0.9442028789510033, + "angularVelocity": 5.958535155683375e-8, + "velocityX": 2.8771417738737903, + "velocityY": 2.4561541639177027, + "timestamp": 2.7771522078983426 + }, + { + "x": 4.405717344151184, + "y": 7.3173296257195375, + "heading": 0.9442028842473172, + "angularVelocity": 7.390175547435562e-8, + "velocityX": 3.228016803073774, + "velocityY": 1.9724464773225439, + "timestamp": 2.8488191646198624 + }, + { + "x": 4.656433083655096, + "y": 7.420495670437542, + "heading": 0.9442028919014063, + "angularVelocity": 1.0680080940487753e-7, + "velocityX": 3.498344997097253, + "velocityY": 1.4395203792296967, + "timestamp": 2.920486121341382 + }, + { + "x": 4.920266408469536, + "y": 7.482894237840949, + "heading": 0.9442029059804885, + "angularVelocity": 1.964515145326873e-7, + "velocityX": 3.6813803304029413, + "velocityY": 0.8706741608391041, + "timestamp": 2.992153078062902 + }, + { + "x": 5.190633947862971, + "y": 7.502968306176006, + "heading": 0.9442029492980424, + "angularVelocity": 6.044285388451107e-7, + "velocityX": 3.772555048542354, + "velocityY": 0.2801021454427761, + "timestamp": 3.0638200347844218 + }, + { + "x": 5.460788726806641, + "y": 7.480216979980469, + "heading": 0.9442042125234751, + "angularVelocity": 0.00001762633003205625, + "velocityX": 3.7695863100958116, + "velocityY": -0.31745908067434947, + "timestamp": 3.1354869915059416 + }, + { + "x": 5.6861880959283475, + "y": 7.430105467011311, + "heading": 0.94453500375596, + "angularVelocity": 0.00541646085327845, + "velocityX": 3.6907473331858487, + "velocityY": -0.8205388221514872, + "timestamp": 3.196558462612879 + }, + { + "x": 5.902931349588688, + "y": 7.350124119825956, + "heading": 0.944535163065715, + "angularVelocity": 0.0000026085789673009567, + "velocityX": 3.5490098687948497, + "velocityY": -1.3096351821180778, + "timestamp": 3.2576299337198167 + }, + { + "x": 6.106969137969694, + "y": 7.241757655990003, + "heading": 0.9445351882967145, + "angularVelocity": 4.1313888378756724e-7, + "velocityX": 3.340967307365667, + "velocityY": -1.7744203942000976, + "timestamp": 3.3187014048267542 + }, + { + "x": 6.294604237914379, + "y": 7.106969701935095, + "heading": 0.9445352004387211, + "angularVelocity": 1.988163445738462e-7, + "velocityX": 3.0723854615542603, + "velocityY": -2.207052681257527, + "timestamp": 3.379772875933692 + }, + { + "x": 6.462436717197839, + "y": 6.94820261304716, + "heading": 0.9445352081247251, + "angularVelocity": 1.2585260943351558e-7, + "velocityX": 2.7481322496649945, + "velocityY": -2.599693212071642, + "timestamp": 3.4408443470406294 + }, + { + "x": 6.607425485391203, + "y": 6.7683332434118935, + "heading": 0.944535213822349, + "angularVelocity": 9.329435992491954e-8, + "velocityX": 2.3740834397043513, + "velocityY": -2.945227393005009, + "timestamp": 3.501915818147567 + }, + { + "x": 6.726943413314801, + "y": 6.570620826362678, + "heading": 0.9445352185350564, + "angularVelocity": 7.716708691487089e-8, + "velocityX": 1.9570173398037076, + "velocityY": -3.237394047754587, + "timestamp": 3.5629872892545045 + }, + { + "x": 6.818825069558712, + "y": 6.358647957429321, + "heading": 0.9445352227854475, + "angularVelocity": 6.959699896596626e-8, + "velocityX": 1.5044939081788649, + "velocityY": -3.470898360417538, + "timestamp": 3.624058760361442 + }, + { + "x": 6.885266799619373, + "y": 6.137378364635986, + "heading": 0.944535226910566, + "angularVelocity": 6.754575173659361e-8, + "velocityX": 1.0879340034945393, + "velocityY": -3.623125311749637, + "timestamp": 3.6851302314683796 + }, + { + "x": 6.951706477654193, + "y": 5.916108155680916, + "heading": 0.9445352310356804, + "angularVelocity": 6.754568510636454e-8, + "velocityX": 1.087900403094797, + "velocityY": -3.6231354009406083, + "timestamp": 3.746201702575317 + }, + { + "x": 7.007755279541016, + "y": 5.691980361938477, + "heading": 0.9445352351916124, + "angularVelocity": 6.805030176915139e-8, + "velocityX": 0.917757520343414, + "velocityY": -3.669926230366784, + "timestamp": 3.8072731736822547 + }, + { + "x": 7.033287944264436, + "y": 5.499297549749691, + "heading": 0.9445352391429122, + "angularVelocity": 7.690359033060341e-8, + "velocityX": 0.49693866281360616, + "velocityY": -3.750158476346985, + "timestamp": 3.858653085599064 + }, + { + "x": 7.048377702483028, + "y": 5.30551704932481, + "heading": 0.9445352430586353, + "angularVelocity": 7.621117031703296e-8, + "velocityX": 0.29368984211229937, + "velocityY": -3.7715226281165486, + "timestamp": 3.9100329975158736 + }, + { + "x": 7.063467124411225, + "y": 5.111736522713165, + "heading": 0.9445352469743582, + "angularVelocity": 7.621116311616718e-8, + "velocityX": 0.2936832969396488, + "velocityY": -3.7715231377858505, + "timestamp": 3.961412909432683 + }, + { + "x": 7.0785565463316305, + "y": 4.917955996100912, + "heading": 0.9445352508900811, + "angularVelocity": 7.621116383171076e-8, + "velocityX": 0.29368329678799265, + "velocityY": -3.7715231377976592, + "timestamp": 4.0127928213494926 + }, + { + "x": 7.093645968252035, + "y": 4.72417546948866, + "heading": 0.9445352548058039, + "angularVelocity": 7.621116339702062e-8, + "velocityX": 0.29368329678798916, + "velocityY": -3.77152313779766, + "timestamp": 4.064172733266302 + }, + { + "x": 7.1087353901724395, + "y": 4.530394942876407, + "heading": 0.9445352587215268, + "angularVelocity": 7.621116320689e-8, + "velocityX": 0.2936832967879892, + "velocityY": -3.77152313779766, + "timestamp": 4.1155526451831115 + }, + { + "x": 7.1238248120928445, + "y": 4.336614416264154, + "heading": 0.9445352626372497, + "angularVelocity": 7.621116316535163e-8, + "velocityX": 0.29368329678798916, + "velocityY": -3.77152313779766, + "timestamp": 4.166932557099921 + }, + { + "x": 7.138914234013249, + "y": 4.142833889651901, + "heading": 0.9445352665529726, + "angularVelocity": 7.621116395669404e-8, + "velocityX": 0.2936832967879891, + "velocityY": -3.7715231377976606, + "timestamp": 4.21831246901673 + }, + { + "x": 7.1540036559336535, + "y": 3.9490533630396487, + "heading": 0.9445352704686956, + "angularVelocity": 7.621116465419702e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.7715231377976597, + "timestamp": 4.26969238093354 + }, + { + "x": 7.1690930778540585, + "y": 3.755272836427396, + "heading": 0.9445352743844184, + "angularVelocity": 7.621116300190572e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.7715231377976597, + "timestamp": 4.321072292850349 + }, + { + "x": 7.184182499774463, + "y": 3.561492309815143, + "heading": 0.9445352783001413, + "angularVelocity": 7.621116375934217e-8, + "velocityX": 0.29368329678798927, + "velocityY": -3.7715231377976597, + "timestamp": 4.372452204767159 + }, + { + "x": 7.199271921694868, + "y": 3.367711783202891, + "heading": 0.9445352822158641, + "angularVelocity": 7.621116358329164e-8, + "velocityX": 0.2936832967879893, + "velocityY": -3.7715231377976597, + "timestamp": 4.423832116683968 + }, + { + "x": 7.2143613436152725, + "y": 3.173931256590638, + "heading": 0.944535286131587, + "angularVelocity": 7.621116395825444e-8, + "velocityX": 0.2936832967879889, + "velocityY": -3.77152313779766, + "timestamp": 4.475212028600778 + }, + { + "x": 7.2294507655356774, + "y": 2.980150729978386, + "heading": 0.94453529004731, + "angularVelocity": 7.621116318952045e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.77152313779766, + "timestamp": 4.526591940517587 + }, + { + "x": 7.244540187456082, + "y": 2.786370203366133, + "heading": 0.9445352939630328, + "angularVelocity": 7.621116338095618e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.77152313779766, + "timestamp": 4.577971852434397 + }, + { + "x": 7.2596296093764865, + "y": 2.5925896767538807, + "heading": 0.9445352978787557, + "angularVelocity": 7.621116372521823e-8, + "velocityX": 0.2936832967879889, + "velocityY": -3.77152313779766, + "timestamp": 4.629351764351206 + }, + { + "x": 7.2747190312968915, + "y": 2.398809150141628, + "heading": 0.9445353017944785, + "angularVelocity": 7.621116368417063e-8, + "velocityX": 0.2936832967879889, + "velocityY": -3.77152313779766, + "timestamp": 4.680731676268016 + }, + { + "x": 7.289808453217296, + "y": 2.2050286235293757, + "heading": 0.9445353057102015, + "angularVelocity": 7.621116284744578e-8, + "velocityX": 0.29368329678798905, + "velocityY": -3.77152313779766, + "timestamp": 4.732111588184825 + }, + { + "x": 7.3048978751377005, + "y": 2.011248096917123, + "heading": 0.9445353096259244, + "angularVelocity": 7.621116384489279e-8, + "velocityX": 0.29368329678798877, + "velocityY": -3.77152313779766, + "timestamp": 4.7834915001016345 + }, + { + "x": 7.3199872970581055, + "y": 1.8174675703048704, + "heading": 0.9445353135416472, + "angularVelocity": 7.621116322392135e-8, + "velocityX": 0.2936832967879892, + "velocityY": -3.77152313779766, + "timestamp": 4.834871412018444 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.1.traj b/src/main/deploy/choreo/Test1.1.traj index 35cbe979..f107b798 100644 --- a/src/main/deploy/choreo/Test1.1.traj +++ b/src/main/deploy/choreo/Test1.1.traj @@ -4,631 +4,595 @@ "x": 1.4869295358657837, "y": 5.564249038696289, "heading": 3.141, - "angularVelocity": 8.029589971503427e-22, - "velocityX": 1.7351458241819956e-21, - "velocityY": 6.201477276822387e-22, + "angularVelocity": 3.7405663636389993e-19, + "velocityX": -1.1249906625590385e-18, + "velocityY": 2.3803295269896554e-19, "timestamp": 0 }, { - "x": 1.506153170031305, - "y": 5.579314508687984, - "heading": 3.1569331073832556, - "angularVelocity": 0.2189381700102546, - "velocityX": 0.2641535755648618, - "velocityY": 0.20701589156378014, - "timestamp": 0.07277446131256805 - }, - { - "x": 1.5445753142190985, - "y": 5.609481391551554, - "heading": 3.1887853931444132, - "angularVelocity": 0.4376849403852184, - "velocityX": 0.5279619181620503, - "velocityY": 0.41452567727023953, - "timestamp": 0.1455489226251361 - }, - { - "x": 1.6021601427736685, - "y": 5.654796415146542, - "heading": 3.2365776272564357, - "angularVelocity": 0.6567171127073494, - "velocityX": 0.7912779774108067, - "velocityY": 0.6226775544288833, - "timestamp": 0.21832338393770417 - }, - { - "x": 1.6788531458253628, - "y": 5.715327275578862, - "heading": 3.3003664786278266, - "angularVelocity": 0.8765279772723583, - "velocityX": 1.0538450119513225, - "velocityY": 0.8317596494783837, - "timestamp": 0.2910978452502722 - }, - { - "x": 1.7745670450726352, - "y": 5.791182757948707, - "heading": 3.380219753358853, - "angularVelocity": 1.097270571169936, - "velocityX": 1.3152127480020601, - "velocityY": 1.0423365697486087, - "timestamp": 0.36387230656284025 - }, - { - "x": 1.8891522077377947, - "y": 5.8825573533612445, - "heading": 3.4761247222373646, - "angularVelocity": 1.317838251891654, - "velocityX": 1.5745243674565124, - "velocityY": 1.255586008669736, - "timestamp": 0.4366467678754083 - }, - { - "x": 2.0223089309237166, - "y": 5.989857408352276, - "heading": 3.5877133538349852, - "angularVelocity": 1.5333487817703626, - "velocityX": 1.829717744168665, - "velocityY": 1.4744190895508729, - "timestamp": 0.5094212291879764 - }, - { - "x": 2.1730917573654285, - "y": 6.114310741146903, - "heading": 3.713064984762661, - "angularVelocity": 1.7224673143135665, - "velocityX": 2.0719195130019052, - "velocityY": 1.7101237240368758, - "timestamp": 0.5821956905005444 - }, - { - "x": 2.304244374220996, - "y": 6.249503988496409, - "heading": 3.8044927424626787, - "angularVelocity": 1.2563165161379026, - "velocityX": 1.8021791503514413, - "velocityY": 1.8577017941616667, - "timestamp": 0.6549701518131125 - }, - { - "x": 2.414916720860036, - "y": 6.372175349075486, - "heading": 3.8745670686179303, - "angularVelocity": 0.9628972154706921, - "velocityX": 1.5207580330096784, - "velocityY": 1.6856374937933305, - "timestamp": 0.7277446131256805 - }, - { - "x": 2.505647288657887, - "y": 6.481209512574969, - "heading": 3.9250064522902046, - "angularVelocity": 0.6930918176863726, - "velocityX": 1.2467363709936818, - "velocityY": 1.4982476205653745, - "timestamp": 0.8005190744382485 - }, - { - "x": 2.576640808380743, - "y": 6.576219510667492, - "heading": 3.9564510372491593, - "angularVelocity": 0.432082689336569, - "velocityX": 0.975527931672858, - "velocityY": 1.305540382971075, - "timestamp": 0.8732935357508166 - }, - { - "x": 2.628005259573955, - "y": 6.657012046349283, - "heading": 3.969203722939461, - "angularVelocity": 0.17523572775796842, - "velocityX": 0.7058032483758355, - "velocityY": 1.1101770349736413, - "timestamp": 0.9460679970633846 - }, - { - "x": 2.6598078437532253, - "y": 6.723475709345355, - "heading": 3.963391945850161, - "angularVelocity": -0.07986011829532402, - "velocityX": 0.4370019867639682, - "velocityY": 0.9132827890076579, - "timestamp": 1.0188424583759528 - }, - { - "x": 2.6720941279928496, - "y": 6.775543377099927, - "heading": 3.9390315825966313, - "angularVelocity": -0.3347378024400806, - "velocityX": 0.16882686615644266, - "velocityY": 0.7154662063514367, - "timestamp": 1.0916169196885208 - }, - { - "x": 2.6648950576782227, + "x": 1.5092825889004946, + "y": 5.579306288055266, + "heading": 3.4062283948642453, + "angularVelocity": 3.352121601432042, + "velocityX": 0.282511802606821, + "velocityY": 0.19030289294706518, + "timestamp": 0.07912254577845217 + }, + { + "x": 1.5673643497428493, + "y": 5.60987650843196, + "heading": 3.626081158643754, + "angularVelocity": 2.77863612218835, + "velocityX": 0.7340734587204384, + "velocityY": 0.38636547997701703, + "timestamp": 0.15824509155690433 + }, + { + "x": 1.639918967505268, + "y": 5.662681063987355, + "heading": 3.7707265189248944, + "angularVelocity": 1.828118128126927, + "velocityX": 0.9169904361466855, + "velocityY": 0.6673768523986875, + "timestamp": 0.23736763733535649 + }, + { + "x": 1.719103738696596, + "y": 5.732106202864848, + "heading": 3.861411095653706, + "angularVelocity": 1.1461281463659418, + "velocityX": 1.000786443513203, + "velocityY": 0.8774381333973873, + "timestamp": 0.31649018311380867 + }, + { + "x": 1.8011465793982657, + "y": 5.8124573886259565, + "heading": 3.9165727578512475, + "angularVelocity": 0.6971674338183805, + "velocityX": 1.0369085055907419, + "velocityY": 1.0155283171258929, + "timestamp": 0.39561272889226085 + }, + { + "x": 1.8844011608490543, + "y": 5.899562457377884, + "heading": 3.9495869202931786, + "angularVelocity": 0.41725354154267424, + "velocityX": 1.0522232396807132, + "velocityY": 1.1008880957373124, + "timestamp": 0.474735274670713 + }, + { + "x": 1.968168332271066, + "y": 5.990724119399961, + "heading": 3.96923386729424, + "angularVelocity": 0.24831034956931133, + "velocityX": 1.0587016709063517, + "velocityY": 1.1521578473642025, + "timestamp": 0.5538578204491652 + }, + { + "x": 2.05213613980173, + "y": 6.084290626509761, + "heading": 3.9810005346780404, + "angularVelocity": 0.1487144690307097, + "velocityX": 1.0612374349756033, + "velocityY": 1.1825517769839073, + "timestamp": 0.6329803662276173 + }, + { + "x": 2.1361397889017213, + "y": 6.179275610733387, + "heading": 3.988261371091814, + "angularVelocity": 0.0917669716303672, + "velocityX": 1.0616904230458737, + "velocityY": 1.2004793739775566, + "timestamp": 0.7121029120060695 + }, + { + "x": 2.2200564987148903, + "y": 6.2750981229467735, + "heading": 3.9931304198236632, + "angularVelocity": 0.061538069635463585, + "velocityX": 1.0605916302053886, + "velocityY": 1.2110645742073904, + "timestamp": 0.7912254577845216 + }, + { + "x": 2.3037490732526433, + "y": 6.3714204686821265, + "heading": 3.997036412999259, + "angularVelocity": 0.049366373859271756, + "velocityX": 1.0577588690345867, + "velocityY": 1.217381781484405, + "timestamp": 0.8703480035629737 + }, + { + "x": 2.3870186605553965, + "y": 6.4680502574631, + "heading": 4.001128510533661, + "angularVelocity": 0.05171847662561717, + "velocityX": 1.0524128929813872, + "velocityY": 1.221267438127472, + "timestamp": 0.9494705493414258 + }, + { + "x": 2.4696215654523987, + "y": 6.564870888609008, + "heading": 4.006432047635611, + "angularVelocity": 0.06702940419537926, + "velocityX": 1.0439869456209716, + "velocityY": 1.2236794227654333, + "timestamp": 1.028593095119878 + }, + { + "x": 2.5522006651677756, + "y": 6.661687676467445, + "heading": 4.0117852212202445, + "angularVelocity": 0.06765674097018534, + "velocityX": 1.043686080913052, + "velocityY": 1.223630848905317, + "timestamp": 1.1077156408983302 + }, + { + "x": 2.626171983754375, + "y": 6.756004010665758, + "heading": 4.047305777483365, + "angularVelocity": 0.44893090728627943, + "velocityX": 0.9348955832857486, + "velocityY": 1.1920285586159498, + "timestamp": 1.1868381866767823 + }, + { + "x": 2.664895057678222, "y": 6.813176155090332, "heading": 3.896073037200959, - "angularVelocity": -0.5902969890929726, - "velocityX": -0.09892303130498589, - "velocityY": 0.5171151707845827, - "timestamp": 1.1643913810010889 - }, - { - "x": 2.659637231596543, - "y": 6.825031823463049, - "heading": 3.877816032306183, - "angularVelocity": -0.6840416135896586, - "velocityX": -0.19699681615987222, - "velocityY": 0.44420049020077496, - "timestamp": 1.191081284849143 - }, - { - "x": 2.6517927610129908, - "y": 6.834893701518374, - "heading": 3.857106562184998, - "angularVelocity": -0.775928989444263, - "velocityX": -0.29391153404714393, - "velocityY": 0.36949844823228756, - "timestamp": 1.2177711886971971 - }, - { - "x": 2.641397527843252, - "y": 6.842708311525597, - "heading": 3.834005474054525, - "angularVelocity": -0.865536581247652, - "velocityX": -0.38948185159896215, - "velocityY": 0.2927927373478789, - "timestamp": 1.2444610925452513 - }, - { - "x": 2.628493660297315, - "y": 6.848415370045661, - "heading": 3.808587288042731, - "angularVelocity": -0.9523521012476956, - "velocityX": -0.4834737367133058, - "velocityY": 0.2138283656829084, - "timestamp": 1.2711509963933054 - }, - { - "x": 2.6131312860336138, - "y": 6.85194659507741, - "heading": 3.7809426101756807, - "angularVelocity": -1.0357728534516963, - "velocityX": -0.5755874712460319, - "velocityY": 0.13230564830254915, - "timestamp": 1.2978409002413596 - }, - { - "x": 2.5953708985643273, - "y": 6.853224371337149, - "heading": 3.7511803744930092, - "angularVelocity": -1.1151121357389708, - "velocityX": -0.6654346741148351, - "velocityY": 0.0478748918322195, - "timestamp": 1.3245308040894137 - }, - { - "x": 2.5752865454465885, - "y": 6.852160328589509, - "heading": 3.7194297145938124, - "angularVelocity": -1.1896131241218921, - "velocityX": -0.752507511157751, - "velocityY": -0.03986686327897337, - "timestamp": 1.3512207079374678 - }, - { - "x": 2.5529701073140734, - "y": 6.848653945190506, - "heading": 3.6858413880737997, - "angularVelocity": -1.2584656247257915, - "velocityX": -0.8361378242335784, - "velocityY": -0.1313748981249787, - "timestamp": 1.377910611785522 - }, - { - "x": 2.5285370169793717, - "y": 6.842591376045072, - "heading": 3.650589102058915, - "angularVelocity": -1.3208097794422922, - "velocityX": -0.9154431755842751, - "velocityY": -0.2271484071260898, - "timestamp": 1.4046005156335761 - }, - { - "x": 2.502133887928051, - "y": 6.833844846711091, - "heading": 3.613872024847483, - "angularVelocity": -1.375691625584805, - "velocityX": -0.9892553079859069, - "velocityY": -0.32770928601974636, - "timestamp": 1.4312904194816303 - }, - { - "x": 2.4739487064187284, - "y": 6.822273243464229, - "heading": 3.5759212160148097, - "angularVelocity": -1.4219162814796094, - "velocityX": -1.0560240932219476, - "velocityY": -0.43355732237695227, - "timestamp": 1.4579803233296844 - }, - { - "x": 2.4442244144939402, - "y": 6.807725217915028, - "heading": 3.5370136666138436, - "angularVelocity": -1.4577628163243674, - "velocityX": -1.113690483637882, - "velocityY": -0.54507598199017, - "timestamp": 1.4846702271777386 - }, - { - "x": 2.4132762941421024, - "y": 6.79004764808573, - "heading": 3.497494895227568, - "angularVelocity": -1.4806636850869153, - "velocityX": -1.1595440930782523, - "velocityY": -0.6623317165148433, - "timestamp": 1.5113601310257927 - }, - { - "x": 2.3815108616317744, - "y": 6.769104478274178, - "heading": 3.45780088591702, - "angularVelocity": -1.4872293859328474, - "velocityX": -1.1901666147307672, - "velocityY": -0.7846850978100616, - "timestamp": 1.5380500348738468 - }, - { - "x": 2.3494374003667597, - "y": 6.744810785756249, - "heading": 3.4184626098399313, - "angularVelocity": -1.4739010039542162, - "velocityX": -1.2017076362510986, - "velocityY": -0.910220308631834, - "timestamp": 1.564739938721901 - }, - { - "x": 2.317656105280672, - "y": 6.717179467626764, - "heading": 3.3800987305060795, - "angularVelocity": -1.4373929390026206, - "velocityX": -1.190760943427104, - "velocityY": -1.0352722994728694, - "timestamp": 1.5914298425699551 - }, - { - "x": 2.2868057964945567, - "y": 6.686361396326711, - "heading": 3.343389515038899, - "angularVelocity": -1.3753970668521929, - "velocityX": -1.1558793527974576, - "velocityY": -1.1546714995865741, - "timestamp": 1.6181197464180093 - }, - { - "x": 2.25748075399746, - "y": 6.652639268574606, - "heading": 3.308925014353095, - "angularVelocity": -1.291293549876024, - "velocityX": -1.0987316651286732, - "velocityY": -1.263478802474685, - "timestamp": 1.6448096502660634 - }, - { - "x": 2.2301787041350396, - "y": 6.616374519308364, - "heading": 3.2770382136187797, - "angularVelocity": -1.194713960599006, - "velocityX": -1.022935489683713, - "velocityY": -1.3587440956212415, - "timestamp": 1.6714995541141175 - }, - { - "x": 2.2052911587380817, - "y": 6.577965167741144, - "heading": 3.2479010535236887, - "angularVelocity": -1.0916922091952401, - "velocityX": -0.9324704029899484, - "velocityY": -1.4390966631383846, - "timestamp": 1.6981894579621717 - }, - { - "x": 2.183095873519221, - "y": 6.537812097974725, - "heading": 3.2217255041499464, - "angularVelocity": -0.9807285002883435, - "velocityX": -0.8315985454731583, - "velocityY": -1.504429165238335, - "timestamp": 1.7248793618102258 - }, - { - "x": 2.163764936777705, - "y": 6.4962772278935255, - "heading": 3.198795874429834, - "angularVelocity": -0.8591124887766933, - "velocityX": -0.72427899521733, - "velocityY": -1.5562015628702863, - "timestamp": 1.75156926565828 - }, - { - "x": 2.1473967460074728, - "y": 6.453660043976164, - "heading": 3.1793849811459, - "angularVelocity": -0.7272747550699427, - "velocityX": -0.61327275150245, - "velocityY": -1.596752995439064, - "timestamp": 1.7782591695063341 - }, - { - "x": 2.134047988560311, - "y": 6.410199882298709, - "heading": 3.163697789085614, - "angularVelocity": -0.5877575336948715, - "velocityX": -0.5001425828716374, - "velocityY": -1.628337139199698, - "timestamp": 1.8049490733543883 - }, - { - "x": 2.1237534679344243, - "y": 6.366088523528368, - "heading": 3.151864532890396, - "angularVelocity": -0.443360765276075, - "velocityX": -0.38570841935190137, - "velocityY": -1.6527357693556206, - "timestamp": 1.8316389772024424 - }, - { - "x": 2.116536197487218, - "y": 6.321482513089734, - "heading": 3.143955647543306, - "angularVelocity": -0.29632498461271933, - "velocityX": -0.27041200628876155, - "velocityY": -1.671269057115243, - "timestamp": 1.8583288810504965 - }, - { - "x": 2.1124122616124033, - "y": 6.276512434450225, - "heading": 3.14, - "angularVelocity": -0.1482076355848078, - "velocityX": -0.15451295359818015, - "velocityY": -1.6849097282449244, - "timestamp": 1.8850187848985507 - }, - { - "x": 2.1113932132720947, + "angularVelocity": -1.9113735382816162, + "velocityX": 0.4894063195625013, + "velocityY": 0.7225771600506798, + "timestamp": 1.2659607324552344 + }, + { + "x": 2.67401621750823, + "y": 6.8287147997218245, + "heading": 3.8216991351700837, + "angularVelocity": -2.7569472556880648, + "velocityX": 0.338109953564034, + "velocityY": 0.5759980652369127, + "timestamp": 1.2929376366660208 + }, + { + "x": 2.6786727556459358, + "y": 6.8400421309827735, + "heading": 3.72670670460024, + "angularVelocity": -3.521250245306635, + "velocityX": 0.17261202773019518, + "velocityY": 0.41988996114756666, + "timestamp": 1.3199145408768072 + }, + { + "x": 2.678518823316929, + "y": 6.846619952079936, + "heading": 3.614568928899329, + "angularVelocity": -4.15680668266135, + "velocityX": -0.00570607834774554, + "velocityY": 0.24383157703223057, + "timestamp": 1.3468914450875935 + }, + { + "x": 2.6741686792707893, + "y": 6.846169187557711, + "heading": 3.4948207380964975, + "angularVelocity": -4.438915224191635, + "velocityX": -0.16125438308832984, + "velocityY": -0.0167092754120811, + "timestamp": 1.3738683492983799 + }, + { + "x": 2.663571596898345, + "y": 6.842490941859769, + "heading": 3.391106479438399, + "angularVelocity": -3.844557472114035, + "velocityX": -0.3928205508549501, + "velocityY": -0.1363479541318252, + "timestamp": 1.4008452535091662 + }, + { + "x": 2.648321534336774, + "y": 6.83512618854535, + "heading": 3.3066791856345255, + "angularVelocity": -3.129613878011262, + "velocityX": -0.5653006898944218, + "velocityY": -0.2730021672197161, + "timestamp": 1.4278221577199526 + }, + { + "x": 2.630755187717048, + "y": 6.821913643765559, + "heading": 3.2403542901418882, + "angularVelocity": -2.4585806797690686, + "velocityX": -0.6511624344467275, + "velocityY": -0.4897724615306165, + "timestamp": 1.454799061930739 + }, + { + "x": 2.610177790375068, + "y": 6.803317677931451, + "heading": 3.192043949934185, + "angularVelocity": -1.7908037123240415, + "velocityX": -0.7627783077407181, + "velocityY": -0.6893291271973806, + "timestamp": 1.4817759661415253 + }, + { + "x": 2.585880089062531, + "y": 6.779956801667491, + "heading": 3.1617447456020207, + "angularVelocity": -1.1231534980960616, + "velocityX": -0.9006853092812849, + "velocityY": -0.8659583798581432, + "timestamp": 1.5087528703523116 + }, + { + "x": 2.557481302008236, + "y": 6.752192795634918, + "heading": 3.1494506971418432, + "angularVelocity": -0.4557249550980476, + "velocityX": -1.0527074134380248, + "velocityY": -1.029176877214801, + "timestamp": 1.535729774563098 + }, + { + "x": 2.5262145192081267, + "y": 6.721488912807054, + "heading": 3.1494498734965517, + "angularVelocity": -0.00003053149779678828, + "velocityX": -1.1590204181995478, + "velocityY": -1.1381544223143767, + "timestamp": 1.5627066787738844 + }, + { + "x": 2.494951526671565, + "y": 6.690781099940809, + "heading": 3.1494491983381225, + "angularVelocity": -0.000025027276073123234, + "velocityX": -1.1588799178840683, + "velocityY": -1.1383001039076166, + "timestamp": 1.5896835829846707 + }, + { + "x": 2.46368853251831, + "y": 6.660073288720487, + "heading": 3.149448523179691, + "angularVelocity": -0.000025027276154336604, + "velocityX": -1.1588799778128431, + "velocityY": -1.138300042895271, + "timestamp": 1.616660487195457 + }, + { + "x": 2.4324255384011386, + "y": 6.629365477463429, + "heading": 3.1494478480212726, + "angularVelocity": -0.000025027275679943262, + "velocityX": -1.1588799764752862, + "velocityY": -1.138300044257102, + "timestamp": 1.6436373914062434 + }, + { + "x": 2.401162544284488, + "y": 6.5986576662058365, + "heading": 3.1494471728628666, + "angularVelocity": -0.000025027275214919653, + "velocityX": -1.1588799764559639, + "velocityY": -1.1383000442768603, + "timestamp": 1.6706142956170298 + }, + { + "x": 2.3698995501679407, + "y": 6.567949854948137, + "heading": 3.1494464977044734, + "angularVelocity": -0.000025027274749328804, + "velocityX": -1.1588799764521394, + "velocityY": -1.1383000442808404, + "timestamp": 1.6975911998278161 + }, + { + "x": 2.3386365560514903, + "y": 6.537242043690337, + "heading": 3.1494458225460926, + "angularVelocity": -0.000025027274284044715, + "velocityX": -1.1588799764485511, + "velocityY": -1.1383000442845796, + "timestamp": 1.7245681040386025 + }, + { + "x": 2.307373561935169, + "y": 6.5065342324324025, + "heading": 3.1494451473877243, + "angularVelocity": -0.00002502727381983999, + "velocityX": -1.1588799764437645, + "velocityY": -1.1383000442895392, + "timestamp": 1.7515450082493889 + }, + { + "x": 2.2761105678205396, + "y": 6.475826421172744, + "heading": 3.1494444722293684, + "angularVelocity": -0.00002502727335405626, + "velocityX": -1.158879976381056, + "velocityY": -1.1383000443534677, + "timestamp": 1.7785219124601752 + }, + { + "x": 2.244847573837531, + "y": 6.445118609779082, + "heading": 3.1494437970710263, + "angularVelocity": -0.000025027272855365495, + "velocityX": -1.1588799715020328, + "velocityY": -1.138300049320807, + "timestamp": 1.8054988166709616 + }, + { + "x": 2.2135845753549397, + "y": 6.414410802966355, + "heading": 3.149443121912666, + "angularVelocity": -0.000025027273518628322, + "velocityX": -1.1588801382959264, + "velocityY": -1.1382998795112838, + "timestamp": 1.832475720881748 + }, + { + "x": 2.182332647777799, + "y": 6.3836917291800725, + "heading": 3.14944244669543, + "angularVelocity": -0.000025029455959603075, + "velocityX": -1.1584697537175366, + "velocityY": -1.1387175320883667, + "timestamp": 1.8594526250925343 + }, + { + "x": 2.153094421241092, + "y": 6.351050761951557, + "heading": 3.149440476891046, + "angularVelocity": -0.00007301817765136917, + "velocityX": -1.0838243820804343, + "velocityY": -1.2099597112211005, + "timestamp": 1.8864295293033206 + }, + { + "x": 2.1314747036692956, + "y": 6.314801767366594, + "heading": 3.1444623769659996, + "angularVelocity": -0.18453191983123554, + "velocityX": -0.801415811201077, + "velocityY": -1.3437047595123932, + "timestamp": 1.913406433514107 + }, + { + "x": 2.1175303161219627, + "y": 6.274679449481932, + "heading": 3.139999999999996, + "angularVelocity": -0.16541471664709217, + "velocityX": -0.5169009549193061, + "velocityY": -1.487283995641412, + "timestamp": 1.9403833377248934 + }, + { + "x": 2.111393213272095, "y": 6.231289386749268, + "heading": 3.139999999999996, + "angularVelocity": 1.2396255317011e-17, + "velocityX": -0.22749470442979314, + "velocityY": -1.6084151981870152, + "timestamp": 1.9673602419356797 + }, + { + "x": 2.1135515859889713, + "y": 6.18871804241084, + "heading": 3.139999999999996, + "angularVelocity": 4.342266970142451e-18, + "velocityX": 0.08225287430098827, + "velocityY": -1.6223404824025023, + "timestamp": 1.9936009385096722 + }, + { + "x": 2.1237593069102005, + "y": 6.1473322849843255, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422681619637e-18, + "velocityX": 0.3890034280317459, + "velocityY": -1.5771592537498764, + "timestamp": 2.0198416350836648 + }, + { + "x": 2.141644452417822, + "y": 6.1086399046863455, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422691347276405e-18, + "velocityX": 0.6815804396497286, + "velocityY": -1.4745180330437861, + "timestamp": 2.0460823316576575 + }, + { + "x": 2.1665552441015765, + "y": 6.0740504370872355, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422696264537505e-18, + "velocityX": 0.9493189943912999, + "velocityY": -1.3181611814907124, + "timestamp": 2.0723230282316503 + }, + { + "x": 2.1935012145011026, + "y": 6.041021760141006, + "heading": 3.139999999999997, + "angularVelocity": 4.34227316559682e-18, + "velocityX": 1.0268770999862573, + "velocityY": -1.2586814093559167, + "timestamp": 2.098563724805643 + }, + { + "x": 2.220442534619127, + "y": 6.00798928987496, + "heading": 3.139999999999997, + "angularVelocity": 4.342268965346379e-18, + "velocityX": 1.0266998835971468, + "velocityY": -1.2588259680112164, + "timestamp": 2.1248044213796358 + }, + { + "x": 2.247383856273639, + "y": 5.974956820862076, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269673161453e-18, + "velocityX": 1.0266999421507437, + "velocityY": -1.2588259202547785, + "timestamp": 2.1510451179536285 + }, + { + "x": 2.274325177898134, + "y": 5.941924351824711, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269691700593e-18, + "velocityX": 1.0266999410068467, + "velocityY": -1.2588259211877415, + "timestamp": 2.1772858145276213 + }, + { + "x": 2.3012664995222347, + "y": 5.908891882787024, + "heading": 3.1399999999999975, + "angularVelocity": 4.3422697108078704e-18, + "velocityX": 1.0266999409917994, + "velocityY": -1.258825921200014, + "timestamp": 2.203526511101614 + }, + { + "x": 2.328207821146328, + "y": 5.87585941374933, + "heading": 3.139999999999998, + "angularVelocity": 4.3422696965810506e-18, + "velocityX": 1.0266999409915183, + "velocityY": -1.2588259212002433, + "timestamp": 2.2297672076756068 + }, + { + "x": 2.355149142770421, + "y": 5.8428269447116365, + "heading": 3.139999999999998, + "angularVelocity": 4.342269684490725e-18, + "velocityX": 1.0266999409915205, + "velocityY": -1.2588259212002417, + "timestamp": 2.2560079042495995 + }, + { + "x": 2.3820904643945138, + "y": 5.809794475673942, + "heading": 3.139999999999998, + "angularVelocity": 4.342269693460261e-18, + "velocityX": 1.026699940991488, + "velocityY": -1.258825921200268, + "timestamp": 2.2822486008235923 + }, + { + "x": 2.4090317860186072, + "y": 5.7767620066362495, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269692405428e-18, + "velocityX": 1.026699940991538, + "velocityY": -1.258825921200227, + "timestamp": 2.308489297397585 + }, + { + "x": 2.4359731076427, + "y": 5.743729537598556, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269716594298e-18, + "velocityX": 1.0266999409914905, + "velocityY": -1.258825921200266, + "timestamp": 2.3347299939715778 + }, + { + "x": 2.4629144292667973, + "y": 5.710697068560865, + "heading": 3.139999999999999, + "angularVelocity": 4.342269681682682e-18, + "velocityX": 1.026699940991683, + "velocityY": -1.258825921200109, + "timestamp": 2.3609706905455705 + }, + { + "x": 2.4898557508911936, + "y": 5.677664599523419, + "heading": 3.139999999999999, + "angularVelocity": 4.3422696993715505e-18, + "velocityX": 1.0266999410030797, + "velocityY": -1.2588259211908142, + "timestamp": 2.3872113871195633 + }, + { + "x": 2.5167970725019893, + "y": 5.644632130474532, + "heading": 3.139999999999999, + "angularVelocity": 4.342269692409483e-18, + "velocityX": 1.026699940484769, + "velocityY": -1.2588259216268345, + "timestamp": 2.413452083693556 + }, + { + "x": 2.5425090771755396, + "y": 5.613106913889164, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374488911e-18, + "velocityX": 0.9798522154718704, + "velocityY": -1.2013864226688296, + "timestamp": 2.4396927802675488 + }, + { + "x": 2.563078686478754, + "y": 5.587886733807481, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374369648e-18, + "velocityX": 0.7838819844288143, + "velocityY": -0.9611093977847911, + "timestamp": 2.4659334768415415 + }, + { + "x": 2.578505895291019, + "y": 5.568971596499493, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374398685e-18, + "velocityX": 0.5879115582455986, + "velocityY": -0.7208321339584491, + "timestamp": 2.4921741734155343 + }, + { + "x": 2.5887907019054897, + "y": 5.5563615040551815, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374398075e-18, + "velocityX": 0.39194106701667647, + "velocityY": -0.48055479048560223, + "timestamp": 2.518414869989527 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374359544e-18, + "velocityX": 0.19597054326510124, + "velocityY": -0.2402774071896897, + "timestamp": 2.5446555665635198 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, "heading": 3.14, - "angularVelocity": 1.1480104978612554e-22, - "velocityX": -0.038181042019109025, - "velocityY": -1.6943878089038094, - "timestamp": 1.9117086887466048 - }, - { - "x": 2.1139722457961088, - "y": 6.184041017313733, - "heading": 3.14, - "angularVelocity": 1.326030822415817e-23, - "velocityX": 0.09287464315336291, - "velocityY": -1.7014812376514836, - "timestamp": 1.9394776516890055 - }, - { - "x": 2.1201947185383916, - "y": 6.136700429098736, - "heading": 3.14, - "angularVelocity": 1.3260179507784046e-23, - "velocityX": 0.224080126981686, - "velocityY": -1.7048021675563763, - "timestamp": 1.9672466146314063 - }, - { - "x": 2.13006161842343, - "y": 6.089396035622716, - "heading": 3.14, - "angularVelocity": 1.326007041519161e-23, - "velocityX": 0.35532115136976367, - "velocityY": -1.7034987433322955, - "timestamp": 1.995015577573807 - }, - { - "x": 2.1435678181376194, - "y": 6.042288248235824, - "heading": 3.14, - "angularVelocity": 1.3260437690465338e-23, - "velocityX": 0.4863775338749469, - "velocityY": -1.6964186773774967, - "timestamp": 2.022784540516208 - }, - { - "x": 2.1606964661173613, - "y": 5.995581737791835, - "heading": 3.14, - "angularVelocity": 1.325999769616951e-23, - "velocityX": 0.6168270675167464, - "velocityY": -1.6819681217793025, - "timestamp": 2.050553503458609 - }, - { - "x": 2.181407907708738, - "y": 5.949543614413858, - "heading": 3.14, - "angularVelocity": 1.3260185229673763e-23, - "velocityX": 0.7458485804578618, - "velocityY": -1.6578985493073877, - "timestamp": 2.07832246640101 - }, - { - "x": 2.2056170475643233, - "y": 5.904529822011414, - "heading": 3.14, - "angularVelocity": 1.3260157963898354e-23, - "velocityX": 0.871805688451557, - "velocityY": -1.6210109284892367, - "timestamp": 2.1060914293434108 - }, - { - "x": 2.2331463169007804, - "y": 5.861019319967013, - "heading": 3.14, - "angularVelocity": 1.326020172336188e-23, - "velocityX": 0.9913682910506778, - "velocityY": -1.566875296519094, - "timestamp": 2.1338603922858117 - }, - { - "x": 2.2636325105277852, - "y": 5.819639483049468, - "heading": 3.14, - "angularVelocity": 1.32601511090322e-23, - "velocityX": 1.0978513562152263, - "velocityY": -1.4901470034504147, - "timestamp": 2.1616293552282126 - }, - { - "x": 2.2963834267173127, - "y": 5.781115195207252, - "heading": 3.14, - "angularVelocity": 1.3260107576996925e-23, - "velocityX": 1.1794072489296905, - "velocityY": -1.387314604514559, - "timestamp": 2.1893983181706136 - }, - { - "x": 2.3303148904385163, - "y": 5.746039013141612, - "heading": 3.14, - "angularVelocity": 1.3260240855176863e-23, - "velocityX": 1.221920450957617, - "velocityY": -1.2631433928013844, - "timestamp": 2.2171672811130145 - }, - { - "x": 2.3642062447518297, - "y": 5.714607213055508, - "heading": 3.14, - "angularVelocity": 1.326023555417506e-23, - "velocityX": 1.2204760539171806, - "velocityY": -1.1319039948052778, - "timestamp": 2.2449362440554155 - }, - { - "x": 2.397064742551735, - "y": 5.68667060145179, - "heading": 3.14, - "angularVelocity": 1.3260077501265654e-23, - "velocityX": 1.1832814163085987, - "velocityY": -1.0060372676381828, - "timestamp": 2.2727052069978164 - }, - { - "x": 2.4282178000518853, - "y": 5.661954954768778, - "heading": 3.14, - "angularVelocity": 1.3260249833501151e-23, - "velocityX": 1.1218660763374204, - "velocityY": -0.8900457224231765, - "timestamp": 2.3004741699402174 - }, - { - "x": 2.457231034537111, - "y": 5.640189613922927, - "heading": 3.14, - "angularVelocity": 1.3260141947431602e-23, - "velocityX": 1.0448079946451803, - "velocityY": -0.7838009972139435, - "timestamp": 2.3282431328826183 - }, - { - "x": 2.483818861573163, - "y": 5.621144668272307, - "heading": 3.14, - "angularVelocity": 1.3260165607649488e-23, - "velocityX": 0.9574656097601072, - "velocityY": -0.6858356824532662, - "timestamp": 2.3560120958250192 - }, - { - "x": 2.507786212179532, - "y": 5.604632755958765, - "heading": 3.14, - "angularVelocity": 1.3260126001429812e-23, - "velocityX": 0.8630985123961222, - "velocityY": -0.5946175356923091, - "timestamp": 2.38378105876742 - }, - { - "x": 2.5289943781295285, - "y": 5.590502031840209, - "heading": 3.14, - "angularVelocity": 1.3260018717887446e-23, - "velocityX": 0.763736333761809, - "velocityY": -0.5088675492802951, - "timestamp": 2.411550021709821 - }, - { - "x": 2.547341071809568, - "y": 5.578628548447832, - "heading": 3.14, - "angularVelocity": 1.3260377381509874e-23, - "velocityX": 0.6606906321310845, - "velocityY": -0.4275810881740915, - "timestamp": 2.439318984652222 - }, - { - "x": 2.562748457639991, - "y": 5.568909973472095, - "heading": 3.14, - "angularVelocity": 1.3260175762620676e-23, - "velocityX": 0.5548419601546171, - "velocityY": -0.3499797596293578, - "timestamp": 2.467087947594623 - }, - { - "x": 2.575155705150767, - "y": 5.561260778200535, - "heading": 3.14, - "angularVelocity": 1.3260285574981282e-23, - "velocityX": 0.4468026961075851, - "velocityY": -0.2754584421256751, - "timestamp": 2.494856910537024 - }, - { - "x": 2.5845141850851863, - "y": 5.555608625671519, - "heading": 3.14, - "angularVelocity": 1.326006595026007e-23, - "velocityX": 0.33701222310069334, - "velocityY": -0.2035420818825477, - "timestamp": 2.522625873479425 - }, - { - "x": 2.5907842648688533, - "y": 5.55189166019714, - "heading": 3.14, - "angularVelocity": 1.3260302513246253e-23, - "velocityX": 0.22579452450827125, - "velocityY": -0.1338532332694335, - "timestamp": 2.550394836421826 - }, - { - "x": 2.59393310546875, - "y": 5.550056457519531, - "heading": 3.14, - "angularVelocity": 1.3260089047889746e-23, - "velocityX": 0.11339424545409957, - "velocityY": -0.06608826845335893, - "timestamp": 2.578163799364227 - }, - { - "x": 2.59393310546875, - "y": 5.550056457519531, - "heading": 3.14, - "angularVelocity": 4.554749859713047e-24, - "velocityX": -4.596278509340535e-25, - "velocityY": -1.5195442763313795e-23, - "timestamp": 2.6059327623066277 + "angularVelocity": 3.0510058962998796e-17, + "velocityX": 1.3525026943280928e-16, + "velocityY": 8.359694800915289e-17, + "timestamp": 2.5708962631375125 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.2.traj b/src/main/deploy/choreo/Test1.2.traj index 714f9e0e..523e0fc4 100644 --- a/src/main/deploy/choreo/Test1.2.traj +++ b/src/main/deploy/choreo/Test1.2.traj @@ -1,445 +1,409 @@ { "samples": [ { - "x": 2.59393310546875, - "y": 5.550056457519531, + "x": 2.5939331054687496, + "y": 5.55005645751953, "heading": 3.14, - "angularVelocity": 4.554749859713047e-24, - "velocityX": -4.596278509340535e-25, - "velocityY": -1.5195442763313795e-23, + "angularVelocity": 3.0510058962998796e-17, + "velocityX": 1.3525026943280928e-16, + "velocityY": 8.359694800915289e-17, "timestamp": 0 }, { - "x": 2.5899783566862378, - "y": 5.5489813066194555, - "heading": 3.1367490851985087, - "angularVelocity": -0.1085265960704612, - "velocityX": -0.13202296888338985, - "velocityY": -0.03589219483504766, - "timestamp": 0.029955005677878788 - }, - { - "x": 2.5820812048170954, - "y": 5.546779098674632, - "heading": 3.130275233897137, - "angularVelocity": -0.21611918124764368, - "velocityX": -0.26363379643671836, - "velocityY": -0.07351719337009978, - "timestamp": 0.059910011355757575 - }, - { - "x": 2.5702568313825034, - "y": 5.5433888083370855, - "heading": 3.1206149262957497, - "angularVelocity": -0.32249393324338926, - "velocityX": -0.3947378131636905, - "velocityY": -0.1131794256360211, - "timestamp": 0.08986501703363636 - }, - { - "x": 2.554524436678559, - "y": 5.5387376635070815, - "heading": 3.107814789143658, - "angularVelocity": -0.42731212571741595, - "velocityX": -0.5252008586852795, - "velocityY": -0.15527103817038457, - "timestamp": 0.11982002271151515 - }, - { - "x": 2.5349090755324424, - "y": 5.532737501107672, - "heading": 3.0919335422287366, - "angularVelocity": -0.5301700519005096, - "velocityX": -0.6548274888361012, - "velocityY": -0.20030583415450579, - "timestamp": 0.14977502838939394 - }, - { - "x": 2.511444606178007, - "y": 5.52527960958652, - "heading": 3.0730442108839475, - "angularVelocity": -0.6305901440285391, - "velocityX": -0.7833238159511905, - "velocityY": -0.24896979160511046, - "timestamp": 0.17973003406727273 - }, - { - "x": 2.4841786133457258, - "y": 5.5162272720643095, - "heading": 3.0512365155339536, - "angularVelocity": -0.7280150631418102, - "velocityX": -0.9102316028741883, - "velocityY": -0.3021978236143635, - "timestamp": 0.2096850397451515 - }, - { - "x": 2.4531809940870857, - "y": 5.505404760057258, - "heading": 3.026619340133933, - "angularVelocity": -0.8218050653951327, - "velocityX": -1.03480598842053, - "velocityY": -0.3612922702613114, - "timestamp": 0.2396400454230303 - }, - { - "x": 2.4185596883958564, - "y": 5.492580836854246, - "heading": 2.999323661656212, - "angularVelocity": -0.9112226107130541, - "velocityX": -1.1557769697502154, - "velocityY": -0.4281061850200961, - "timestamp": 0.2695950511009091 - }, - { - "x": 2.380491125275952, - "y": 5.477444069032361, - "heading": 2.9695096535475827, - "angularVelocity": -0.9952930214480606, - "velocityX": -1.27085815069692, - "velocityY": -0.5053168069690293, - "timestamp": 0.2995500567787879 - }, - { - "x": 2.3392828051052046, - "y": 5.459567907623698, - "heading": 2.9373981254397274, - "angularVelocity": -1.0719920554570026, - "velocityX": -1.3756739228789079, - "velocityY": -0.5967670846367005, - "timestamp": 0.32950506245666666 - }, - { - "x": 2.2955095509891357, - "y": 5.438376261665861, - "heading": 2.9034010779477137, - "angularVelocity": -1.1349371072601777, - "velocityX": -1.4613001441823985, - "velocityY": -0.7074492385586875, - "timestamp": 0.35946006813454545 - }, - { - "x": 2.2502877957375587, - "y": 5.413212847808716, - "heading": 2.8684201412995205, - "angularVelocity": -1.1677826746007212, - "velocityX": -1.5096560400578645, - "velocityY": -0.8400403634617765, - "timestamp": 0.38941507381242424 - }, - { - "x": 2.205434968136323, - "y": 5.383827174903616, - "heading": 2.8338778225381454, - "angularVelocity": -1.15314011730883, - "velocityX": -1.4973399799539564, - "velocityY": -0.9809937351072376, - "timestamp": 0.419370079490303 - }, - { - "x": 2.1627117665288242, - "y": 5.35082401983714, - "heading": 2.8010931886625263, - "angularVelocity": -1.0944626159703914, - "velocityX": -1.426245819043514, - "velocityY": -1.1017575967561364, - "timestamp": 0.4493250851681818 - }, - { - "x": 2.1231203758060415, - "y": 5.315102619215835, - "heading": 2.770927863402835, - "angularVelocity": -1.0070211831729985, - "velocityX": -1.3216953169206167, - "velocityY": -1.1925018811692434, - "timestamp": 0.4792800908460606 - }, - { - "x": 2.0871120973907478, - "y": 5.277368588566571, - "heading": 2.743899688375806, - "angularVelocity": -0.9022924354505801, - "velocityX": -1.2020788379247493, - "velocityY": -1.2596903187079023, - "timestamp": 0.5092350965239394 - }, - { - "x": 2.0548974336097707, - "y": 5.238102066889186, - "heading": 2.7202319811811844, - "angularVelocity": -0.7901085864957776, - "velocityX": -1.0754350751055555, - "velocityY": -1.310850082942305, - "timestamp": 0.5391901022018182 - }, - { - "x": 2.026587153392112, - "y": 5.197634903381892, - "heading": 2.6999923980627787, - "angularVelocity": -0.6756661419481035, - "velocityX": -0.9450934685873016, - "velocityY": -1.350931591950202, - "timestamp": 0.569145107879697 - }, - { - "x": 2.002245678733676, - "y": 5.1562068665429, - "heading": 2.6831832138201195, - "angularVelocity": -0.5611477568529534, - "velocityX": -0.812601236674494, - "velocityY": -1.3830088127670148, - "timestamp": 0.5991001135575758 - }, - { - "x": 1.9819131544340292, - "y": 5.113998389763002, - "heading": 2.66978325115483, - "angularVelocity": -0.44733634202529504, - "velocityX": -0.6787688347748394, - "velocityY": -1.4090625531434064, - "timestamp": 0.6290551192354545 - }, - { - "x": 1.9656157831702816, - "y": 5.071149791653941, - "heading": 2.659765787935313, - "angularVelocity": -0.33441700286222414, - "velocityX": -0.5440616983685925, - "velocityY": -1.4304319808793762, - "timestamp": 0.6590101249133333 - }, - { - "x": 1.9533711923999129, - "y": 5.027773084636261, - "heading": 2.6531059130951715, - "angularVelocity": -0.22232927984587103, - "velocityX": -0.4087660974610013, - "velocityY": -1.4480620529380386, - "timestamp": 0.6889651305912121 - }, - { - "x": 1.9451914498185667, - "y": 4.983959581247479, + "x": 2.5906368423225947, + "y": 5.5485199070890205, + "heading": 3.101591851410996, + "angularVelocity": -1.2805791583146104, + "velocityX": -0.10990183178303778, + "velocityY": -0.05123065102809662, + "timestamp": 0.029992795322007382 + }, + { + "x": 2.584621501308477, + "y": 5.544975862182894, + "heading": 3.0243371709692783, + "angularVelocity": -2.5757746022741834, + "velocityX": -0.20055953270102006, + "velocityY": -0.11816320779855842, + "timestamp": 0.059985590644014763 + }, + { + "x": 2.5759613994637824, + "y": 5.536844199254011, + "heading": 2.9135350884263085, + "angularVelocity": -3.6942899570776517, + "velocityX": -0.28873940397378167, + "velocityY": -0.2711205421688905, + "timestamp": 0.08997838596602215 + }, + { + "x": 2.56275074361348, + "y": 5.521892690270415, + "heading": 2.8248688434153846, + "angularVelocity": -2.956251461684603, + "velocityX": -0.44046097433337356, + "velocityY": -0.49850335131840534, + "timestamp": 0.11997118128802953 + }, + { + "x": 2.543962903792877, + "y": 5.501410245111836, + "heading": 2.7589030524712514, + "angularVelocity": -2.199387894182165, + "velocityX": -0.6264117638620101, + "velocityY": -0.6829121773583486, + "timestamp": 0.1499639766100369 + }, + { + "x": 2.518778715441927, + "y": 5.477002144640871, + "heading": 2.7154691851313926, + "angularVelocity": -1.448143358205586, + "velocityX": -0.839674597873837, + "velocityY": -0.8137987876364632, + "timestamp": 0.1799567719320443 + }, + { + "x": 2.486733800013198, + "y": 5.449761748396881, + "heading": 2.6943182531197865, + "angularVelocity": -0.705200425130341, + "velocityX": -1.0684204351295292, + "velocityY": -0.9082313252703665, + "timestamp": 0.20994956725405167 + }, + { + "x": 2.4478196867061426, + "y": 5.4204460978412365, + "heading": 2.694317441639593, + "angularVelocity": -0.000027055837428819432, + "velocityX": -1.2974487002285997, + "velocityY": -0.9774230858209427, + "timestamp": 0.23994236257605905 + }, + { + "x": 2.408901839144878, + "y": 5.391135398464328, + "heading": 2.6943166423252785, + "angularVelocity": -0.000026650210696695328, + "velocityX": -1.2975732052797133, + "velocityY": -0.9772580068715555, + "timestamp": 0.26993515789806644 + }, + { + "x": 2.3699840051756724, + "y": 5.361824681040097, + "heading": 2.6943158430110494, + "angularVelocity": -0.00002665020784043696, + "velocityX": -1.2975727521022344, + "velocityY": -0.9772586085935405, + "timestamp": 0.2999279532200738 + }, + { + "x": 2.3310661711945366, + "y": 5.332513963631788, + "heading": 2.6943150436964616, + "angularVelocity": -0.000026650219800690006, + "velocityX": -1.297572752499997, + "velocityY": -0.9772586080626477, + "timestamp": 0.3299207485420812 + }, + { + "x": 2.292148337213513, + "y": 5.303203246223414, + "heading": 2.694314244381513, + "angularVelocity": -0.000026650231834296552, + "velocityX": -1.2975727524962546, + "velocityY": -0.9772586080648237, + "timestamp": 0.3599135438640886 + }, + { + "x": 2.25323050323266, + "y": 5.273892528814898, + "heading": 2.6943134450662036, + "angularVelocity": -0.00002665024386857193, + "velocityX": -1.297572752490578, + "velocityY": -0.9772586080695672, + "timestamp": 0.38990633918609596 + }, + { + "x": 2.214312669251978, + "y": 5.2445818114062375, + "heading": 2.694312645750533, + "angularVelocity": -0.00002665025590264624, + "velocityX": -1.2975727524848706, + "velocityY": -0.9772586080743522, + "timestamp": 0.41989913450810334 + }, + { + "x": 2.1753948352714723, + "y": 5.215271093997427, + "heading": 2.694311846434502, + "angularVelocity": -0.000026650267936849635, + "velocityX": -1.2975727524790048, + "velocityY": -0.9772586080793467, + "timestamp": 0.4498919298301107 + }, + { + "x": 2.136477001291495, + "y": 5.185960376588, + "heading": 2.6943110471181098, + "angularVelocity": -0.000026650279973527825, + "velocityX": -1.2975727524613627, + "velocityY": -0.9772586080999772, + "timestamp": 0.4798847251521181 + }, + { + "x": 2.097559167357152, + "y": 5.1566496591180675, + "heading": 2.694310247801348, + "angularVelocity": -0.000026650292283470146, + "velocityX": -1.2975727509398636, + "velocityY": -0.9772586101172457, + "timestamp": 0.5098775204741255 + }, + { + "x": 2.058641302557044, + "y": 5.127338982627939, + "heading": 2.694309448489961, + "angularVelocity": -0.000026650113093683626, + "velocityX": -1.2975737800459084, + "velocityY": -0.9772572437956306, + "timestamp": 0.5398703157961329 + }, + { + "x": 2.0197569370912083, + "y": 5.0979839038638834, + "heading": 2.6943086027536207, + "angularVelocity": -0.000028197983267341566, + "velocityX": -1.2964568673313885, + "velocityY": -0.9787376751348899, + "timestamp": 0.5698631111181403 + }, + { + "x": 1.9877778714785685, + "y": 5.067888184962114, + "heading": 2.678621118987198, + "angularVelocity": -0.5230417371285907, + "velocityX": -1.0662249139900737, + "velocityY": -1.0034316101155079, + "timestamp": 0.5998559064401476 + }, + { + "x": 1.9640654183560038, + "y": 5.031205594348394, + "heading": 2.6596531558314034, + "angularVelocity": -0.6324173173088112, + "velocityX": -0.7906049725553251, + "velocityY": -1.223046742388833, + "timestamp": 0.629848701762155 + }, + { + "x": 1.9481708008996033, + "y": 4.987987936601904, "heading": 2.649783348569472, - "angularVelocity": -0.11091850762536054, - "velocityX": -0.27306763581709725, - "velocityY": -1.4626438018383596, - "timestamp": 0.7189201362690909 + "angularVelocity": -0.32907260414160233, + "velocityX": -0.5299478519964009, + "velocityY": -1.4409346405525878, + "timestamp": 0.6598414970841624 }, { - "x": 1.941084861755371, + "x": 1.9410848617553718, "y": 4.939785003662109, "heading": 2.649783348569472, - "angularVelocity": 6.132292113202646e-25, - "velocityX": -0.13709188064779135, - "velocityY": -1.4746976869375825, - "timestamp": 0.7488751419469697 + "angularVelocity": 6.681257540282107e-18, + "velocityX": -0.23625470944103766, + "velocityY": -1.6071503980304298, + "timestamp": 0.6898342924061698 }, { - "x": 1.9419022015392158, - "y": 4.889062683793002, + "x": 1.9456173734407256, + "y": 4.888006870907468, "heading": 2.649783348569472, - "angularVelocity": 4.614715107153485e-26, - "velocityX": 0.023936675164732843, - "velocityY": -1.4854577230996109, - "timestamp": 0.7830210614708646 + "angularVelocity": -1.484655388526889e-19, + "velocityX": 0.14165581617647935, + "velocityY": -1.6182360167593688, + "timestamp": 0.7218309432150796 }, { - "x": 1.948223657679148, - "y": 4.838070468392074, + "x": 1.9619962278910381, + "y": 4.838678856610239, "heading": 2.649783348569472, - "angularVelocity": 4.653945144562686e-26, - "velocityX": 0.18513064600613907, - "velocityY": -1.4933619042018718, - "timestamp": 0.8171669809947595 + "angularVelocity": -1.4846556369937314e-19, + "velocityX": 0.5118927774045035, + "velocityY": -1.5416618005383362, + "timestamp": 0.7538275940239894 }, { - "x": 1.9600538072506943, - "y": 4.786928471515363, + "x": 1.9893341653194017, + "y": 4.794473003544163, "heading": 2.649783348569472, - "angularVelocity": 4.629261284078221e-26, - "velocityX": 0.3464586614300294, - "velocityY": -1.4977484159101515, - "timestamp": 0.8513129005186544 + "angularVelocity": -1.4846558519027202e-19, + "velocityX": 0.8543999680324345, + "velocityY": -1.381577507287736, + "timestamp": 0.7858242448328991 }, { - "x": 1.977394685635232, - "y": 4.735788039854744, + "x": 2.026135553103086, + "y": 4.757769076744815, "heading": 2.649783348569472, - "angularVelocity": 4.640038416252255e-26, - "velocityX": 0.5078462851879747, - "velocityY": -1.4977025768725385, - "timestamp": 0.8854588200425493 + "angularVelocity": -1.4846559448429508e-19, + "velocityX": 1.1501637469359431, + "velocityY": -1.1471177723711978, + "timestamp": 0.8178208956418089 }, { - "x": 2.0002427480647933, - "y": 4.684845229382865, + "x": 2.0637808893572873, + "y": 4.721931260837698, "heading": 2.649783348569472, - "angularVelocity": 4.6490332666751425e-26, - "velocityX": 0.6691300966012094, - "velocityY": -1.4919150276866968, - "timestamp": 0.9196047395664442 + "angularVelocity": -1.4846636373679086e-19, + "velocityX": 1.1765398972283652, + "velocityY": -1.120048973905845, + "timestamp": 0.8498175464507187 }, { - "x": 2.0285822715254445, - "y": 4.634362832128866, + "x": 2.1014301321789435, + "y": 4.6860975489840255, "heading": 2.649783348569472, - "angularVelocity": 4.622795940866032e-26, - "velocityX": 0.8299534426308102, - "velocityY": -1.478431331119124, - "timestamp": 0.9537506590903391 + "angularVelocity": -1.484648310808098e-19, + "velocityX": 1.1766619902345172, + "velocityY": -1.1199207088167735, + "timestamp": 0.8818141972596285 }, { - "x": 2.0623699922411927, - "y": 4.5847082412928, + "x": 2.139079370280558, + "y": 4.650263832171171, "heading": 2.649783348569472, - "angularVelocity": 4.634016030607817e-26, - "velocityX": 0.9895097624213414, - "velocityY": -1.454188129311252, - "timestamp": 0.987896578614234 + "angularVelocity": -1.48465589810743e-19, + "velocityX": 1.1766618427177973, + "velocityY": -1.1199208638074638, + "timestamp": 0.9138108480685383 }, { - "x": 2.101495965027088, - "y": 4.536421557771227, + "x": 2.17672860838618, + "y": 4.614430115362526, "heading": 2.649783348569472, - "angularVelocity": 4.645177261974479e-26, - "velocityX": 1.1458462191511727, - "velocityY": -1.4141274915083855, - "timestamp": 1.0220424981381289 + "angularVelocity": -1.4846559463647802e-19, + "velocityX": 1.1766618428430395, + "velocityY": -1.1199208636758753, + "timestamp": 0.9458074988774481 }, { - "x": 2.145673252803998, - "y": 4.4903376692314145, + "x": 2.2143778464918284, + "y": 4.57859639855391, "heading": 2.649783348569472, - "angularVelocity": 4.6233288536561975e-26, - "velocityX": 1.2937794147261192, - "velocityY": -1.3496162698902718, - "timestamp": 1.0561884176620238 + "angularVelocity": -1.4846559409367017e-19, + "velocityX": 1.176661842843871, + "velocityY": -1.1199208636750018, + "timestamp": 0.9778041496863579 }, { - "x": 2.1941116699379357, - "y": 4.447748182132556, + "x": 2.2520270845974713, + "y": 4.5427626817452875, "heading": 2.649783348569472, - "angularVelocity": 4.6479494614855375e-26, - "velocityX": 1.4185711736373225, - "velocityY": -1.2472789631292271, - "timestamp": 1.0903343371859187 + "angularVelocity": -1.4846559515966518e-19, + "velocityX": 1.1766618428436975, + "velocityY": -1.1199208636751843, + "timestamp": 1.0098008004952677 }, { - "x": 2.2448201323304646, - "y": 4.410180142680831, + "x": 2.289676322703114, + "y": 4.506928964936664, "heading": 2.649783348569472, - "angularVelocity": 4.6329904581670297e-26, - "velocityX": 1.485051891985029, - "velocityY": -1.1002204648621148, - "timestamp": 1.1244802567098136 + "angularVelocity": -1.4846559610313848e-19, + "velocityX": 1.1766618428436995, + "velocityY": -1.1199208636751825, + "timestamp": 1.0417974513041774 }, { - "x": 2.294852293597042, - "y": 4.3780811675391975, + "x": 2.3273255608087573, + "y": 4.471095248128042, "heading": 2.649783348569472, - "angularVelocity": 4.6446166642422895e-26, - "velocityX": 1.4652456856979683, - "velocityY": -0.9400530309096446, - "timestamp": 1.1586261762337084 + "angularVelocity": -1.484655947429445e-19, + "velocityX": 1.1766618428436957, + "velocityY": -1.1199208636751863, + "timestamp": 1.0737941021130872 }, { - "x": 2.342149827765447, - "y": 4.350766530180341, + "x": 2.3649747989143997, + "y": 4.435261531319418, "heading": 2.649783348569472, - "angularVelocity": 4.633506801188826e-26, - "velocityX": 1.3851591882100787, - "velocityY": -0.7999385501902068, - "timestamp": 1.1927720957576033 + "angularVelocity": -1.4846559574023035e-19, + "velocityX": 1.1766618428436864, + "velocityY": -1.1199208636751963, + "timestamp": 1.105790752921997 }, { - "x": 2.3856944951859207, - "y": 4.3274872473595005, + "x": 2.4026240370200447, + "y": 4.399427814510799, "heading": 2.649783348569472, - "angularVelocity": 4.6381778689211406e-26, - "velocityX": 1.2752524467821442, - "velocityY": -0.6817588498253565, - "timestamp": 1.2269180152814982 + "angularVelocity": -1.484655924580371e-19, + "velocityX": 1.1766618428437539, + "velocityY": -1.1199208636751252, + "timestamp": 1.1377874037309068 }, { - "x": 2.4249725857215654, - "y": 4.307695719032298, + "x": 2.4402732751256884, + "y": 4.363594097702177, "heading": 2.649783348569472, - "angularVelocity": 4.6432377599415885e-26, - "velocityX": 1.150301151156826, - "velocityY": -0.5796162060697082, - "timestamp": 1.2610639348053931 + "angularVelocity": -1.4846559654067578e-19, + "velocityX": 1.1766618428437225, + "velocityY": -1.1199208636751585, + "timestamp": 1.1697840545398166 }, { - "x": 2.4596964858202144, - "y": 4.291007007021927, + "x": 2.4779225132411047, + "y": 4.327760380902407, "heading": 2.649783348569472, - "angularVelocity": 4.6331179794053255e-26, - "velocityX": 1.0169267831358226, - "velocityY": -0.4887468910800007, - "timestamp": 1.295209854329288 + "angularVelocity": -1.4846559492892923e-19, + "velocityX": 1.1766618431491565, + "velocityY": -1.1199208633985085, + "timestamp": 1.2017807053487264 }, { - "x": 2.4896890256889828, - "y": 4.277143601542489, + "x": 2.5129728702731393, + "y": 4.294400221963306, "heading": 2.649783348569472, - "angularVelocity": 4.6467164999430504e-26, - "velocityX": 0.8783638070657234, - "velocityY": -0.4060047488173824, - "timestamp": 1.329355773853183 + "angularVelocity": -1.484655203554499e-19, + "velocityX": 1.0954383082580312, + "velocityY": -1.0426140891540008, + "timestamp": 1.2337773561576362 }, { - "x": 2.514832571254234, - "y": 4.265898475268687, + "x": 2.53926064365096, + "y": 4.2693800974206075, "heading": 2.649783348569472, - "angularVelocity": 4.632856845272505e-26, - "velocityX": 0.7363557905551701, - "velocityY": -0.3293256245723021, - "timestamp": 1.3635016933770778 + "angularVelocity": -1.484655203554444e-19, + "velocityX": 0.8215789063304317, + "velocityY": -0.7819607337071065, + "timestamp": 1.265774006966546 }, { - "x": 2.5350443768209585, - "y": 4.257112226026017, + "x": 2.5567858275270727, + "y": 4.252700012844841, "heading": 2.649783348569472, - "angularVelocity": 4.629729705599566e-26, - "velocityX": 0.5919244773180152, - "velocityY": -0.2573147645510306, - "timestamp": 1.3976476129009727 - }, - { - "x": 2.550263563230007, - "y": 4.2506587632470065, - "heading": 2.649783348569472, - "angularVelocity": 4.62810585566047e-26, - "velocityX": 0.445710252388941, - "velocityY": -0.18899660249284694, - "timestamp": 1.4317935324248676 - }, - { - "x": 2.5604437164785643, - "y": 4.246436059535304, - "heading": 2.649783348569472, - "angularVelocity": 4.6519462435401186e-26, - "velocityX": 0.29813674343821717, - "velocityY": -0.12366642253542871, - "timestamp": 1.4659394519487625 + "angularVelocity": -1.4846552035544855e-19, + "velocityX": 0.5477193216495216, + "velocityY": -0.5213072041629537, + "timestamp": 1.2977706577754557 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 4.6699948971014915e-26, - "velocityX": 0.14949673474910496, - "velocityY": -0.06080051354533503, - "timestamp": 1.5000853714726574 + "angularVelocity": -1.4846552035527083e-19, + "velocityX": 0.2738596760533411, + "velocityY": -0.260653616588761, + "timestamp": 1.3297673085843655 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 1.535197914331468e-26, - "velocityX": 1.1963425727073795e-24, - "velocityY": -7.168329076335369e-24, - "timestamp": 1.5342312909965523 + "angularVelocity": -4.948850678520552e-20, + "velocityX": 6.0427487543509175e-18, + "velocityY": -9.335003878092332e-18, + "timestamp": 1.3617639593932753 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.traj b/src/main/deploy/choreo/Test1.traj index e709ba2f..a894f9a7 100644 --- a/src/main/deploy/choreo/Test1.traj +++ b/src/main/deploy/choreo/Test1.traj @@ -4,1063 +4,991 @@ "x": 1.4869295358657837, "y": 5.564249038696289, "heading": 3.141, - "angularVelocity": 8.029589971503427e-22, - "velocityX": 1.7351458241819956e-21, - "velocityY": 6.201477276822387e-22, + "angularVelocity": 3.7405663636389993e-19, + "velocityX": -1.1249906625590385e-18, + "velocityY": 2.3803295269896554e-19, "timestamp": 0 }, { - "x": 1.506153170031305, - "y": 5.579314508687984, - "heading": 3.1569331073832556, - "angularVelocity": 0.2189381700102546, - "velocityX": 0.2641535755648618, - "velocityY": 0.20701589156378014, - "timestamp": 0.07277446131256805 - }, - { - "x": 1.5445753142190985, - "y": 5.609481391551554, - "heading": 3.1887853931444132, - "angularVelocity": 0.4376849403852184, - "velocityX": 0.5279619181620503, - "velocityY": 0.41452567727023953, - "timestamp": 0.1455489226251361 - }, - { - "x": 1.6021601427736685, - "y": 5.654796415146542, - "heading": 3.2365776272564357, - "angularVelocity": 0.6567171127073494, - "velocityX": 0.7912779774108067, - "velocityY": 0.6226775544288833, - "timestamp": 0.21832338393770417 - }, - { - "x": 1.6788531458253628, - "y": 5.715327275578862, - "heading": 3.3003664786278266, - "angularVelocity": 0.8765279772723583, - "velocityX": 1.0538450119513225, - "velocityY": 0.8317596494783837, - "timestamp": 0.2910978452502722 - }, - { - "x": 1.7745670450726352, - "y": 5.791182757948707, - "heading": 3.380219753358853, - "angularVelocity": 1.097270571169936, - "velocityX": 1.3152127480020601, - "velocityY": 1.0423365697486087, - "timestamp": 0.36387230656284025 - }, - { - "x": 1.8891522077377947, - "y": 5.8825573533612445, - "heading": 3.4761247222373646, - "angularVelocity": 1.317838251891654, - "velocityX": 1.5745243674565124, - "velocityY": 1.255586008669736, - "timestamp": 0.4366467678754083 - }, - { - "x": 2.0223089309237166, - "y": 5.989857408352276, - "heading": 3.5877133538349852, - "angularVelocity": 1.5333487817703626, - "velocityX": 1.829717744168665, - "velocityY": 1.4744190895508729, - "timestamp": 0.5094212291879764 - }, - { - "x": 2.1730917573654285, - "y": 6.114310741146903, - "heading": 3.713064984762661, - "angularVelocity": 1.7224673143135665, - "velocityX": 2.0719195130019052, - "velocityY": 1.7101237240368758, - "timestamp": 0.5821956905005444 - }, - { - "x": 2.304244374220996, - "y": 6.249503988496409, - "heading": 3.8044927424626787, - "angularVelocity": 1.2563165161379026, - "velocityX": 1.8021791503514413, - "velocityY": 1.8577017941616667, - "timestamp": 0.6549701518131125 - }, - { - "x": 2.414916720860036, - "y": 6.372175349075486, - "heading": 3.8745670686179303, - "angularVelocity": 0.9628972154706921, - "velocityX": 1.5207580330096784, - "velocityY": 1.6856374937933305, - "timestamp": 0.7277446131256805 - }, - { - "x": 2.505647288657887, - "y": 6.481209512574969, - "heading": 3.9250064522902046, - "angularVelocity": 0.6930918176863726, - "velocityX": 1.2467363709936818, - "velocityY": 1.4982476205653745, - "timestamp": 0.8005190744382485 - }, - { - "x": 2.576640808380743, - "y": 6.576219510667492, - "heading": 3.9564510372491593, - "angularVelocity": 0.432082689336569, - "velocityX": 0.975527931672858, - "velocityY": 1.305540382971075, - "timestamp": 0.8732935357508166 - }, - { - "x": 2.628005259573955, - "y": 6.657012046349283, - "heading": 3.969203722939461, - "angularVelocity": 0.17523572775796842, - "velocityX": 0.7058032483758355, - "velocityY": 1.1101770349736413, - "timestamp": 0.9460679970633846 - }, - { - "x": 2.6598078437532253, - "y": 6.723475709345355, - "heading": 3.963391945850161, - "angularVelocity": -0.07986011829532402, - "velocityX": 0.4370019867639682, - "velocityY": 0.9132827890076579, - "timestamp": 1.0188424583759528 - }, - { - "x": 2.6720941279928496, - "y": 6.775543377099927, - "heading": 3.9390315825966313, - "angularVelocity": -0.3347378024400806, - "velocityX": 0.16882686615644266, - "velocityY": 0.7154662063514367, - "timestamp": 1.0916169196885208 - }, - { - "x": 2.6648950576782227, + "x": 1.5092825889004946, + "y": 5.579306288055266, + "heading": 3.4062283948642453, + "angularVelocity": 3.352121601432042, + "velocityX": 0.282511802606821, + "velocityY": 0.19030289294706518, + "timestamp": 0.07912254577845217 + }, + { + "x": 1.5673643497428493, + "y": 5.60987650843196, + "heading": 3.626081158643754, + "angularVelocity": 2.77863612218835, + "velocityX": 0.7340734587204384, + "velocityY": 0.38636547997701703, + "timestamp": 0.15824509155690433 + }, + { + "x": 1.639918967505268, + "y": 5.662681063987355, + "heading": 3.7707265189248944, + "angularVelocity": 1.828118128126927, + "velocityX": 0.9169904361466855, + "velocityY": 0.6673768523986875, + "timestamp": 0.23736763733535649 + }, + { + "x": 1.719103738696596, + "y": 5.732106202864848, + "heading": 3.861411095653706, + "angularVelocity": 1.1461281463659418, + "velocityX": 1.000786443513203, + "velocityY": 0.8774381333973873, + "timestamp": 0.31649018311380867 + }, + { + "x": 1.8011465793982657, + "y": 5.8124573886259565, + "heading": 3.9165727578512475, + "angularVelocity": 0.6971674338183805, + "velocityX": 1.0369085055907419, + "velocityY": 1.0155283171258929, + "timestamp": 0.39561272889226085 + }, + { + "x": 1.8844011608490543, + "y": 5.899562457377884, + "heading": 3.9495869202931786, + "angularVelocity": 0.41725354154267424, + "velocityX": 1.0522232396807132, + "velocityY": 1.1008880957373124, + "timestamp": 0.474735274670713 + }, + { + "x": 1.968168332271066, + "y": 5.990724119399961, + "heading": 3.96923386729424, + "angularVelocity": 0.24831034956931133, + "velocityX": 1.0587016709063517, + "velocityY": 1.1521578473642025, + "timestamp": 0.5538578204491652 + }, + { + "x": 2.05213613980173, + "y": 6.084290626509761, + "heading": 3.9810005346780404, + "angularVelocity": 0.1487144690307097, + "velocityX": 1.0612374349756033, + "velocityY": 1.1825517769839073, + "timestamp": 0.6329803662276173 + }, + { + "x": 2.1361397889017213, + "y": 6.179275610733387, + "heading": 3.988261371091814, + "angularVelocity": 0.0917669716303672, + "velocityX": 1.0616904230458737, + "velocityY": 1.2004793739775566, + "timestamp": 0.7121029120060695 + }, + { + "x": 2.2200564987148903, + "y": 6.2750981229467735, + "heading": 3.9931304198236632, + "angularVelocity": 0.061538069635463585, + "velocityX": 1.0605916302053886, + "velocityY": 1.2110645742073904, + "timestamp": 0.7912254577845216 + }, + { + "x": 2.3037490732526433, + "y": 6.3714204686821265, + "heading": 3.997036412999259, + "angularVelocity": 0.049366373859271756, + "velocityX": 1.0577588690345867, + "velocityY": 1.217381781484405, + "timestamp": 0.8703480035629737 + }, + { + "x": 2.3870186605553965, + "y": 6.4680502574631, + "heading": 4.001128510533661, + "angularVelocity": 0.05171847662561717, + "velocityX": 1.0524128929813872, + "velocityY": 1.221267438127472, + "timestamp": 0.9494705493414258 + }, + { + "x": 2.4696215654523987, + "y": 6.564870888609008, + "heading": 4.006432047635611, + "angularVelocity": 0.06702940419537926, + "velocityX": 1.0439869456209716, + "velocityY": 1.2236794227654333, + "timestamp": 1.028593095119878 + }, + { + "x": 2.5522006651677756, + "y": 6.661687676467445, + "heading": 4.0117852212202445, + "angularVelocity": 0.06765674097018534, + "velocityX": 1.043686080913052, + "velocityY": 1.223630848905317, + "timestamp": 1.1077156408983302 + }, + { + "x": 2.626171983754375, + "y": 6.756004010665758, + "heading": 4.047305777483365, + "angularVelocity": 0.44893090728627943, + "velocityX": 0.9348955832857486, + "velocityY": 1.1920285586159498, + "timestamp": 1.1868381866767823 + }, + { + "x": 2.664895057678222, "y": 6.813176155090332, "heading": 3.896073037200959, - "angularVelocity": -0.5902969890929726, - "velocityX": -0.09892303130498589, - "velocityY": 0.5171151707845827, - "timestamp": 1.1643913810010889 - }, - { - "x": 2.659637231596543, - "y": 6.825031823463049, - "heading": 3.877816032306183, - "angularVelocity": -0.6840416135896586, - "velocityX": -0.19699681615987222, - "velocityY": 0.44420049020077496, - "timestamp": 1.191081284849143 - }, - { - "x": 2.6517927610129908, - "y": 6.834893701518374, - "heading": 3.857106562184998, - "angularVelocity": -0.775928989444263, - "velocityX": -0.29391153404714393, - "velocityY": 0.36949844823228756, - "timestamp": 1.2177711886971971 - }, - { - "x": 2.641397527843252, - "y": 6.842708311525597, - "heading": 3.834005474054525, - "angularVelocity": -0.865536581247652, - "velocityX": -0.38948185159896215, - "velocityY": 0.2927927373478789, - "timestamp": 1.2444610925452513 - }, - { - "x": 2.628493660297315, - "y": 6.848415370045661, - "heading": 3.808587288042731, - "angularVelocity": -0.9523521012476956, - "velocityX": -0.4834737367133058, - "velocityY": 0.2138283656829084, - "timestamp": 1.2711509963933054 - }, - { - "x": 2.6131312860336138, - "y": 6.85194659507741, - "heading": 3.7809426101756807, - "angularVelocity": -1.0357728534516963, - "velocityX": -0.5755874712460319, - "velocityY": 0.13230564830254915, - "timestamp": 1.2978409002413596 - }, - { - "x": 2.5953708985643273, - "y": 6.853224371337149, - "heading": 3.7511803744930092, - "angularVelocity": -1.1151121357389708, - "velocityX": -0.6654346741148351, - "velocityY": 0.0478748918322195, - "timestamp": 1.3245308040894137 - }, - { - "x": 2.5752865454465885, - "y": 6.852160328589509, - "heading": 3.7194297145938124, - "angularVelocity": -1.1896131241218921, - "velocityX": -0.752507511157751, - "velocityY": -0.03986686327897337, - "timestamp": 1.3512207079374678 - }, - { - "x": 2.5529701073140734, - "y": 6.848653945190506, - "heading": 3.6858413880737997, - "angularVelocity": -1.2584656247257915, - "velocityX": -0.8361378242335784, - "velocityY": -0.1313748981249787, - "timestamp": 1.377910611785522 - }, - { - "x": 2.5285370169793717, - "y": 6.842591376045072, - "heading": 3.650589102058915, - "angularVelocity": -1.3208097794422922, - "velocityX": -0.9154431755842751, - "velocityY": -0.2271484071260898, - "timestamp": 1.4046005156335761 - }, - { - "x": 2.502133887928051, - "y": 6.833844846711091, - "heading": 3.613872024847483, - "angularVelocity": -1.375691625584805, - "velocityX": -0.9892553079859069, - "velocityY": -0.32770928601974636, - "timestamp": 1.4312904194816303 - }, - { - "x": 2.4739487064187284, - "y": 6.822273243464229, - "heading": 3.5759212160148097, - "angularVelocity": -1.4219162814796094, - "velocityX": -1.0560240932219476, - "velocityY": -0.43355732237695227, - "timestamp": 1.4579803233296844 - }, - { - "x": 2.4442244144939402, - "y": 6.807725217915028, - "heading": 3.5370136666138436, - "angularVelocity": -1.4577628163243674, - "velocityX": -1.113690483637882, - "velocityY": -0.54507598199017, - "timestamp": 1.4846702271777386 - }, - { - "x": 2.4132762941421024, - "y": 6.79004764808573, - "heading": 3.497494895227568, - "angularVelocity": -1.4806636850869153, - "velocityX": -1.1595440930782523, - "velocityY": -0.6623317165148433, - "timestamp": 1.5113601310257927 - }, - { - "x": 2.3815108616317744, - "y": 6.769104478274178, - "heading": 3.45780088591702, - "angularVelocity": -1.4872293859328474, - "velocityX": -1.1901666147307672, - "velocityY": -0.7846850978100616, - "timestamp": 1.5380500348738468 - }, - { - "x": 2.3494374003667597, - "y": 6.744810785756249, - "heading": 3.4184626098399313, - "angularVelocity": -1.4739010039542162, - "velocityX": -1.2017076362510986, - "velocityY": -0.910220308631834, - "timestamp": 1.564739938721901 - }, - { - "x": 2.317656105280672, - "y": 6.717179467626764, - "heading": 3.3800987305060795, - "angularVelocity": -1.4373929390026206, - "velocityX": -1.190760943427104, - "velocityY": -1.0352722994728694, - "timestamp": 1.5914298425699551 - }, - { - "x": 2.2868057964945567, - "y": 6.686361396326711, - "heading": 3.343389515038899, - "angularVelocity": -1.3753970668521929, - "velocityX": -1.1558793527974576, - "velocityY": -1.1546714995865741, - "timestamp": 1.6181197464180093 - }, - { - "x": 2.25748075399746, - "y": 6.652639268574606, - "heading": 3.308925014353095, - "angularVelocity": -1.291293549876024, - "velocityX": -1.0987316651286732, - "velocityY": -1.263478802474685, - "timestamp": 1.6448096502660634 - }, - { - "x": 2.2301787041350396, - "y": 6.616374519308364, - "heading": 3.2770382136187797, - "angularVelocity": -1.194713960599006, - "velocityX": -1.022935489683713, - "velocityY": -1.3587440956212415, - "timestamp": 1.6714995541141175 - }, - { - "x": 2.2052911587380817, - "y": 6.577965167741144, - "heading": 3.2479010535236887, - "angularVelocity": -1.0916922091952401, - "velocityX": -0.9324704029899484, - "velocityY": -1.4390966631383846, - "timestamp": 1.6981894579621717 - }, - { - "x": 2.183095873519221, - "y": 6.537812097974725, - "heading": 3.2217255041499464, - "angularVelocity": -0.9807285002883435, - "velocityX": -0.8315985454731583, - "velocityY": -1.504429165238335, - "timestamp": 1.7248793618102258 - }, - { - "x": 2.163764936777705, - "y": 6.4962772278935255, - "heading": 3.198795874429834, - "angularVelocity": -0.8591124887766933, - "velocityX": -0.72427899521733, - "velocityY": -1.5562015628702863, - "timestamp": 1.75156926565828 - }, - { - "x": 2.1473967460074728, - "y": 6.453660043976164, - "heading": 3.1793849811459, - "angularVelocity": -0.7272747550699427, - "velocityX": -0.61327275150245, - "velocityY": -1.596752995439064, - "timestamp": 1.7782591695063341 - }, - { - "x": 2.134047988560311, - "y": 6.410199882298709, - "heading": 3.163697789085614, - "angularVelocity": -0.5877575336948715, - "velocityX": -0.5001425828716374, - "velocityY": -1.628337139199698, - "timestamp": 1.8049490733543883 - }, - { - "x": 2.1237534679344243, - "y": 6.366088523528368, - "heading": 3.151864532890396, - "angularVelocity": -0.443360765276075, - "velocityX": -0.38570841935190137, - "velocityY": -1.6527357693556206, - "timestamp": 1.8316389772024424 - }, - { - "x": 2.116536197487218, - "y": 6.321482513089734, - "heading": 3.143955647543306, - "angularVelocity": -0.29632498461271933, - "velocityX": -0.27041200628876155, - "velocityY": -1.671269057115243, - "timestamp": 1.8583288810504965 - }, - { - "x": 2.1124122616124033, - "y": 6.276512434450225, - "heading": 3.14, - "angularVelocity": -0.1482076355848078, - "velocityX": -0.15451295359818015, - "velocityY": -1.6849097282449244, - "timestamp": 1.8850187848985507 - }, - { - "x": 2.1113932132720947, + "angularVelocity": -1.9113735382816162, + "velocityX": 0.4894063195625013, + "velocityY": 0.7225771600506798, + "timestamp": 1.2659607324552344 + }, + { + "x": 2.67401621750823, + "y": 6.8287147997218245, + "heading": 3.8216991351700837, + "angularVelocity": -2.7569472556880648, + "velocityX": 0.338109953564034, + "velocityY": 0.5759980652369127, + "timestamp": 1.2929376366660208 + }, + { + "x": 2.6786727556459358, + "y": 6.8400421309827735, + "heading": 3.72670670460024, + "angularVelocity": -3.521250245306635, + "velocityX": 0.17261202773019518, + "velocityY": 0.41988996114756666, + "timestamp": 1.3199145408768072 + }, + { + "x": 2.678518823316929, + "y": 6.846619952079936, + "heading": 3.614568928899329, + "angularVelocity": -4.15680668266135, + "velocityX": -0.00570607834774554, + "velocityY": 0.24383157703223057, + "timestamp": 1.3468914450875935 + }, + { + "x": 2.6741686792707893, + "y": 6.846169187557711, + "heading": 3.4948207380964975, + "angularVelocity": -4.438915224191635, + "velocityX": -0.16125438308832984, + "velocityY": -0.0167092754120811, + "timestamp": 1.3738683492983799 + }, + { + "x": 2.663571596898345, + "y": 6.842490941859769, + "heading": 3.391106479438399, + "angularVelocity": -3.844557472114035, + "velocityX": -0.3928205508549501, + "velocityY": -0.1363479541318252, + "timestamp": 1.4008452535091662 + }, + { + "x": 2.648321534336774, + "y": 6.83512618854535, + "heading": 3.3066791856345255, + "angularVelocity": -3.129613878011262, + "velocityX": -0.5653006898944218, + "velocityY": -0.2730021672197161, + "timestamp": 1.4278221577199526 + }, + { + "x": 2.630755187717048, + "y": 6.821913643765559, + "heading": 3.2403542901418882, + "angularVelocity": -2.4585806797690686, + "velocityX": -0.6511624344467275, + "velocityY": -0.4897724615306165, + "timestamp": 1.454799061930739 + }, + { + "x": 2.610177790375068, + "y": 6.803317677931451, + "heading": 3.192043949934185, + "angularVelocity": -1.7908037123240415, + "velocityX": -0.7627783077407181, + "velocityY": -0.6893291271973806, + "timestamp": 1.4817759661415253 + }, + { + "x": 2.585880089062531, + "y": 6.779956801667491, + "heading": 3.1617447456020207, + "angularVelocity": -1.1231534980960616, + "velocityX": -0.9006853092812849, + "velocityY": -0.8659583798581432, + "timestamp": 1.5087528703523116 + }, + { + "x": 2.557481302008236, + "y": 6.752192795634918, + "heading": 3.1494506971418432, + "angularVelocity": -0.4557249550980476, + "velocityX": -1.0527074134380248, + "velocityY": -1.029176877214801, + "timestamp": 1.535729774563098 + }, + { + "x": 2.5262145192081267, + "y": 6.721488912807054, + "heading": 3.1494498734965517, + "angularVelocity": -0.00003053149779678828, + "velocityX": -1.1590204181995478, + "velocityY": -1.1381544223143767, + "timestamp": 1.5627066787738844 + }, + { + "x": 2.494951526671565, + "y": 6.690781099940809, + "heading": 3.1494491983381225, + "angularVelocity": -0.000025027276073123234, + "velocityX": -1.1588799178840683, + "velocityY": -1.1383001039076166, + "timestamp": 1.5896835829846707 + }, + { + "x": 2.46368853251831, + "y": 6.660073288720487, + "heading": 3.149448523179691, + "angularVelocity": -0.000025027276154336604, + "velocityX": -1.1588799778128431, + "velocityY": -1.138300042895271, + "timestamp": 1.616660487195457 + }, + { + "x": 2.4324255384011386, + "y": 6.629365477463429, + "heading": 3.1494478480212726, + "angularVelocity": -0.000025027275679943262, + "velocityX": -1.1588799764752862, + "velocityY": -1.138300044257102, + "timestamp": 1.6436373914062434 + }, + { + "x": 2.401162544284488, + "y": 6.5986576662058365, + "heading": 3.1494471728628666, + "angularVelocity": -0.000025027275214919653, + "velocityX": -1.1588799764559639, + "velocityY": -1.1383000442768603, + "timestamp": 1.6706142956170298 + }, + { + "x": 2.3698995501679407, + "y": 6.567949854948137, + "heading": 3.1494464977044734, + "angularVelocity": -0.000025027274749328804, + "velocityX": -1.1588799764521394, + "velocityY": -1.1383000442808404, + "timestamp": 1.6975911998278161 + }, + { + "x": 2.3386365560514903, + "y": 6.537242043690337, + "heading": 3.1494458225460926, + "angularVelocity": -0.000025027274284044715, + "velocityX": -1.1588799764485511, + "velocityY": -1.1383000442845796, + "timestamp": 1.7245681040386025 + }, + { + "x": 2.307373561935169, + "y": 6.5065342324324025, + "heading": 3.1494451473877243, + "angularVelocity": -0.00002502727381983999, + "velocityX": -1.1588799764437645, + "velocityY": -1.1383000442895392, + "timestamp": 1.7515450082493889 + }, + { + "x": 2.2761105678205396, + "y": 6.475826421172744, + "heading": 3.1494444722293684, + "angularVelocity": -0.00002502727335405626, + "velocityX": -1.158879976381056, + "velocityY": -1.1383000443534677, + "timestamp": 1.7785219124601752 + }, + { + "x": 2.244847573837531, + "y": 6.445118609779082, + "heading": 3.1494437970710263, + "angularVelocity": -0.000025027272855365495, + "velocityX": -1.1588799715020328, + "velocityY": -1.138300049320807, + "timestamp": 1.8054988166709616 + }, + { + "x": 2.2135845753549397, + "y": 6.414410802966355, + "heading": 3.149443121912666, + "angularVelocity": -0.000025027273518628322, + "velocityX": -1.1588801382959264, + "velocityY": -1.1382998795112838, + "timestamp": 1.832475720881748 + }, + { + "x": 2.182332647777799, + "y": 6.3836917291800725, + "heading": 3.14944244669543, + "angularVelocity": -0.000025029455959603075, + "velocityX": -1.1584697537175366, + "velocityY": -1.1387175320883667, + "timestamp": 1.8594526250925343 + }, + { + "x": 2.153094421241092, + "y": 6.351050761951557, + "heading": 3.149440476891046, + "angularVelocity": -0.00007301817765136917, + "velocityX": -1.0838243820804343, + "velocityY": -1.2099597112211005, + "timestamp": 1.8864295293033206 + }, + { + "x": 2.1314747036692956, + "y": 6.314801767366594, + "heading": 3.1444623769659996, + "angularVelocity": -0.18453191983123554, + "velocityX": -0.801415811201077, + "velocityY": -1.3437047595123932, + "timestamp": 1.913406433514107 + }, + { + "x": 2.1175303161219627, + "y": 6.274679449481932, + "heading": 3.139999999999996, + "angularVelocity": -0.16541471664709217, + "velocityX": -0.5169009549193061, + "velocityY": -1.487283995641412, + "timestamp": 1.9403833377248934 + }, + { + "x": 2.111393213272095, "y": 6.231289386749268, + "heading": 3.139999999999996, + "angularVelocity": 1.2396255317011e-17, + "velocityX": -0.22749470442979314, + "velocityY": -1.6084151981870152, + "timestamp": 1.9673602419356797 + }, + { + "x": 2.1135515859889713, + "y": 6.18871804241084, + "heading": 3.139999999999996, + "angularVelocity": 4.342266970142451e-18, + "velocityX": 0.08225287430098827, + "velocityY": -1.6223404824025023, + "timestamp": 1.9936009385096722 + }, + { + "x": 2.1237593069102005, + "y": 6.1473322849843255, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422681619637e-18, + "velocityX": 0.3890034280317459, + "velocityY": -1.5771592537498764, + "timestamp": 2.0198416350836648 + }, + { + "x": 2.141644452417822, + "y": 6.1086399046863455, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422691347276405e-18, + "velocityX": 0.6815804396497286, + "velocityY": -1.4745180330437861, + "timestamp": 2.0460823316576575 + }, + { + "x": 2.1665552441015765, + "y": 6.0740504370872355, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422696264537505e-18, + "velocityX": 0.9493189943912999, + "velocityY": -1.3181611814907124, + "timestamp": 2.0723230282316503 + }, + { + "x": 2.1935012145011026, + "y": 6.041021760141006, + "heading": 3.139999999999997, + "angularVelocity": 4.34227316559682e-18, + "velocityX": 1.0268770999862573, + "velocityY": -1.2586814093559167, + "timestamp": 2.098563724805643 + }, + { + "x": 2.220442534619127, + "y": 6.00798928987496, + "heading": 3.139999999999997, + "angularVelocity": 4.342268965346379e-18, + "velocityX": 1.0266998835971468, + "velocityY": -1.2588259680112164, + "timestamp": 2.1248044213796358 + }, + { + "x": 2.247383856273639, + "y": 5.974956820862076, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269673161453e-18, + "velocityX": 1.0266999421507437, + "velocityY": -1.2588259202547785, + "timestamp": 2.1510451179536285 + }, + { + "x": 2.274325177898134, + "y": 5.941924351824711, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269691700593e-18, + "velocityX": 1.0266999410068467, + "velocityY": -1.2588259211877415, + "timestamp": 2.1772858145276213 + }, + { + "x": 2.3012664995222347, + "y": 5.908891882787024, + "heading": 3.1399999999999975, + "angularVelocity": 4.3422697108078704e-18, + "velocityX": 1.0266999409917994, + "velocityY": -1.258825921200014, + "timestamp": 2.203526511101614 + }, + { + "x": 2.328207821146328, + "y": 5.87585941374933, + "heading": 3.139999999999998, + "angularVelocity": 4.3422696965810506e-18, + "velocityX": 1.0266999409915183, + "velocityY": -1.2588259212002433, + "timestamp": 2.2297672076756068 + }, + { + "x": 2.355149142770421, + "y": 5.8428269447116365, + "heading": 3.139999999999998, + "angularVelocity": 4.342269684490725e-18, + "velocityX": 1.0266999409915205, + "velocityY": -1.2588259212002417, + "timestamp": 2.2560079042495995 + }, + { + "x": 2.3820904643945138, + "y": 5.809794475673942, + "heading": 3.139999999999998, + "angularVelocity": 4.342269693460261e-18, + "velocityX": 1.026699940991488, + "velocityY": -1.258825921200268, + "timestamp": 2.2822486008235923 + }, + { + "x": 2.4090317860186072, + "y": 5.7767620066362495, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269692405428e-18, + "velocityX": 1.026699940991538, + "velocityY": -1.258825921200227, + "timestamp": 2.308489297397585 + }, + { + "x": 2.4359731076427, + "y": 5.743729537598556, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269716594298e-18, + "velocityX": 1.0266999409914905, + "velocityY": -1.258825921200266, + "timestamp": 2.3347299939715778 + }, + { + "x": 2.4629144292667973, + "y": 5.710697068560865, + "heading": 3.139999999999999, + "angularVelocity": 4.342269681682682e-18, + "velocityX": 1.026699940991683, + "velocityY": -1.258825921200109, + "timestamp": 2.3609706905455705 + }, + { + "x": 2.4898557508911936, + "y": 5.677664599523419, + "heading": 3.139999999999999, + "angularVelocity": 4.3422696993715505e-18, + "velocityX": 1.0266999410030797, + "velocityY": -1.2588259211908142, + "timestamp": 2.3872113871195633 + }, + { + "x": 2.5167970725019893, + "y": 5.644632130474532, + "heading": 3.139999999999999, + "angularVelocity": 4.342269692409483e-18, + "velocityX": 1.026699940484769, + "velocityY": -1.2588259216268345, + "timestamp": 2.413452083693556 + }, + { + "x": 2.5425090771755396, + "y": 5.613106913889164, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374488911e-18, + "velocityX": 0.9798522154718704, + "velocityY": -1.2013864226688296, + "timestamp": 2.4396927802675488 + }, + { + "x": 2.563078686478754, + "y": 5.587886733807481, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374369648e-18, + "velocityX": 0.7838819844288143, + "velocityY": -0.9611093977847911, + "timestamp": 2.4659334768415415 + }, + { + "x": 2.578505895291019, + "y": 5.568971596499493, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374398685e-18, + "velocityX": 0.5879115582455986, + "velocityY": -0.7208321339584491, + "timestamp": 2.4921741734155343 + }, + { + "x": 2.5887907019054897, + "y": 5.5563615040551815, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374398075e-18, + "velocityX": 0.39194106701667647, + "velocityY": -0.48055479048560223, + "timestamp": 2.518414869989527 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374359544e-18, + "velocityX": 0.19597054326510124, + "velocityY": -0.2402774071896897, + "timestamp": 2.5446555665635198 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, "heading": 3.14, - "angularVelocity": 1.1480104978612554e-22, - "velocityX": -0.038181042019109025, - "velocityY": -1.6943878089038094, - "timestamp": 1.9117086887466048 - }, - { - "x": 2.1139722457961088, - "y": 6.184041017313733, - "heading": 3.14, - "angularVelocity": 1.326030822415817e-23, - "velocityX": 0.09287464315336291, - "velocityY": -1.7014812376514836, - "timestamp": 1.9394776516890055 - }, - { - "x": 2.1201947185383916, - "y": 6.136700429098736, - "heading": 3.14, - "angularVelocity": 1.3260179507784046e-23, - "velocityX": 0.224080126981686, - "velocityY": -1.7048021675563763, - "timestamp": 1.9672466146314063 - }, - { - "x": 2.13006161842343, - "y": 6.089396035622716, - "heading": 3.14, - "angularVelocity": 1.326007041519161e-23, - "velocityX": 0.35532115136976367, - "velocityY": -1.7034987433322955, - "timestamp": 1.995015577573807 - }, - { - "x": 2.1435678181376194, - "y": 6.042288248235824, - "heading": 3.14, - "angularVelocity": 1.3260437690465338e-23, - "velocityX": 0.4863775338749469, - "velocityY": -1.6964186773774967, - "timestamp": 2.022784540516208 - }, - { - "x": 2.1606964661173613, - "y": 5.995581737791835, - "heading": 3.14, - "angularVelocity": 1.325999769616951e-23, - "velocityX": 0.6168270675167464, - "velocityY": -1.6819681217793025, - "timestamp": 2.050553503458609 - }, - { - "x": 2.181407907708738, - "y": 5.949543614413858, - "heading": 3.14, - "angularVelocity": 1.3260185229673763e-23, - "velocityX": 0.7458485804578618, - "velocityY": -1.6578985493073877, - "timestamp": 2.07832246640101 - }, - { - "x": 2.2056170475643233, - "y": 5.904529822011414, - "heading": 3.14, - "angularVelocity": 1.3260157963898354e-23, - "velocityX": 0.871805688451557, - "velocityY": -1.6210109284892367, - "timestamp": 2.1060914293434108 - }, - { - "x": 2.2331463169007804, - "y": 5.861019319967013, - "heading": 3.14, - "angularVelocity": 1.326020172336188e-23, - "velocityX": 0.9913682910506778, - "velocityY": -1.566875296519094, - "timestamp": 2.1338603922858117 - }, - { - "x": 2.2636325105277852, - "y": 5.819639483049468, - "heading": 3.14, - "angularVelocity": 1.32601511090322e-23, - "velocityX": 1.0978513562152263, - "velocityY": -1.4901470034504147, - "timestamp": 2.1616293552282126 - }, - { - "x": 2.2963834267173127, - "y": 5.781115195207252, - "heading": 3.14, - "angularVelocity": 1.3260107576996925e-23, - "velocityX": 1.1794072489296905, - "velocityY": -1.387314604514559, - "timestamp": 2.1893983181706136 - }, - { - "x": 2.3303148904385163, - "y": 5.746039013141612, - "heading": 3.14, - "angularVelocity": 1.3260240855176863e-23, - "velocityX": 1.221920450957617, - "velocityY": -1.2631433928013844, - "timestamp": 2.2171672811130145 - }, - { - "x": 2.3642062447518297, - "y": 5.714607213055508, - "heading": 3.14, - "angularVelocity": 1.326023555417506e-23, - "velocityX": 1.2204760539171806, - "velocityY": -1.1319039948052778, - "timestamp": 2.2449362440554155 - }, - { - "x": 2.397064742551735, - "y": 5.68667060145179, - "heading": 3.14, - "angularVelocity": 1.3260077501265654e-23, - "velocityX": 1.1832814163085987, - "velocityY": -1.0060372676381828, - "timestamp": 2.2727052069978164 - }, - { - "x": 2.4282178000518853, - "y": 5.661954954768778, - "heading": 3.14, - "angularVelocity": 1.3260249833501151e-23, - "velocityX": 1.1218660763374204, - "velocityY": -0.8900457224231765, - "timestamp": 2.3004741699402174 - }, - { - "x": 2.457231034537111, - "y": 5.640189613922927, - "heading": 3.14, - "angularVelocity": 1.3260141947431602e-23, - "velocityX": 1.0448079946451803, - "velocityY": -0.7838009972139435, - "timestamp": 2.3282431328826183 - }, - { - "x": 2.483818861573163, - "y": 5.621144668272307, - "heading": 3.14, - "angularVelocity": 1.3260165607649488e-23, - "velocityX": 0.9574656097601072, - "velocityY": -0.6858356824532662, - "timestamp": 2.3560120958250192 - }, - { - "x": 2.507786212179532, - "y": 5.604632755958765, - "heading": 3.14, - "angularVelocity": 1.3260126001429812e-23, - "velocityX": 0.8630985123961222, - "velocityY": -0.5946175356923091, - "timestamp": 2.38378105876742 - }, - { - "x": 2.5289943781295285, - "y": 5.590502031840209, - "heading": 3.14, - "angularVelocity": 1.3260018717887446e-23, - "velocityX": 0.763736333761809, - "velocityY": -0.5088675492802951, - "timestamp": 2.411550021709821 - }, - { - "x": 2.547341071809568, - "y": 5.578628548447832, - "heading": 3.14, - "angularVelocity": 1.3260377381509874e-23, - "velocityX": 0.6606906321310845, - "velocityY": -0.4275810881740915, - "timestamp": 2.439318984652222 - }, - { - "x": 2.562748457639991, - "y": 5.568909973472095, - "heading": 3.14, - "angularVelocity": 1.3260175762620676e-23, - "velocityX": 0.5548419601546171, - "velocityY": -0.3499797596293578, - "timestamp": 2.467087947594623 - }, - { - "x": 2.575155705150767, - "y": 5.561260778200535, - "heading": 3.14, - "angularVelocity": 1.3260285574981282e-23, - "velocityX": 0.4468026961075851, - "velocityY": -0.2754584421256751, - "timestamp": 2.494856910537024 - }, - { - "x": 2.5845141850851863, - "y": 5.555608625671519, - "heading": 3.14, - "angularVelocity": 1.326006595026007e-23, - "velocityX": 0.33701222310069334, - "velocityY": -0.2035420818825477, - "timestamp": 2.522625873479425 - }, - { - "x": 2.5907842648688533, - "y": 5.55189166019714, - "heading": 3.14, - "angularVelocity": 1.3260302513246253e-23, - "velocityX": 0.22579452450827125, - "velocityY": -0.1338532332694335, - "timestamp": 2.550394836421826 - }, - { - "x": 2.59393310546875, - "y": 5.550056457519531, - "heading": 3.14, - "angularVelocity": 1.3260089047889746e-23, - "velocityX": 0.11339424545409957, - "velocityY": -0.06608826845335893, - "timestamp": 2.578163799364227 - }, - { - "x": 2.59393310546875, - "y": 5.550056457519531, - "heading": 3.14, - "angularVelocity": 4.554749859713047e-24, - "velocityX": -4.596278509340535e-25, - "velocityY": -1.5195442763313795e-23, - "timestamp": 2.6059327623066277 - }, - { - "x": 2.5899783566862378, - "y": 5.5489813066194555, - "heading": 3.1367490851985087, - "angularVelocity": -0.1085265960704612, - "velocityX": -0.13202296888338985, - "velocityY": -0.03589219483504766, - "timestamp": 2.6358877679845065 - }, - { - "x": 2.5820812048170954, - "y": 5.546779098674632, - "heading": 3.130275233897137, - "angularVelocity": -0.21611918124764368, - "velocityX": -0.26363379643671836, - "velocityY": -0.07351719337009978, - "timestamp": 2.6658427736623853 - }, - { - "x": 2.5702568313825034, - "y": 5.5433888083370855, - "heading": 3.1206149262957497, - "angularVelocity": -0.32249393324338926, - "velocityX": -0.3947378131636905, - "velocityY": -0.1131794256360211, - "timestamp": 2.695797779340264 - }, - { - "x": 2.554524436678559, - "y": 5.5387376635070815, - "heading": 3.107814789143658, - "angularVelocity": -0.42731212571741595, - "velocityX": -0.5252008586852795, - "velocityY": -0.15527103817038457, - "timestamp": 2.725752785018143 - }, - { - "x": 2.5349090755324424, - "y": 5.532737501107672, - "heading": 3.0919335422287366, - "angularVelocity": -0.5301700519005096, - "velocityX": -0.6548274888361012, - "velocityY": -0.20030583415450579, - "timestamp": 2.7557077906960217 - }, - { - "x": 2.511444606178007, - "y": 5.52527960958652, - "heading": 3.0730442108839475, - "angularVelocity": -0.6305901440285391, - "velocityX": -0.7833238159511905, - "velocityY": -0.24896979160511046, - "timestamp": 2.7856627963739005 - }, - { - "x": 2.4841786133457258, - "y": 5.5162272720643095, - "heading": 3.0512365155339536, - "angularVelocity": -0.7280150631418102, - "velocityX": -0.9102316028741883, - "velocityY": -0.3021978236143635, - "timestamp": 2.8156178020517793 - }, - { - "x": 2.4531809940870857, - "y": 5.505404760057258, - "heading": 3.026619340133933, - "angularVelocity": -0.8218050653951327, - "velocityX": -1.03480598842053, - "velocityY": -0.3612922702613114, - "timestamp": 2.845572807729658 - }, - { - "x": 2.4185596883958564, - "y": 5.492580836854246, - "heading": 2.999323661656212, - "angularVelocity": -0.9112226107130541, - "velocityX": -1.1557769697502154, - "velocityY": -0.4281061850200961, - "timestamp": 2.875527813407537 - }, - { - "x": 2.380491125275952, - "y": 5.477444069032361, - "heading": 2.9695096535475827, - "angularVelocity": -0.9952930214480606, - "velocityX": -1.27085815069692, - "velocityY": -0.5053168069690293, - "timestamp": 2.9054828190854156 - }, - { - "x": 2.3392828051052046, - "y": 5.459567907623698, - "heading": 2.9373981254397274, - "angularVelocity": -1.0719920554570026, - "velocityX": -1.3756739228789079, - "velocityY": -0.5967670846367005, - "timestamp": 2.9354378247632944 - }, - { - "x": 2.2955095509891357, - "y": 5.438376261665861, - "heading": 2.9034010779477137, - "angularVelocity": -1.1349371072601777, - "velocityX": -1.4613001441823985, - "velocityY": -0.7074492385586875, - "timestamp": 2.965392830441173 - }, - { - "x": 2.2502877957375587, - "y": 5.413212847808716, - "heading": 2.8684201412995205, - "angularVelocity": -1.1677826746007212, - "velocityX": -1.5096560400578645, - "velocityY": -0.8400403634617765, - "timestamp": 2.995347836119052 - }, - { - "x": 2.205434968136323, - "y": 5.383827174903616, - "heading": 2.8338778225381454, - "angularVelocity": -1.15314011730883, - "velocityX": -1.4973399799539564, - "velocityY": -0.9809937351072376, - "timestamp": 3.0253028417969308 - }, - { - "x": 2.1627117665288242, - "y": 5.35082401983714, - "heading": 2.8010931886625263, - "angularVelocity": -1.0944626159703914, - "velocityX": -1.426245819043514, - "velocityY": -1.1017575967561364, - "timestamp": 3.0552578474748096 - }, - { - "x": 2.1231203758060415, - "y": 5.315102619215835, - "heading": 2.770927863402835, - "angularVelocity": -1.0070211831729985, - "velocityX": -1.3216953169206167, - "velocityY": -1.1925018811692434, - "timestamp": 3.0852128531526883 - }, - { - "x": 2.0871120973907478, - "y": 5.277368588566571, - "heading": 2.743899688375806, - "angularVelocity": -0.9022924354505801, - "velocityX": -1.2020788379247493, - "velocityY": -1.2596903187079023, - "timestamp": 3.115167858830567 - }, - { - "x": 2.0548974336097707, - "y": 5.238102066889186, - "heading": 2.7202319811811844, - "angularVelocity": -0.7901085864957776, - "velocityX": -1.0754350751055555, - "velocityY": -1.310850082942305, - "timestamp": 3.145122864508446 - }, - { - "x": 2.026587153392112, - "y": 5.197634903381892, - "heading": 2.6999923980627787, - "angularVelocity": -0.6756661419481035, - "velocityX": -0.9450934685873016, - "velocityY": -1.350931591950202, - "timestamp": 3.1750778701863247 - }, - { - "x": 2.002245678733676, - "y": 5.1562068665429, - "heading": 2.6831832138201195, - "angularVelocity": -0.5611477568529534, - "velocityX": -0.812601236674494, - "velocityY": -1.3830088127670148, - "timestamp": 3.2050328758642035 - }, - { - "x": 1.9819131544340292, - "y": 5.113998389763002, - "heading": 2.66978325115483, - "angularVelocity": -0.44733634202529504, - "velocityX": -0.6787688347748394, - "velocityY": -1.4090625531434064, - "timestamp": 3.2349878815420823 - }, - { - "x": 1.9656157831702816, - "y": 5.071149791653941, - "heading": 2.659765787935313, - "angularVelocity": -0.33441700286222414, - "velocityX": -0.5440616983685925, - "velocityY": -1.4304319808793762, - "timestamp": 3.264942887219961 - }, - { - "x": 1.9533711923999129, - "y": 5.027773084636261, - "heading": 2.6531059130951715, - "angularVelocity": -0.22232927984587103, - "velocityX": -0.4087660974610013, - "velocityY": -1.4480620529380386, - "timestamp": 3.29489789289784 - }, - { - "x": 1.9451914498185667, - "y": 4.983959581247479, + "angularVelocity": 3.0510058962998796e-17, + "velocityX": 1.3525026943280928e-16, + "velocityY": 8.359694800915289e-17, + "timestamp": 2.5708962631375125 + }, + { + "x": 2.5906368423225947, + "y": 5.5485199070890205, + "heading": 3.101591851410996, + "angularVelocity": -1.2805791583146104, + "velocityX": -0.10990183178303778, + "velocityY": -0.05123065102809662, + "timestamp": 2.60088905845952 + }, + { + "x": 2.584621501308477, + "y": 5.544975862182894, + "heading": 3.0243371709692783, + "angularVelocity": -2.5757746022741834, + "velocityX": -0.20055953270102006, + "velocityY": -0.11816320779855842, + "timestamp": 2.6308818537815273 + }, + { + "x": 2.5759613994637824, + "y": 5.536844199254011, + "heading": 2.9135350884263085, + "angularVelocity": -3.6942899570776517, + "velocityX": -0.28873940397378167, + "velocityY": -0.2711205421688905, + "timestamp": 2.6608746491035347 + }, + { + "x": 2.56275074361348, + "y": 5.521892690270415, + "heading": 2.8248688434153846, + "angularVelocity": -2.956251461684603, + "velocityX": -0.44046097433337356, + "velocityY": -0.49850335131840534, + "timestamp": 2.690867444425542 + }, + { + "x": 2.543962903792877, + "y": 5.501410245111836, + "heading": 2.7589030524712514, + "angularVelocity": -2.199387894182165, + "velocityX": -0.6264117638620101, + "velocityY": -0.6829121773583486, + "timestamp": 2.7208602397475494 + }, + { + "x": 2.518778715441927, + "y": 5.477002144640871, + "heading": 2.7154691851313926, + "angularVelocity": -1.448143358205586, + "velocityX": -0.839674597873837, + "velocityY": -0.8137987876364632, + "timestamp": 2.750853035069557 + }, + { + "x": 2.486733800013198, + "y": 5.449761748396881, + "heading": 2.6943182531197865, + "angularVelocity": -0.705200425130341, + "velocityX": -1.0684204351295292, + "velocityY": -0.9082313252703665, + "timestamp": 2.780845830391564 + }, + { + "x": 2.4478196867061426, + "y": 5.4204460978412365, + "heading": 2.694317441639593, + "angularVelocity": -0.000027055837428819432, + "velocityX": -1.2974487002285997, + "velocityY": -0.9774230858209427, + "timestamp": 2.8108386257135716 + }, + { + "x": 2.408901839144878, + "y": 5.391135398464328, + "heading": 2.6943166423252785, + "angularVelocity": -0.000026650210696695328, + "velocityX": -1.2975732052797133, + "velocityY": -0.9772580068715555, + "timestamp": 2.840831421035579 + }, + { + "x": 2.3699840051756724, + "y": 5.361824681040097, + "heading": 2.6943158430110494, + "angularVelocity": -0.00002665020784043696, + "velocityX": -1.2975727521022344, + "velocityY": -0.9772586085935405, + "timestamp": 2.8708242163575863 + }, + { + "x": 2.3310661711945366, + "y": 5.332513963631788, + "heading": 2.6943150436964616, + "angularVelocity": -0.000026650219800690006, + "velocityX": -1.297572752499997, + "velocityY": -0.9772586080626477, + "timestamp": 2.9008170116795937 + }, + { + "x": 2.292148337213513, + "y": 5.303203246223414, + "heading": 2.694314244381513, + "angularVelocity": -0.000026650231834296552, + "velocityX": -1.2975727524962546, + "velocityY": -0.9772586080648237, + "timestamp": 2.930809807001601 + }, + { + "x": 2.25323050323266, + "y": 5.273892528814898, + "heading": 2.6943134450662036, + "angularVelocity": -0.00002665024386857193, + "velocityX": -1.297572752490578, + "velocityY": -0.9772586080695672, + "timestamp": 2.9608026023236085 + }, + { + "x": 2.214312669251978, + "y": 5.2445818114062375, + "heading": 2.694312645750533, + "angularVelocity": -0.00002665025590264624, + "velocityX": -1.2975727524848706, + "velocityY": -0.9772586080743522, + "timestamp": 2.990795397645616 + }, + { + "x": 2.1753948352714723, + "y": 5.215271093997427, + "heading": 2.694311846434502, + "angularVelocity": -0.000026650267936849635, + "velocityX": -1.2975727524790048, + "velocityY": -0.9772586080793467, + "timestamp": 3.0207881929676232 + }, + { + "x": 2.136477001291495, + "y": 5.185960376588, + "heading": 2.6943110471181098, + "angularVelocity": -0.000026650279973527825, + "velocityX": -1.2975727524613627, + "velocityY": -0.9772586080999772, + "timestamp": 3.0507809882896306 + }, + { + "x": 2.097559167357152, + "y": 5.1566496591180675, + "heading": 2.694310247801348, + "angularVelocity": -0.000026650292283470146, + "velocityX": -1.2975727509398636, + "velocityY": -0.9772586101172457, + "timestamp": 3.080773783611638 + }, + { + "x": 2.058641302557044, + "y": 5.127338982627939, + "heading": 2.694309448489961, + "angularVelocity": -0.000026650113093683626, + "velocityX": -1.2975737800459084, + "velocityY": -0.9772572437956306, + "timestamp": 3.1107665789336454 + }, + { + "x": 2.0197569370912083, + "y": 5.0979839038638834, + "heading": 2.6943086027536207, + "angularVelocity": -0.000028197983267341566, + "velocityX": -1.2964568673313885, + "velocityY": -0.9787376751348899, + "timestamp": 3.1407593742556528 + }, + { + "x": 1.9877778714785685, + "y": 5.067888184962114, + "heading": 2.678621118987198, + "angularVelocity": -0.5230417371285907, + "velocityX": -1.0662249139900737, + "velocityY": -1.0034316101155079, + "timestamp": 3.17075216957766 + }, + { + "x": 1.9640654183560038, + "y": 5.031205594348394, + "heading": 2.6596531558314034, + "angularVelocity": -0.6324173173088112, + "velocityX": -0.7906049725553251, + "velocityY": -1.223046742388833, + "timestamp": 3.2007449648996675 + }, + { + "x": 1.9481708008996033, + "y": 4.987987936601904, "heading": 2.649783348569472, - "angularVelocity": -0.11091850762536054, - "velocityX": -0.27306763581709725, - "velocityY": -1.4626438018383596, - "timestamp": 3.3248528985757186 + "angularVelocity": -0.32907260414160233, + "velocityX": -0.5299478519964009, + "velocityY": -1.4409346405525878, + "timestamp": 3.230737760221675 }, { - "x": 1.941084861755371, + "x": 1.9410848617553718, "y": 4.939785003662109, "heading": 2.649783348569472, - "angularVelocity": 6.132292113202646e-25, - "velocityX": -0.13709188064779135, - "velocityY": -1.4746976869375825, - "timestamp": 3.3548079042535974 - }, - { - "x": 1.9419022015392158, - "y": 4.889062683793002, - "heading": 2.649783348569472, - "angularVelocity": 4.614715107153485e-26, - "velocityX": 0.023936675164732843, - "velocityY": -1.4854577230996109, - "timestamp": 3.3889538237774923 - }, - { - "x": 1.948223657679148, - "y": 4.838070468392074, - "heading": 2.649783348569472, - "angularVelocity": 4.653945144562686e-26, - "velocityX": 0.18513064600613907, - "velocityY": -1.4933619042018718, - "timestamp": 3.423099743301387 + "angularVelocity": 6.681257540282107e-18, + "velocityX": -0.23625470944103766, + "velocityY": -1.6071503980304298, + "timestamp": 3.2607305555436823 }, { - "x": 1.9600538072506943, - "y": 4.786928471515363, + "x": 1.9456173734407256, + "y": 4.888006870907468, "heading": 2.649783348569472, - "angularVelocity": 4.629261284078221e-26, - "velocityX": 0.3464586614300294, - "velocityY": -1.4977484159101515, - "timestamp": 3.457245662825282 + "angularVelocity": -1.484655388526889e-19, + "velocityX": 0.14165581617647935, + "velocityY": -1.6182360167593688, + "timestamp": 3.292727206352592 }, { - "x": 1.977394685635232, - "y": 4.735788039854744, + "x": 1.9619962278910381, + "y": 4.838678856610239, "heading": 2.649783348569472, - "angularVelocity": 4.640038416252255e-26, - "velocityX": 0.5078462851879747, - "velocityY": -1.4977025768725385, - "timestamp": 3.491391582349177 + "angularVelocity": -1.4846556369937314e-19, + "velocityX": 0.5118927774045035, + "velocityY": -1.5416618005383362, + "timestamp": 3.324723857161502 }, { - "x": 2.0002427480647933, - "y": 4.684845229382865, + "x": 1.9893341653194017, + "y": 4.794473003544163, "heading": 2.649783348569472, - "angularVelocity": 4.6490332666751425e-26, - "velocityX": 0.6691300966012094, - "velocityY": -1.4919150276866968, - "timestamp": 3.525537501873072 + "angularVelocity": -1.4846558519027202e-19, + "velocityX": 0.8543999680324345, + "velocityY": -1.381577507287736, + "timestamp": 3.3567205079704117 }, { - "x": 2.0285822715254445, - "y": 4.634362832128866, + "x": 2.026135553103086, + "y": 4.757769076744815, "heading": 2.649783348569472, - "angularVelocity": 4.622795940866032e-26, - "velocityX": 0.8299534426308102, - "velocityY": -1.478431331119124, - "timestamp": 3.559683421396967 + "angularVelocity": -1.4846559448429508e-19, + "velocityX": 1.1501637469359431, + "velocityY": -1.1471177723711978, + "timestamp": 3.3887171587793214 }, { - "x": 2.0623699922411927, - "y": 4.5847082412928, + "x": 2.0637808893572873, + "y": 4.721931260837698, "heading": 2.649783348569472, - "angularVelocity": 4.634016030607817e-26, - "velocityX": 0.9895097624213414, - "velocityY": -1.454188129311252, - "timestamp": 3.5938293409208617 + "angularVelocity": -1.4846636373679086e-19, + "velocityX": 1.1765398972283652, + "velocityY": -1.120048973905845, + "timestamp": 3.4207138095882312 }, { - "x": 2.101495965027088, - "y": 4.536421557771227, + "x": 2.1014301321789435, + "y": 4.6860975489840255, "heading": 2.649783348569472, - "angularVelocity": 4.645177261974479e-26, - "velocityX": 1.1458462191511727, - "velocityY": -1.4141274915083855, - "timestamp": 3.6279752604447566 + "angularVelocity": -1.484648310808098e-19, + "velocityX": 1.1766619902345172, + "velocityY": -1.1199207088167735, + "timestamp": 3.452710460397141 }, { - "x": 2.145673252803998, - "y": 4.4903376692314145, + "x": 2.139079370280558, + "y": 4.650263832171171, "heading": 2.649783348569472, - "angularVelocity": 4.6233288536561975e-26, - "velocityX": 1.2937794147261192, - "velocityY": -1.3496162698902718, - "timestamp": 3.6621211799686515 + "angularVelocity": -1.48465589810743e-19, + "velocityX": 1.1766618427177973, + "velocityY": -1.1199208638074638, + "timestamp": 3.484707111206051 }, { - "x": 2.1941116699379357, - "y": 4.447748182132556, + "x": 2.17672860838618, + "y": 4.614430115362526, "heading": 2.649783348569472, - "angularVelocity": 4.6479494614855375e-26, - "velocityX": 1.4185711736373225, - "velocityY": -1.2472789631292271, - "timestamp": 3.6962670994925464 + "angularVelocity": -1.4846559463647802e-19, + "velocityX": 1.1766618428430395, + "velocityY": -1.1199208636758753, + "timestamp": 3.5167037620149606 }, { - "x": 2.2448201323304646, - "y": 4.410180142680831, + "x": 2.2143778464918284, + "y": 4.57859639855391, "heading": 2.649783348569472, - "angularVelocity": 4.6329904581670297e-26, - "velocityX": 1.485051891985029, - "velocityY": -1.1002204648621148, - "timestamp": 3.7304130190164413 + "angularVelocity": -1.4846559409367017e-19, + "velocityX": 1.176661842843871, + "velocityY": -1.1199208636750018, + "timestamp": 3.5487004128238704 }, { - "x": 2.294852293597042, - "y": 4.3780811675391975, + "x": 2.2520270845974713, + "y": 4.5427626817452875, "heading": 2.649783348569472, - "angularVelocity": 4.6446166642422895e-26, - "velocityX": 1.4652456856979683, - "velocityY": -0.9400530309096446, - "timestamp": 3.764558938540336 + "angularVelocity": -1.4846559515966518e-19, + "velocityX": 1.1766618428436975, + "velocityY": -1.1199208636751843, + "timestamp": 3.58069706363278 }, { - "x": 2.342149827765447, - "y": 4.350766530180341, + "x": 2.289676322703114, + "y": 4.506928964936664, "heading": 2.649783348569472, - "angularVelocity": 4.633506801188826e-26, - "velocityX": 1.3851591882100787, - "velocityY": -0.7999385501902068, - "timestamp": 3.798704858064231 + "angularVelocity": -1.4846559610313848e-19, + "velocityX": 1.1766618428436995, + "velocityY": -1.1199208636751825, + "timestamp": 3.61269371444169 }, { - "x": 2.3856944951859207, - "y": 4.3274872473595005, + "x": 2.3273255608087573, + "y": 4.471095248128042, "heading": 2.649783348569472, - "angularVelocity": 4.6381778689211406e-26, - "velocityX": 1.2752524467821442, - "velocityY": -0.6817588498253565, - "timestamp": 3.832850777588126 + "angularVelocity": -1.484655947429445e-19, + "velocityX": 1.1766618428436957, + "velocityY": -1.1199208636751863, + "timestamp": 3.6446903652505998 }, { - "x": 2.4249725857215654, - "y": 4.307695719032298, + "x": 2.3649747989143997, + "y": 4.435261531319418, "heading": 2.649783348569472, - "angularVelocity": 4.6432377599415885e-26, - "velocityX": 1.150301151156826, - "velocityY": -0.5796162060697082, - "timestamp": 3.866996697112021 + "angularVelocity": -1.4846559574023035e-19, + "velocityX": 1.1766618428436864, + "velocityY": -1.1199208636751963, + "timestamp": 3.6766870160595095 }, { - "x": 2.4596964858202144, - "y": 4.291007007021927, + "x": 2.4026240370200447, + "y": 4.399427814510799, "heading": 2.649783348569472, - "angularVelocity": 4.6331179794053255e-26, - "velocityX": 1.0169267831358226, - "velocityY": -0.4887468910800007, - "timestamp": 3.9011426166359158 + "angularVelocity": -1.484655924580371e-19, + "velocityX": 1.1766618428437539, + "velocityY": -1.1199208636751252, + "timestamp": 3.7086836668684193 }, { - "x": 2.4896890256889828, - "y": 4.277143601542489, + "x": 2.4402732751256884, + "y": 4.363594097702177, "heading": 2.649783348569472, - "angularVelocity": 4.6467164999430504e-26, - "velocityX": 0.8783638070657234, - "velocityY": -0.4060047488173824, - "timestamp": 3.9352885361598107 + "angularVelocity": -1.4846559654067578e-19, + "velocityX": 1.1766618428437225, + "velocityY": -1.1199208636751585, + "timestamp": 3.740680317677329 }, { - "x": 2.514832571254234, - "y": 4.265898475268687, + "x": 2.4779225132411047, + "y": 4.327760380902407, "heading": 2.649783348569472, - "angularVelocity": 4.632856845272505e-26, - "velocityX": 0.7363557905551701, - "velocityY": -0.3293256245723021, - "timestamp": 3.9694344556837056 + "angularVelocity": -1.4846559492892923e-19, + "velocityX": 1.1766618431491565, + "velocityY": -1.1199208633985085, + "timestamp": 3.772676968486239 }, { - "x": 2.5350443768209585, - "y": 4.257112226026017, + "x": 2.5129728702731393, + "y": 4.294400221963306, "heading": 2.649783348569472, - "angularVelocity": 4.629729705599566e-26, - "velocityX": 0.5919244773180152, - "velocityY": -0.2573147645510306, - "timestamp": 4.0035803752076005 + "angularVelocity": -1.484655203554499e-19, + "velocityX": 1.0954383082580312, + "velocityY": -1.0426140891540008, + "timestamp": 3.8046736192951487 }, { - "x": 2.550263563230007, - "y": 4.2506587632470065, + "x": 2.53926064365096, + "y": 4.2693800974206075, "heading": 2.649783348569472, - "angularVelocity": 4.62810585566047e-26, - "velocityX": 0.445710252388941, - "velocityY": -0.18899660249284694, - "timestamp": 4.037726294731495 + "angularVelocity": -1.484655203554444e-19, + "velocityX": 0.8215789063304317, + "velocityY": -0.7819607337071065, + "timestamp": 3.8366702701040585 }, { - "x": 2.5604437164785643, - "y": 4.246436059535304, + "x": 2.5567858275270727, + "y": 4.252700012844841, "heading": 2.649783348569472, - "angularVelocity": 4.6519462435401186e-26, - "velocityX": 0.29813674343821717, - "velocityY": -0.12366642253542871, - "timestamp": 4.07187221425539 + "angularVelocity": -1.4846552035544855e-19, + "velocityX": 0.5477193216495216, + "velocityY": -0.5213072041629537, + "timestamp": 3.8686669209129683 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 4.6699948971014915e-26, - "velocityX": 0.14949673474910496, - "velocityY": -0.06080051354533503, - "timestamp": 4.106018133779285 + "angularVelocity": -1.4846552035527083e-19, + "velocityX": 0.2738596760533411, + "velocityY": -0.260653616588761, + "timestamp": 3.900663571721878 }, { "x": 2.5655484199523926, "y": 4.244359970092773, "heading": 2.649783348569472, - "angularVelocity": 1.535197914331468e-26, - "velocityX": 1.1963425727073795e-24, - "velocityY": -7.168329076335369e-24, - "timestamp": 4.14016405330318 + "angularVelocity": -4.948850678520552e-20, + "velocityX": 6.0427487543509175e-18, + "velocityY": -9.335003878092332e-18, + "timestamp": 3.932660222530788 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.1.traj b/src/main/deploy/choreo/Test2.1.traj index 6858b3e4..60dd1202 100644 --- a/src/main/deploy/choreo/Test2.1.traj +++ b/src/main/deploy/choreo/Test2.1.traj @@ -1,688 +1,796 @@ { "samples": [ { - "x": 0.3137660622596754, + "x": 0.3137660622596741, "y": 6.865104675292969, - "heading": 3.135568639860517, - "angularVelocity": -8.5693301650697e-28, - "velocityX": -3.4362695197914885e-17, - "velocityY": -4.743570524232907e-17, + "heading": 3.135568639860519, + "angularVelocity": 1.3323616061473399e-26, + "velocityX": -9.030177506128314e-25, + "velocityY": 7.578270673418084e-24, "timestamp": 0 }, { - "x": 0.3176438425782737, - "y": 6.865425616765028, - "heading": 3.1355686398605154, - "angularVelocity": -5.1868461530169816e-17, - "velocityX": 0.13515003367410133, - "velocityY": 0.011185587421808195, - "timestamp": 0.028692411042588198 - }, - { - "x": 0.3253999903453479, - "y": 6.866060324505518, - "heading": 3.1355686398605136, - "angularVelocity": -5.186846153227154e-17, - "velocityX": 0.2703205302461453, - "velocityY": 0.022121101623313478, - "timestamp": 0.057384822085176396 - }, - { - "x": 0.3370351106624937, - "y": 6.867001226604551, - "heading": 3.135568639860512, - "angularVelocity": -5.186846153153239e-17, - "velocityX": 0.40551211607398097, - "velocityY": 0.032792716430754286, - "timestamp": 0.0860772331277646 - }, - { - "x": 0.352549826865152, - "y": 6.86824032070545, - "heading": 3.13556863986051, - "angularVelocity": -5.186846153220684e-17, - "velocityX": 0.5407254266512643, - "velocityY": 0.04318542973123231, - "timestamp": 0.11476964417035279 - }, - { - "x": 0.3719447806630506, - "y": 6.869769136338039, - "heading": 3.1355686398605083, - "angularVelocity": -5.1868461532349994e-17, - "velocityX": 0.6759611023663795, - "velocityY": 0.053282926635904025, - "timestamp": 0.14346205521294098 - }, - { - "x": 0.3952206321171861, - "y": 6.871578692739439, - "heading": 3.135568639860506, - "angularVelocity": -5.1868461531715824e-17, - "velocityX": 0.8112197828054859, - "velocityY": 0.06306742220841621, - "timestamp": 0.17215446625552916 - }, - { - "x": 0.42237805939934603, - "y": 6.8736594514681, - "heading": 3.1355686398605043, - "angularVelocity": -5.1868461532243226e-17, - "velocityX": 0.9465020991734979, - "velocityY": 0.07251947999676765, - "timestamp": 0.20084687729811734 - }, - { - "x": 0.45341775826418784, - "y": 6.876001262985059, - "heading": 3.1355686398605025, - "angularVelocity": -5.1868461531961763e-17, - "velocityX": 1.081808664276005, - "velocityY": 0.08161780177636822, - "timestamp": 0.22953928834070553 - }, - { - "x": 0.4883404411428946, - "y": 6.878593306214626, - "heading": 3.1355686398605007, - "angularVelocity": -5.186846153181617e-17, - "velocityX": 1.2171400593303083, - "velocityY": 0.09033898286621334, - "timestamp": 0.2582316993832937 - }, - { - "x": 0.5271468357396778, - "y": 6.881424019896891, - "heading": 3.135568639860499, - "angularVelocity": -5.186846153207198e-17, - "velocityX": 1.3524968166384297, - "velocityY": 0.09865722605409555, - "timestamp": 0.2869241104258819 - }, - { - "x": 0.5698376829754908, - "y": 6.884481024296066, - "heading": 3.135568639860497, - "angularVelocity": -5.18684615321628e-17, - "velocityX": 1.4878793968358233, - "velocityY": 0.10654400547371551, - "timestamp": 0.3156165214684701 - }, - { - "x": 0.6164137340738928, - "y": 6.887751031517755, - "heading": 3.1355686398604954, - "angularVelocity": -5.186846153204876e-17, - "velocityX": 1.6232881589932495, - "velocityY": 0.11396766959864212, - "timestamp": 0.34430893251105826 - }, - { - "x": 0.666875746517318, - "y": 6.891219742296381, - "heading": 3.1355686398604936, - "angularVelocity": -5.186846153160267e-17, - "velocityX": 1.758723321247663, - "velocityY": 0.12089296969422078, - "timestamp": 0.37300134355364645 - }, - { - "x": 0.7212244785112947, - "y": 6.894871726616115, - "heading": 3.135568639860492, - "angularVelocity": -5.186846153215831e-17, - "velocityX": 1.894184908800638, - "velocityY": 0.1272804963763128, - "timestamp": 0.40169375459623463 - }, - { - "x": 0.7794606814696723, - "y": 6.898690284891088, - "heading": 3.13556863986049, - "angularVelocity": -5.186846153213201e-17, - "velocityX": 2.02967268494572, - "velocityY": 0.1330860020548787, - "timestamp": 0.4303861656388228 - }, - { - "x": 0.8415850898614354, - "y": 6.9026572856068995, - "heading": 3.1355686398604883, - "angularVelocity": -5.186846153171342e-17, - "velocityX": 2.165186059113343, - "velocityY": 0.13825958055332968, - "timestamp": 0.459078576681411 - }, - { - "x": 0.9075984075182041, - "y": 6.906752974251311, - "heading": 3.1355686398604865, - "angularVelocity": -5.1868461532457525e-17, - "velocityX": 2.3007239635171715, - "velocityY": 0.14274466646709222, - "timestamp": 0.4877709877239992 - }, - { - "x": 0.9775012891595924, - "y": 6.910955746947304, - "heading": 3.1355686398604847, - "angularVelocity": -5.186846153166985e-17, - "velocityX": 2.43628468648487, - "velocityY": 0.14647680495568935, - "timestamp": 0.5164633987665874 - }, - { - "x": 1.051294315403438, - "y": 6.915241880318482, - "heading": 3.1355686398604825, - "angularVelocity": -5.1868461532007456e-17, - "velocityX": 2.571865645390099, - "velocityY": 0.1493821263335398, - "timestamp": 0.5451558098091757 - }, - { - "x": 1.1289779588157673, - "y": 6.919585206580955, - "heading": 3.1355686398604807, - "angularVelocity": -5.186846153206435e-17, - "velocityX": 2.7074630743656276, - "velocityY": 0.15137543708073178, - "timestamp": 0.5738482208517639 - }, - { - "x": 1.210552537505391, - "y": 6.923956719399065, - "heading": 3.135568639860479, - "angularVelocity": -5.1868461531968826e-17, - "velocityX": 2.843071590203452, - "velocityY": 0.1523578067950584, - "timestamp": 0.6025406318943521 - }, - { - "x": 1.29601815119489, - "y": 6.9283240912670765, - "heading": 3.135568639860477, - "angularVelocity": -5.186846153216223e-17, - "velocityX": 2.978683581614699, - "velocityY": 0.15221348465724693, - "timestamp": 0.6312330429369404 - }, - { - "x": 1.3853745923008893, - "y": 6.932651076487761, - "heading": 3.1355686398604754, - "angularVelocity": -5.1868461531841376e-17, - "velocityX": 3.1142883382426785, - "velocityY": 0.15080591220658612, - "timestamp": 0.6599254539795286 - }, - { - "x": 1.478621220826743, - "y": 6.936896764298285, - "heading": 3.1355686398604736, - "angularVelocity": -5.186846153215644e-17, - "velocityX": 3.2498707894378844, - "velocityY": 0.1479725006107481, - "timestamp": 0.6886178650221169 - }, - { - "x": 1.5757567859462194, - "y": 6.941014632922591, - "heading": 3.135568639860472, - "angularVelocity": -5.186846153173054e-17, - "velocityX": 3.3854096463101833, - "velocityY": 0.14351769247253987, - "timestamp": 0.7173102760647051 - }, - { - "x": 1.676779167520274, - "y": 6.94495133505747, - "heading": 3.13556863986047, - "angularVelocity": -5.186846153388186e-17, - "velocityX": 3.52087461120313, - "velocityY": 0.13720360164351214, - "timestamp": 0.7460026871072933 - }, - { - "x": 1.7816849946975712, - "y": 6.948645114898683, - "heading": 3.1355686398604683, - "angularVelocity": -1.6596882408000968e-16, - "velocityX": 3.6562220937649346, - "velocityY": 0.12873717150261788, - "timestamp": 0.7746950981498816 - }, - { - "x": 1.8121461784699802, - "y": 6.949692765946351, - "heading": 3.1362037838100756, - "angularVelocity": 0.07689012502825784, - "velocityX": 3.687611651202223, - "velocityY": 0.12682797354934053, - "timestamp": 0.7829555080448543 - }, - { - "x": 1.8428693964358074, - "y": 6.950724930078729, - "heading": 3.1374589452138397, - "angularVelocity": 0.15194904607940982, - "velocityX": 3.7193333450103565, - "velocityY": 0.12495313737441759, - "timestamp": 0.7912159179398269 - }, - { - "x": 1.8738574968140793, - "y": 6.95174197296817, - "heading": 3.139318096432083, - "angularVelocity": 0.22506767120320992, - "velocityX": 3.751399842413374, - "velocityY": 0.1231225692636009, - "timestamp": 0.7994763278347996 - }, - { - "x": 1.9051134303883683, - "y": 6.952744343319344, - "heading": 3.1417642609142864, - "angularVelocity": 0.2961311258529596, - "velocityX": 3.783823559810237, - "velocityY": 0.12134632105632295, - "timestamp": 0.8077367377297723 - }, - { - "x": 1.9366402470506743, - "y": 6.953732574884012, - "heading": 3.1447794649491048, - "angularVelocity": 0.36501869436959006, - "velocityX": 3.8166164952046153, - "velocityY": 0.11963468849991928, - "timestamp": 0.815997147624745 - }, - { - "x": 1.96844109067084, - "y": 6.9547072895390265, - "heading": 3.148344689757031, - "angularVelocity": 0.43160386146187707, - "velocityX": 3.849790025494764, - "velocityY": 0.117998339961998, - "timestamp": 0.8242575575197176 - }, - { - "x": 2.0005191919654135, - "y": 6.955669201722795, - "heading": 3.152439824908413, - "angularVelocity": 0.495754472653526, - "velocityX": 3.883354664287875, - "velocityY": 0.11644848088619933, - "timestamp": 0.8325179674146903 - }, - { - "x": 2.032877859008433, - "y": 6.9566191245667985, - "heading": 3.1570436241813726, - "angularVelocity": 0.5573330296555058, - "velocityX": 3.91731977643278, - "velocityY": 0.11499705899340439, - "timestamp": 0.840778377309663 - }, - { - "x": 2.065520465002766, - "y": 6.957557978090485, - "heading": 3.1621336650538163, - "angularVelocity": 0.6161971303075195, - "velocityX": 3.951693246384564, - "velocityY": 0.11365701407324179, - "timestamp": 0.8490387872046357 - }, - { - "x": 2.0984504329177724, - "y": 6.958486799841183, - "heading": 3.1676863130152344, - "angularVelocity": 0.6722000520579139, - "velocityX": 3.9864810988430395, - "velocityY": 0.11244257397721802, - "timestamp": 0.8572991970996083 - }, - { - "x": 2.1316712166001497, - "y": 6.959406758344203, - "heading": 3.1736766917418935, - "angularVelocity": 0.7251914617828771, - "velocityX": 4.021687071799445, - "velocityY": 0.1113695948160438, - "timestamp": 0.865559606994581 - }, - { - "x": 2.165186277981829, - "y": 6.960319169668765, - "heading": 3.1800786598480077, - "angularVelocity": 0.7750182118701426, - "velocityX": 4.057312144046869, - "velocityY": 0.11045593816320934, - "timestamp": 0.8738200168895537 - }, - { - "x": 2.1989990600414004, - "y": 6.961225517299853, - "heading": 3.1868647943382302, - "angularVelocity": 0.8215251514763765, - "velocityX": 4.093354021105941, - "velocityY": 0.10972187126423899, - "timestamp": 0.8820804267845264 - }, - { - "x": 2.2331129552203137, - "y": 6.962127475323903, - "heading": 3.19400637995892, - "angularVelocity": 0.8645558406293681, - "velocityX": 4.1298065849823535, - "velocityY": 0.10919046821108658, - "timestamp": 0.890340836679499 - }, - { - "x": 2.2675312690432237, - "y": 6.963026934685406, - "heading": 3.201473402262516, - "angularVelocity": 0.9039529997346885, - "velocityX": 4.1666593135838506, - "velocityY": 0.10888798170273117, - "timestamp": 0.8986012465744717 - }, - { - "x": 2.302257178726397, - "y": 6.963926031963223, - "heading": 3.2092345401896574, - "angularVelocity": 0.9395584511933502, - "velocityX": 4.203896673978105, - "velocityY": 0.10884414808067534, - "timestamp": 0.9068616564694444 - }, - { - "x": 2.3372936865473584, - "y": 6.964827179782931, - "heading": 3.2172571510612844, - "angularVelocity": 0.9712122005605541, - "velocityX": 4.241497488191654, - "velocityY": 0.10909238538572823, - "timestamp": 0.9151220663644171 - }, - { - "x": 2.372643567633051, - "y": 6.965733097685614, - "heading": 3.225507236598066, - "angularVelocity": 0.9987501397242283, - "velocityX": 4.279434257518503, - "velocityY": 0.10966984861524744, - "timestamp": 0.9233824762593897 - }, - { - "x": 2.4083093114869696, - "y": 6.9666468421059164, - "heading": 3.233949372180996, - "angularVelocity": 1.0219995969049733, - "velocityX": 4.31767240456481, - "velocityY": 0.11061732189065114, - "timestamp": 0.9316428861543624 - }, - { - "x": 2.44429305579275, - "y": 6.967571834188082, - "heading": 3.2425465717794997, - "angularVelocity": 1.0407715486052915, - "velocityX": 4.356169338240552, - "velocityY": 0.11197895672538041, - "timestamp": 0.9399032960493351 - }, - { - "x": 2.480596509367599, - "y": 6.968511884597095, - "heading": 3.251260045781759, - "angularVelocity": 1.054847654420144, - "velocityX": 4.394873140247142, - "velocityY": 0.11380190825438415, - "timestamp": 0.9481637059443078 - }, - { - "x": 2.517220857768171, - "y": 6.969471215284249, - "heading": 3.260048785175124, - "angularVelocity": 1.0639592350877263, - "velocityX": 4.433720464992984, - "velocityY": 0.11613596653794503, - "timestamp": 0.9564241158392804 - }, - { - "x": 2.55416663843719, - "y": 6.970454479070239, - "heading": 3.2688688682505744, - "angularVelocity": 1.0677536814268536, - "velocityX": 4.4726328522150816, - "velocityY": 0.11903329235341958, - "timestamp": 0.9646845257342531 - }, - { - "x": 2.5914335596417093, - "y": 6.971466777824258, - "heading": 3.2776723283732965, - "angularVelocity": 1.065741317277861, - "velocityX": 4.511509922431134, - "velocityY": 0.12254824722845813, - "timestamp": 0.9729449356292258 - }, - { - "x": 2.6290202140808088, - "y": 6.9725136756896955, - "heading": 3.286405336491996, - "angularVelocity": 1.0572124422077387, - "velocityX": 4.550216625687468, - "velocityY": 0.12673679378462502, - "timestamp": 0.9812053455241985 - }, - { - "x": 2.9717276780335036, - "y": 6.984363087388876, - "heading": 3.35108767309623, - "angularVelocity": 0.9203746590757935, - "velocityX": 4.876435853393211, - "velocityY": 0.16860705449782717, - "timestamp": 1.0514836131475132 - }, - { - "x": 3.3366815448761877, - "y": 6.999743371642888, - "heading": 3.3973428408952557, - "angularVelocity": 0.6581717131524953, - "velocityX": 5.192983253355129, - "velocityY": 0.21884836912099448, - "timestamp": 1.1217618807708278 - }, - { - "x": 3.71402485435163, - "y": 7.027526643127482, - "heading": 3.3973428368677423, - "angularVelocity": -5.730809398962461e-8, - "velocityX": 5.369274488921224, - "velocityY": 0.3953323327989187, - "timestamp": 1.1920401483941425 - }, - { - "x": 4.090360227255461, - "y": 7.066661937541599, - "heading": 3.3973428243317834, - "angularVelocity": -1.783760359331945e-7, - "velocityX": 5.354932408421835, - "velocityY": 0.5568619679682045, - "timestamp": 1.2623184160174572 - }, - { - "x": 4.466694898854889, - "y": 7.105803975313799, - "heading": 3.3973428117958613, - "angularVelocity": -1.7837551225317533e-7, - "velocityX": 5.354922429456401, - "velocityY": 0.5569579202208789, - "timestamp": 1.3325966836407719 - }, - { - "x": 4.8430291421579215, - "y": 7.144950130722809, - "heading": 3.397342799228156, - "angularVelocity": -1.7882775854425254e-7, - "velocityX": 5.354916335162799, - "velocityY": 0.5570165106920909, - "timestamp": 1.4028749512640866 - }, - { - "x": 5.214361577079249, - "y": 7.183676617694927, - "heading": 3.378511132398759, - "angularVelocity": -0.26795860891639617, - "velocityX": 5.283744854264688, - "velocityY": 0.55104498562348, - "timestamp": 1.4731532188874013 - }, - { - "x": 5.562524060208366, - "y": 7.219962031584869, - "heading": 3.355414523374288, - "angularVelocity": -0.32864511043819594, - "velocityX": 4.9540561385951625, - "velocityY": 0.5163105909843465, - "timestamp": 1.543431486510716 - }, - { - "x": 5.887470499962918, - "y": 7.253817931255832, - "heading": 3.331666055733143, - "angularVelocity": -0.3379205043646441, - "velocityX": 4.6237115788942855, - "velocityY": 0.48174066914158636, - "timestamp": 1.6137097541340306 - }, - { - "x": 6.189199807150463, - "y": 7.285249171486667, - "heading": 3.308363686841363, - "angularVelocity": -0.33157289841972754, - "velocityX": 4.29335152091094, - "velocityY": 0.4472398266744939, - "timestamp": 1.6839880217573453 - }, - { - "x": 6.467713626267079, - "y": 7.31425833816948, - "heading": 3.2860397702782755, - "angularVelocity": -0.3176503536305448, - "velocityX": 3.963014862708697, - "velocityY": 0.41277577925367004, - "timestamp": 1.75426628938066 - }, - { - "x": 6.723013594809074, - "y": 7.34084703024564, - "heading": 3.265008761402615, - "angularVelocity": -0.29925337642620564, - "velocityX": 3.6327015046867763, - "velocityY": 0.37833448340917136, - "timestamp": 1.8245445570039747 - }, - { - "x": 6.955101065031071, - "y": 7.365016333172441, - "heading": 3.245478308115181, - "angularVelocity": -0.27790174612893576, - "velocityX": 3.302407388098493, - "velocityY": 0.34390863269912847, - "timestamp": 1.8948228246272893 - }, - { - "x": 7.163977127241494, - "y": 7.386767031992689, - "heading": 3.2275957745381954, - "angularVelocity": -0.2544532496565816, - "velocityX": 2.972128785672115, - "velocityY": 0.3094939524808096, - "timestamp": 1.965101092250604 - }, - { - "x": 7.349642665971283, - "y": 7.4060997209757735, - "heading": 3.2114711639593976, - "angularVelocity": -0.22943949992088322, - "velocityX": 2.6418627693690517, - "velocityY": 0.2750877282106925, - "timestamp": 2.035379359873919 - }, - { - "x": 7.512098408217057, - "y": 7.423014865697383, - "heading": 3.1971897253730983, - "angularVelocity": -0.2032127294720464, - "velocityX": 2.3116070976097918, - "velocityY": 0.24068812868678965, - "timestamp": 2.1056576274972336 - }, - { - "x": 7.651344959888608, - "y": 7.43751284079522, - "heading": 3.184819461924971, - "angularVelocity": -0.17601833207433357, - "velocityX": 1.981360047431725, - "velocityY": 0.20629385993890986, - "timestamp": 2.1759358951205483 - }, - { - "x": 7.767382832665261, - "y": 7.449593954241994, - "heading": 3.174415882660127, - "angularVelocity": -0.14803408815663477, - "velocityX": 1.6511202780154728, - "velocityY": 0.1719039733808395, - "timestamp": 2.246214162743863 - }, - { - "x": 7.860212463836391, - "y": 7.459258463658005, - "heading": 3.1660251568616267, - "angularVelocity": -0.11939289459260129, - "velocityX": 1.3208867308552614, - "velocityY": 0.13751775254062693, - "timestamp": 2.3164924303671777 - }, - { - "x": 7.929834231127341, - "y": 7.466506587678373, - "heading": 3.159686290431417, - "angularVelocity": -0.09019668020541635, - "velocityX": 0.9906585584055106, - "velocityY": 0.10313464269233455, - "timestamp": 2.3867706979904924 - }, - { - "x": 7.976248463934981, - "y": 7.471338514116222, - "heading": 3.1554326760982865, - "angularVelocity": -0.06052531567696676, - "velocityX": 0.6604350729932147, - "velocityY": 0.06875420526505516, - "timestamp": 2.457048965613807 + "x": 0.3285749308509426, + "y": 6.865947459797136, + "heading": 3.135568639860519, + "angularVelocity": 3.3337145202478786e-22, + "velocityX": 0.41797046933441173, + "velocityY": 0.023787032249159984, + "timestamp": 0.03543041836149472 + }, + { + "x": 0.35819266531481686, + "y": 6.867633028650745, + "heading": 3.135568639860519, + "angularVelocity": 3.3337147019213317e-22, + "velocityX": 0.8359408619363751, + "velocityY": 0.04757406013141577, + "timestamp": 0.07086083672298944 + }, + { + "x": 0.4026192574953566, + "y": 6.87016138138964, + "heading": 3.135568639860519, + "angularVelocity": 3.3337143389947597e-22, + "velocityX": 1.2539110243423486, + "velocityY": 0.07136107491303612, + "timestamp": 0.10629125508448417 + }, + { + "x": 0.4600803298886451, + "y": 6.873431536777097, + "heading": 3.135568639860519, + "angularVelocity": 3.331660178480041e-22, + "velocityX": 1.621800561512317, + "velocityY": 0.09229796143220789, + "timestamp": 0.14172167344597889 + }, + { + "x": 0.5175414022820589, + "y": 6.8767016921645565, + "heading": 3.135568639860519, + "angularVelocity": 3.3316629368551667e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233462, + "timestamp": 0.1771520918074736 + }, + { + "x": 0.5750024746754727, + "y": 6.879971847552017, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614681891394e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233254, + "timestamp": 0.2125825101689683 + }, + { + "x": 0.6324635470688867, + "y": 6.883242002939478, + "heading": 3.135568639860519, + "angularVelocity": 3.331683104806914e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.24801292853046303 + }, + { + "x": 0.6899246194623005, + "y": 6.886512158326939, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614878052174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.28344334689195777 + }, + { + "x": 0.7473856918557144, + "y": 6.8897823137144, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614758461554e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.3188737652534525 + }, + { + "x": 0.8048467642491284, + "y": 6.89305246910186, + "heading": 3.135568639860519, + "angularVelocity": 3.331638534198991e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233299, + "timestamp": 0.35430418361494725 + }, + { + "x": 0.8623078366425422, + "y": 6.89632262448932, + "heading": 3.135568639860519, + "angularVelocity": 3.3317076614902997e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.389734601976442 + }, + { + "x": 0.9197689090359561, + "y": 6.899592779876781, + "heading": 3.135568639860519, + "angularVelocity": 3.331661479116866e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233396, + "timestamp": 0.42516502033793674 + }, + { + "x": 0.97722998142937, + "y": 6.902862935264242, + "heading": 3.135568639860519, + "angularVelocity": 3.33166145502926e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.4605954386994315 + }, + { + "x": 1.0346910538227838, + "y": 6.906133090651703, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600659908063e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233301, + "timestamp": 0.4960258570609262 + }, + { + "x": 1.0921521262161977, + "y": 6.9094032460391634, + "heading": 3.135568639860519, + "angularVelocity": 3.331661460674642e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5314562754224209 + }, + { + "x": 1.1496131986096116, + "y": 6.912673401426624, + "heading": 3.135568639860519, + "angularVelocity": 3.331661475383932e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5668866937839157 + }, + { + "x": 1.2070742710030253, + "y": 6.915943556814084, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614753921174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.6023171121454104 + }, + { + "x": 1.2645353433964392, + "y": 6.919213712201545, + "heading": 3.135568639860519, + "angularVelocity": 3.331662731932369e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.6377475305069051 + }, + { + "x": 1.3219964157898532, + "y": 6.922483867589006, + "heading": 3.135568639860519, + "angularVelocity": 3.3316627143346654e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233393, + "timestamp": 0.6731779488683999 + }, + { + "x": 1.379457488183267, + "y": 6.925754022976466, + "heading": 3.135568639860519, + "angularVelocity": 3.331661483573468e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233157, + "timestamp": 0.7086083672298946 + }, + { + "x": 1.436918560576681, + "y": 6.929024178363926, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614890219236e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233392, + "timestamp": 0.7440387855913894 + }, + { + "x": 1.494379632970095, + "y": 6.932294333751387, + "heading": 3.135568639860519, + "angularVelocity": 3.331662743669949e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233207, + "timestamp": 0.7794692039528841 + }, + { + "x": 1.5518407053635086, + "y": 6.935564489138847, + "heading": 3.135568639860519, + "angularVelocity": 3.331638909213181e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.8148996223143788 + }, + { + "x": 1.6093017777569225, + "y": 6.938834644526308, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600948450887e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233622, + "timestamp": 0.8503300406758736 + }, + { + "x": 1.666762850150336, + "y": 6.942104799913777, + "heading": 3.135568639860519, + "angularVelocity": 3.331671581625241e-22, + "velocityX": 1.621800561515844, + "velocityY": 0.09229796143255317, + "timestamp": 0.8857604590373683 + }, + { + "x": 1.7242239225437261, + "y": 6.945374955301653, + "heading": 3.135568639860519, + "angularVelocity": 3.3334679678699274e-22, + "velocityX": 1.6218005615151887, + "velocityY": 0.09229796144406777, + "timestamp": 0.9211908773988631 + }, + { + "x": 1.7816849946975708, + "y": 6.948645114898682, + "heading": 3.135568639860519, + "angularVelocity": 1.7645788213167404e-22, + "velocityX": 1.6218005547541783, + "velocityY": 0.09229808024460076, + "timestamp": 0.9566212957603578 + }, + { + "x": 1.8178857407766151, + "y": 6.94853231479887, + "heading": 3.1384797599068888, + "angularVelocity": 0.12817174361754557, + "velocityX": 1.5938582646199322, + "velocityY": -0.004966399613447973, + "timestamp": 0.9793339464913666 + }, + { + "x": 1.8540603970824046, + "y": 6.948528771707492, + "heading": 3.1414999940123267, + "angularVelocity": 0.13297585302602363, + "velocityX": 1.592709575567121, + "velocityY": -0.00015599638369279018, + "timestamp": 1.0020465972223755 + }, + { + "x": 1.8901908310793394, + "y": 6.948640868874459, + "heading": 3.1447030512216227, + "angularVelocity": 0.14102524831780386, + "velocityX": 1.5907625413183106, + "velocityY": 0.0049354506567203535, + "timestamp": 1.0247592479533845 + }, + { + "x": 1.926257669084107, + "y": 6.9488776015696425, + "heading": 3.148167194864803, + "angularVelocity": 0.15252044704983372, + "velocityX": 1.5879625162168578, + "velocityY": 0.010422944375284186, + "timestamp": 1.0474718986843934 + }, + { + "x": 1.9622398024006038, + "y": 6.94925072993569, + "heading": 3.151976954699645, + "angularVelocity": 0.16773734954856945, + "velocityX": 1.584233110553301, + "velocityY": 0.01642821749276163, + "timestamp": 1.0701845494154023 + }, + { + "x": 1.9981171224268564, + "y": 6.949774504499785, + "heading": 3.156211459419019, + "angularVelocity": 0.1864381559653407, + "velocityX": 1.5796183567984214, + "velocityY": 0.023060917472754097, + "timestamp": 1.0928972001464112 + }, + { + "x": 2.033847488660219, + "y": 6.950468453992633, + "heading": 3.161038496208879, + "angularVelocity": 0.2125263513725333, + "velocityX": 1.5731482272380108, + "velocityY": 0.03055343478252191, + "timestamp": 1.1156098508774202 + }, + { + "x": 2.0694053227389695, + "y": 6.951354952814641, + "heading": 3.166554769842392, + "angularVelocity": 0.24287229609803831, + "velocityX": 1.565551925218666, + "velocityY": 0.039031059496573456, + "timestamp": 1.138322501608429 + }, + { + "x": 2.104784911670157, + "y": 6.952454847185295, + "heading": 3.1727732170956275, + "angularVelocity": 0.27378782542301205, + "velocityX": 1.5577040896809682, + "velocityY": 0.04842650836667849, + "timestamp": 1.161035152339438 + }, + { + "x": 2.1400047240253706, + "y": 6.9537191803371154, + "heading": 3.1796054213345153, + "angularVelocity": 0.3008105183231903, + "velocityX": 1.550669394441415, + "velocityY": 0.05566647269821489, + "timestamp": 1.183747803070447 + }, + { + "x": 2.175095855891208, + "y": 6.955084615506324, + "heading": 3.18691368664007, + "angularVelocity": 0.3217706903570311, + "velocityX": 1.5450038078527182, + "velocityY": 0.060117825320333544, + "timestamp": 1.2064604538014558 + }, + { + "x": 2.2101380445057046, + "y": 6.956471002562802, + "heading": 3.194369840668752, + "angularVelocity": 0.3282819833311086, + "velocityX": 1.5428489184071512, + "velocityY": 0.06104030185188484, + "timestamp": 1.2291731045324648 + }, + { + "x": 2.2451610724837057, + "y": 6.957849512193169, + "heading": 3.2018529792356185, + "angularVelocity": 0.32947006738627316, + "velocityX": 1.5420053076493287, + "velocityY": 0.060693471963871995, + "timestamp": 1.2518857552634737 + }, + { + "x": 2.2801629678458126, + "y": 6.959221595789265, + "heading": 3.2093707720743723, + "angularVelocity": 0.3309958369804056, + "velocityX": 1.5410748739388525, + "velocityY": 0.060410544429463274, + "timestamp": 1.2745984059944826 + }, + { + "x": 2.315144049729391, + "y": 6.960586594052822, + "heading": 3.2169217131634897, + "angularVelocity": 0.3324552989672944, + "velocityX": 1.540158491312511, + "velocityY": 0.06009858909567994, + "timestamp": 1.2973110567254915 + }, + { + "x": 2.3501040543802802, + "y": 6.961944386116353, + "heading": 3.2245066187504112, + "angularVelocity": 0.3339506989629348, + "velocityX": 1.539230496031878, + "velocityY": 0.05978131216881497, + "timestamp": 1.3200237074565004 + }, + { + "x": 2.3850426980471595, + "y": 6.963294864074251, + "heading": 3.232126373384074, + "angularVelocity": 0.3354850441678861, + "velocityX": 1.5382900076554706, + "velocityY": 0.05945928433854099, + "timestamp": 1.3427363581875094 + }, + { + "x": 2.419959710539657, + "y": 6.964637896582436, + "heading": 3.239781798872735, + "angularVelocity": 0.3370555722150803, + "velocityX": 1.5373376232491214, + "velocityY": 0.05913147364836824, + "timestamp": 1.3654490089185183 + }, + { + "x": 2.454854803951486, + "y": 6.965973356645481, + "heading": 3.247473776388813, + "angularVelocity": 0.3386648968090985, + "velocityX": 1.5363725628109135, + "velocityY": 0.05879807156202029, + "timestamp": 1.3881616596495272 + }, + { + "x": 2.48972762858419, + "y": 6.967301155248667, + "heading": 3.2552034126870217, + "angularVelocity": 0.3403229499609042, + "velocityX": 1.5353921057348447, + "velocityY": 0.05846075030657316, + "timestamp": 1.4108743103805361 + }, + { + "x": 2.5245780070203394, + "y": 6.968621054178632, + "heading": 3.262971149366171, + "angularVelocity": 0.34200044596925644, + "velocityX": 1.5344038372663136, + "velocityY": 0.05811294091547401, + "timestamp": 1.433586961111545 + }, + { + "x": 2.559405532428149, + "y": 6.969932985582836, + "heading": 3.2707782920481354, + "angularVelocity": 0.3437354263236719, + "velocityX": 1.5333976566751537, + "velocityY": 0.05776214409057404, + "timestamp": 1.456299611842554 + }, + { + "x": 2.594207801112396, + "y": 6.97123839692077, + "heading": 3.2786335848605472, + "angularVelocity": 0.34585539598366793, + "velocityX": 1.5322856454060756, + "velocityY": 0.05747507648462563, + "timestamp": 1.4790122625735629 + }, + { + "x": 2.6290202140808105, + "y": 6.972513675689697, + "heading": 3.286405336491994, + "angularVelocity": 0.3421772175995684, + "velocityX": 1.5327322812606092, + "velocityY": 0.05614838990089804, + "timestamp": 1.5017249133045718 + }, + { + "x": 2.7840510289336455, + "y": 6.98699854094784, + "heading": 3.286405287600397, + "angularVelocity": -5.100684883763116e-7, + "velocityX": 1.6173808639626774, + "velocityY": 0.1511154018498607, + "timestamp": 1.5975779175703702 + }, + { + "x": 2.93908160001489, + "y": 7.001486015045406, + "heading": 3.2864052387079807, + "angularVelocity": -5.100770354356606e-7, + "velocityX": 1.6173783207811374, + "velocityY": 0.15114261893547631, + "timestamp": 1.6934309218361685 + }, + { + "x": 3.094112171101734, + "y": 7.015973489083046, + "heading": 3.286405189815564, + "angularVelocity": -5.100770390190521e-7, + "velocityX": 1.617378320839561, + "velocityY": 0.15114261831028078, + "timestamp": 1.7892839261019668 + }, + { + "x": 3.2491427421885786, + "y": 7.030460963120681, + "heading": 3.286405140923147, + "angularVelocity": -5.100770430673946e-7, + "velocityX": 1.617378320839565, + "velocityY": 0.15114261831023715, + "timestamp": 1.8851369303677652 + }, + { + "x": 3.404173313275423, + "y": 7.0449484371583155, + "heading": 3.28640509203073, + "angularVelocity": -5.100770460466358e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023496, + "timestamp": 1.9809899346335635 + }, + { + "x": 3.5592038843622675, + "y": 7.05943591119595, + "heading": 3.286405043138312, + "angularVelocity": -5.100770488976204e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023276, + "timestamp": 2.076842938899362 + }, + { + "x": 3.714234455449112, + "y": 7.073923385233584, + "heading": 3.2864049942458946, + "angularVelocity": -5.100770524050277e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831023054, + "timestamp": 2.1726959431651602 + }, + { + "x": 3.8692650265359565, + "y": 7.088410859271219, + "heading": 3.2864049453534765, + "angularVelocity": -5.100770565197187e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831022827, + "timestamp": 2.2685489474309586 + }, + { + "x": 4.0242955976228005, + "y": 7.102898333308853, + "heading": 3.2864048964610575, + "angularVelocity": -5.100770606946053e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831022607, + "timestamp": 2.364401951696757 + }, + { + "x": 4.179326168709644, + "y": 7.117385807346487, + "heading": 3.286404847568639, + "angularVelocity": -5.100770634020749e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.151142618310224, + "timestamp": 2.4602549559625553 + }, + { + "x": 4.334356739796489, + "y": 7.13187328138412, + "heading": 3.2864047986762195, + "angularVelocity": -5.100770666030285e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831022163, + "timestamp": 2.5561079602283536 + }, + { + "x": 4.489387310883332, + "y": 7.146360755421753, + "heading": 3.2864047497838, + "angularVelocity": -5.100770706092546e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831021944, + "timestamp": 2.651960964494152 + }, + { + "x": 4.644417881970177, + "y": 7.160848229459387, + "heading": 3.28640470089138, + "angularVelocity": -5.100770740673254e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021727, + "timestamp": 2.7478139687599503 + }, + { + "x": 4.79944845305702, + "y": 7.175335703497019, + "heading": 3.28640465199896, + "angularVelocity": -5.100770777885376e-7, + "velocityX": 1.6173783208395638, + "velocityY": 0.15114261831021514, + "timestamp": 2.8436669730257487 + }, + { + "x": 4.954479024143865, + "y": 7.1898231775346515, + "heading": 3.286404603106539, + "angularVelocity": -5.100770810299081e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021283, + "timestamp": 2.939519977291547 + }, + { + "x": 5.109509595230708, + "y": 7.2043106515722855, + "heading": 3.2864045542141187, + "angularVelocity": -5.100770846564483e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021056, + "timestamp": 3.0353729815573454 + }, + { + "x": 5.264540166317553, + "y": 7.218798125609918, + "heading": 3.286404505321697, + "angularVelocity": -5.100770889061609e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.1511426183102084, + "timestamp": 3.1312259858231437 + }, + { + "x": 5.419570737404396, + "y": 7.233285599647549, + "heading": 3.2864044564292754, + "angularVelocity": -5.100770922378371e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020614, + "timestamp": 3.227078990088942 + }, + { + "x": 5.574601308491241, + "y": 7.247773073685181, + "heading": 3.2864044075368533, + "angularVelocity": -5.100770952675197e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020392, + "timestamp": 3.3229319943547404 + }, + { + "x": 5.7296318795780845, + "y": 7.262260547722812, + "heading": 3.286404358644431, + "angularVelocity": -5.100770991840634e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.1511426183102018, + "timestamp": 3.4187849986205388 + }, + { + "x": 5.884662450664929, + "y": 7.276748021760444, + "heading": 3.286404309752008, + "angularVelocity": -5.100771023487481e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.15114261831019962, + "timestamp": 3.514638002886337 + }, + { + "x": 6.039693021751773, + "y": 7.291235495798076, + "heading": 3.286404260859585, + "angularVelocity": -5.100771061559551e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.15114261831019746, + "timestamp": 3.6104910071521354 + }, + { + "x": 6.194723592838616, + "y": 7.305722969835706, + "heading": 3.2864042119671617, + "angularVelocity": -5.100771091660107e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.1511426183101951, + "timestamp": 3.706344011417934 + }, + { + "x": 6.349754163925461, + "y": 7.320210443873337, + "heading": 3.2864041630747383, + "angularVelocity": -5.100771129976334e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831019293, + "timestamp": 3.802197015683732 + }, + { + "x": 6.504784735012304, + "y": 7.334697917910969, + "heading": 3.286404114182314, + "angularVelocity": -5.100771163404491e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.1511426183101907, + "timestamp": 3.8980500199495305 + }, + { + "x": 6.659815306099149, + "y": 7.349185391948599, + "heading": 3.28640406528989, + "angularVelocity": -5.100771202032421e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.1511426183101885, + "timestamp": 3.993903024215329 + }, + { + "x": 6.814845877185992, + "y": 7.363672865986229, + "heading": 3.2864040163974653, + "angularVelocity": -5.100771233944824e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831018627, + "timestamp": 4.089756028481127 + }, + { + "x": 6.969876448272837, + "y": 7.378160340023859, + "heading": 3.2864039675050396, + "angularVelocity": -5.100771270486632e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.15114261831018408, + "timestamp": 4.1856090327469255 + }, + { + "x": 7.12490701935968, + "y": 7.392647814061489, + "heading": 3.286403918612615, + "angularVelocity": -5.100771304657537e-7, + "velocityX": 1.617378320839562, + "velocityY": 0.15114261831018191, + "timestamp": 4.281462037012724 + }, + { + "x": 7.279937590446525, + "y": 7.407135288099118, + "heading": 3.286403869720189, + "angularVelocity": -5.10077134427039e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.1511426183101796, + "timestamp": 4.377315041278522 + }, + { + "x": 7.4349681615333685, + "y": 7.421622762136748, + "heading": 3.286403820827763, + "angularVelocity": -5.100771387503622e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.15114261831017736, + "timestamp": 4.473168045544321 + }, + { + "x": 7.589998732620213, + "y": 7.436110236174376, + "heading": 3.2864037719353365, + "angularVelocity": -5.100771410554518e-7, + "velocityX": 1.6173783208395616, + "velocityY": 0.15114261831017525, + "timestamp": 4.569021049810119 + }, + { + "x": 7.745029303707056, + "y": 7.450597710212007, + "heading": 3.28640372304291, + "angularVelocity": -5.100771448287922e-7, + "velocityX": 1.6173783208395598, + "velocityY": 0.1511426183101905, + "timestamp": 4.664874054075917 + }, + { + "x": 7.90005987484092, + "y": 7.465085183746354, + "heading": 3.2864036741504377, + "angularVelocity": -5.1007762076643e-7, + "velocityX": 1.6173783213301043, + "velocityY": 0.15114261305961757, + "timestamp": 4.760727058341716 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -0.03044253542482025, - "velocityX": 0.33021570985126986, - "velocityY": 0.03437608724320182, - "timestamp": 2.5273272332371217 + "angularVelocity": -1.3886935284235506, + "velocityX": 1.0369583915052876, + "velocityY": 0.09044288486721058, + "timestamp": 4.856580062607514 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -3.79407884851814e-18, - "velocityX": 2.546987187300363e-16, - "velocityY": -1.0256209789219734e-16, - "timestamp": 2.5976055008604364 + "angularVelocity": 5.239369635456734e-24, + "velocityX": 2.4359888438060422e-22, + "velocityY": -8.774946070326848e-22, + "timestamp": 4.952433066873312 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.2.traj b/src/main/deploy/choreo/Test2.2.traj index a0cb7c71..2c12c192 100644 --- a/src/main/deploy/choreo/Test2.2.traj +++ b/src/main/deploy/choreo/Test2.2.traj @@ -4,298 +4,433 @@ "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -3.79407884851814e-18, - "velocityX": 2.546987187300363e-16, - "velocityY": -1.0256209789219734e-16, + "angularVelocity": 5.239369635456734e-24, + "velocityX": 2.4359888438060422e-22, + "velocityY": -8.774946070326848e-22, "timestamp": 0 }, { - "x": 7.9777911181971035, - "y": 7.466979623755239, + "x": 7.928671024084623, + "y": 7.451618987983874, "heading": 3.153293227446569, - "angularVelocity": 4.0010921973360205e-29, - "velocityX": -0.31261406582002, - "velocityY": -0.09775939742848679, - "timestamp": 0.06930057261307221 + "angularVelocity": 2.2670564289793535e-36, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 0.07922514236752409 }, { - "x": 7.934462450883723, - "y": 7.453430059384792, + "x": 7.805841482889084, + "y": 7.413208234129291, "heading": 3.153293227446569, - "angularVelocity": -2.051733815514594e-34, - "velocityX": -0.6252281284210741, - "velocityY": -0.1955187938503514, - "timestamp": 0.13860114522614442 + "angularVelocity": 3.3814188338755363e-34, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 0.15845028473504819 }, { - "x": 7.869469450281316, - "y": 7.433105712944096, + "x": 7.683011941693537, + "y": 7.374797480274705, "heading": 3.153293227446569, - "angularVelocity": -1.98522229630168e-34, - "velocityX": -0.9378421873262779, - "velocityY": -0.29327818911646475, - "timestamp": 0.20790171783921663 + "angularVelocity": 6.0567834914455396e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.23767542710257228 }, { - "x": 7.782812116686986, - "y": 7.40600658452606, + "x": 7.56018240049799, + "y": 7.336386726420119, "heading": 3.153293227446569, - "angularVelocity": -1.9190015159804115e-34, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190667, - "timestamp": 0.27720229045228884 + "angularVelocity": -6.892446902508999e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.31690056947009637 }, { - "x": 7.6744904504495075, - "y": 7.372132674239751, + "x": 7.437352859302442, + "y": 7.297975972565533, "heading": 3.153293227446569, - "angularVelocity": -8.200637720207044e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.4887969753935167, - "timestamp": 0.34650286306536104 + "angularVelocity": -4.625140327997389e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.39612571183762046 }, { - "x": 7.544504451984089, - "y": 7.331483982215012, + "x": 7.314523318106895, + "y": 7.2595652187109465, "heading": 3.153293227446569, - "angularVelocity": 6.610929004011549e-29, - "velocityX": -1.8756843351233508, - "velocityY": -0.5865563658715173, - "timestamp": 0.41580343567843325 + "angularVelocity": 2.407409636834091e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.47535085420514456 }, { - "x": 7.392854121793352, - "y": 7.28406050860902, + "x": 7.191693776911348, + "y": 7.22115446485636, "heading": 3.153293227446569, - "angularVelocity": -6.483737543755527e-29, - "velocityX": -2.188298371464413, - "velocityY": -0.6843157540814643, - "timestamp": 0.48510400829150546 + "angularVelocity": -8.071167864078362e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.5545759965726687 }, { - "x": 7.219539460498179, - "y": 7.229862253615935, + "x": 7.0688642357158, + "y": 7.182743711001775, "heading": 3.153293227446569, - "angularVelocity": 8.073376116568391e-29, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 0.5544045809045777 + "angularVelocity": 6.706721989227974e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.6338011389401927 }, { - "x": 7.024560468885024, - "y": 7.168889217481695, + "x": 6.946034694520253, + "y": 7.1443329571471885, "heading": 3.153293227446569, - "angularVelocity": -3.526812750348743e-30, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 0.6237051535176499 + "angularVelocity": -3.932169261853096e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.7130262813077168 }, { - "x": 6.807917147982327, - "y": 7.101141400527908, + "x": 6.823205153324706, + "y": 7.105922203292602, "heading": 3.153293227446569, - "angularVelocity": -9.438045628849687e-29, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 0.6930057261307221 + "angularVelocity": -5.711604766985455e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.7922514236752409 }, { - "x": 6.569609499192503, - "y": 7.026618803193132, + "x": 6.700375612129158, + "y": 7.067511449438016, "heading": 3.153293227446569, - "angularVelocity": 9.790681244582267e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 0.7623062987437943 + "angularVelocity": 9.147104299173202e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.871476566042765 }, { - "x": 6.309637524541267, - "y": 6.94532142611084, + "x": 6.577546070933611, + "y": 7.0291006955834305, "heading": 3.153293227446569, - "angularVelocity": -1.3895984503554753e-34, - "velocityX": -3.7513683487544087, - "velocityY": -1.1731126312072742, - "timestamp": 0.8316068713568665 + "angularVelocity": 4.987399043745874e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 0.9507017084102891 }, { - "x": 6.028001227211883, - "y": 6.85724927027649, + "x": 6.4547165297380635, + "y": 6.990689941728844, "heading": 3.153293227446569, - "angularVelocity": -1.323503512813964e-34, - "velocityX": -4.063982254545725, - "velocityY": -1.2708719785922367, - "timestamp": 0.9009074439699387 + "angularVelocity": 1.0726945561042014e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.0299268507778132 }, { - "x": 5.724700612934229, - "y": 6.762402337481905, + "x": 6.331886988542516, + "y": 6.952279187874258, "heading": 3.153293227446569, - "angularVelocity": -2.1989550257903952e-29, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 0.9702080165830109 + "angularVelocity": 1.8749531736864053e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.1091519931453373 }, { - "x": 5.399735695078014, - "y": 6.660780631908008, + "x": 6.209057447346969, + "y": 6.913868434019672, "heading": 3.153293227446569, - "angularVelocity": 1.8024841235265752e-29, - "velocityX": -4.689209707841794, - "velocityY": -1.4663905613202457, - "timestamp": 1.0395085891960831 + "angularVelocity": -8.352063684746622e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.1883771355128614 }, { - "x": 5.053106540487171, - "y": 6.552384174457959, + "x": 6.0862279061514215, + "y": 6.875457680165086, "heading": 3.153293227446569, - "angularVelocity": 1.896211807861914e-29, - "velocityX": -5.001822373477154, - "velocityY": -1.564149520888688, - "timestamp": 1.1088091618091553 + "angularVelocity": 6.429027209138877e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.2676022778803855 }, { - "x": 4.7281416226309565, - "y": 6.4507624688840615, + "x": 5.963398364955874, + "y": 6.8370469263105, "heading": 3.153293227446569, - "angularVelocity": -1.4997927313419928e-29, - "velocityX": -4.689209707841795, - "velocityY": -1.4663905613202457, - "timestamp": 1.1781097344222276 + "angularVelocity": -7.349253697576567e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.3468274202479096 }, { - "x": 4.424841008353304, - "y": 6.355915536089477, + "x": 5.840568823760327, + "y": 6.798636172455914, "heading": 3.153293227446569, - "angularVelocity": -4.4185947102012545e-35, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 1.2474103070352998 + "angularVelocity": -5.316547406258321e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.4260525626154337 }, { - "x": 4.14320471102392, - "y": 6.267843380255127, + "x": 5.7177392825647795, + "y": 6.760225418601328, "heading": 3.153293227446569, - "angularVelocity": -9.264733448000688e-35, - "velocityX": -4.063982254545726, - "velocityY": -1.2708719785922367, - "timestamp": 1.316710879648372 + "angularVelocity": 3.261106516242251e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.5052777049829578 }, { - "x": 3.883232736372683, - "y": 6.186546003172835, + "x": 5.594909741369232, + "y": 6.721814664746741, "heading": 3.153293227446569, - "angularVelocity": 3.5265482976182584e-30, - "velocityX": -3.751368348754409, - "velocityY": -1.1731126312072742, - "timestamp": 1.3860114522614442 + "angularVelocity": -2.0479976334874182e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.5845028473504819 }, { - "x": 3.644925087582859, - "y": 6.112023405838059, + "x": 5.472080200173685, + "y": 6.683403910892156, "heading": 3.153293227446569, - "angularVelocity": 2.156768838744686e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 1.4553120248745168 + "angularVelocity": -7.212902306224552e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.663727989718006 }, { - "x": 3.4282817666801613, - "y": 6.044275588884272, + "x": 5.3492506589781375, + "y": 6.64499315703757, "heading": 3.153293227446569, - "angularVelocity": -2.105657187574439e-28, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 1.5246125974875895 + "angularVelocity": 2.340395125909778e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.74295313208553 }, { - "x": 3.2333027750670063, - "y": 5.983302552750032, + "x": 5.22642111778259, + "y": 6.606582403182983, "heading": 3.153293227446569, - "angularVelocity": 1.8547117767579628e-28, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 1.5939131701006621 + "angularVelocity": -2.132648663400479e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.8221782744530541 }, { - "x": 3.059988113771833, - "y": 5.929104297756947, + "x": 5.103591576587043, + "y": 6.568171649328397, "heading": 3.153293227446569, - "angularVelocity": -5.955568710068408e-35, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 1.6632137427137348 + "angularVelocity": -2.6423732915608732e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 1.9014034168205782 }, { - "x": 2.908337783581096, - "y": 5.881680824150955, + "x": 4.980762035391495, + "y": 6.529760895473811, "heading": 3.153293227446569, - "angularVelocity": -2.0810713408746708e-29, - "velocityX": -2.1882983714644135, - "velocityY": -0.6843157540814642, - "timestamp": 1.7325143153268074 + "angularVelocity": -1.5836433593674017e-28, + "velocityX": -1.5503858689927383, + "velocityY": -0.4848303544397451, + "timestamp": 1.9806285591881023 }, { - "x": 2.7783517851156776, - "y": 5.841032132126216, + "x": 4.857932494195948, + "y": 6.491350141619225, "heading": 3.153293227446569, - "angularVelocity": 2.081061415014159e-29, - "velocityX": -1.875684335123351, - "velocityY": -0.5865563658715173, - "timestamp": 1.80181488793988 + "angularVelocity": 3.462450644614712e-28, + "velocityX": -1.5503858689927386, + "velocityY": -0.4848303544397447, + "timestamp": 2.0598537015556264 }, { - "x": 2.6700301188781994, - "y": 5.807158221839908, + "x": 4.735102953000401, + "y": 6.452939387764639, "heading": 3.153293227446569, - "angularVelocity": 7.326893682940581e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.48879697539351663, - "timestamp": 1.8711154605529527 + "angularVelocity": 9.425461275139336e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.1390788439231505 }, { - "x": 2.583372785283869, - "y": 5.780059093421871, + "x": 4.612273411804853, + "y": 6.414528633910053, "heading": 3.153293227446569, - "angularVelocity": -7.326900961882318e-29, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190656, - "timestamp": 1.9404160331660254 + "angularVelocity": 2.8373243283513945e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.2183039862906746 }, { - "x": 2.5183797846814615, - "y": 5.759734746981175, + "x": 4.489443870609306, + "y": 6.3761178800554665, "heading": 3.153293227446569, - "angularVelocity": -2.64698564160025e-35, - "velocityX": -0.9378421873262781, - "velocityY": -0.2932781891164647, - "timestamp": 2.009716605779098 + "angularVelocity": 4.424546575494848e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.2975291286581987 }, { - "x": 2.4750511173680816, - "y": 5.746185182610728, + "x": 4.366614329413759, + "y": 6.337707126200881, "heading": 3.153293227446569, - "angularVelocity": -4.5099737006589516e-29, - "velocityX": -0.6252281284210743, - "velocityY": -0.1955187938503513, - "timestamp": 2.0790171783921707 + "angularVelocity": 2.6048393166300173e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.376754271025723 + }, + { + "x": 4.243784788218211, + "y": 6.299296372346295, + "heading": 3.153293227446569, + "angularVelocity": -8.141761105066643e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.455979413393247 + }, + { + "x": 4.120955247022664, + "y": 6.2608856184917085, + "heading": 3.153293227446569, + "angularVelocity": -1.5769317536066289e-37, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.535204555760771 + }, + { + "x": 3.998125705827117, + "y": 6.222474864637123, + "heading": 3.153293227446569, + "angularVelocity": -1.9751877406999063e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.614429698128295 + }, + { + "x": 3.8752961646315702, + "y": 6.184064110782536, + "heading": 3.153293227446569, + "angularVelocity": 6.296656188647329e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.693654840495819 + }, + { + "x": 3.7524666234360233, + "y": 6.145653356927951, + "heading": 3.153293227446569, + "angularVelocity": 3.349857109653225e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.7728799828633433 + }, + { + "x": 3.6296370822404764, + "y": 6.107242603073365, + "heading": 3.153293227446569, + "angularVelocity": 1.1073851910178502e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.8521051252308673 + }, + { + "x": 3.5068075410449295, + "y": 6.068831849218778, + "heading": 3.153293227446569, + "angularVelocity": -1.0776722091283702e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 2.9313302675983914 + }, + { + "x": 3.3839779998493826, + "y": 6.030421095364192, + "heading": 3.153293227446569, + "angularVelocity": -2.2657744543933122e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.0105554099659155 + }, + { + "x": 3.2611484586538357, + "y": 5.992010341509607, + "heading": 3.153293227446569, + "angularVelocity": 6.322058172960319e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.0897805523334396 + }, + { + "x": 3.138318917458289, + "y": 5.95359958765502, + "heading": 3.153293227446569, + "angularVelocity": -1.267303885279781e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.169005694700963 + }, + { + "x": 3.015489376262742, + "y": 5.915188833800434, + "heading": 3.153293227446569, + "angularVelocity": 4.539047579816208e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.248230837068486 + }, + { + "x": 2.892659835067195, + "y": 5.876778079945848, + "heading": 3.153293227446569, + "angularVelocity": -2.4352191603126546e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.3274559794360092 + }, + { + "x": 2.769830293871648, + "y": 5.838367326091261, + "heading": 3.153293227446569, + "angularVelocity": 1.9777124768596482e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.4066811218035324 + }, + { + "x": 2.6470007526761012, + "y": 5.799956572236676, + "heading": 3.153293227446569, + "angularVelocity": 1.6826010379363495e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 3.4859062641710556 + }, + { + "x": 2.5241712114805623, + "y": 5.761545818382093, + "heading": 3.153293227446569, + "angularVelocity": -1.9705474475500694e-33, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 3.565131406538579 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": 1.9207053764808128e-28, - "velocityX": -0.31261406582002027, - "velocityY": -0.09775939742848665, - "timestamp": 2.1483177510052434 + "angularVelocity": 2.748235734113482e-34, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 3.644356548906102 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": -4.899028012023128e-29, - "velocityX": 1.275108562409782e-22, - "velocityY": 3.987692290669026e-23, - "timestamp": 2.217618323618316 + "angularVelocity": -7.888608661057978e-31, + "velocityX": -1.038819230645035e-25, + "velocityY": -4.640517458430809e-27, + "timestamp": 3.7235816912736253 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.traj b/src/main/deploy/choreo/Test2.traj index 68c886e7..027e49e3 100644 --- a/src/main/deploy/choreo/Test2.traj +++ b/src/main/deploy/choreo/Test2.traj @@ -1,976 +1,1219 @@ { "samples": [ { - "x": 0.3137660622596754, + "x": 0.3137660622596741, "y": 6.865104675292969, - "heading": 3.135568639860517, - "angularVelocity": -8.5693301650697e-28, - "velocityX": -3.4362695197914885e-17, - "velocityY": -4.743570524232907e-17, + "heading": 3.135568639860519, + "angularVelocity": 1.3323616061473399e-26, + "velocityX": -9.030177506128314e-25, + "velocityY": 7.578270673418084e-24, "timestamp": 0 }, { - "x": 0.3176438425782737, - "y": 6.865425616765028, - "heading": 3.1355686398605154, - "angularVelocity": -5.1868461530169816e-17, - "velocityX": 0.13515003367410133, - "velocityY": 0.011185587421808195, - "timestamp": 0.028692411042588198 - }, - { - "x": 0.3253999903453479, - "y": 6.866060324505518, - "heading": 3.1355686398605136, - "angularVelocity": -5.186846153227154e-17, - "velocityX": 0.2703205302461453, - "velocityY": 0.022121101623313478, - "timestamp": 0.057384822085176396 - }, - { - "x": 0.3370351106624937, - "y": 6.867001226604551, - "heading": 3.135568639860512, - "angularVelocity": -5.186846153153239e-17, - "velocityX": 0.40551211607398097, - "velocityY": 0.032792716430754286, - "timestamp": 0.0860772331277646 - }, - { - "x": 0.352549826865152, - "y": 6.86824032070545, - "heading": 3.13556863986051, - "angularVelocity": -5.186846153220684e-17, - "velocityX": 0.5407254266512643, - "velocityY": 0.04318542973123231, - "timestamp": 0.11476964417035279 - }, - { - "x": 0.3719447806630506, - "y": 6.869769136338039, - "heading": 3.1355686398605083, - "angularVelocity": -5.1868461532349994e-17, - "velocityX": 0.6759611023663795, - "velocityY": 0.053282926635904025, - "timestamp": 0.14346205521294098 - }, - { - "x": 0.3952206321171861, - "y": 6.871578692739439, - "heading": 3.135568639860506, - "angularVelocity": -5.1868461531715824e-17, - "velocityX": 0.8112197828054859, - "velocityY": 0.06306742220841621, - "timestamp": 0.17215446625552916 - }, - { - "x": 0.42237805939934603, - "y": 6.8736594514681, - "heading": 3.1355686398605043, - "angularVelocity": -5.1868461532243226e-17, - "velocityX": 0.9465020991734979, - "velocityY": 0.07251947999676765, - "timestamp": 0.20084687729811734 - }, - { - "x": 0.45341775826418784, - "y": 6.876001262985059, - "heading": 3.1355686398605025, - "angularVelocity": -5.1868461531961763e-17, - "velocityX": 1.081808664276005, - "velocityY": 0.08161780177636822, - "timestamp": 0.22953928834070553 - }, - { - "x": 0.4883404411428946, - "y": 6.878593306214626, - "heading": 3.1355686398605007, - "angularVelocity": -5.186846153181617e-17, - "velocityX": 1.2171400593303083, - "velocityY": 0.09033898286621334, - "timestamp": 0.2582316993832937 - }, - { - "x": 0.5271468357396778, - "y": 6.881424019896891, - "heading": 3.135568639860499, - "angularVelocity": -5.186846153207198e-17, - "velocityX": 1.3524968166384297, - "velocityY": 0.09865722605409555, - "timestamp": 0.2869241104258819 - }, - { - "x": 0.5698376829754908, - "y": 6.884481024296066, - "heading": 3.135568639860497, - "angularVelocity": -5.18684615321628e-17, - "velocityX": 1.4878793968358233, - "velocityY": 0.10654400547371551, - "timestamp": 0.3156165214684701 - }, - { - "x": 0.6164137340738928, - "y": 6.887751031517755, - "heading": 3.1355686398604954, - "angularVelocity": -5.186846153204876e-17, - "velocityX": 1.6232881589932495, - "velocityY": 0.11396766959864212, - "timestamp": 0.34430893251105826 - }, - { - "x": 0.666875746517318, - "y": 6.891219742296381, - "heading": 3.1355686398604936, - "angularVelocity": -5.186846153160267e-17, - "velocityX": 1.758723321247663, - "velocityY": 0.12089296969422078, - "timestamp": 0.37300134355364645 - }, - { - "x": 0.7212244785112947, - "y": 6.894871726616115, - "heading": 3.135568639860492, - "angularVelocity": -5.186846153215831e-17, - "velocityX": 1.894184908800638, - "velocityY": 0.1272804963763128, - "timestamp": 0.40169375459623463 - }, - { - "x": 0.7794606814696723, - "y": 6.898690284891088, - "heading": 3.13556863986049, - "angularVelocity": -5.186846153213201e-17, - "velocityX": 2.02967268494572, - "velocityY": 0.1330860020548787, - "timestamp": 0.4303861656388228 - }, - { - "x": 0.8415850898614354, - "y": 6.9026572856068995, - "heading": 3.1355686398604883, - "angularVelocity": -5.186846153171342e-17, - "velocityX": 2.165186059113343, - "velocityY": 0.13825958055332968, - "timestamp": 0.459078576681411 - }, - { - "x": 0.9075984075182041, - "y": 6.906752974251311, - "heading": 3.1355686398604865, - "angularVelocity": -5.1868461532457525e-17, - "velocityX": 2.3007239635171715, - "velocityY": 0.14274466646709222, - "timestamp": 0.4877709877239992 - }, - { - "x": 0.9775012891595924, - "y": 6.910955746947304, - "heading": 3.1355686398604847, - "angularVelocity": -5.186846153166985e-17, - "velocityX": 2.43628468648487, - "velocityY": 0.14647680495568935, - "timestamp": 0.5164633987665874 - }, - { - "x": 1.051294315403438, - "y": 6.915241880318482, - "heading": 3.1355686398604825, - "angularVelocity": -5.1868461532007456e-17, - "velocityX": 2.571865645390099, - "velocityY": 0.1493821263335398, - "timestamp": 0.5451558098091757 - }, - { - "x": 1.1289779588157673, - "y": 6.919585206580955, - "heading": 3.1355686398604807, - "angularVelocity": -5.186846153206435e-17, - "velocityX": 2.7074630743656276, - "velocityY": 0.15137543708073178, - "timestamp": 0.5738482208517639 - }, - { - "x": 1.210552537505391, - "y": 6.923956719399065, - "heading": 3.135568639860479, - "angularVelocity": -5.1868461531968826e-17, - "velocityX": 2.843071590203452, - "velocityY": 0.1523578067950584, - "timestamp": 0.6025406318943521 - }, - { - "x": 1.29601815119489, - "y": 6.9283240912670765, - "heading": 3.135568639860477, - "angularVelocity": -5.186846153216223e-17, - "velocityX": 2.978683581614699, - "velocityY": 0.15221348465724693, - "timestamp": 0.6312330429369404 - }, - { - "x": 1.3853745923008893, - "y": 6.932651076487761, - "heading": 3.1355686398604754, - "angularVelocity": -5.1868461531841376e-17, - "velocityX": 3.1142883382426785, - "velocityY": 0.15080591220658612, - "timestamp": 0.6599254539795286 - }, - { - "x": 1.478621220826743, - "y": 6.936896764298285, - "heading": 3.1355686398604736, - "angularVelocity": -5.186846153215644e-17, - "velocityX": 3.2498707894378844, - "velocityY": 0.1479725006107481, - "timestamp": 0.6886178650221169 - }, - { - "x": 1.5757567859462194, - "y": 6.941014632922591, - "heading": 3.135568639860472, - "angularVelocity": -5.186846153173054e-17, - "velocityX": 3.3854096463101833, - "velocityY": 0.14351769247253987, - "timestamp": 0.7173102760647051 - }, - { - "x": 1.676779167520274, - "y": 6.94495133505747, - "heading": 3.13556863986047, - "angularVelocity": -5.186846153388186e-17, - "velocityX": 3.52087461120313, - "velocityY": 0.13720360164351214, - "timestamp": 0.7460026871072933 - }, - { - "x": 1.7816849946975712, - "y": 6.948645114898683, - "heading": 3.1355686398604683, - "angularVelocity": -1.6596882408000968e-16, - "velocityX": 3.6562220937649346, - "velocityY": 0.12873717150261788, - "timestamp": 0.7746950981498816 - }, - { - "x": 1.8121461784699802, - "y": 6.949692765946351, - "heading": 3.1362037838100756, - "angularVelocity": 0.07689012502825784, - "velocityX": 3.687611651202223, - "velocityY": 0.12682797354934053, - "timestamp": 0.7829555080448543 - }, - { - "x": 1.8428693964358074, - "y": 6.950724930078729, - "heading": 3.1374589452138397, - "angularVelocity": 0.15194904607940982, - "velocityX": 3.7193333450103565, - "velocityY": 0.12495313737441759, - "timestamp": 0.7912159179398269 - }, - { - "x": 1.8738574968140793, - "y": 6.95174197296817, - "heading": 3.139318096432083, - "angularVelocity": 0.22506767120320992, - "velocityX": 3.751399842413374, - "velocityY": 0.1231225692636009, - "timestamp": 0.7994763278347996 - }, - { - "x": 1.9051134303883683, - "y": 6.952744343319344, - "heading": 3.1417642609142864, - "angularVelocity": 0.2961311258529596, - "velocityX": 3.783823559810237, - "velocityY": 0.12134632105632295, - "timestamp": 0.8077367377297723 - }, - { - "x": 1.9366402470506743, - "y": 6.953732574884012, - "heading": 3.1447794649491048, - "angularVelocity": 0.36501869436959006, - "velocityX": 3.8166164952046153, - "velocityY": 0.11963468849991928, - "timestamp": 0.815997147624745 - }, - { - "x": 1.96844109067084, - "y": 6.9547072895390265, - "heading": 3.148344689757031, - "angularVelocity": 0.43160386146187707, - "velocityX": 3.849790025494764, - "velocityY": 0.117998339961998, - "timestamp": 0.8242575575197176 - }, - { - "x": 2.0005191919654135, - "y": 6.955669201722795, - "heading": 3.152439824908413, - "angularVelocity": 0.495754472653526, - "velocityX": 3.883354664287875, - "velocityY": 0.11644848088619933, - "timestamp": 0.8325179674146903 - }, - { - "x": 2.032877859008433, - "y": 6.9566191245667985, - "heading": 3.1570436241813726, - "angularVelocity": 0.5573330296555058, - "velocityX": 3.91731977643278, - "velocityY": 0.11499705899340439, - "timestamp": 0.840778377309663 - }, - { - "x": 2.065520465002766, - "y": 6.957557978090485, - "heading": 3.1621336650538163, - "angularVelocity": 0.6161971303075195, - "velocityX": 3.951693246384564, - "velocityY": 0.11365701407324179, - "timestamp": 0.8490387872046357 - }, - { - "x": 2.0984504329177724, - "y": 6.958486799841183, - "heading": 3.1676863130152344, - "angularVelocity": 0.6722000520579139, - "velocityX": 3.9864810988430395, - "velocityY": 0.11244257397721802, - "timestamp": 0.8572991970996083 - }, - { - "x": 2.1316712166001497, - "y": 6.959406758344203, - "heading": 3.1736766917418935, - "angularVelocity": 0.7251914617828771, - "velocityX": 4.021687071799445, - "velocityY": 0.1113695948160438, - "timestamp": 0.865559606994581 - }, - { - "x": 2.165186277981829, - "y": 6.960319169668765, - "heading": 3.1800786598480077, - "angularVelocity": 0.7750182118701426, - "velocityX": 4.057312144046869, - "velocityY": 0.11045593816320934, - "timestamp": 0.8738200168895537 - }, - { - "x": 2.1989990600414004, - "y": 6.961225517299853, - "heading": 3.1868647943382302, - "angularVelocity": 0.8215251514763765, - "velocityX": 4.093354021105941, - "velocityY": 0.10972187126423899, - "timestamp": 0.8820804267845264 - }, - { - "x": 2.2331129552203137, - "y": 6.962127475323903, - "heading": 3.19400637995892, - "angularVelocity": 0.8645558406293681, - "velocityX": 4.1298065849823535, - "velocityY": 0.10919046821108658, - "timestamp": 0.890340836679499 - }, - { - "x": 2.2675312690432237, - "y": 6.963026934685406, - "heading": 3.201473402262516, - "angularVelocity": 0.9039529997346885, - "velocityX": 4.1666593135838506, - "velocityY": 0.10888798170273117, - "timestamp": 0.8986012465744717 - }, - { - "x": 2.302257178726397, - "y": 6.963926031963223, - "heading": 3.2092345401896574, - "angularVelocity": 0.9395584511933502, - "velocityX": 4.203896673978105, - "velocityY": 0.10884414808067534, - "timestamp": 0.9068616564694444 - }, - { - "x": 2.3372936865473584, - "y": 6.964827179782931, - "heading": 3.2172571510612844, - "angularVelocity": 0.9712122005605541, - "velocityX": 4.241497488191654, - "velocityY": 0.10909238538572823, - "timestamp": 0.9151220663644171 - }, - { - "x": 2.372643567633051, - "y": 6.965733097685614, - "heading": 3.225507236598066, - "angularVelocity": 0.9987501397242283, - "velocityX": 4.279434257518503, - "velocityY": 0.10966984861524744, - "timestamp": 0.9233824762593897 - }, - { - "x": 2.4083093114869696, - "y": 6.9666468421059164, - "heading": 3.233949372180996, - "angularVelocity": 1.0219995969049733, - "velocityX": 4.31767240456481, - "velocityY": 0.11061732189065114, - "timestamp": 0.9316428861543624 - }, - { - "x": 2.44429305579275, - "y": 6.967571834188082, - "heading": 3.2425465717794997, - "angularVelocity": 1.0407715486052915, - "velocityX": 4.356169338240552, - "velocityY": 0.11197895672538041, - "timestamp": 0.9399032960493351 - }, - { - "x": 2.480596509367599, - "y": 6.968511884597095, - "heading": 3.251260045781759, - "angularVelocity": 1.054847654420144, - "velocityX": 4.394873140247142, - "velocityY": 0.11380190825438415, - "timestamp": 0.9481637059443078 - }, - { - "x": 2.517220857768171, - "y": 6.969471215284249, - "heading": 3.260048785175124, - "angularVelocity": 1.0639592350877263, - "velocityX": 4.433720464992984, - "velocityY": 0.11613596653794503, - "timestamp": 0.9564241158392804 - }, - { - "x": 2.55416663843719, - "y": 6.970454479070239, - "heading": 3.2688688682505744, - "angularVelocity": 1.0677536814268536, - "velocityX": 4.4726328522150816, - "velocityY": 0.11903329235341958, - "timestamp": 0.9646845257342531 - }, - { - "x": 2.5914335596417093, - "y": 6.971466777824258, - "heading": 3.2776723283732965, - "angularVelocity": 1.065741317277861, - "velocityX": 4.511509922431134, - "velocityY": 0.12254824722845813, - "timestamp": 0.9729449356292258 - }, - { - "x": 2.6290202140808088, - "y": 6.9725136756896955, - "heading": 3.286405336491996, - "angularVelocity": 1.0572124422077387, - "velocityX": 4.550216625687468, - "velocityY": 0.12673679378462502, - "timestamp": 0.9812053455241985 - }, - { - "x": 2.9717276780335036, - "y": 6.984363087388876, - "heading": 3.35108767309623, - "angularVelocity": 0.9203746590757935, - "velocityX": 4.876435853393211, - "velocityY": 0.16860705449782717, - "timestamp": 1.0514836131475132 - }, - { - "x": 3.3366815448761877, - "y": 6.999743371642888, - "heading": 3.3973428408952557, - "angularVelocity": 0.6581717131524953, - "velocityX": 5.192983253355129, - "velocityY": 0.21884836912099448, - "timestamp": 1.1217618807708278 - }, - { - "x": 3.71402485435163, - "y": 7.027526643127482, - "heading": 3.3973428368677423, - "angularVelocity": -5.730809398962461e-8, - "velocityX": 5.369274488921224, - "velocityY": 0.3953323327989187, - "timestamp": 1.1920401483941425 - }, - { - "x": 4.090360227255461, - "y": 7.066661937541599, - "heading": 3.3973428243317834, - "angularVelocity": -1.783760359331945e-7, - "velocityX": 5.354932408421835, - "velocityY": 0.5568619679682045, - "timestamp": 1.2623184160174572 - }, - { - "x": 4.466694898854889, - "y": 7.105803975313799, - "heading": 3.3973428117958613, - "angularVelocity": -1.7837551225317533e-7, - "velocityX": 5.354922429456401, - "velocityY": 0.5569579202208789, - "timestamp": 1.3325966836407719 - }, - { - "x": 4.8430291421579215, - "y": 7.144950130722809, - "heading": 3.397342799228156, - "angularVelocity": -1.7882775854425254e-7, - "velocityX": 5.354916335162799, - "velocityY": 0.5570165106920909, - "timestamp": 1.4028749512640866 - }, - { - "x": 5.214361577079249, - "y": 7.183676617694927, - "heading": 3.378511132398759, - "angularVelocity": -0.26795860891639617, - "velocityX": 5.283744854264688, - "velocityY": 0.55104498562348, - "timestamp": 1.4731532188874013 - }, - { - "x": 5.562524060208366, - "y": 7.219962031584869, - "heading": 3.355414523374288, - "angularVelocity": -0.32864511043819594, - "velocityX": 4.9540561385951625, - "velocityY": 0.5163105909843465, - "timestamp": 1.543431486510716 - }, - { - "x": 5.887470499962918, - "y": 7.253817931255832, - "heading": 3.331666055733143, - "angularVelocity": -0.3379205043646441, - "velocityX": 4.6237115788942855, - "velocityY": 0.48174066914158636, - "timestamp": 1.6137097541340306 - }, - { - "x": 6.189199807150463, - "y": 7.285249171486667, - "heading": 3.308363686841363, - "angularVelocity": -0.33157289841972754, - "velocityX": 4.29335152091094, - "velocityY": 0.4472398266744939, - "timestamp": 1.6839880217573453 - }, - { - "x": 6.467713626267079, - "y": 7.31425833816948, - "heading": 3.2860397702782755, - "angularVelocity": -0.3176503536305448, - "velocityX": 3.963014862708697, - "velocityY": 0.41277577925367004, - "timestamp": 1.75426628938066 - }, - { - "x": 6.723013594809074, - "y": 7.34084703024564, - "heading": 3.265008761402615, - "angularVelocity": -0.29925337642620564, - "velocityX": 3.6327015046867763, - "velocityY": 0.37833448340917136, - "timestamp": 1.8245445570039747 - }, - { - "x": 6.955101065031071, - "y": 7.365016333172441, - "heading": 3.245478308115181, - "angularVelocity": -0.27790174612893576, - "velocityX": 3.302407388098493, - "velocityY": 0.34390863269912847, - "timestamp": 1.8948228246272893 - }, - { - "x": 7.163977127241494, - "y": 7.386767031992689, - "heading": 3.2275957745381954, - "angularVelocity": -0.2544532496565816, - "velocityX": 2.972128785672115, - "velocityY": 0.3094939524808096, - "timestamp": 1.965101092250604 - }, - { - "x": 7.349642665971283, - "y": 7.4060997209757735, - "heading": 3.2114711639593976, - "angularVelocity": -0.22943949992088322, - "velocityX": 2.6418627693690517, - "velocityY": 0.2750877282106925, - "timestamp": 2.035379359873919 - }, - { - "x": 7.512098408217057, - "y": 7.423014865697383, - "heading": 3.1971897253730983, - "angularVelocity": -0.2032127294720464, - "velocityX": 2.3116070976097918, - "velocityY": 0.24068812868678965, - "timestamp": 2.1056576274972336 - }, - { - "x": 7.651344959888608, - "y": 7.43751284079522, - "heading": 3.184819461924971, - "angularVelocity": -0.17601833207433357, - "velocityX": 1.981360047431725, - "velocityY": 0.20629385993890986, - "timestamp": 2.1759358951205483 - }, - { - "x": 7.767382832665261, - "y": 7.449593954241994, - "heading": 3.174415882660127, - "angularVelocity": -0.14803408815663477, - "velocityX": 1.6511202780154728, - "velocityY": 0.1719039733808395, - "timestamp": 2.246214162743863 - }, - { - "x": 7.860212463836391, - "y": 7.459258463658005, - "heading": 3.1660251568616267, - "angularVelocity": -0.11939289459260129, - "velocityX": 1.3208867308552614, - "velocityY": 0.13751775254062693, - "timestamp": 2.3164924303671777 - }, - { - "x": 7.929834231127341, - "y": 7.466506587678373, - "heading": 3.159686290431417, - "angularVelocity": -0.09019668020541635, - "velocityX": 0.9906585584055106, - "velocityY": 0.10313464269233455, - "timestamp": 2.3867706979904924 - }, - { - "x": 7.976248463934981, - "y": 7.471338514116222, - "heading": 3.1554326760982865, - "angularVelocity": -0.06052531567696676, - "velocityX": 0.6604350729932147, - "velocityY": 0.06875420526505516, - "timestamp": 2.457048965613807 + "x": 0.3285749308509426, + "y": 6.865947459797136, + "heading": 3.135568639860519, + "angularVelocity": 3.3337145202478786e-22, + "velocityX": 0.41797046933441173, + "velocityY": 0.023787032249159984, + "timestamp": 0.03543041836149472 + }, + { + "x": 0.35819266531481686, + "y": 6.867633028650745, + "heading": 3.135568639860519, + "angularVelocity": 3.3337147019213317e-22, + "velocityX": 0.8359408619363751, + "velocityY": 0.04757406013141577, + "timestamp": 0.07086083672298944 + }, + { + "x": 0.4026192574953566, + "y": 6.87016138138964, + "heading": 3.135568639860519, + "angularVelocity": 3.3337143389947597e-22, + "velocityX": 1.2539110243423486, + "velocityY": 0.07136107491303612, + "timestamp": 0.10629125508448417 + }, + { + "x": 0.4600803298886451, + "y": 6.873431536777097, + "heading": 3.135568639860519, + "angularVelocity": 3.331660178480041e-22, + "velocityX": 1.621800561512317, + "velocityY": 0.09229796143220789, + "timestamp": 0.14172167344597889 + }, + { + "x": 0.5175414022820589, + "y": 6.8767016921645565, + "heading": 3.135568639860519, + "angularVelocity": 3.3316629368551667e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233462, + "timestamp": 0.1771520918074736 + }, + { + "x": 0.5750024746754727, + "y": 6.879971847552017, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614681891394e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233254, + "timestamp": 0.2125825101689683 + }, + { + "x": 0.6324635470688867, + "y": 6.883242002939478, + "heading": 3.135568639860519, + "angularVelocity": 3.331683104806914e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.24801292853046303 + }, + { + "x": 0.6899246194623005, + "y": 6.886512158326939, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614878052174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.28344334689195777 + }, + { + "x": 0.7473856918557144, + "y": 6.8897823137144, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614758461554e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.3188737652534525 + }, + { + "x": 0.8048467642491284, + "y": 6.89305246910186, + "heading": 3.135568639860519, + "angularVelocity": 3.331638534198991e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233299, + "timestamp": 0.35430418361494725 + }, + { + "x": 0.8623078366425422, + "y": 6.89632262448932, + "heading": 3.135568639860519, + "angularVelocity": 3.3317076614902997e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.389734601976442 + }, + { + "x": 0.9197689090359561, + "y": 6.899592779876781, + "heading": 3.135568639860519, + "angularVelocity": 3.331661479116866e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233396, + "timestamp": 0.42516502033793674 + }, + { + "x": 0.97722998142937, + "y": 6.902862935264242, + "heading": 3.135568639860519, + "angularVelocity": 3.33166145502926e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.4605954386994315 + }, + { + "x": 1.0346910538227838, + "y": 6.906133090651703, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600659908063e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233301, + "timestamp": 0.4960258570609262 + }, + { + "x": 1.0921521262161977, + "y": 6.9094032460391634, + "heading": 3.135568639860519, + "angularVelocity": 3.331661460674642e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5314562754224209 + }, + { + "x": 1.1496131986096116, + "y": 6.912673401426624, + "heading": 3.135568639860519, + "angularVelocity": 3.331661475383932e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5668866937839157 + }, + { + "x": 1.2070742710030253, + "y": 6.915943556814084, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614753921174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.6023171121454104 + }, + { + "x": 1.2645353433964392, + "y": 6.919213712201545, + "heading": 3.135568639860519, + "angularVelocity": 3.331662731932369e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.6377475305069051 + }, + { + "x": 1.3219964157898532, + "y": 6.922483867589006, + "heading": 3.135568639860519, + "angularVelocity": 3.3316627143346654e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233393, + "timestamp": 0.6731779488683999 + }, + { + "x": 1.379457488183267, + "y": 6.925754022976466, + "heading": 3.135568639860519, + "angularVelocity": 3.331661483573468e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233157, + "timestamp": 0.7086083672298946 + }, + { + "x": 1.436918560576681, + "y": 6.929024178363926, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614890219236e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233392, + "timestamp": 0.7440387855913894 + }, + { + "x": 1.494379632970095, + "y": 6.932294333751387, + "heading": 3.135568639860519, + "angularVelocity": 3.331662743669949e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233207, + "timestamp": 0.7794692039528841 + }, + { + "x": 1.5518407053635086, + "y": 6.935564489138847, + "heading": 3.135568639860519, + "angularVelocity": 3.331638909213181e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.8148996223143788 + }, + { + "x": 1.6093017777569225, + "y": 6.938834644526308, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600948450887e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233622, + "timestamp": 0.8503300406758736 + }, + { + "x": 1.666762850150336, + "y": 6.942104799913777, + "heading": 3.135568639860519, + "angularVelocity": 3.331671581625241e-22, + "velocityX": 1.621800561515844, + "velocityY": 0.09229796143255317, + "timestamp": 0.8857604590373683 + }, + { + "x": 1.7242239225437261, + "y": 6.945374955301653, + "heading": 3.135568639860519, + "angularVelocity": 3.3334679678699274e-22, + "velocityX": 1.6218005615151887, + "velocityY": 0.09229796144406777, + "timestamp": 0.9211908773988631 + }, + { + "x": 1.7816849946975708, + "y": 6.948645114898682, + "heading": 3.135568639860519, + "angularVelocity": 1.7645788213167404e-22, + "velocityX": 1.6218005547541783, + "velocityY": 0.09229808024460076, + "timestamp": 0.9566212957603578 + }, + { + "x": 1.8178857407766151, + "y": 6.94853231479887, + "heading": 3.1384797599068888, + "angularVelocity": 0.12817174361754557, + "velocityX": 1.5938582646199322, + "velocityY": -0.004966399613447973, + "timestamp": 0.9793339464913666 + }, + { + "x": 1.8540603970824046, + "y": 6.948528771707492, + "heading": 3.1414999940123267, + "angularVelocity": 0.13297585302602363, + "velocityX": 1.592709575567121, + "velocityY": -0.00015599638369279018, + "timestamp": 1.0020465972223755 + }, + { + "x": 1.8901908310793394, + "y": 6.948640868874459, + "heading": 3.1447030512216227, + "angularVelocity": 0.14102524831780386, + "velocityX": 1.5907625413183106, + "velocityY": 0.0049354506567203535, + "timestamp": 1.0247592479533845 + }, + { + "x": 1.926257669084107, + "y": 6.9488776015696425, + "heading": 3.148167194864803, + "angularVelocity": 0.15252044704983372, + "velocityX": 1.5879625162168578, + "velocityY": 0.010422944375284186, + "timestamp": 1.0474718986843934 + }, + { + "x": 1.9622398024006038, + "y": 6.94925072993569, + "heading": 3.151976954699645, + "angularVelocity": 0.16773734954856945, + "velocityX": 1.584233110553301, + "velocityY": 0.01642821749276163, + "timestamp": 1.0701845494154023 + }, + { + "x": 1.9981171224268564, + "y": 6.949774504499785, + "heading": 3.156211459419019, + "angularVelocity": 0.1864381559653407, + "velocityX": 1.5796183567984214, + "velocityY": 0.023060917472754097, + "timestamp": 1.0928972001464112 + }, + { + "x": 2.033847488660219, + "y": 6.950468453992633, + "heading": 3.161038496208879, + "angularVelocity": 0.2125263513725333, + "velocityX": 1.5731482272380108, + "velocityY": 0.03055343478252191, + "timestamp": 1.1156098508774202 + }, + { + "x": 2.0694053227389695, + "y": 6.951354952814641, + "heading": 3.166554769842392, + "angularVelocity": 0.24287229609803831, + "velocityX": 1.565551925218666, + "velocityY": 0.039031059496573456, + "timestamp": 1.138322501608429 + }, + { + "x": 2.104784911670157, + "y": 6.952454847185295, + "heading": 3.1727732170956275, + "angularVelocity": 0.27378782542301205, + "velocityX": 1.5577040896809682, + "velocityY": 0.04842650836667849, + "timestamp": 1.161035152339438 + }, + { + "x": 2.1400047240253706, + "y": 6.9537191803371154, + "heading": 3.1796054213345153, + "angularVelocity": 0.3008105183231903, + "velocityX": 1.550669394441415, + "velocityY": 0.05566647269821489, + "timestamp": 1.183747803070447 + }, + { + "x": 2.175095855891208, + "y": 6.955084615506324, + "heading": 3.18691368664007, + "angularVelocity": 0.3217706903570311, + "velocityX": 1.5450038078527182, + "velocityY": 0.060117825320333544, + "timestamp": 1.2064604538014558 + }, + { + "x": 2.2101380445057046, + "y": 6.956471002562802, + "heading": 3.194369840668752, + "angularVelocity": 0.3282819833311086, + "velocityX": 1.5428489184071512, + "velocityY": 0.06104030185188484, + "timestamp": 1.2291731045324648 + }, + { + "x": 2.2451610724837057, + "y": 6.957849512193169, + "heading": 3.2018529792356185, + "angularVelocity": 0.32947006738627316, + "velocityX": 1.5420053076493287, + "velocityY": 0.060693471963871995, + "timestamp": 1.2518857552634737 + }, + { + "x": 2.2801629678458126, + "y": 6.959221595789265, + "heading": 3.2093707720743723, + "angularVelocity": 0.3309958369804056, + "velocityX": 1.5410748739388525, + "velocityY": 0.060410544429463274, + "timestamp": 1.2745984059944826 + }, + { + "x": 2.315144049729391, + "y": 6.960586594052822, + "heading": 3.2169217131634897, + "angularVelocity": 0.3324552989672944, + "velocityX": 1.540158491312511, + "velocityY": 0.06009858909567994, + "timestamp": 1.2973110567254915 + }, + { + "x": 2.3501040543802802, + "y": 6.961944386116353, + "heading": 3.2245066187504112, + "angularVelocity": 0.3339506989629348, + "velocityX": 1.539230496031878, + "velocityY": 0.05978131216881497, + "timestamp": 1.3200237074565004 + }, + { + "x": 2.3850426980471595, + "y": 6.963294864074251, + "heading": 3.232126373384074, + "angularVelocity": 0.3354850441678861, + "velocityX": 1.5382900076554706, + "velocityY": 0.05945928433854099, + "timestamp": 1.3427363581875094 + }, + { + "x": 2.419959710539657, + "y": 6.964637896582436, + "heading": 3.239781798872735, + "angularVelocity": 0.3370555722150803, + "velocityX": 1.5373376232491214, + "velocityY": 0.05913147364836824, + "timestamp": 1.3654490089185183 + }, + { + "x": 2.454854803951486, + "y": 6.965973356645481, + "heading": 3.247473776388813, + "angularVelocity": 0.3386648968090985, + "velocityX": 1.5363725628109135, + "velocityY": 0.05879807156202029, + "timestamp": 1.3881616596495272 + }, + { + "x": 2.48972762858419, + "y": 6.967301155248667, + "heading": 3.2552034126870217, + "angularVelocity": 0.3403229499609042, + "velocityX": 1.5353921057348447, + "velocityY": 0.05846075030657316, + "timestamp": 1.4108743103805361 + }, + { + "x": 2.5245780070203394, + "y": 6.968621054178632, + "heading": 3.262971149366171, + "angularVelocity": 0.34200044596925644, + "velocityX": 1.5344038372663136, + "velocityY": 0.05811294091547401, + "timestamp": 1.433586961111545 + }, + { + "x": 2.559405532428149, + "y": 6.969932985582836, + "heading": 3.2707782920481354, + "angularVelocity": 0.3437354263236719, + "velocityX": 1.5333976566751537, + "velocityY": 0.05776214409057404, + "timestamp": 1.456299611842554 + }, + { + "x": 2.594207801112396, + "y": 6.97123839692077, + "heading": 3.2786335848605472, + "angularVelocity": 0.34585539598366793, + "velocityX": 1.5322856454060756, + "velocityY": 0.05747507648462563, + "timestamp": 1.4790122625735629 + }, + { + "x": 2.6290202140808105, + "y": 6.972513675689697, + "heading": 3.286405336491994, + "angularVelocity": 0.3421772175995684, + "velocityX": 1.5327322812606092, + "velocityY": 0.05614838990089804, + "timestamp": 1.5017249133045718 + }, + { + "x": 2.7840510289336455, + "y": 6.98699854094784, + "heading": 3.286405287600397, + "angularVelocity": -5.100684883763116e-7, + "velocityX": 1.6173808639626774, + "velocityY": 0.1511154018498607, + "timestamp": 1.5975779175703702 + }, + { + "x": 2.93908160001489, + "y": 7.001486015045406, + "heading": 3.2864052387079807, + "angularVelocity": -5.100770354356606e-7, + "velocityX": 1.6173783207811374, + "velocityY": 0.15114261893547631, + "timestamp": 1.6934309218361685 + }, + { + "x": 3.094112171101734, + "y": 7.015973489083046, + "heading": 3.286405189815564, + "angularVelocity": -5.100770390190521e-7, + "velocityX": 1.617378320839561, + "velocityY": 0.15114261831028078, + "timestamp": 1.7892839261019668 + }, + { + "x": 3.2491427421885786, + "y": 7.030460963120681, + "heading": 3.286405140923147, + "angularVelocity": -5.100770430673946e-7, + "velocityX": 1.617378320839565, + "velocityY": 0.15114261831023715, + "timestamp": 1.8851369303677652 + }, + { + "x": 3.404173313275423, + "y": 7.0449484371583155, + "heading": 3.28640509203073, + "angularVelocity": -5.100770460466358e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023496, + "timestamp": 1.9809899346335635 + }, + { + "x": 3.5592038843622675, + "y": 7.05943591119595, + "heading": 3.286405043138312, + "angularVelocity": -5.100770488976204e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023276, + "timestamp": 2.076842938899362 + }, + { + "x": 3.714234455449112, + "y": 7.073923385233584, + "heading": 3.2864049942458946, + "angularVelocity": -5.100770524050277e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831023054, + "timestamp": 2.1726959431651602 + }, + { + "x": 3.8692650265359565, + "y": 7.088410859271219, + "heading": 3.2864049453534765, + "angularVelocity": -5.100770565197187e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831022827, + "timestamp": 2.2685489474309586 + }, + { + "x": 4.0242955976228005, + "y": 7.102898333308853, + "heading": 3.2864048964610575, + "angularVelocity": -5.100770606946053e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831022607, + "timestamp": 2.364401951696757 + }, + { + "x": 4.179326168709644, + "y": 7.117385807346487, + "heading": 3.286404847568639, + "angularVelocity": -5.100770634020749e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.151142618310224, + "timestamp": 2.4602549559625553 + }, + { + "x": 4.334356739796489, + "y": 7.13187328138412, + "heading": 3.2864047986762195, + "angularVelocity": -5.100770666030285e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831022163, + "timestamp": 2.5561079602283536 + }, + { + "x": 4.489387310883332, + "y": 7.146360755421753, + "heading": 3.2864047497838, + "angularVelocity": -5.100770706092546e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831021944, + "timestamp": 2.651960964494152 + }, + { + "x": 4.644417881970177, + "y": 7.160848229459387, + "heading": 3.28640470089138, + "angularVelocity": -5.100770740673254e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021727, + "timestamp": 2.7478139687599503 + }, + { + "x": 4.79944845305702, + "y": 7.175335703497019, + "heading": 3.28640465199896, + "angularVelocity": -5.100770777885376e-7, + "velocityX": 1.6173783208395638, + "velocityY": 0.15114261831021514, + "timestamp": 2.8436669730257487 + }, + { + "x": 4.954479024143865, + "y": 7.1898231775346515, + "heading": 3.286404603106539, + "angularVelocity": -5.100770810299081e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021283, + "timestamp": 2.939519977291547 + }, + { + "x": 5.109509595230708, + "y": 7.2043106515722855, + "heading": 3.2864045542141187, + "angularVelocity": -5.100770846564483e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021056, + "timestamp": 3.0353729815573454 + }, + { + "x": 5.264540166317553, + "y": 7.218798125609918, + "heading": 3.286404505321697, + "angularVelocity": -5.100770889061609e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.1511426183102084, + "timestamp": 3.1312259858231437 + }, + { + "x": 5.419570737404396, + "y": 7.233285599647549, + "heading": 3.2864044564292754, + "angularVelocity": -5.100770922378371e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020614, + "timestamp": 3.227078990088942 + }, + { + "x": 5.574601308491241, + "y": 7.247773073685181, + "heading": 3.2864044075368533, + "angularVelocity": -5.100770952675197e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020392, + "timestamp": 3.3229319943547404 + }, + { + "x": 5.7296318795780845, + "y": 7.262260547722812, + "heading": 3.286404358644431, + "angularVelocity": -5.100770991840634e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.1511426183102018, + "timestamp": 3.4187849986205388 + }, + { + "x": 5.884662450664929, + "y": 7.276748021760444, + "heading": 3.286404309752008, + "angularVelocity": -5.100771023487481e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.15114261831019962, + "timestamp": 3.514638002886337 + }, + { + "x": 6.039693021751773, + "y": 7.291235495798076, + "heading": 3.286404260859585, + "angularVelocity": -5.100771061559551e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.15114261831019746, + "timestamp": 3.6104910071521354 + }, + { + "x": 6.194723592838616, + "y": 7.305722969835706, + "heading": 3.2864042119671617, + "angularVelocity": -5.100771091660107e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.1511426183101951, + "timestamp": 3.706344011417934 + }, + { + "x": 6.349754163925461, + "y": 7.320210443873337, + "heading": 3.2864041630747383, + "angularVelocity": -5.100771129976334e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831019293, + "timestamp": 3.802197015683732 + }, + { + "x": 6.504784735012304, + "y": 7.334697917910969, + "heading": 3.286404114182314, + "angularVelocity": -5.100771163404491e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.1511426183101907, + "timestamp": 3.8980500199495305 + }, + { + "x": 6.659815306099149, + "y": 7.349185391948599, + "heading": 3.28640406528989, + "angularVelocity": -5.100771202032421e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.1511426183101885, + "timestamp": 3.993903024215329 + }, + { + "x": 6.814845877185992, + "y": 7.363672865986229, + "heading": 3.2864040163974653, + "angularVelocity": -5.100771233944824e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831018627, + "timestamp": 4.089756028481127 + }, + { + "x": 6.969876448272837, + "y": 7.378160340023859, + "heading": 3.2864039675050396, + "angularVelocity": -5.100771270486632e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.15114261831018408, + "timestamp": 4.1856090327469255 + }, + { + "x": 7.12490701935968, + "y": 7.392647814061489, + "heading": 3.286403918612615, + "angularVelocity": -5.100771304657537e-7, + "velocityX": 1.617378320839562, + "velocityY": 0.15114261831018191, + "timestamp": 4.281462037012724 + }, + { + "x": 7.279937590446525, + "y": 7.407135288099118, + "heading": 3.286403869720189, + "angularVelocity": -5.10077134427039e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.1511426183101796, + "timestamp": 4.377315041278522 + }, + { + "x": 7.4349681615333685, + "y": 7.421622762136748, + "heading": 3.286403820827763, + "angularVelocity": -5.100771387503622e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.15114261831017736, + "timestamp": 4.473168045544321 + }, + { + "x": 7.589998732620213, + "y": 7.436110236174376, + "heading": 3.2864037719353365, + "angularVelocity": -5.100771410554518e-7, + "velocityX": 1.6173783208395616, + "velocityY": 0.15114261831017525, + "timestamp": 4.569021049810119 + }, + { + "x": 7.745029303707056, + "y": 7.450597710212007, + "heading": 3.28640372304291, + "angularVelocity": -5.100771448287922e-7, + "velocityX": 1.6173783208395598, + "velocityY": 0.1511426183101905, + "timestamp": 4.664874054075917 + }, + { + "x": 7.90005987484092, + "y": 7.465085183746354, + "heading": 3.2864036741504377, + "angularVelocity": -5.1007762076643e-7, + "velocityX": 1.6173783213301043, + "velocityY": 0.15114261305961757, + "timestamp": 4.760727058341716 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -0.03044253542482025, - "velocityX": 0.33021570985126986, - "velocityY": 0.03437608724320182, - "timestamp": 2.5273272332371217 + "angularVelocity": -1.3886935284235506, + "velocityX": 1.0369583915052876, + "velocityY": 0.09044288486721058, + "timestamp": 4.856580062607514 }, { "x": 7.999455451965332, "y": 7.473754405975342, "heading": 3.153293227446569, - "angularVelocity": -3.79407884851814e-18, - "velocityX": 2.546987187300363e-16, - "velocityY": -1.0256209789219734e-16, - "timestamp": 2.5976055008604364 + "angularVelocity": 5.239369635456734e-24, + "velocityX": 2.4359888438060422e-22, + "velocityY": -8.774946070326848e-22, + "timestamp": 4.952433066873312 }, { - "x": 7.9777911181971035, - "y": 7.466979623755239, + "x": 7.928671024084623, + "y": 7.451618987983874, "heading": 3.153293227446569, - "angularVelocity": 4.0010921973360205e-29, - "velocityX": -0.31261406582002, - "velocityY": -0.09775939742848679, - "timestamp": 2.6669060734735086 + "angularVelocity": 2.2670564289793535e-36, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 5.031658209240836 }, { - "x": 7.934462450883723, - "y": 7.453430059384792, + "x": 7.805841482889084, + "y": 7.413208234129291, "heading": 3.153293227446569, - "angularVelocity": -2.051733815514594e-34, - "velocityX": -0.6252281284210741, - "velocityY": -0.1955187938503514, - "timestamp": 2.736206646086581 + "angularVelocity": 3.3814188338755363e-34, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 5.1108833516083605 }, { - "x": 7.869469450281316, - "y": 7.433105712944096, + "x": 7.683011941693537, + "y": 7.374797480274705, "heading": 3.153293227446569, - "angularVelocity": -1.98522229630168e-34, - "velocityX": -0.9378421873262779, - "velocityY": -0.29327818911646475, - "timestamp": 2.805507218699653 + "angularVelocity": 6.0567834914455396e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.190108493975885 }, { - "x": 7.782812116686986, - "y": 7.40600658452606, + "x": 7.56018240049799, + "y": 7.336386726420119, "heading": 3.153293227446569, - "angularVelocity": -1.9190015159804115e-34, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190667, - "timestamp": 2.8748077913127252 + "angularVelocity": -6.892446902508999e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.269333636343409 }, { - "x": 7.6744904504495075, - "y": 7.372132674239751, + "x": 7.437352859302442, + "y": 7.297975972565533, "heading": 3.153293227446569, - "angularVelocity": -8.200637720207044e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.4887969753935167, - "timestamp": 2.9441083639257974 + "angularVelocity": -4.625140327997389e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.348558778710933 }, { - "x": 7.544504451984089, - "y": 7.331483982215012, + "x": 7.314523318106895, + "y": 7.2595652187109465, "heading": 3.153293227446569, - "angularVelocity": 6.610929004011549e-29, - "velocityX": -1.8756843351233508, - "velocityY": -0.5865563658715173, - "timestamp": 3.0134089365388697 + "angularVelocity": 2.407409636834091e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.427783921078457 }, { - "x": 7.392854121793352, - "y": 7.28406050860902, + "x": 7.191693776911348, + "y": 7.22115446485636, "heading": 3.153293227446569, - "angularVelocity": -6.483737543755527e-29, - "velocityX": -2.188298371464413, - "velocityY": -0.6843157540814643, - "timestamp": 3.082709509151942 + "angularVelocity": -8.071167864078362e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.507009063445981 }, { - "x": 7.219539460498179, - "y": 7.229862253615935, + "x": 7.0688642357158, + "y": 7.182743711001775, "heading": 3.153293227446569, - "angularVelocity": 8.073376116568391e-29, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 3.152010081765014 + "angularVelocity": 6.706721989227974e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.586234205813505 }, { - "x": 7.024560468885024, - "y": 7.168889217481695, + "x": 6.946034694520253, + "y": 7.1443329571471885, "heading": 3.153293227446569, - "angularVelocity": -3.526812750348743e-30, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 3.2213106543780863 + "angularVelocity": -3.932169261853096e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.665459348181029 }, { - "x": 6.807917147982327, - "y": 7.101141400527908, + "x": 6.823205153324706, + "y": 7.105922203292602, "heading": 3.153293227446569, - "angularVelocity": -9.438045628849687e-29, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 3.2906112269911585 + "angularVelocity": -5.711604766985455e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.744684490548553 }, { - "x": 6.569609499192503, - "y": 7.026618803193132, + "x": 6.700375612129158, + "y": 7.067511449438016, "heading": 3.153293227446569, - "angularVelocity": 9.790681244582267e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 3.3599117996042307 + "angularVelocity": 9.147104299173202e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.823909632916077 }, { - "x": 6.309637524541267, - "y": 6.94532142611084, + "x": 6.577546070933611, + "y": 7.0291006955834305, "heading": 3.153293227446569, - "angularVelocity": -1.3895984503554753e-34, - "velocityX": -3.7513683487544087, - "velocityY": -1.1731126312072742, - "timestamp": 3.429212372217303 + "angularVelocity": 4.987399043745874e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.903134775283601 }, { - "x": 6.028001227211883, - "y": 6.85724927027649, + "x": 6.4547165297380635, + "y": 6.990689941728844, "heading": 3.153293227446569, - "angularVelocity": -1.323503512813964e-34, - "velocityX": -4.063982254545725, - "velocityY": -1.2708719785922367, - "timestamp": 3.498512944830375 + "angularVelocity": 1.0726945561042014e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.9823599176511255 }, { - "x": 5.724700612934229, - "y": 6.762402337481905, + "x": 6.331886988542516, + "y": 6.952279187874258, "heading": 3.153293227446569, - "angularVelocity": -2.1989550257903952e-29, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 3.5678135174434473 + "angularVelocity": 1.8749531736864053e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.06158506001865 }, { - "x": 5.399735695078014, - "y": 6.660780631908008, + "x": 6.209057447346969, + "y": 6.913868434019672, "heading": 3.153293227446569, - "angularVelocity": 1.8024841235265752e-29, - "velocityX": -4.689209707841794, - "velocityY": -1.4663905613202457, - "timestamp": 3.6371140900565195 + "angularVelocity": -8.352063684746622e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.140810202386174 }, { - "x": 5.053106540487171, - "y": 6.552384174457959, + "x": 6.0862279061514215, + "y": 6.875457680165086, "heading": 3.153293227446569, - "angularVelocity": 1.896211807861914e-29, - "velocityX": -5.001822373477154, - "velocityY": -1.564149520888688, - "timestamp": 3.7064146626695917 + "angularVelocity": 6.429027209138877e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.220035344753698 }, { - "x": 4.7281416226309565, - "y": 6.4507624688840615, + "x": 5.963398364955874, + "y": 6.8370469263105, "heading": 3.153293227446569, - "angularVelocity": -1.4997927313419928e-29, - "velocityX": -4.689209707841795, - "velocityY": -1.4663905613202457, - "timestamp": 3.775715235282664 + "angularVelocity": -7.349253697576567e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.299260487121222 }, { - "x": 4.424841008353304, - "y": 6.355915536089477, + "x": 5.840568823760327, + "y": 6.798636172455914, "heading": 3.153293227446569, - "angularVelocity": -4.4185947102012545e-35, - "velocityX": -4.376596077655505, - "velocityY": -1.3686313001213652, - "timestamp": 3.845015807895736 + "angularVelocity": -5.316547406258321e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.378485629488746 }, { - "x": 4.14320471102392, - "y": 6.267843380255127, + "x": 5.7177392825647795, + "y": 6.760225418601328, "heading": 3.153293227446569, - "angularVelocity": -9.264733448000688e-35, - "velocityX": -4.063982254545726, - "velocityY": -1.2708719785922367, - "timestamp": 3.9143163805088084 + "angularVelocity": 3.261106516242251e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.45771077185627 }, { - "x": 3.883232736372683, - "y": 6.186546003172835, + "x": 5.594909741369232, + "y": 6.721814664746741, "heading": 3.153293227446569, - "angularVelocity": 3.5265482976182584e-30, - "velocityX": -3.751368348754409, - "velocityY": -1.1731126312072742, - "timestamp": 3.9836169531218806 + "angularVelocity": -2.0479976334874182e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.536935914223794 }, { - "x": 3.644925087582859, - "y": 6.112023405838059, + "x": 5.472080200173685, + "y": 6.683403910892156, "heading": 3.153293227446569, - "angularVelocity": 2.156768838744686e-29, - "velocityX": -3.438754397028912, - "velocityY": -1.0753532694579622, - "timestamp": 4.052917525734953 + "angularVelocity": -7.212902306224552e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.616161056591318 }, { - "x": 3.4282817666801613, - "y": 6.044275588884272, + "x": 5.3492506589781375, + "y": 6.64499315703757, "heading": 3.153293227446569, - "angularVelocity": -2.105657187574439e-28, - "velocityX": -3.1261404160725776, - "velocityY": -0.9775938985677018, - "timestamp": 4.122218098348026 + "angularVelocity": 2.340395125909778e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.695386198958842 }, { - "x": 3.2333027750670063, - "y": 5.983302552750032, + "x": 5.22642111778259, + "y": 6.606582403182983, "heading": 3.153293227446569, - "angularVelocity": 1.8547117767579628e-28, - "velocityX": -2.813526414879514, - "velocityY": -0.8798345213490937, - "timestamp": 4.1915186709610985 + "angularVelocity": -2.132648663400479e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.774611341326366 }, { - "x": 3.059988113771833, - "y": 5.929104297756947, + "x": 5.103591576587043, + "y": 6.568171649328397, "heading": 3.153293227446569, - "angularVelocity": -5.955568710068408e-35, - "velocityX": -2.5009123988461828, - "velocityY": -0.7820751394896976, - "timestamp": 4.260819243574171 + "angularVelocity": -2.6423732915608732e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.8538364836938905 }, { - "x": 2.908337783581096, - "y": 5.881680824150955, + "x": 4.980762035391495, + "y": 6.529760895473811, "heading": 3.153293227446569, - "angularVelocity": -2.0810713408746708e-29, - "velocityX": -2.1882983714644135, - "velocityY": -0.6843157540814642, - "timestamp": 4.330119816187244 + "angularVelocity": -1.5836433593674017e-28, + "velocityX": -1.5503858689927383, + "velocityY": -0.4848303544397451, + "timestamp": 6.933061626061415 }, { - "x": 2.7783517851156776, - "y": 5.841032132126216, + "x": 4.857932494195948, + "y": 6.491350141619225, "heading": 3.153293227446569, - "angularVelocity": 2.081061415014159e-29, - "velocityX": -1.875684335123351, - "velocityY": -0.5865563658715173, - "timestamp": 4.3994203888003165 + "angularVelocity": 3.462450644614712e-28, + "velocityX": -1.5503858689927386, + "velocityY": -0.4848303544397447, + "timestamp": 7.012286768428939 }, { - "x": 2.6700301188781994, - "y": 5.807158221839908, + "x": 4.735102953000401, + "y": 6.452939387764639, "heading": 3.153293227446569, - "angularVelocity": 7.326893682940581e-29, - "velocityX": -1.5630702915295283, - "velocityY": -0.48879697539351663, - "timestamp": 4.468720961413389 + "angularVelocity": 9.425461275139336e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.091511910796463 }, { - "x": 2.583372785283869, - "y": 5.780059093421871, + "x": 4.612273411804853, + "y": 6.414528633910053, "heading": 3.153293227446569, - "angularVelocity": -7.326900961882318e-29, - "velocityX": -1.2504562419442953, - "velocityY": -0.39103758304190656, - "timestamp": 4.538021534026462 + "angularVelocity": 2.8373243283513945e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.170737053163987 }, { - "x": 2.5183797846814615, - "y": 5.759734746981175, + "x": 4.489443870609306, + "y": 6.3761178800554665, "heading": 3.153293227446569, - "angularVelocity": -2.64698564160025e-35, - "velocityX": -0.9378421873262781, - "velocityY": -0.2932781891164647, - "timestamp": 4.6073221066395345 + "angularVelocity": 4.424546575494848e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.249962195531511 }, { - "x": 2.4750511173680816, - "y": 5.746185182610728, + "x": 4.366614329413759, + "y": 6.337707126200881, "heading": 3.153293227446569, - "angularVelocity": -4.5099737006589516e-29, - "velocityX": -0.6252281284210743, - "velocityY": -0.1955187938503513, - "timestamp": 4.676622679252607 + "angularVelocity": 2.6048393166300173e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.329187337899035 + }, + { + "x": 4.243784788218211, + "y": 6.299296372346295, + "heading": 3.153293227446569, + "angularVelocity": -8.141761105066643e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.408412480266559 + }, + { + "x": 4.120955247022664, + "y": 6.2608856184917085, + "heading": 3.153293227446569, + "angularVelocity": -1.5769317536066289e-37, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.487637622634083 + }, + { + "x": 3.998125705827117, + "y": 6.222474864637123, + "heading": 3.153293227446569, + "angularVelocity": -1.9751877406999063e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.566862765001607 + }, + { + "x": 3.8752961646315702, + "y": 6.184064110782536, + "heading": 3.153293227446569, + "angularVelocity": 6.296656188647329e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.6460879073691315 + }, + { + "x": 3.7524666234360233, + "y": 6.145653356927951, + "heading": 3.153293227446569, + "angularVelocity": 3.349857109653225e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.725313049736656 + }, + { + "x": 3.6296370822404764, + "y": 6.107242603073365, + "heading": 3.153293227446569, + "angularVelocity": 1.1073851910178502e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.80453819210418 + }, + { + "x": 3.5068075410449295, + "y": 6.068831849218778, + "heading": 3.153293227446569, + "angularVelocity": -1.0776722091283702e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.883763334471704 + }, + { + "x": 3.3839779998493826, + "y": 6.030421095364192, + "heading": 3.153293227446569, + "angularVelocity": -2.2657744543933122e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.962988476839228 + }, + { + "x": 3.2611484586538357, + "y": 5.992010341509607, + "heading": 3.153293227446569, + "angularVelocity": 6.322058172960319e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.042213619206752 + }, + { + "x": 3.138318917458289, + "y": 5.95359958765502, + "heading": 3.153293227446569, + "angularVelocity": -1.267303885279781e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.121438761574275 + }, + { + "x": 3.015489376262742, + "y": 5.915188833800434, + "heading": 3.153293227446569, + "angularVelocity": 4.539047579816208e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.200663903941798 + }, + { + "x": 2.892659835067195, + "y": 5.876778079945848, + "heading": 3.153293227446569, + "angularVelocity": -2.4352191603126546e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.279889046309322 + }, + { + "x": 2.769830293871648, + "y": 5.838367326091261, + "heading": 3.153293227446569, + "angularVelocity": 1.9777124768596482e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.359114188676845 + }, + { + "x": 2.6470007526761012, + "y": 5.799956572236676, + "heading": 3.153293227446569, + "angularVelocity": 1.6826010379363495e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.438339331044368 + }, + { + "x": 2.5241712114805623, + "y": 5.761545818382093, + "heading": 3.153293227446569, + "angularVelocity": -1.9705474475500694e-33, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 8.517564473411891 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": 1.9207053764808128e-28, - "velocityX": -0.31261406582002027, - "velocityY": -0.09775939742848665, - "timestamp": 4.74592325186568 + "angularVelocity": 2.748235734113482e-34, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 8.596789615779414 }, { "x": 2.4533867835998535, "y": 5.739410400390625, "heading": 3.153293227446569, - "angularVelocity": -4.899028012023128e-29, - "velocityX": 1.275108562409782e-22, - "velocityY": 3.987692290669026e-23, - "timestamp": 4.815223824478752 + "angularVelocity": -7.888608661057978e-31, + "velocityX": -1.038819230645035e-25, + "velocityY": -4.640517458430809e-27, + "timestamp": 8.676014758146938 } ] } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontWing1.auto b/src/main/deploy/pathplanner/autos/FrontWing1.auto new file mode 100644 index 00000000..ac1f7ee3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontWing1.auto @@ -0,0 +1,69 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2263859510421753, + "y": 5.594110488891602 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing1.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "FrontWing1.2" + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontWing2.auto b/src/main/deploy/pathplanner/autos/FrontWing2.auto new file mode 100644 index 00000000..e02712a3 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontWing2.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2263859510421753, + "y": 5.594110488891602 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing2.1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontWing3.auto b/src/main/deploy/pathplanner/autos/FrontWing3.auto new file mode 100644 index 00000000..62bec56a --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontWing3.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2263859510421753, + "y": 5.594110488891602 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing3Contested5.1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto b/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto new file mode 100644 index 00000000..1fd0c9df --- /dev/null +++ b/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto @@ -0,0 +1,88 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 1.2263859510421753, + "y": 5.594110488891602 + }, + "rotation": 179.96604345059157 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing3Contested5.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.1 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "FrontWing3Contested5.2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PathPlannerTest.auto b/src/main/deploy/pathplanner/autos/PathPlannerTest.auto new file mode 100644 index 00000000..e2736b56 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/PathPlannerTest.auto @@ -0,0 +1,19 @@ +{ + "version": 1.0, + "startingPose": null, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "OnePieceAmpStart" + } + } + ] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/AmpLineupPath.path b/src/main/deploy/pathplanner/paths/AmpLineupPath.path new file mode 100644 index 00000000..c8b2a305 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/AmpLineupPath.path @@ -0,0 +1,52 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 1.8725109148331507, + "y": 7.096994083000375 + }, + "prevControl": null, + "nextControl": { + "x": 1.8816898899058623, + "y": 7.409079235472567 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 1.8725109148331507, + "y": 7.583479761854086 + }, + "prevControl": { + "x": 1.8908688649785739, + "y": 7.115352033145799 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 90.0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 90.0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/OnePieceAmpStart.path b/src/main/deploy/pathplanner/paths/OnePieceAmpStart.path new file mode 100644 index 00000000..3450caf8 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/OnePieceAmpStart.path @@ -0,0 +1,84 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.5232015791445568, + "y": 6.977667407055125 + }, + "prevControl": null, + "nextControl": { + "x": 1.5232015791445608, + "y": 6.977667407055125 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4691442945593995, + "y": 6.977667407055125 + }, + "prevControl": { + "x": 1.4691442945593995, + "y": 6.977667407055125 + }, + "nextControl": { + "x": 3.4691442945593995, + "y": 6.977667407055125 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 7.765412911513948, + "y": 7.42743718561799 + }, + "prevControl": { + "x": 5.520899364773203, + "y": 7.193373321263846 + }, + "nextControl": { + "x": 10.009926458254693, + "y": 7.661501049972133 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.4691442945593995, + "y": 5.527389345566705 + }, + "prevControl": { + "x": 5.945808174080279, + "y": 6.5485503224058625 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 1.5, + "maxAcceleration": 1.5, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 0, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ 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 500e5429..ecad226e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -45,7 +45,7 @@ private Constants() { throw new IllegalStateException("Constants class should not be constructed"); } - public static final Mode currentMode = Mode.SIM; + public static final Mode currentMode = Mode.REAL; public enum Mode { /** Running on a real robot. */ @@ -87,35 +87,35 @@ private DriveConstants() { public static final Transform3d LEFT_CAMERA_TRANSFORMATION = new Transform3d( new Translation3d( Units.inchesToMeters(11.0351), // 11.0351 - Units.inchesToMeters(10.023204), // 10.023204 - Units.inchesToMeters(7.1374)), // 4.1374 + Units.inchesToMeters(10.023204 - 2.0), // 10.023204 + Units.inchesToMeters(7.1374 - 3.5)), // 4.1374 new Rotation3d( - Units.degreesToRadians(0.0), - Units.degreesToRadians(-30.0), // -120.0 + 91.0 - Units.degreesToRadians(-14.7)) // 165.3224 + 180 + Units.degreesToRadians(0.0 + 4.0), + Units.degreesToRadians(-30.0 - 1.0), // -120.0 + 91.0 + Units.degreesToRadians(-14.7 - 6.5)) // 165.3224 + 180 ); public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d( new Translation3d( Units.inchesToMeters(11.0351), //11.0351 Units.inchesToMeters(-10.023204), //-10.023204 - Units.inchesToMeters(7.1374)), // 7.1374 + Units.inchesToMeters(7.1374 - 3.5)), // 7.1374 new Rotation3d( - Units.degreesToRadians(0.0), - Units.degreesToRadians(-30.0), // -30.0 - 1 - Units.degreesToRadians(14.7)) // 165.3224) + Units.degreesToRadians(0.0 + 7.0), + Units.degreesToRadians(-30.0 + 0.85), // -30.0 - 1 + Units.degreesToRadians(14.7 - 12.5)) // 165.3224) ); public static final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( - MAX_LINEAR_SPEED * 0.25, - MAX_LINEAR_SPEED * 0.25, - MAX_ANGULAR_SPEED * 0.25, - MAX_ANGULAR_SPEED * 0.25 + MAX_LINEAR_SPEED, + MAX_LINEAR_SPEED * 0.5, + MAX_ANGULAR_SPEED, + MAX_ANGULAR_SPEED * 0.5 ); public static final HolonomicPathFollowerConfig HOLONOMIC_CONFIG = new HolonomicPathFollowerConfig( - new PIDConstants(7.5, 0.75), new PIDConstants(7.5, 0.75), - DriveConstants.MAX_LINEAR_SPEED * 0.25, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig()); + new PIDConstants(4.5, 0.75), new PIDConstants(4.5, 0.75), + DriveConstants.MAX_LINEAR_SPEED * 0.5, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig()); public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants( 0, diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 3f35eda3..63182ded 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -58,7 +58,7 @@ public void robotInit() { // DriverStation.startDataLog(DataLogManager.getLog()); SignalLogger.setPath(logPath); - SignalLogger.start(); +// SignalLogger.start(); // Instantiate our RobotContainer. This will perform all our button bindings, // and put our autonomous chooser on the dashboard. diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index d216dec6..afdd483c 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -30,6 +30,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -183,8 +184,7 @@ public RobotContainer() { private void configureButtonBindings() { Trigger intakeTrigger = controller.y().and(controller.x().negate()) .and(controller.a().negate()) // make sure we don't amp - .and(controller.b().negate()) - .debounce(0.1, Debouncer.DebounceType.kBoth); + .and(controller.b().negate()); Trigger spinUpTrigger = controller.x().and(controller.y().negate()) .and(controller.a().negate()) // make sure we don't amp @@ -192,8 +192,7 @@ private void configureButtonBindings() { Trigger shootTrigger = controller.x().and(controller.y()) .and(controller.a().negate()) // make sure we don't amp - .and(controller.b().negate()) - .debounce(0.1, Debouncer.DebounceType.kBoth); + .and(controller.b().negate()); Trigger ampLineupTrigger = controller.b().and(controller.a().negate()) .debounce(0.1, Debouncer.DebounceType.kBoth); @@ -225,12 +224,16 @@ private void configureButtonBindings() { () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) ))); - ampLineupTrigger.whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); + ampLineupTrigger.whileTrue(new ScheduleCommand(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP)) + .andThen(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), () -> m_shooter.setKickerPower(0.0), m_shooter) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); + controller.rightBumper().whileTrue(m_climber.setClimberPowerFactory(0.25)); + controller.leftBumper().whileTrue(m_climber.setClimberPowerFactory(-0.25)); + controller.pov(0).onTrue(Commands.runOnce(SignalLogger::stop)); controller.pov(180).whileTrue(m_driveSubsystem.pathfollowFactory( FieldConstants.AMP_LINEUP @@ -258,14 +261,14 @@ private void configureButtonBindings() { * Use this method to configure any named commands needed for PathPlanner autos */ private void configureNamedCommands() { - NamedCommands.registerCommand("Run Intake", m_shooter.intakeCommand(0.75, 0.5, 0.1) + NamedCommands.registerCommand("Intake", m_shooter.intakeCommand(0.75, 0.5, 0.1) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); NamedCommands.registerCommand("AimAndShoot", new ShooterAutoCommand(m_armSubsystem, m_shooter) .raceWith(DriveCommands.alignmentDrive( m_driveSubsystem, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), + () -> 0.0, + () -> 0.0, () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) ))); } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 11ec5791..b4ae68db 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -42,7 +42,7 @@ public static Command joystickDrive( () -> { double xInput = setSensitivity(xSupplier.getAsDouble(), 0.25); double yInput = setSensitivity(ySupplier.getAsDouble(), 0.25); - double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.0); + double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.0) * 0.85; // Apply deadband double linearMagnitude = diff --git a/src/main/java/frc/robot/commands/auto/AutoFactory.java b/src/main/java/frc/robot/commands/auto/AutoFactory.java new file mode 100644 index 00000000..5cc21fc9 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/AutoFactory.java @@ -0,0 +1,14 @@ +package frc.robot.commands.auto; + +public class AutoFactory { + public enum AUTOS { + ONE_PIECE_AMP_START("OneAmpStart"), + THREE_PIECE_CENTER_START("ThreeCenterStart"); + + AUTOS(String name) { + this.name = name; + } + + public final String name; + } +} diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java index 5176a41a..343a5b1d 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java @@ -21,6 +21,7 @@ import lib.properties.phoenix6.PidPropertyPublic; import frc.robot.Constants.ArmConstants; +import org.littletonrobotics.junction.Logger; public class ArmIOKraken implements ArmIO { // Physical devices @@ -84,7 +85,7 @@ public ArmIOKraken() { armConfig.Feedback.SensorToMechanismRatio = ArmConstants.ARM_SENSOR_MECHANISM_RATIO; m_armMaster = TalonFXFactory.createTalon(ArmConstants.ARM_MASTER_ID, armConfig); - m_armFollower = TalonFXFactory.createTalon(ArmConstants.ARM_MASTER_ID, armConfig); + m_armFollower = TalonFXFactory.createTalon(ArmConstants.ARM_FOLLOWER_ID, armConfig); // Wrist Configuration TalonFXConfiguration wristConfig = new TalonFXConfiguration(); @@ -242,6 +243,9 @@ public void updateInputs(ArmIOInputsAutoLogged inputs) { m_armMaxVelDegS.updateIfChanged(); m_wristMaxVelDegS.updateIfChanged(); m_accelTimeSecs.updateIfChanged(); + + Logger.recordOutput("Arm Prev Vel Mult", m_prevArmVelocityMult); + Logger.recordOutput("Wrist Prev Vel Mult", m_prevWristVelocityMult); } @Override diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 1405fb6a..4600f977 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -145,8 +145,8 @@ public DriveSubsystem( Units.inchesToMeters(0.5), Units.degreesToRadians(0.75)), VecBuilder.fill( - Units.inchesToMeters(4.0), - Units.inchesToMeters(4.0), + Units.inchesToMeters(2.0), + Units.inchesToMeters(2.0), Units.degreesToRadians(25.0)) ); @@ -239,9 +239,10 @@ public void periodic() { } // make sure we're not moving too fast before trying to update vision poses - if ((kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond <= Units.inchesToMeters(20)) - && (kinematics.toChassisSpeeds(getModuleStates()).vyMetersPerSecond <= Units.inchesToMeters(20)) - && (kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond <= Units.degreesToRadians(60))) { + if ((kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond <= Units.inchesToMeters(30)) + && (kinematics.toChassisSpeeds(getModuleStates()).vyMetersPerSecond <= Units.inchesToMeters(30)) + && (kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond <= Units.degreesToRadians(60)) + || DriverStation.isTeleop()) { for (VisionSubsystem camera : m_cameras) { camera.updateInputs(); camera.getPose(m_wpiPoseEstimator.getEstimatedPosition()).ifPresent( @@ -255,12 +256,6 @@ public void periodic() { m_thetaPidProperty.updateIfChanged(); m_field.setRobotPose(getVisionPose()); - - Logger.recordOutput("Swerve/LeftCamPose", - new Pose3d(getVisionPose()).plus(DriveConstants.LEFT_CAMERA_TRANSFORMATION)); - - Logger.recordOutput("Swerve/RightCamPose", - new Pose3d(getVisionPose()).plus(DriveConstants.RIGHT_CAMERA_TRANSFORMATION)); } /** @@ -352,7 +347,7 @@ private SwerveModulePosition[] getModulePositions() { } /** Returns the robot pose with vision updates */ - @AutoLogOutput(key = "Odometry/RobotVision") + @AutoLogOutput(key = "Odometry/RobotPose") public Pose2d getVisionPose() { return m_wpiPoseEstimator.getEstimatedPosition(); } @@ -380,7 +375,7 @@ public double getMaxAngularSpeedRadPerSec() { public Command pathfollowFactory(Pose2d pose) { return AutoBuilder.pathfindToPoseFlipped( - pose, DriveConstants.DEFAULT_CONSTRAINTS); + pose, DriveConstants.DEFAULT_CONSTRAINTS).withInterruptBehavior(Command.InterruptionBehavior.kCancelSelf); } /** Returns an array of module translations. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 49890e33..5c7e9b95 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -27,8 +27,8 @@ public class ShooterSubsystem extends SubsystemBase { private final DataLogTable m_logTable = DataLogUtil.getTable("Shooter"); - private double m_leftSpeedSetpoint = 0.0; - private double m_rightSpeedSetpoint = 0.0; + private double m_leftSpeedSetpoint = 3800.0; + private double m_rightSpeedSetpoint = 3800.0; private final GosDoubleProperty m_leftPower = new GosDoubleProperty(false, "Shooter/Left RPM", 3600); diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index 3c0aaede..dd6dc86b 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -14,6 +14,7 @@ import edu.wpi.first.math.numbers.N3; import edu.wpi.first.math.util.Units; import lib.logger.DataLogUtil; +import lib.utils.FieldConstants; import lib.utils.PoseEstimator; import org.littletonrobotics.junction.Logger; import org.photonvision.EstimatedRobotPose; @@ -32,11 +33,11 @@ public class VisionSubsystem { private AprilTagFieldLayout m_aprilTagFieldLayout; private final String m_name; - private final double xyStdDevCoefficient = Units.inchesToMeters(8.0); - private final double thetaStdDevCoefficient = Units.degreesToRadians(24.0); + private final double xyStdDevCoefficient = Units.inchesToMeters(4.0); + private final double thetaStdDevCoefficient = Units.degreesToRadians(12.0); - private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(4.0); - private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(12.0); + private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(2.0); + private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(6.0); private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); @@ -47,12 +48,13 @@ public VisionSubsystem(String camName, Transform3d camPose) { try { m_aprilTagFieldLayout = AprilTagFieldLayout.loadFromResource(AprilTagFields.k2024Crescendo.m_resourceFile); m_photonPoseEstimator = new PhotonPoseEstimator( - m_aprilTagFieldLayout, + FieldConstants.APRIL_TAG_FIELD_LAYOUT, PhotonPoseEstimator.PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, m_camera, robotToCam); - m_photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.CLOSEST_TO_REFERENCE_POSE); + m_photonPoseEstimator.setMultiTagFallbackStrategy(PhotonPoseEstimator.PoseStrategy.LOWEST_AMBIGUITY); + } catch (IOException e){ throw new IllegalStateException(e); } @@ -103,6 +105,11 @@ public Optional getPose(Pose2d prevEstima double xyStdDev = xyStdDevCoefficient * Math.pow(avgDist, 2.0); double thetaStdDev = thetaStdDevCoefficient * Math.pow(avgDist, 2.0); + if (camResult.getTargets().size() > 1.0) { + xyStdDev = xyStdDevMultiTagCoefficient * Math.pow(avgDist, 2.0); + thetaStdDev = thetaStdDevMultiTagCoefficient * Math.pow(avgDist, 2.0); + } + Logger.recordOutput("Vision/" + m_name + "/Estimated Pose", estPose.estimatedPose); return Optional.of(new PoseEstimator.TimestampedVisionUpdate(estPose.timestampSeconds, diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index b36cc3ed..26ac4737 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -14,24 +14,25 @@ public class AimbotUtils { static { // angle measurements, meters -> degrees - m_angleLerpTable.put(Units.inchesToMeters(18.0), 57.0); - m_angleLerpTable.put(Units.inchesToMeters(46.0), 45.0); - m_angleLerpTable.put(Units.inchesToMeters(97.0), 33.5); - m_angleLerpTable.put(Units.inchesToMeters(116.0), 29.75); - m_angleLerpTable.put(Units.inchesToMeters(155.0), 24.65); + m_angleLerpTable.put(Units.inchesToMeters(18.0), 57.0 + 3.0); + m_angleLerpTable.put(Units.inchesToMeters(46.0), 45.0 + 3.0); + m_angleLerpTable.put(Units.inchesToMeters(97.0), 33.5 + 2.5); + m_angleLerpTable.put(Units.inchesToMeters(116.0), 29.75 + 2.5); + m_angleLerpTable.put(Units.inchesToMeters(155.0), 24.65 + 1.5); + // m_angleLerpTable.put(Units.inchesToMeters(229.0), 0.0); //flywheel measurements, meters -> RPM m_leftSpeedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); - m_leftSpeedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); - m_leftSpeedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); - m_leftSpeedLerpTable.put(Units.inchesToMeters(116.0), 3800.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(46.0), 3700.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(97.0), 3800.0); + m_leftSpeedLerpTable.put(Units.inchesToMeters(116.0), 4000.0); m_leftSpeedLerpTable.put(Units.inchesToMeters(155.0), 5000.0); // m_leftSpeedLerpTable.put(Units.inchesToMeters(229.0), 0.0); m_rightSpeedLerpTable.put(Units.inchesToMeters(18.0), 3600.0); m_rightSpeedLerpTable.put(Units.inchesToMeters(46.0), 3600.0); - m_rightSpeedLerpTable.put(Units.inchesToMeters(97.0), 3600.0); + m_rightSpeedLerpTable.put(Units.inchesToMeters(97.0), 3700.0); m_rightSpeedLerpTable.put(Units.inchesToMeters(116.0), 3800.0); m_rightSpeedLerpTable.put(Units.inchesToMeters(155.0), 4600.0); // m_rightSpeedLerpTable.put(Units.inchesToMeters(229.0), 0.0); diff --git a/src/main/java/lib/utils/FieldConstants.java b/src/main/java/lib/utils/FieldConstants.java index 4bee1313..6a552e45 100644 --- a/src/main/java/lib/utils/FieldConstants.java +++ b/src/main/java/lib/utils/FieldConstants.java @@ -32,7 +32,7 @@ private FieldConstants() {} public static final Pose2d AMP_LINEUP = AllianceFlipUtil.apply(new Pose2d( AMP_CENTER.getX(), - AMP_CENTER.getY() - Units.inchesToMeters(20), + AMP_CENTER.getY() - Units.inchesToMeters(10), Rotation2d.fromDegrees(90) )); From 34a6146e84fcd345ebaaa8c40a240316a06f58b3 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 11:40:12 -0400 Subject: [PATCH 03/30] move and auto align at the same time --- src/main/java/frc/robot/RobotContainer.java | 12 +- .../frc/robot/commands/DriveCommands.java | 2 +- .../robot/commands/alignmentDriveCommand.java | 116 ++++++++++++++++++ .../subsystems/drive/DriveSubsystem.java | 6 +- 4 files changed, 129 insertions(+), 7 deletions(-) create mode 100644 src/main/java/frc/robot/commands/alignmentDriveCommand.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index afdd483c..5a5283cf 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -35,10 +35,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DriveConstants; -import frc.robot.commands.DriveCommands; -import frc.robot.commands.FeedForwardCharacterization; -import frc.robot.commands.IntakeControlCommand; -import frc.robot.commands.ShooterAutoCommand; +import frc.robot.commands.*; import frc.robot.subsystems.arm.*; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberIOKraken; @@ -224,8 +221,13 @@ private void configureButtonBindings() { () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) ))); - ampLineupTrigger.whileTrue(new ScheduleCommand(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP)) + ampLineupTrigger.whileTrue(new alignmentDriveCommand(m_driveSubsystem, + () -> -controller.getLeftY(), + () -> -controller.getLeftX(), + () -> -controller.getRightX(), + () -> FieldConstants.AMP_LINEUP) .andThen(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); + ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), () -> m_shooter.setKickerPower(0.0), m_shooter) diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index b4ae68db..555d427b 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -26,7 +26,7 @@ import java.util.function.Supplier; public class DriveCommands { - private static final double DEADBAND = 0.1; + public static final double DEADBAND = 0.1; private DriveCommands() {} diff --git a/src/main/java/frc/robot/commands/alignmentDriveCommand.java b/src/main/java/frc/robot/commands/alignmentDriveCommand.java new file mode 100644 index 00000000..e5d21049 --- /dev/null +++ b/src/main/java/frc/robot/commands/alignmentDriveCommand.java @@ -0,0 +1,116 @@ +package frc.robot.commands; + +import com.pathplanner.lib.commands.PathfindHolonomic; +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.drive.DriveSubsystem; +import lib.utils.AllianceFlipUtil; + +import java.util.function.DoubleSupplier; +import java.util.function.Supplier; + +import static frc.robot.commands.DriveCommands.DEADBAND; + + +public class alignmentDriveCommand extends Command { + private final DriveSubsystem driveSubsystem; + private final Supplier pointSupplier; + private Command alignmentCommand; + + private final DoubleSupplier xSupplier; + private final DoubleSupplier ySupplier; + private final DoubleSupplier thetaSupplier; + + public alignmentDriveCommand(DriveSubsystem driveSubsystem, + DoubleSupplier xSupplier, + DoubleSupplier ySupplier, + DoubleSupplier thetaSupplier, + Supplier point) { + this.driveSubsystem = driveSubsystem; + this.pointSupplier = point; + + + + this.xSupplier = xSupplier; + this.ySupplier =ySupplier; + this.thetaSupplier = thetaSupplier; + + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.driveSubsystem); + } + + @Override + public void initialize() { + Pose2d point = AllianceFlipUtil.apply(pointSupplier.get()); + alignmentCommand = new PathfindHolonomic( + point, + DriveConstants.DEFAULT_CONSTRAINTS, + 0.0, + driveSubsystem::getVisionPose, + driveSubsystem::getRobotRelativeSpeeds, + driveSubsystem::runVelocity, + DriveConstants.HOLONOMIC_CONFIG, + 0.0, + driveSubsystem + ); + } + + @Override + public void execute() { + double xInput = DriveCommands.setSensitivity(xSupplier.getAsDouble(), 0.25); + double yInput = DriveCommands.setSensitivity(ySupplier.getAsDouble(), 0.25); + double omegaInput = DriveCommands.setSensitivity(thetaSupplier.getAsDouble(), 0.0) * 0.85; + + // if we have actual driver input + if (xInput > DEADBAND + || yInput > DEADBAND + || omegaInput > DEADBAND) { + // Apply deadband + double linearMagnitude = + MathUtil.applyDeadband( + Math.hypot(xInput, yInput), DEADBAND); + Rotation2d linearDirection = + new Rotation2d(xInput, yInput); + double omega = MathUtil.applyDeadband(omegaInput, DEADBAND); + + // Calcaulate new linear velocity + Translation2d linearVelocity = + new Pose2d(new Translation2d(), linearDirection) + .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d())) + .getTranslation(); + + Rotation2d heading; + + // if red change heading goal + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + heading = driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); + } else { + heading = driveSubsystem.getRotation(); + } + + // Convert to field relative speeds & send command + driveSubsystem.runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds( + linearVelocity.getX() * driveSubsystem.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getY() * driveSubsystem.getMaxLinearSpeedMetersPerSec(), + omega * driveSubsystem.getMaxAngularSpeedRadPerSec(), + heading)); + } else { + alignmentCommand.execute(); + } + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 4600f977..69092632 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -395,4 +395,8 @@ public Command runSysidQuasistatic(SysIdRoutine.Direction direction) { public Command runSysidDynamic(SysIdRoutine.Direction direction) { return m_sysId.dynamic(direction); } - } + + public ChassisSpeeds getRobotRelativeSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); + } +} From fc384e1c2d35d5078ac4d0b36795beebfea79d1e Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 11:56:56 -0400 Subject: [PATCH 04/30] use average rotation from last 5 updates for aimbot --- src/main/java/frc/robot/RobotContainer.java | 2 +- .../subsystems/drive/DriveSubsystem.java | 29 ++++++++++++++++++- 2 files changed, 29 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5a5283cf..5a04e0b1 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -227,7 +227,7 @@ private void configureButtonBindings() { () -> -controller.getRightX(), () -> FieldConstants.AMP_LINEUP) .andThen(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); - + ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), () -> m_shooter.setKickerPower(0.0), m_shooter) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 69092632..f2555ef3 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -81,6 +81,15 @@ public class DriveSubsystem extends SubsystemBase { new SwerveModulePosition() }; + private Rotation2d[] m_prevRotations = new Rotation2d[] { + new Rotation2d(), + new Rotation2d(), + new Rotation2d(), + new Rotation2d(), + new Rotation2d() + }; + private Rotation2d m_avgRotationRads = new Rotation2d(); + private final VisionSubsystem[] m_cameras; private final PIDController m_thetaPid; @@ -234,6 +243,24 @@ public void periodic() { m_rawGyroRotation = m_rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } + // Update average heading, used for aiming + m_prevRotations = new Rotation2d[] { + m_rawGyroRotation, + m_prevRotations[1], + m_prevRotations[2], + m_prevRotations[3], + m_prevRotations[4] + }; + + double totalRotationRads = 0.0; + + for (Rotation2d mPrevRotation : m_prevRotations) { + totalRotationRads += mPrevRotation.getRadians(); + } + + totalRotationRads /= m_prevRotations.length; + m_avgRotationRads = new Rotation2d(totalRotationRads); + // Apply update m_wpiPoseEstimator.updateWithTime(sampleTimestamps[i], m_rawGyroRotation, modulePositions); } @@ -291,7 +318,7 @@ public double alignToPoint(Pose3d point) { double desiredRotation = Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY())) + Units.degreesToRadians(90); - return m_thetaPid.calculate(getRotation().getRadians(), desiredRotation); + return m_thetaPid.calculate(m_avgRotationRads.getRadians(), desiredRotation); } /** Stops the drive. */ From 8bde48a915f2c46ff9ef7f7cba5d0a8ec6ea5b0e Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 12:18:25 -0400 Subject: [PATCH 05/30] use a secondary variable for velocity multipliers on arm --- .../frc/robot/subsystems/arm/ArmIOKraken.java | 26 ++++++------------- .../subsystems/drive/DriveSubsystem.java | 8 +++--- 2 files changed, 12 insertions(+), 22 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java index 343a5b1d..4b37ddad 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java @@ -41,8 +41,8 @@ public class ArmIOKraken implements ArmIO { private final HeavyDoubleProperty m_wristMaxVelDegS; private final HeavyDoubleProperty m_accelTimeSecs; - private final MotionMagicConfigs m_armMMConfigs = new MotionMagicConfigs(); - private final MotionMagicConfigs m_wristMMConfigs = new MotionMagicConfigs(); + private double m_armMaxVel; + private double m_wristMaxVel; // Control outputs private final DynamicMotionMagicVoltage m_wristDynMMRequest; @@ -51,8 +51,6 @@ public class ArmIOKraken implements ArmIO { private final Follower m_wristFollowerRequest; private final NeutralOut m_stopRequest; - private boolean m_tracking = false; - private double m_prevArmVelocityMult = 0.0; private double m_prevWristVelocityMult = 0.0; @@ -141,16 +139,16 @@ public ArmIOKraken() { .build(); m_armMaxVelDegS = new HeavyDoubleProperty( - (double maxVel) -> m_armDynMMRequest.Velocity = Units.degreesToRotations(maxVel), + (double maxVel) -> m_armMaxVel = Units.degreesToRotations(maxVel), new GosDoubleProperty(false, "Arm/Arm Max Vel DegsS", 120)); m_wristMaxVelDegS = new HeavyDoubleProperty( - (double maxVel) -> m_wristDynMMRequest.Velocity = Units.degreesToRotations(maxVel), + (double maxVel) -> m_wristMaxVel = Units.degreesToRotations(maxVel), new GosDoubleProperty(false, "Arm/Wrist Max Vel DegsS", 120)); m_accelTimeSecs = new HeavyDoubleProperty((double accel) -> { - m_armDynMMRequest.Acceleration = m_armDynMMRequest.Velocity / accel; - m_wristDynMMRequest.Acceleration = m_wristDynMMRequest.Velocity / accel; + m_armDynMMRequest.Acceleration = m_armMaxVel / accel; + m_wristDynMMRequest.Acceleration = m_wristMaxVel / accel; }, new GosDoubleProperty(false, "Arm/Acceleration Time Secs", 1)); m_armMaxVelDegS.updateIfChanged(true); @@ -256,11 +254,7 @@ public void setArmVoltage(double voltage) { @Override public void setArmAngle(double degrees, double velocityMult) { - if (m_prevArmVelocityMult != velocityMult) { - m_armMaxVelDegS.updateIfChanged(true); - } - m_prevArmVelocityMult = velocityMult; - m_armDynMMRequest.Velocity = m_armDynMMRequest.Velocity * velocityMult; + m_armDynMMRequest.Velocity = m_armMaxVel * velocityMult; m_armMaster.setControl(m_armDynMMRequest.withPosition(degrees / 360)); m_armFollower.setControl(m_armFollowerRequest); @@ -274,11 +268,7 @@ public void setWristVoltage(double voltage) { @Override public void setWristAngle(double degrees, double velocityMult) { - if (m_prevWristVelocityMult != velocityMult) { - m_wristMaxVelDegS.updateIfChanged(true); - } - m_prevWristVelocityMult = velocityMult; - m_wristDynMMRequest.Velocity = m_wristDynMMRequest.Velocity * velocityMult; + m_wristDynMMRequest.Velocity = m_wristMaxVel * velocityMult; m_wristMaster.setControl(m_wristDynMMRequest.withPosition(degrees / 360)); m_wristFollower.setControl(m_wristFollowerRequest); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index f2555ef3..c42c1e93 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -293,16 +293,16 @@ public void periodic() { public void runVelocity(ChassisSpeeds speeds) { // Calculate module setpoints ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] m_setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); - SwerveDriveKinematics.desaturateWheelSpeeds(m_setpointStates, DriveConstants.MAX_LINEAR_SPEED); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(discreteSpeeds); + SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, DriveConstants.MAX_LINEAR_SPEED); // Send setpoints to modules for (int i = 0; i < 4; i++) { // The module returns the optimized state, useful for logging - m_optimizedStates[i] = modules[i].runSetpoint(m_setpointStates[i]); + m_optimizedStates[i] = modules[i].runSetpoint(setpointStates[i]); } - Logger.recordOutput("SwerveStates/Setpoints", m_setpointStates); + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); Logger.recordOutput("SwerveStates/Optimized", m_optimizedStates); } From e7bfe10efe2ba706161fe00d0f5b12ba4c3fc404 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 13:24:24 -0400 Subject: [PATCH 06/30] aim back further for aimbot --- src/main/java/lib/utils/FieldConstants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/lib/utils/FieldConstants.java b/src/main/java/lib/utils/FieldConstants.java index 6a552e45..491efc94 100644 --- a/src/main/java/lib/utils/FieldConstants.java +++ b/src/main/java/lib/utils/FieldConstants.java @@ -93,7 +93,7 @@ public static final class Speaker { public static final Translation3d CENTER_SPEAKER = new Translation3d( - (Units.inchesToMeters(18.055) / 2.0), + (Units.inchesToMeters(18.055) / 2.0) - Units.inchesToMeters(2.0), Units.inchesToMeters((238.815 + 197.765) / 2.0), (Units.inchesToMeters(83.091 + 78.324) / 2.0) - Units.inchesToMeters(1.0)); From 0334ef74674a20ed5a2d9314904ec066f56e3807 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 14:38:23 -0400 Subject: [PATCH 07/30] finish alignment drive --- src/main/java/frc/robot/RobotContainer.java | 5 +---- ...lignmentDriveCommand.java => AlignmentDriveCommand.java} | 6 +++--- 2 files changed, 4 insertions(+), 7 deletions(-) rename src/main/java/frc/robot/commands/{alignmentDriveCommand.java => AlignmentDriveCommand.java} (96%) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5a04e0b1..a2bd84c5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,8 +19,6 @@ import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; @@ -30,7 +28,6 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; @@ -221,7 +218,7 @@ private void configureButtonBindings() { () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) ))); - ampLineupTrigger.whileTrue(new alignmentDriveCommand(m_driveSubsystem, + ampLineupTrigger.whileTrue(new AlignmentDriveCommand(m_driveSubsystem, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX(), diff --git a/src/main/java/frc/robot/commands/alignmentDriveCommand.java b/src/main/java/frc/robot/commands/AlignmentDriveCommand.java similarity index 96% rename from src/main/java/frc/robot/commands/alignmentDriveCommand.java rename to src/main/java/frc/robot/commands/AlignmentDriveCommand.java index e5d21049..40e10a3b 100644 --- a/src/main/java/frc/robot/commands/alignmentDriveCommand.java +++ b/src/main/java/frc/robot/commands/AlignmentDriveCommand.java @@ -19,7 +19,7 @@ import static frc.robot.commands.DriveCommands.DEADBAND; -public class alignmentDriveCommand extends Command { +public class AlignmentDriveCommand extends Command { private final DriveSubsystem driveSubsystem; private final Supplier pointSupplier; private Command alignmentCommand; @@ -28,7 +28,7 @@ public class alignmentDriveCommand extends Command { private final DoubleSupplier ySupplier; private final DoubleSupplier thetaSupplier; - public alignmentDriveCommand(DriveSubsystem driveSubsystem, + public AlignmentDriveCommand(DriveSubsystem driveSubsystem, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier thetaSupplier, @@ -111,6 +111,6 @@ public void execute() { @Override public boolean isFinished() { - return false; + return alignmentCommand.isFinished(); } } From cb1f9ecabfe4efea3f9f4cf733e2c28089a46b9d Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 16:12:19 -0400 Subject: [PATCH 08/30] new auto factory --- src/main/java/frc/robot/RobotContainer.java | 5 +- .../frc/robot/commands/auto/AutoFactory.java | 61 ++++++++++++++++--- 2 files changed, 57 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a2bd84c5..a0663b63 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -33,6 +33,7 @@ import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DriveConstants; import frc.robot.commands.*; +import frc.robot.commands.auto.AutoFactory; import frc.robot.subsystems.arm.*; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberIOKraken; @@ -68,7 +69,7 @@ public class RobotContainer { private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs - private final SendableChooser autoChooser; + private final AutoFactory m_autonFactory = new AutoFactory(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -285,6 +286,6 @@ private void configureDashboard() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return autoChooser.getSelected(); + return m_autonFactory.getSelectedAutonomous(); } } diff --git a/src/main/java/frc/robot/commands/auto/AutoFactory.java b/src/main/java/frc/robot/commands/auto/AutoFactory.java index 5cc21fc9..b3fa9269 100644 --- a/src/main/java/frc/robot/commands/auto/AutoFactory.java +++ b/src/main/java/frc/robot/commands/auto/AutoFactory.java @@ -1,14 +1,61 @@ package frc.robot.commands.auto; -public class AutoFactory { - public enum AUTOS { - ONE_PIECE_AMP_START("OneAmpStart"), - THREE_PIECE_CENTER_START("ThreeCenterStart"); +import com.pathplanner.lib.auto.AutoBuilder; +import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; - AUTOS(String name) { - this.name = name; +import java.util.EnumMap; +import java.util.HashMap; +import java.util.Map; + +public final class AutoFactory { + + public enum AutoModes { + FOUR_NOTE("FourNoteSpeaker"), + TWO_NOTE_LEFT_3("TwoNoteSpeakerLeft3"), + TWO_NOTE_LEFT_4("TwoNoteSpeakerLeft4"), + TWO_NOTE_MIDDLE_2("TwoNoteSpeakerMiddle2"), + TWO_NOTE_MIDDLE_5("TwoNoteSpeakerMiddle5"), + TWO_NOTE_MIDDLE_6("TwoNoteSpeakerMiddle6"), + TWO_NOTE_RIGHT_6("TwoNoteSpeakerRight6"), + TWO_NOTE_RIGHT_7("TwoNotesSpeakerRight7"), + LEAVE_WING("LeaveWing"), + PRELOAD_AND_LEAVE_WING("OneNoteSpeakerAndLeaveWing"), + PRELOAD_AND_SHOOT("JustShoot"); + + + public final String m_modeName; + + AutoModes(String modeName) { + m_modeName = modeName; + } + } + + private static final AutoModes DEFAULT_MODE = AutoModes.TWO_NOTE_RIGHT_6; + + private final LoggedDashboardChooser m_autonChooser; + + private final EnumMap m_modes; + + public AutoFactory() { + m_autonChooser = new LoggedDashboardChooser<>("Auto Chooser"); + m_modes = new EnumMap<>(new HashMap<>()); + + for (AutoModes mode : AutoModes.values()) { + if (mode == DEFAULT_MODE) { + m_autonChooser.addDefaultOption(mode.m_modeName, mode); + } else { + m_autonChooser.addOption(mode.m_modeName, mode); + } + + m_modes.put(mode, AutoBuilder.buildAuto(mode.m_modeName)); } + } - public final String name; + public Command getSelectedAutonomous() { + AutoModes mode = m_autonChooser.get(); + return m_modes.get(mode); } } From e820639ed06ba5f6f14c40c131bf1db178516301 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 16:14:07 -0400 Subject: [PATCH 09/30] configured autofactory and set defaults --- src/main/java/frc/robot/RobotContainer.java | 18 ---------------- .../frc/robot/commands/auto/AutoFactory.java | 21 +++++-------------- 2 files changed, 5 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a0663b63..f1eed556 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -148,24 +148,6 @@ public RobotContainer() { configureNamedCommands(); configureDashboard(); - autoChooser = AutoBuilder.buildAutoChooser(); - - // Set up feedforward characterization - autoChooser.addOption( - "Drive Quasistatic Forward", - m_driveSubsystem.runSysidQuasistatic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Drive Quasistatic Backwards", - m_driveSubsystem.runSysidQuasistatic(SysIdRoutine.Direction.kReverse)); - autoChooser.addOption( - "Drive Dynamic Forward", - m_driveSubsystem.runSysidDynamic(SysIdRoutine.Direction.kForward)); - autoChooser.addOption( - "Drive Dynamic Backwards", - m_driveSubsystem.runSysidDynamic(SysIdRoutine.Direction.kReverse)); - - SmartDashboard.putData("Auto Chooser", autoChooser); - // Configure the button bindings configureButtonBindings(); } diff --git a/src/main/java/frc/robot/commands/auto/AutoFactory.java b/src/main/java/frc/robot/commands/auto/AutoFactory.java index b3fa9269..50cfe43e 100644 --- a/src/main/java/frc/robot/commands/auto/AutoFactory.java +++ b/src/main/java/frc/robot/commands/auto/AutoFactory.java @@ -1,30 +1,19 @@ package frc.robot.commands.auto; import com.pathplanner.lib.auto.AutoBuilder; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; import java.util.EnumMap; import java.util.HashMap; -import java.util.Map; public final class AutoFactory { public enum AutoModes { - FOUR_NOTE("FourNoteSpeaker"), - TWO_NOTE_LEFT_3("TwoNoteSpeakerLeft3"), - TWO_NOTE_LEFT_4("TwoNoteSpeakerLeft4"), - TWO_NOTE_MIDDLE_2("TwoNoteSpeakerMiddle2"), - TWO_NOTE_MIDDLE_5("TwoNoteSpeakerMiddle5"), - TWO_NOTE_MIDDLE_6("TwoNoteSpeakerMiddle6"), - TWO_NOTE_RIGHT_6("TwoNoteSpeakerRight6"), - TWO_NOTE_RIGHT_7("TwoNotesSpeakerRight7"), - LEAVE_WING("LeaveWing"), - PRELOAD_AND_LEAVE_WING("OneNoteSpeakerAndLeaveWing"), - PRELOAD_AND_SHOOT("JustShoot"); - + FRONT_WING_1("FrontWing1"), + FRONT_WING_2("FrontWing2"), + FRONT_WING_3("FrontWing3"), + FRONT_WING_3_CONTESTED_5("FrontWing3Contested5"); public final String m_modeName; @@ -33,7 +22,7 @@ public enum AutoModes { } } - private static final AutoModes DEFAULT_MODE = AutoModes.TWO_NOTE_RIGHT_6; + private static final AutoModes DEFAULT_MODE = AutoModes.FRONT_WING_1; private final LoggedDashboardChooser m_autonChooser; From 862d7136e59c5d75dc3d275023f4ec129620cf7a Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 16:16:12 -0400 Subject: [PATCH 10/30] initialize and end alignmentDrive --- .../java/frc/robot/commands/AlignmentDriveCommand.java | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/commands/AlignmentDriveCommand.java b/src/main/java/frc/robot/commands/AlignmentDriveCommand.java index 40e10a3b..f97c1b88 100644 --- a/src/main/java/frc/robot/commands/AlignmentDriveCommand.java +++ b/src/main/java/frc/robot/commands/AlignmentDriveCommand.java @@ -61,6 +61,8 @@ public void initialize() { 0.0, driveSubsystem ); + + alignmentCommand.initialize(); } @Override @@ -113,4 +115,9 @@ public void execute() { public boolean isFinished() { return alignmentCommand.isFinished(); } + + @Override + public void end(boolean interrupted) { + alignmentCommand.end(interrupted); + } } From 4f2b8f530c38c634354cbe66bc771d9afe5cf53e Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 20:02:26 -0400 Subject: [PATCH 11/30] fix build errors and drive after alignment --- src/main/java/frc/robot/Constants.java | 10 +++--- src/main/java/frc/robot/RobotContainer.java | 28 +++++----------- .../robot/commands/AlignmentDriveCommand.java | 33 +++++++++++++++---- .../frc/robot/commands/DriveCommands.java | 2 +- .../frc/robot/commands/auto/AutoFactory.java | 4 +-- .../frc/robot/subsystems/arm/ArmIOKraken.java | 9 +++-- src/main/java/lib/utils/AimbotUtils.java | 17 ++++++++++ 7 files changed, 68 insertions(+), 35 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index ecad226e..5cf079f8 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -107,10 +107,10 @@ private DriveConstants() { ); public static final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( - MAX_LINEAR_SPEED, MAX_LINEAR_SPEED * 0.5, - MAX_ANGULAR_SPEED, - MAX_ANGULAR_SPEED * 0.5 + MAX_LINEAR_SPEED * 0.25, + MAX_ANGULAR_SPEED * 0.5, + MAX_ANGULAR_SPEED * 0.25 ); public static final HolonomicPathFollowerConfig HOLONOMIC_CONFIG = new HolonomicPathFollowerConfig( @@ -169,8 +169,8 @@ private ArmConstants() {} public static final int WRIST_MASTER_ID = 21; public static final int WRIST_FOLLOWER_ID = 22; public static final int WRIST_ENCODER_ID = 23; - public static final int ARM_MASTER_ID = 18; - public static final int ARM_FOLLOWER_ID = 19; + public static final int ARM_MASTER_ID = 19; + public static final int ARM_FOLLOWER_ID = 18; public static final int ARM_ENCODER_ID = 20; /* Because the absolute encoders are on a 2/1 ratio, we have to move our offset down a little into a rotation lower diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f1eed556..934d99d3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,8 +13,6 @@ package frc.robot; -import com.ctre.phoenix6.SignalLogger; -import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.math.geometry.Pose2d; @@ -24,16 +22,14 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; -import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; -import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants.DriveConstants; import frc.robot.commands.*; -import frc.robot.commands.auto.AutoFactory; +import frc.robot.commands.auto.ShooterAutoCommand; import frc.robot.subsystems.arm.*; import frc.robot.subsystems.climber.ClimberIO; import frc.robot.subsystems.climber.ClimberIOKraken; @@ -69,7 +65,7 @@ public class RobotContainer { private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs - private final AutoFactory m_autonFactory = new AutoFactory(); +// private final AutoFactory m_autonFactory = new AutoFactory(); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -201,12 +197,9 @@ private void configureButtonBindings() { () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) ))); - ampLineupTrigger.whileTrue(new AlignmentDriveCommand(m_driveSubsystem, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> -controller.getRightX(), - () -> FieldConstants.AMP_LINEUP) - .andThen(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); + ampLineupTrigger.whileTrue(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP) + .finallyDo(() -> m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AMP))) + .onFalse(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.STOW)); ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), () -> m_shooter.setKickerPower(0.0), @@ -216,10 +209,7 @@ private void configureButtonBindings() { controller.rightBumper().whileTrue(m_climber.setClimberPowerFactory(0.25)); controller.leftBumper().whileTrue(m_climber.setClimberPowerFactory(-0.25)); - controller.pov(0).onTrue(Commands.runOnce(SignalLogger::stop)); - controller.pov(180).whileTrue(m_driveSubsystem.pathfollowFactory( - FieldConstants.AMP_LINEUP - )); + controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); m_driveSubsystem.setDefaultCommand( DriveCommands.joystickDrive( @@ -246,7 +236,7 @@ private void configureNamedCommands() { NamedCommands.registerCommand("Intake", m_shooter.intakeCommand(0.75, 0.5, 0.1) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); - NamedCommands.registerCommand("AimAndShoot", new ShooterAutoCommand(m_armSubsystem, m_shooter) + NamedCommands.registerCommand("AimAndShoot", new ShooterAutoCommand(m_armSubsystem, m_shooter, m_driveSubsystem) .raceWith(DriveCommands.alignmentDrive( m_driveSubsystem, () -> 0.0, @@ -268,6 +258,6 @@ private void configureDashboard() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return m_autonFactory.getSelectedAutonomous(); + return new InstantCommand(); } } diff --git a/src/main/java/frc/robot/commands/AlignmentDriveCommand.java b/src/main/java/frc/robot/commands/AlignmentDriveCommand.java index f97c1b88..bd47a887 100644 --- a/src/main/java/frc/robot/commands/AlignmentDriveCommand.java +++ b/src/main/java/frc/robot/commands/AlignmentDriveCommand.java @@ -7,20 +7,22 @@ import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants.DriveConstants; +import frc.robot.subsystems.arm.ArmSubsystem; import frc.robot.subsystems.drive.DriveSubsystem; import lib.utils.AllianceFlipUtil; import java.util.function.DoubleSupplier; import java.util.function.Supplier; -import static frc.robot.commands.DriveCommands.DEADBAND; - public class AlignmentDriveCommand extends Command { + private static final double DEADBAND = 0.25; private final DriveSubsystem driveSubsystem; + private final ArmSubsystem armSubsystem; private final Supplier pointSupplier; private Command alignmentCommand; @@ -28,23 +30,25 @@ public class AlignmentDriveCommand extends Command { private final DoubleSupplier ySupplier; private final DoubleSupplier thetaSupplier; + private boolean reinit = false; + public AlignmentDriveCommand(DriveSubsystem driveSubsystem, + ArmSubsystem armSubsystem, DoubleSupplier xSupplier, DoubleSupplier ySupplier, DoubleSupplier thetaSupplier, Supplier point) { this.driveSubsystem = driveSubsystem; + this.armSubsystem = armSubsystem; this.pointSupplier = point; - - this.xSupplier = xSupplier; this.ySupplier =ySupplier; this.thetaSupplier = thetaSupplier; // each subsystem used by the command must be passed into the // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.driveSubsystem); + addRequirements(this.driveSubsystem, this.armSubsystem); } @Override @@ -99,6 +103,9 @@ public void execute() { heading = driveSubsystem.getRotation(); } + // Mark that we should re-check auto alignment +// reinit = true; + // Convert to field relative speeds & send command driveSubsystem.runVelocity( ChassisSpeeds.fromFieldRelativeSpeeds( @@ -107,17 +114,31 @@ public void execute() { omega * driveSubsystem.getMaxAngularSpeedRadPerSec(), heading)); } else { + // if we've driven in the last loop cycle recheck auto alignment, and we're not too close to our goal + if (reinit + && driveSubsystem.getVisionPose().getTranslation().getDistance(pointSupplier.get().getTranslation()) + > Units.inchesToMeters(5.0)) { + alignmentCommand.initialize(); + } + alignmentCommand.execute(); } + + // if we've finished auto aligning move arm to amp + if (alignmentCommand.isFinished()) { + armSubsystem.setDesiredState(ArmSubsystem.ArmState.AMP); + } } @Override public boolean isFinished() { - return alignmentCommand.isFinished(); + // finishing is handled by the "whileTrue" trigger method + return false; } @Override public void end(boolean interrupted) { alignmentCommand.end(interrupted); + armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 555d427b..b4ae68db 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -26,7 +26,7 @@ import java.util.function.Supplier; public class DriveCommands { - public static final double DEADBAND = 0.1; + private static final double DEADBAND = 0.1; private DriveCommands() {} diff --git a/src/main/java/frc/robot/commands/auto/AutoFactory.java b/src/main/java/frc/robot/commands/auto/AutoFactory.java index 50cfe43e..69d25819 100644 --- a/src/main/java/frc/robot/commands/auto/AutoFactory.java +++ b/src/main/java/frc/robot/commands/auto/AutoFactory.java @@ -26,11 +26,11 @@ public enum AutoModes { private final LoggedDashboardChooser m_autonChooser; - private final EnumMap m_modes; + private final HashMap m_modes; public AutoFactory() { m_autonChooser = new LoggedDashboardChooser<>("Auto Chooser"); - m_modes = new EnumMap<>(new HashMap<>()); + m_modes = new HashMap<>(); for (AutoModes mode : AutoModes.values()) { if (mode == DEFAULT_MODE) { diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java index 4b37ddad..e465eddb 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmIOKraken.java @@ -82,8 +82,11 @@ public ArmIOKraken() { armConfig.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RotorSensor; armConfig.Feedback.SensorToMechanismRatio = ArmConstants.ARM_SENSOR_MECHANISM_RATIO; - m_armMaster = TalonFXFactory.createTalon(ArmConstants.ARM_MASTER_ID, armConfig); - m_armFollower = TalonFXFactory.createTalon(ArmConstants.ARM_FOLLOWER_ID, armConfig); + m_armMaster = new TalonFX(ArmConstants.ARM_MASTER_ID, CANBUS); + m_armFollower = new TalonFX(ArmConstants.ARM_FOLLOWER_ID, CANBUS); + + m_armMaster.getConfigurator().apply(armConfig); + m_armFollower.getConfigurator().apply(armConfig); // Wrist Configuration TalonFXConfiguration wristConfig = new TalonFXConfiguration(); @@ -280,10 +283,12 @@ public void resetPosition() { m_armMaster.setPosition( (m_armEncoder.getAbsolutePosition().getValueAsDouble() - Units.degreesToRotations(ArmConstants.OFFSET_NUDGE)) / ArmConstants.ARM_CANCODER_MECHANISM_RATIO); + m_armFollower.setPosition(m_armMaster.getPosition().getValueAsDouble()); m_wristMaster.setPosition( (m_wristEncoder.getAbsolutePosition().getValueAsDouble() - Units.degreesToRotations(ArmConstants.OFFSET_NUDGE)) / ArmConstants.WRIST_CANCODER_MECHANISM_RATIO); + m_wristFollower.setPosition(m_wristMaster.getPosition().getValueAsDouble()); } @Override diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 26ac4737..19aa6c6f 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -38,6 +38,7 @@ public class AimbotUtils { // m_rightSpeedLerpTable.put(Units.inchesToMeters(229.0), 0.0); } + /** Linear interpolation tables for aiming */ public static double getWristAngle(double distance) { return m_angleLerpTable.get(distance); } @@ -50,6 +51,22 @@ public static double getRightSpeed(double distance) { return m_rightSpeedLerpTable.get(distance); } + /** Gets the distance from the drivebase to the speaker in meters */ + public static double getDistanceFromSpeaker(Pose2d drivePose) { + return FieldConstants.CENTER_SPEAKER.toTranslation2d().getDistance(drivePose.getTranslation()); + } + + /** Gets the angle the drivebase should be at to aim at the speaker */ + public static Rotation2d getDrivebaseAimingAngle(Pose2d drivePose) { + Transform3d robotToPoint = + new Transform3d( + new Pose3d(new Pose2d(drivePose.getTranslation(), new Rotation2d())), + new Pose3d(FieldConstants.CENTER_SPEAKER, new Rotation3d())); + + return new Rotation2d(Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY())) + + Units.degreesToRadians(90)); + } + /** Gets the top point of the shooter for checking limits*/ public static Translation2d calculateArmPosition(double armAngle, double wristAngle) { return ArmConstants.PIVOT_JOINT_TRANSLATION From 218527a13cabacd5aa615d57ad7c7a85d240aa7a Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 20:02:44 -0400 Subject: [PATCH 12/30] moved some files to auto --- .../robot/commands/ShooterAutoCommand.java | 41 ---------- .../{ => auto}/IntakeControlCommand.java | 2 +- .../commands/auto/ShooterAutoCommand.java | 77 +++++++++++++++++++ 3 files changed, 78 insertions(+), 42 deletions(-) delete mode 100644 src/main/java/frc/robot/commands/ShooterAutoCommand.java rename src/main/java/frc/robot/commands/{ => auto}/IntakeControlCommand.java (98%) create mode 100644 src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java diff --git a/src/main/java/frc/robot/commands/ShooterAutoCommand.java b/src/main/java/frc/robot/commands/ShooterAutoCommand.java deleted file mode 100644 index cde298a7..00000000 --- a/src/main/java/frc/robot/commands/ShooterAutoCommand.java +++ /dev/null @@ -1,41 +0,0 @@ -package frc.robot.commands; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.arm.ArmSubsystem; -import frc.robot.subsystems.shooter.ShooterSubsystem; - - -public class ShooterAutoCommand extends Command { - private final ArmSubsystem armSubsystem; - private final ShooterSubsystem shooterSubsystem; - - public ShooterAutoCommand(ArmSubsystem armSubsystem, ShooterSubsystem shooterSubsystem) { - this.armSubsystem = armSubsystem; - this.shooterSubsystem = shooterSubsystem; - // each subsystem used by the command must be passed into the - // addRequirements() method (which takes a vararg of Subsystem) - addRequirements(this.armSubsystem, this.shooterSubsystem); - } - - @Override - public void initialize() { - - } - - @Override - public void execute() { - shooterSubsystem.runShooterVelocity(shooterSubsystem.atSpeed()).execute(); - armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM); - } - - @Override - public boolean isFinished() { - return !shooterSubsystem.hasPiece(); - } - - @Override - public void end(boolean interrupted) { - shooterSubsystem.setShooterPowerFactory(0.0, 0.0, 0.0).execute(); - armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); - } -} diff --git a/src/main/java/frc/robot/commands/IntakeControlCommand.java b/src/main/java/frc/robot/commands/auto/IntakeControlCommand.java similarity index 98% rename from src/main/java/frc/robot/commands/IntakeControlCommand.java rename to src/main/java/frc/robot/commands/auto/IntakeControlCommand.java index 630aff06..3deaf298 100644 --- a/src/main/java/frc/robot/commands/IntakeControlCommand.java +++ b/src/main/java/frc/robot/commands/auto/IntakeControlCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands; +package frc.robot.commands.auto; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java new file mode 100644 index 00000000..3141628d --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java @@ -0,0 +1,77 @@ +package frc.robot.commands.auto; + +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation3d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.arm.ArmSubsystem; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; +import lib.utils.FieldConstants; + + +public class ShooterAutoCommand extends Command { + private final ArmSubsystem m_armSubsystem; + private final ShooterSubsystem m_shooterSubsystem; + private final DriveSubsystem m_driveSubsystem; + + private final Timer m_timer; + + private boolean m_previousHadPiece; + private boolean m_hasChanged; + + public ShooterAutoCommand(ArmSubsystem armSubsystem, + ShooterSubsystem shooterSubsystem, + DriveSubsystem driveSubsystem) { + this.m_armSubsystem = armSubsystem; + this.m_shooterSubsystem = shooterSubsystem; + this.m_driveSubsystem = driveSubsystem; + + m_timer = new Timer(); + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.m_armSubsystem, this.m_shooterSubsystem, driveSubsystem); + } + + @Override + public void initialize() { + m_timer.restart(); + m_previousHadPiece = m_shooterSubsystem.hasPiece(); + } + + @Override + public void execute() { + // Calculate output to align to speaker + double omega = m_driveSubsystem.alignToPoint(new Pose3d(FieldConstants.CENTER_SPEAKER, new Rotation3d())); + m_driveSubsystem.runVelocity(new ChassisSpeeds(0.0, 0.0, omega)); + + // only actually shoot if we're aligned close enough to speaker and flywheels are at speed + m_shooterSubsystem.runShooterVelocity(m_shooterSubsystem.atSpeed() + && omega < 1).execute(); + m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM); + + // check to see if the state of having a note has changed, mark if it has + if (m_previousHadPiece != m_shooterSubsystem.hasPiece()) { + m_hasChanged = true; + } + + m_previousHadPiece = m_shooterSubsystem.hasPiece(); + } + + @Override + public boolean isFinished() { + // if magazine is empty, we've detected a note has run through the system, and we're not taking too + // short or too long to run + + // too long check may change as system gets tuned + return (!m_shooterSubsystem.hasPiece() && m_hasChanged + && m_timer.hasElapsed(0.5)) || m_timer.hasElapsed(5.0); + } + + @Override + public void end(boolean interrupted) { + m_shooterSubsystem.setShooterPowerFactory(0.0, 0.0, 0.0).execute(); + m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); + } +} From c35367b2b8ac0128f89309e6811722ab9ba92da7 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Tue, 12 Mar 2024 21:52:41 -0400 Subject: [PATCH 13/30] setup data to log with Dan --- src/main/java/frc/robot/Constants.java | 10 +++++----- src/main/java/frc/robot/RobotContainer.java | 15 +++++++------- .../frc/robot/commands/DriveCommands.java | 3 ++- .../commands/auto/ShooterAutoCommand.java | 3 ++- .../robot/subsystems/arm/ArmSubsystem.java | 9 ++++++++- .../subsystems/drive/DriveSubsystem.java | 20 +++++++++---------- .../subsystems/shooter/ShooterSubsystem.java | 20 +++++++++++-------- src/main/java/lib/utils/AimbotUtils.java | 15 ++++++++++++-- src/main/java/lib/utils/FieldConstants.java | 8 ++++---- 9 files changed, 63 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5cf079f8..7bfa9caf 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -107,14 +107,14 @@ private DriveConstants() { ); public static final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( - MAX_LINEAR_SPEED * 0.5, - MAX_LINEAR_SPEED * 0.25, - MAX_ANGULAR_SPEED * 0.5, - MAX_ANGULAR_SPEED * 0.25 + MAX_LINEAR_SPEED * 0.75, + MAX_LINEAR_SPEED * 0.45, + MAX_ANGULAR_SPEED * 0.75, + MAX_ANGULAR_SPEED * 0.45 ); public static final HolonomicPathFollowerConfig HOLONOMIC_CONFIG = new HolonomicPathFollowerConfig( - new PIDConstants(4.5, 0.75), new PIDConstants(4.5, 0.75), + new PIDConstants(4.0, 0.0), new PIDConstants(4.0, 0.0), DriveConstants.MAX_LINEAR_SPEED * 0.5, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig()); public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants( diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 934d99d3..6c28dc30 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,6 +47,7 @@ import frc.robot.subsystems.vision.VisionSubsystem; import lib.utils.AllianceFlipUtil; import lib.utils.FieldConstants; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -66,6 +67,8 @@ public class RobotContainer { // Dashboard inputs // private final AutoFactory m_autonFactory = new AutoFactory(); + private final LoggedDashboardNumber m_leftRPM = new LoggedDashboardNumber("Shooter/LeftRPM", 3600); + private final LoggedDashboardNumber m_rightRPM = new LoggedDashboardNumber("Shooter/RightRPM", 3600); /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { @@ -209,7 +212,9 @@ private void configureButtonBindings() { controller.rightBumper().whileTrue(m_climber.setClimberPowerFactory(0.25)); controller.leftBumper().whileTrue(m_climber.setClimberPowerFactory(-0.25)); - controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); + controller.pov(180).whileTrue( + m_shooter.runShooterVelocity(true, m_leftRPM.get(), m_rightRPM.get()) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST))); m_driveSubsystem.setDefaultCommand( DriveCommands.joystickDrive( @@ -236,13 +241,7 @@ private void configureNamedCommands() { NamedCommands.registerCommand("Intake", m_shooter.intakeCommand(0.75, 0.5, 0.1) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); - NamedCommands.registerCommand("AimAndShoot", new ShooterAutoCommand(m_armSubsystem, m_shooter, m_driveSubsystem) - .raceWith(DriveCommands.alignmentDrive( - m_driveSubsystem, - () -> 0.0, - () -> 0.0, - () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) - ))); + NamedCommands.registerCommand("AimAndShoot", new ShooterAutoCommand(m_armSubsystem, m_shooter, m_driveSubsystem)); } private void configureDashboard() { diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index b4ae68db..1d569f3e 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import frc.robot.subsystems.drive.DriveSubsystem; +import lib.utils.AimbotUtils; import org.opencv.core.Mat; import java.util.function.DoubleSupplier; @@ -96,7 +97,7 @@ public static Command alignmentDrive( new Rotation2d(xInput, yInput); // Calculate omega - double omega = driveSubsystem.alignToPoint(new Pose3d(point.get(), new Rotation3d())); + double omega = driveSubsystem.alignToAngle(AimbotUtils.getDrivebaseAimingAngle(driveSubsystem.getVisionPose())); // Calcaulate new linear velocity Translation2d linearVelocity = diff --git a/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java index 3141628d..319d7053 100644 --- a/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java @@ -8,6 +8,7 @@ import frc.robot.subsystems.arm.ArmSubsystem; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; +import lib.utils.AimbotUtils; import lib.utils.FieldConstants; @@ -43,7 +44,7 @@ public void initialize() { @Override public void execute() { // Calculate output to align to speaker - double omega = m_driveSubsystem.alignToPoint(new Pose3d(FieldConstants.CENTER_SPEAKER, new Rotation3d())); + double omega = m_driveSubsystem.alignToAngle(AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose())); m_driveSubsystem.runVelocity(new ChassisSpeeds(0.0, 0.0, omega)); // only actually shoot if we're aligned close enough to speaker and flywheels are at speed diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 3175eb3a..8477e485 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -30,7 +30,8 @@ public enum ArmState { TRANSITION_AMP, SOURCE, TRANSITION_SOURCE, - DISABLED + DISABLED, + MANUAL_WRIST } private final ArmIO m_io; @@ -170,6 +171,12 @@ public void handleState() { m_desiredArmPoseDegs = ArmSetpoints.AMP_SETPOINT.armAngle(); m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristAngle(); } + case MANUAL_WRIST -> { + m_desiredWristPoseDegs = ArmSetpoints.WRIST_ANGLE.getValue(); + + m_desiredArmPoseDegs = ArmConstants.WRIST_ARM_GAP.getValue() - m_desiredWristPoseDegs; + m_desiredArmPoseDegs = m_desiredArmPoseDegs >= 0 ? m_desiredArmPoseDegs : 0; + } default -> { m_armVelocityMult = 1.0; m_wristVelocityMult = 1.0; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index c42c1e93..52647205 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -52,6 +52,7 @@ import lib.logger.DataLogUtil; import lib.logger.DataLogUtil.DataLogTable; +import lib.utils.AimbotUtils; import lib.utils.AllianceFlipUtil; import lib.utils.LocalADStarAK; import lib.utils.PoseEstimator; @@ -282,6 +283,11 @@ public void periodic() { m_wpiPoseEstimator.updateWithTime(Timer.getFPGATimestamp(), gyroInputs.yawPosition, getModulePositions()); m_thetaPidProperty.updateIfChanged(); + Logger.recordOutput("Drive/DistanceToTarget", + Units.inchesToMeters(AimbotUtils.getDistanceFromSpeaker(getVisionPose()))); + Logger.recordOutput("Drive/AngleToTarget", + AimbotUtils.getDrivebaseAimingAngle(getVisionPose()).getDegrees()); + m_field.setRobotPose(getVisionPose()); } @@ -307,18 +313,12 @@ public void runVelocity(ChassisSpeeds speeds) { } /** - * Calculates theta output to align to an arbitrary point + * Calculates theta output to align to an arbitrary angle * - * @param point the desired point to align to + * @param angle the desired angle to hold relative to the field */ - public double alignToPoint(Pose3d point) { - Transform3d robotToPoint = new Transform3d(new Pose3d( - new Pose2d(m_wpiPoseEstimator.getEstimatedPosition().getTranslation(), new Rotation2d())), point); - - double desiredRotation = Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY())) - + Units.degreesToRadians(90); - - return m_thetaPid.calculate(m_avgRotationRads.getRadians(), desiredRotation); + public double alignToAngle(Rotation2d angle) { + return m_thetaPid.calculate(getVisionPose().getRotation().getRadians(), angle.getRadians()); } /** Stops the drive. */ diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 5c7e9b95..b641e5a1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -73,16 +73,20 @@ public void setIntakePower(double power) { } public Command runShooterVelocity(boolean runKicker) { - return runEnd(() -> { -// m_io.setLeftVelocityRpm(m_leftPower.getValue()); -// m_io.setRightVelocityRpm(m_rightPower.getValue()); + Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d()); + Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d(); + double groundDistance = m_poseSupplier.get().getTranslation().getDistance(speakerPoseGround); + + m_leftSpeedSetpoint = AimbotUtils.getLeftSpeed(groundDistance); + m_rightSpeedSetpoint = AimbotUtils.getRightSpeed(groundDistance); - Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d()); - Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d(); - double groundDistance = m_poseSupplier.get().getTranslation().getDistance(speakerPoseGround); + return runShooterVelocity(runKicker, m_leftSpeedSetpoint, m_rightSpeedSetpoint); + } - m_leftSpeedSetpoint = AimbotUtils.getLeftSpeed(groundDistance); - m_rightSpeedSetpoint = AimbotUtils.getRightSpeed(groundDistance); + public Command runShooterVelocity(boolean runKicker, double leftRPM, double rightRPM) { + return runEnd(() -> { + m_leftSpeedSetpoint = leftRPM; + m_rightSpeedSetpoint = rightRPM; m_io.setLeftVelocityRpm(m_leftSpeedSetpoint); m_io.setRightVelocityRpm(m_rightSpeedSetpoint); diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 19aa6c6f..92fe1a31 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -61,10 +61,21 @@ public static Rotation2d getDrivebaseAimingAngle(Pose2d drivePose) { Transform3d robotToPoint = new Transform3d( new Pose3d(new Pose2d(drivePose.getTranslation(), new Rotation2d())), - new Pose3d(FieldConstants.CENTER_SPEAKER, new Rotation3d())); + new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d())); - return new Rotation2d(Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY())) + Rotation2d angle = Rotation2d.fromRadians(Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY())) + Units.degreesToRadians(90)); + + // normalize rotation between +/-180 degrees + while (angle.getDegrees() > 180) { + angle = Rotation2d.fromDegrees(angle.getDegrees() - 180.0); + } + + while (angle.getDegrees() < 180) { + angle = Rotation2d.fromDegrees(angle.getDegrees() + 180.0); + } + + return angle; } /** Gets the top point of the shooter for checking limits*/ diff --git a/src/main/java/lib/utils/FieldConstants.java b/src/main/java/lib/utils/FieldConstants.java index 491efc94..500c8c42 100644 --- a/src/main/java/lib/utils/FieldConstants.java +++ b/src/main/java/lib/utils/FieldConstants.java @@ -32,7 +32,7 @@ private FieldConstants() {} public static final Pose2d AMP_LINEUP = AllianceFlipUtil.apply(new Pose2d( AMP_CENTER.getX(), - AMP_CENTER.getY() - Units.inchesToMeters(10), + AMP_CENTER.getY() - Units.inchesToMeters(18.5), Rotation2d.fromDegrees(90) )); @@ -93,9 +93,9 @@ public static final class Speaker { public static final Translation3d CENTER_SPEAKER = new Translation3d( - (Units.inchesToMeters(18.055) / 2.0) - Units.inchesToMeters(2.0), - Units.inchesToMeters((238.815 + 197.765) / 2.0), - (Units.inchesToMeters(83.091 + 78.324) / 2.0) - Units.inchesToMeters(1.0)); + 0.0, // - Units.inchesToMeters(2.0), + FIELD_WIDTH - Units.inchesToMeters(104.0), + (Units.inchesToMeters(83.091 + 78.324) / 2.0));// - Units.inchesToMeters(1.0)); public static final double APRIL_TAG_WIDTH = Units.inchesToMeters(6.50); public static final AprilTagFieldLayout APRIL_TAG_FIELD_LAYOUT; From 39d179680f6c3855a881ed156bbe6c49b7920523 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Wed, 13 Mar 2024 00:13:29 -0400 Subject: [PATCH 14/30] check aimbot util and manual antidefense setpoint --- src/main/java/frc/robot/RobotContainer.java | 14 ++++++++++---- .../frc/robot/subsystems/arm/ArmSubsystem.java | 8 ++++++-- .../frc/robot/subsystems/drive/DriveSubsystem.java | 2 +- src/main/java/lib/utils/AimbotUtils.java | 2 +- 4 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6c28dc30..7ea8832f 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -209,12 +209,18 @@ private void configureButtonBindings() { m_shooter) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); - controller.rightBumper().whileTrue(m_climber.setClimberPowerFactory(0.25)); - controller.leftBumper().whileTrue(m_climber.setClimberPowerFactory(-0.25)); +// controller.rightBumper().whileTrue(m_climber.setClimberPowerFactory(0.25)); +// controller.leftBumper().whileTrue(m_climber.setClimberPowerFactory(-0.25)); - controller.pov(180).whileTrue( + controller.leftBumper().whileTrue( + m_shooter.runShooterVelocity(false, m_leftRPM.get(), m_rightRPM.get()) + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE))); + + controller.rightBumper().whileTrue( m_shooter.runShooterVelocity(true, m_leftRPM.get(), m_rightRPM.get()) - .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST))); + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE))); + + controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); m_driveSubsystem.setDefaultCommand( DriveCommands.joystickDrive( diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 8477e485..447d6aa0 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -125,8 +125,8 @@ public void periodic() { public void handleState() { switch(m_desiredState) { case STOW -> { - if (m_inputs.armPositionDegs > 67.5 && m_currentState == ArmState.AMP) { - m_wristVelocityMult = 0.35; + if (m_inputs.armPositionDegs > 60.0 && m_currentState == ArmState.AMP) { + m_wristVelocityMult = 0.15; m_armVelocityMult = 1.0; } else { m_currentState = ArmState.STOW; @@ -150,6 +150,10 @@ public void handleState() { m_desiredArmPoseDegs = ArmConstants.WRIST_ARM_GAP.getValue() - m_desiredWristPoseDegs; m_desiredArmPoseDegs = m_desiredArmPoseDegs >= 0 ? m_desiredArmPoseDegs : 0; } + case ANTI_DEFENSE -> { + m_desiredArmPoseDegs = 45.0; + m_desiredWristPoseDegs = 35.0; + } case INTAKE -> { m_armVelocityMult = 1.0; m_wristVelocityMult = 1.0; diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 52647205..0b65e7ac 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -284,7 +284,7 @@ public void periodic() { m_thetaPidProperty.updateIfChanged(); Logger.recordOutput("Drive/DistanceToTarget", - Units.inchesToMeters(AimbotUtils.getDistanceFromSpeaker(getVisionPose()))); + Units.metersToInches(AimbotUtils.getDistanceFromSpeaker(getVisionPose()))); Logger.recordOutput("Drive/AngleToTarget", AimbotUtils.getDrivebaseAimingAngle(getVisionPose()).getDegrees()); diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 92fe1a31..fe34cf16 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -53,7 +53,7 @@ public static double getRightSpeed(double distance) { /** Gets the distance from the drivebase to the speaker in meters */ public static double getDistanceFromSpeaker(Pose2d drivePose) { - return FieldConstants.CENTER_SPEAKER.toTranslation2d().getDistance(drivePose.getTranslation()); + return AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER).toTranslation2d().getDistance(drivePose.getTranslation()); } /** Gets the angle the drivebase should be at to aim at the speaker */ From e36fb61c9e11c64978e4edc9b29ca9d7a183aa85 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Wed, 13 Mar 2024 14:49:33 -0400 Subject: [PATCH 15/30] static arm and wrist angle for shooting test --- src/main/java/frc/robot/Constants.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 13 +++++++------ .../java/frc/robot/subsystems/arm/ArmSubsystem.java | 4 ++-- 3 files changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 7bfa9caf..a740f88e 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -242,7 +242,7 @@ private ArmSetpoints() { public static final ArmPose AMP_SETPOINT = new ArmPose("ArmPoses/Amp", true, 94.0, 145.0); - public static final ArmPose STATIC_SHOOTER = new ArmPose(0.0, 55.0); + public static final ArmPose STATIC_SHOOTER = new ArmPose("ArmPoses/ShooterTesting", false, 0.0, 55.0); public static final GosDoubleProperty WRIST_ANGLE = new GosDoubleProperty(false, "Wrist Angle", 45.0); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 7ea8832f..b067c86a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -29,6 +29,7 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.DriveConstants; import frc.robot.commands.*; +import frc.robot.commands.auto.AutoFactory; import frc.robot.commands.auto.ShooterAutoCommand; import frc.robot.subsystems.arm.*; import frc.robot.subsystems.climber.ClimberIO; @@ -66,7 +67,7 @@ public class RobotContainer { private final CommandXboxController controller = new CommandXboxController(0); // Dashboard inputs -// private final AutoFactory m_autonFactory = new AutoFactory(); + private final AutoFactory m_autonFactory; private final LoggedDashboardNumber m_leftRPM = new LoggedDashboardNumber("Shooter/LeftRPM", 3600); private final LoggedDashboardNumber m_rightRPM = new LoggedDashboardNumber("Shooter/RightRPM", 3600); @@ -147,6 +148,8 @@ public RobotContainer() { configureNamedCommands(); configureDashboard(); + m_autonFactory = new AutoFactory(); + // Configure the button bindings configureButtonBindings(); } @@ -201,16 +204,14 @@ private void configureButtonBindings() { ))); ampLineupTrigger.whileTrue(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP) - .finallyDo(() -> m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AMP))) - .onFalse(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.STOW)); + .finallyDo(() -> m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP).schedule())) + .whileFalse(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.STOW)); ampDepositeTrigger.whileTrue(Commands.runEnd(() -> m_shooter.setKickerPower(-0.5), () -> m_shooter.setKickerPower(0.0), m_shooter) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP))); -// controller.rightBumper().whileTrue(m_climber.setClimberPowerFactory(0.25)); -// controller.leftBumper().whileTrue(m_climber.setClimberPowerFactory(-0.25)); controller.leftBumper().whileTrue( m_shooter.runShooterVelocity(false, m_leftRPM.get(), m_rightRPM.get()) @@ -263,6 +264,6 @@ private void configureDashboard() { * @return the command to run in autonomous */ public Command getAutonomousCommand() { - return new InstantCommand(); + return m_autonFactory.getSelectedAutonomous(); } } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 447d6aa0..a9acea88 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -176,10 +176,10 @@ public void handleState() { m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristAngle(); } case MANUAL_WRIST -> { - m_desiredWristPoseDegs = ArmSetpoints.WRIST_ANGLE.getValue(); + m_desiredWristPoseDegs = ArmSetpoints.STATIC_SHOOTER.wristAngle(); m_desiredArmPoseDegs = ArmConstants.WRIST_ARM_GAP.getValue() - m_desiredWristPoseDegs; - m_desiredArmPoseDegs = m_desiredArmPoseDegs >= 0 ? m_desiredArmPoseDegs : 0; + m_desiredArmPoseDegs = Math.max(m_desiredArmPoseDegs, ArmSetpoints.STATIC_SHOOTER.armAngle()); } default -> { m_armVelocityMult = 1.0; From 770abd148456e1c06d5f7f3ae137e4b87fee3554 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Wed, 13 Mar 2024 15:02:10 -0400 Subject: [PATCH 16/30] removed uneeded logging method --- .../robot/subsystems/arm/ArmSubsystem.java | 21 ------------------- 1 file changed, 21 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index a9acea88..7ab66130 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -266,26 +266,5 @@ public Command setWristPowerFactory(double power) { }, () -> m_io.setWristVoltage(0.0)); } - - /** Logging util */ - public void setupLogging() { - logUtil.addDouble("ArmAngleDegs", () -> m_inputs.armPositionDegs, true); - logUtil.addDouble("WristAngleDegs", () -> m_inputs.wristPositionDegs, true); - - logUtil.addDouble("ArmSetpointDegs", () -> m_desiredArmPoseDegs, true); - logUtil.addDouble("WristSetpointDegs", () -> m_desiredWristPoseDegs, true); - - logUtil.addDouble("ArmAppliedOutput", () -> m_inputs.armAppliedOutput, false); - logUtil.addDouble("WristAppliedOutput", () -> m_inputs.wristAppliedOutput, false); - - logUtil.addDouble("ArmClosedLoopOutput", () -> m_inputs.armClosedLoopOutput, false); - logUtil.addDouble("WristClosedLoopOutput", () -> m_inputs.wristClosedLoopOutput, false); - - logUtil.addDouble("ArmAppliedOutput", () -> m_inputs.armAppliedOutput, false); - logUtil.addDouble("WristAppliedOutput", () -> m_inputs.wristAppliedOutput, false); - - logUtil.addDouble("WristArmGap", () -> m_wristGap, true); - logUtil.addString("Arm Current State", () -> m_desiredState.toString(), true); - } } From fb38b3728cce6006f71de22cc73b2374fe0c6156 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Wed, 13 Mar 2024 22:47:30 -0400 Subject: [PATCH 17/30] more data collection, rough distance->wrist model --- src/main/java/frc/robot/Constants.java | 16 ++++++------ src/main/java/frc/robot/Robot.java | 2 +- src/main/java/frc/robot/RobotContainer.java | 4 +-- .../subsystems/drive/DriveSubsystem.java | 14 +++++------ .../subsystems/shooter/ShooterSubsystem.java | 12 +++++---- .../subsystems/vision/VisionSubsystem.java | 4 +-- src/main/java/lib/utils/AimbotUtils.java | 25 ++++++++----------- 7 files changed, 37 insertions(+), 40 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index a740f88e..6d3aea16 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -88,22 +88,22 @@ private DriveConstants() { new Translation3d( Units.inchesToMeters(11.0351), // 11.0351 Units.inchesToMeters(10.023204 - 2.0), // 10.023204 - Units.inchesToMeters(7.1374 - 3.5)), // 4.1374 + Units.inchesToMeters(7.1374 - 2.0)), // 4.1374 new Rotation3d( - Units.degreesToRadians(0.0 + 4.0), - Units.degreesToRadians(-30.0 - 1.0), // -120.0 + 91.0 - Units.degreesToRadians(-14.7 - 6.5)) // 165.3224 + 180 + Units.degreesToRadians(0.0 + 1.0), + Units.degreesToRadians(-30.0 + 4.0), // -120.0 + 91.0 + Units.degreesToRadians(-14.7 - 3.0)) // 165.3224 + 180 ); public static final Transform3d RIGHT_CAMERA_TRANSFORMATION = new Transform3d( new Translation3d( Units.inchesToMeters(11.0351), //11.0351 Units.inchesToMeters(-10.023204), //-10.023204 - Units.inchesToMeters(7.1374 - 3.5)), // 7.1374 + Units.inchesToMeters(7.1374 - 0.0)), // 7.1374 new Rotation3d( - Units.degreesToRadians(0.0 + 7.0), - Units.degreesToRadians(-30.0 + 0.85), // -30.0 - 1 - Units.degreesToRadians(14.7 - 12.5)) // 165.3224) + Units.degreesToRadians(0.0 + 0.5), + Units.degreesToRadians(-30.0 - 2.5), // -30.0 - 1 + Units.degreesToRadians(14.7 - 3.0)) // 165.3224) ); public static final PathConstraints DEFAULT_CONSTRAINTS = new PathConstraints( diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 63182ded..05bef1f0 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -114,7 +114,7 @@ public void robotPeriodic() { // This must be called from the robot's periodic block in order for anything in // the Command-based framework to work. CommandScheduler.getInstance().run(); - TalonFXFactory.handleFaults(); +// TalonFXFactory.handleFaults(); // DataLogUtil.updateTables(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b067c86a..cc1dda4e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -215,11 +215,11 @@ private void configureButtonBindings() { controller.leftBumper().whileTrue( m_shooter.runShooterVelocity(false, m_leftRPM.get(), m_rightRPM.get()) - .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE))); + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST))); controller.rightBumper().whileTrue( m_shooter.runShooterVelocity(true, m_leftRPM.get(), m_rightRPM.get()) - .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE))); + .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST))); controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 0b65e7ac..d5f7e606 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -157,7 +157,7 @@ public DriveSubsystem( VecBuilder.fill( Units.inchesToMeters(2.0), Units.inchesToMeters(2.0), - Units.degreesToRadians(25.0)) + Units.degreesToRadians(30.0)) ); // Configure AutoBuilder for PathPlanner @@ -267,10 +267,10 @@ public void periodic() { } // make sure we're not moving too fast before trying to update vision poses - if ((kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond <= Units.inchesToMeters(30)) - && (kinematics.toChassisSpeeds(getModuleStates()).vyMetersPerSecond <= Units.inchesToMeters(30)) - && (kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond <= Units.degreesToRadians(60)) - || DriverStation.isTeleop()) { + if ((kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond <= DriveConstants.MAX_LINEAR_SPEED / 2.0) + && (kinematics.toChassisSpeeds(getModuleStates()).vyMetersPerSecond <= DriveConstants.MAX_LINEAR_SPEED / 2.0) + && (kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond <= DriveConstants.MAX_ANGULAR_SPEED / 2.0) + /* || DriverStation.isTeleop() */) { for (VisionSubsystem camera : m_cameras) { camera.updateInputs(); camera.getPose(m_wpiPoseEstimator.getEstimatedPosition()).ifPresent( @@ -286,7 +286,7 @@ public void periodic() { Logger.recordOutput("Drive/DistanceToTarget", Units.metersToInches(AimbotUtils.getDistanceFromSpeaker(getVisionPose()))); Logger.recordOutput("Drive/AngleToTarget", - AimbotUtils.getDrivebaseAimingAngle(getVisionPose()).getDegrees()); + -AimbotUtils.getDrivebaseAimingAngle(getVisionPose()).getDegrees()); m_field.setRobotPose(getVisionPose()); } @@ -356,7 +356,7 @@ public double getCharacterizationVelocity() { } /** Returns the module states (turn angles and drive velocities) for all the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") +// @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index b641e5a1..ad713901 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -77,10 +77,10 @@ public Command runShooterVelocity(boolean runKicker) { Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d(); double groundDistance = m_poseSupplier.get().getTranslation().getDistance(speakerPoseGround); - m_leftSpeedSetpoint = AimbotUtils.getLeftSpeed(groundDistance); - m_rightSpeedSetpoint = AimbotUtils.getRightSpeed(groundDistance); + double leftSpeedSetpoint = AimbotUtils.getLeftSpeed(groundDistance); + double rightSpeedSetpoint = AimbotUtils.getRightSpeed(groundDistance); - return runShooterVelocity(runKicker, m_leftSpeedSetpoint, m_rightSpeedSetpoint); + return runShooterVelocity(runKicker, 3750, 4500); } public Command runShooterVelocity(boolean runKicker, double leftRPM, double rightRPM) { @@ -88,8 +88,8 @@ public Command runShooterVelocity(boolean runKicker, double leftRPM, double righ m_leftSpeedSetpoint = leftRPM; m_rightSpeedSetpoint = rightRPM; - m_io.setLeftVelocityRpm(m_leftSpeedSetpoint); - m_io.setRightVelocityRpm(m_rightSpeedSetpoint); + m_io.setLeftVelocityRpm(m_leftPower.getValue()); + m_io.setRightVelocityRpm(m_rightPower.getValue()); if (runKicker) { m_io.setKickerVoltage(12.0); @@ -136,6 +136,8 @@ public Command intakeCommand(double intakePower, double kickerPower, double time () -> { setIntakePower(0.0); setKickerPower(0.0); + setShooterPowerRight(0.0); + setShooterPowerLeft(0.0); }); } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index dd6dc86b..d2acceb2 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -34,10 +34,10 @@ public class VisionSubsystem { private final String m_name; private final double xyStdDevCoefficient = Units.inchesToMeters(4.0); - private final double thetaStdDevCoefficient = Units.degreesToRadians(12.0); + private final double thetaStdDevCoefficient = Units.degreesToRadians(14.0); private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(2.0); - private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(6.0); + private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(7.0); private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index fe34cf16..ea8d0abf 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -1,5 +1,6 @@ package lib.utils; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.*; import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; import edu.wpi.first.math.util.Units; @@ -40,7 +41,8 @@ public class AimbotUtils { /** Linear interpolation tables for aiming */ public static double getWristAngle(double distance) { - return m_angleLerpTable.get(distance); +// return m_angleLerpTable.get(distance); + return 52.409 + -0.1224 * distance; } public static double getLeftSpeed(double distance) { @@ -53,7 +55,8 @@ public static double getRightSpeed(double distance) { /** Gets the distance from the drivebase to the speaker in meters */ public static double getDistanceFromSpeaker(Pose2d drivePose) { - return AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER).toTranslation2d().getDistance(drivePose.getTranslation()); + return AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER).toTranslation2d() + .getDistance(drivePose.getTranslation()); } /** Gets the angle the drivebase should be at to aim at the speaker */ @@ -63,19 +66,11 @@ public static Rotation2d getDrivebaseAimingAngle(Pose2d drivePose) { new Pose3d(new Pose2d(drivePose.getTranslation(), new Rotation2d())), new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d())); - Rotation2d angle = Rotation2d.fromRadians(Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY())) - + Units.degreesToRadians(90)); - - // normalize rotation between +/-180 degrees - while (angle.getDegrees() > 180) { - angle = Rotation2d.fromDegrees(angle.getDegrees() - 180.0); - } - - while (angle.getDegrees() < 180) { - angle = Rotation2d.fromDegrees(angle.getDegrees() + 180.0); - } - - return angle; + return Rotation2d.fromRadians( + MathUtil.angleModulus( + Math.PI * 2 - (Math.atan2(robotToPoint.getX(), robotToPoint.getY())) + + Units.degreesToRadians(90) + )); } /** Gets the top point of the shooter for checking limits*/ From 587574fc2f1846385cc701ba81992e5831bc8571 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Wed, 13 Mar 2024 23:14:28 -0400 Subject: [PATCH 18/30] initial wrist alg fix and nudge factor --- src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java | 3 ++- .../java/frc/robot/subsystems/drive/DriveSubsystem.java | 2 ++ src/main/java/lib/utils/AimbotUtils.java | 6 +++++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 7ab66130..2268c93c 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -5,6 +5,7 @@ import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; @@ -143,7 +144,7 @@ public void handleState() { Pose3d speakerPose = new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d()); Translation2d speakerPoseGround = speakerPose.getTranslation().toTranslation2d(); - double groundDistance = m_poseSupplier.get().getTranslation().getDistance(speakerPoseGround); + double groundDistance = Units.metersToInches(AimbotUtils.getDistanceFromSpeaker(m_poseSupplier.get())); m_desiredWristPoseDegs = AimbotUtils.getWristAngle(groundDistance); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index d5f7e606..43f495c5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -137,6 +137,8 @@ public DriveSubsystem( m_thetaPid = new PIDController(0.0, 0.0, 0.0); m_thetaPid.enableContinuousInput(-Math.PI, Math.PI); + m_thetaPid.setTolerance(Units.degreesToRadians(4)); + m_thetaPidProperty = new WpiPidPropertyBuilder("Drive/Theta Alignment", false, m_thetaPid) .addP(0.5) .addI(0.0) diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index ea8d0abf..b2968154 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.util.Units; import frc.robot.Constants.ArmConstants; import frc.robot.subsystems.arm.ArmPose; +import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; public class AimbotUtils { @@ -13,6 +14,9 @@ public class AimbotUtils { private static final InterpolatingDoubleTreeMap m_leftSpeedLerpTable = new InterpolatingDoubleTreeMap(); private static final InterpolatingDoubleTreeMap m_rightSpeedLerpTable = new InterpolatingDoubleTreeMap(); + private static final LoggedDashboardNumber m_offsetNudge = + new LoggedDashboardNumber("Wrist Angle Nudge", 0.015); + static { // angle measurements, meters -> degrees m_angleLerpTable.put(Units.inchesToMeters(18.0), 57.0 + 3.0); @@ -42,7 +46,7 @@ public class AimbotUtils { /** Linear interpolation tables for aiming */ public static double getWristAngle(double distance) { // return m_angleLerpTable.get(distance); - return 52.409 + -0.1224 * distance; + return 52.409 - ((0.1224 + m_offsetNudge.get()) * distance); } public static double getLeftSpeed(double distance) { From fc0299c218d059a33195664db6321c5c85d1ea47 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Wed, 13 Mar 2024 23:31:09 -0400 Subject: [PATCH 19/30] changed default nudge value and deviances + tolerences --- src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java | 4 ++-- src/main/java/lib/utils/AimbotUtils.java | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 43f495c5..19bf6a66 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -137,7 +137,7 @@ public DriveSubsystem( m_thetaPid = new PIDController(0.0, 0.0, 0.0); m_thetaPid.enableContinuousInput(-Math.PI, Math.PI); - m_thetaPid.setTolerance(Units.degreesToRadians(4)); + m_thetaPid.setTolerance(Units.degreesToRadians(6)); m_thetaPidProperty = new WpiPidPropertyBuilder("Drive/Theta Alignment", false, m_thetaPid) .addP(0.5) @@ -159,7 +159,7 @@ public DriveSubsystem( VecBuilder.fill( Units.inchesToMeters(2.0), Units.inchesToMeters(2.0), - Units.degreesToRadians(30.0)) + Units.degreesToRadians(35.0)) ); // Configure AutoBuilder for PathPlanner diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index b2968154..3e721756 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -15,7 +15,7 @@ public class AimbotUtils { private static final InterpolatingDoubleTreeMap m_rightSpeedLerpTable = new InterpolatingDoubleTreeMap(); private static final LoggedDashboardNumber m_offsetNudge = - new LoggedDashboardNumber("Wrist Angle Nudge", 0.015); + new LoggedDashboardNumber("Wrist Angle Nudge", 0.01); static { // angle measurements, meters -> degrees From 8445c258bbda0adb760bdbaf752f04347832df44 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 14 Mar 2024 11:41:48 -0400 Subject: [PATCH 20/30] update WPILib and better aimbot command --- build.gradle | 2 +- .../frc/robot/commands/AimbotCommand.java | 127 ++++++++++++++++++ 2 files changed, 128 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/AimbotCommand.java diff --git a/build.gradle b/build.gradle index 30e6c78e..771bdadb 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2024.3.1" + id "edu.wpi.first.GradleRIO" version "2024.3.2" id "org.sonarqube" version "4.4.1.3373" id "com.peterabeles.gversion" version "1.10" } diff --git a/src/main/java/frc/robot/commands/AimbotCommand.java b/src/main/java/frc/robot/commands/AimbotCommand.java new file mode 100644 index 00000000..e5685f40 --- /dev/null +++ b/src/main/java/frc/robot/commands/AimbotCommand.java @@ -0,0 +1,127 @@ +package frc.robot.commands; + +import com.gos.lib.properties.pid.PidProperty; +import com.gos.lib.properties.pid.WpiPidPropertyBuilder; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.GenericHID; +import edu.wpi.first.wpilibj.XboxController; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.Constants; +import frc.robot.subsystems.arm.ArmSubsystem; +import frc.robot.subsystems.drive.DriveSubsystem; +import frc.robot.subsystems.shooter.ShooterSubsystem; +import lib.utils.AimbotUtils; + + +public class AimbotCommand extends Command { + private final ArmSubsystem m_armSubsystem; + private final DriveSubsystem m_driveSubsystem; + private final ShooterSubsystem m_shooterSubsystem; + private final XboxController m_driverController; + + private final PIDController m_smallController; + private final PIDController m_fastController; + + private final PidProperty m_smallProperty; + private final PidProperty m_fastProperty; + + private boolean m_runKicker; + + public AimbotCommand(ArmSubsystem armSubsystem, + DriveSubsystem driveSubsystem, + ShooterSubsystem shooterSubsystem, + XboxController driverController, + boolean runKicker) { + this.m_armSubsystem = armSubsystem; + this.m_driveSubsystem = driveSubsystem; + this.m_shooterSubsystem = shooterSubsystem; + this.m_driverController = driverController; + + m_smallController = new PIDController(0.0, 0.0, 0.0); + m_fastController = new PIDController(0.0, 0.0, 0.0); + + m_smallProperty = new WpiPidPropertyBuilder("Drive/Aimbot Small", false, m_smallController) + .addP(1.0) + .addI(0.0) + .addD(0.0) + .build(); + m_fastProperty = new WpiPidPropertyBuilder("Drive/Aimbot Fast", false, m_fastController) + .addP(3.0) + .addI(0.0) + .addD(0.0) + .build(); + + m_runKicker = runKicker; + + // each subsystem used by the command must be passed into the + // addRequirements() method (which takes a vararg of Subsystem) + addRequirements(this.m_armSubsystem, this.m_driveSubsystem, this.m_shooterSubsystem); + } + + @Override + public void initialize() { + + } + + @Override + public void execute() { + m_smallProperty.updateIfChanged(); + m_fastProperty.updateIfChanged(); + + // get our desired rotation and error from it + double desiredRotationDegs = AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose()).getDegrees(); + double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees()); + + // if we're far from our setpoint, move faster + double omega; + if (error > 15.0) { + omega = m_fastController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); + } else { + omega = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); + } + + double x = -DriveCommands.setSensitivity(-m_driverController.getLeftY(), 0.25); + double y = -DriveCommands.setSensitivity(-m_driverController.getLeftX(), 0.25); + + + Rotation2d heading; + + // if red change heading goal + if (DriverStation.getAlliance().isPresent() + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); + } else { + heading = m_driveSubsystem.getRotation(); + } + + // Convert to field relative speeds & send command + m_driveSubsystem.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( + x * Constants.DriveConstants.MAX_LINEAR_SPEED, + y * Constants.DriveConstants.MAX_LINEAR_SPEED, + omega * Constants.DriveConstants.MAX_ANGULAR_SPEED, + heading + )); + + m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM); + m_shooterSubsystem.runShooterVelocity(m_runKicker); + + // set shooter speeds and rumble controller + if (m_shooterSubsystem.atSpeed() && error < 10.0) { + m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 1.0); + } else { + m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0); + } + } + + @Override + public void end(boolean interrupted) { + m_driveSubsystem.stopWithX(); + m_shooterSubsystem.runShooterVelocity(false, 0.0, 0.0); + m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); + m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0); + } +} From 0d33fb2fa900dc76f934a2f398b1a6f2d8f31083 Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 14 Mar 2024 11:46:49 -0400 Subject: [PATCH 21/30] update advantagekit to match wpilib --- vendordeps/AdvantageKit.json | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json index 60360535..6fba7872 100644 --- a/vendordeps/AdvantageKit.json +++ b/vendordeps/AdvantageKit.json @@ -1,7 +1,7 @@ { "fileName": "AdvantageKit.json", "name": "AdvantageKit", - "version": "3.1.1", + "version": "3.2.0", "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003", "frcYear": "2024", "mavenUrls": [], @@ -10,24 +10,24 @@ { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "wpilib-shim", - "version": "3.1.1" + "version": "3.2.0" }, { "groupId": "org.littletonrobotics.akit.junction", "artifactId": "junction-core", - "version": "3.1.1" + "version": "3.2.0" }, { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-api", - "version": "3.1.1" + "version": "3.2.0" } ], "jniDependencies": [ { "groupId": "org.littletonrobotics.akit.conduit", "artifactId": "conduit-wpilibio", - "version": "3.1.1", + "version": "3.2.0", "skipInvalidPlatforms": false, "isJar": false, "validPlatforms": [ From d09e7f4ee27e21a0bf8a350b5dd48b8bca9c98aa Mon Sep 17 00:00:00 2001 From: GearBoxFox <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 14 Mar 2024 15:16:00 -0400 Subject: [PATCH 22/30] created new paths --- 2024Crescendo.chor | 750 -- Crescendo2024.chor | 6461 ++++++++--------- src/main/deploy/choreo/AmpWing3.1.traj | 176 + src/main/deploy/choreo/AmpWing3.2.traj | 95 + src/main/deploy/choreo/AmpWing3.traj | 257 + src/main/deploy/choreo/FrontWing1.1.traj | 311 +- src/main/deploy/choreo/FrontWing1.2.traj | 167 +- src/main/deploy/choreo/FrontWing1.traj | 455 +- src/main/deploy/choreo/FrontWing2.1.traj | 212 +- src/main/deploy/choreo/FrontWing2.traj | 212 +- src/main/deploy/choreo/FrontWing3.1.traj | 158 + src/main/deploy/choreo/FrontWing3.traj | 158 + .../deploy/choreo/FrontWing3Contested5.1.traj | 329 +- .../deploy/choreo/FrontWing3Contested5.2.traj | 1355 ++-- .../deploy/choreo/FrontWing3Contested5.traj | 1661 ++--- .../deploy/choreo/SourceContested1.1.traj | 527 ++ .../deploy/choreo/SourceContested1.2.traj | 338 + src/main/deploy/choreo/SourceContested1.traj | 851 +++ src/main/deploy/choreo/SourceWing1.1.traj | 185 + src/main/deploy/choreo/SourceWing1.2.traj | 86 + src/main/deploy/choreo/SourceWing1.traj | 257 + src/main/deploy/choreo/Test1.1.traj | 1589 ++-- src/main/deploy/choreo/Test1.traj | 994 --- src/main/deploy/choreo/Test2.1.traj | 2012 +++-- src/main/deploy/choreo/Test2.traj | 1219 ---- src/main/java/lib/utils/AimbotUtils.java | 12 +- 26 files changed, 10690 insertions(+), 10137 deletions(-) delete mode 100644 2024Crescendo.chor create mode 100644 src/main/deploy/choreo/AmpWing3.1.traj create mode 100644 src/main/deploy/choreo/AmpWing3.2.traj create mode 100644 src/main/deploy/choreo/AmpWing3.traj create mode 100644 src/main/deploy/choreo/FrontWing3.1.traj create mode 100644 src/main/deploy/choreo/FrontWing3.traj create mode 100644 src/main/deploy/choreo/SourceContested1.1.traj create mode 100644 src/main/deploy/choreo/SourceContested1.2.traj create mode 100644 src/main/deploy/choreo/SourceContested1.traj create mode 100644 src/main/deploy/choreo/SourceWing1.1.traj create mode 100644 src/main/deploy/choreo/SourceWing1.2.traj create mode 100644 src/main/deploy/choreo/SourceWing1.traj delete mode 100644 src/main/deploy/choreo/Test1.traj delete mode 100644 src/main/deploy/choreo/Test2.traj diff --git a/2024Crescendo.chor b/2024Crescendo.chor deleted file mode 100644 index f02e322e..00000000 --- a/2024Crescendo.chor +++ /dev/null @@ -1,750 +0,0 @@ -{ - "version": "v0.2.2", - "robotConfiguration": { - "mass": 74.08797700309194, - "rotationalInertia": 6, - "motorMaxTorque": 1.162295081967213, - "motorMaxVelocity": 4800, - "gearing": 6.75, - "wheelbase": 0.5778496879611685, - "trackWidth": 0.5778496879611685, - "bumperLength": 0.8762995267982555, - "bumperWidth": 0.8762995267982555, - "wheelRadius": 0.050799972568014815 - }, - "paths": { - "NewPath": { - "waypoints": [ - { - "x": 1.373390793800354, - "y": 5.677787780761719, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 1.5862760543823242, - "y": 2.3567769527435303, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 - }, - { - "x": 2.395240306854248, - "y": 4.7978620529174805, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 16 - }, - { - "x": 5.460788726806641, - "y": 7.480216979980469, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 11 - }, - { - "x": 7.007755279541016, - "y": 5.691980361938477, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 20 - }, - { - "x": 7.3199872970581055, - "y": 1.8174675703048706, - "heading": 0.9445353135416473, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.373390793800354, - "y": 5.677787780761719, - "heading": 0, - "angularVelocity": 1.7597793138716488e-8, - "velocityX": -0.24611796880717063, - "velocityY": -3.3913013287386686, - "timestamp": 0 - }, - { - "x": 1.352399870888487, - "y": 5.388550285801571, - "heading": 3.752867032965106e-9, - "angularVelocity": 4.895503936124812e-8, - "velocityX": -0.27382037475691573, - "velocityY": -3.773017489334137, - "timestamp": 0.07665946308963235 - }, - { - "x": 1.3314089479766167, - "y": 5.099312790841379, - "heading": 7.505725037785982e-9, - "angularVelocity": 4.8954921591779524e-8, - "velocityX": -0.27382037475695803, - "velocityY": -3.7730174893347197, - "timestamp": 0.1533189261792647 - }, - { - "x": 1.3104180250647461, - "y": 4.8100752958811865, - "heading": 1.1258583081678353e-8, - "angularVelocity": 4.895492210145562e-8, - "velocityX": -0.27382037475695814, - "velocityY": -3.77301748933472, - "timestamp": 0.22997838926889708 - }, - { - "x": 1.2894271021528758, - "y": 4.520837800920995, - "heading": 1.5011441030111597e-8, - "angularVelocity": 4.895492085621956e-8, - "velocityX": -0.2738203747569581, - "velocityY": -3.77301748933472, - "timestamp": 0.3066378523585294 - }, - { - "x": 1.2684361792410237, - "y": 4.231600305960802, - "heading": 1.876429908089443e-8, - "angularVelocity": 4.895492219133962e-8, - "velocityX": -0.2738203747567213, - "velocityY": -3.773017489334737, - "timestamp": 0.38329731544816176 - }, - { - "x": 1.2474452563299916, - "y": 3.9423628110005486, - "heading": 2.251715713543734e-8, - "angularVelocity": 4.8954922240388724e-8, - "velocityX": -0.27382037474602466, - "velocityY": -3.7730174893355133, - "timestamp": 0.4599567785377941 - }, - { - "x": 1.2264544553180678, - "y": 3.6531253071936995, - "heading": 2.6270015297030064e-8, - "angularVelocity": 4.8954923636821905e-8, - "velocityX": -0.27381878460824305, - "velocityY": -3.773017604736736, - "timestamp": 0.5366162416274265 - }, - { - "x": 1.221016329808459, - "y": 3.3631781154569036, - "heading": 3.0068452251973244e-8, - "angularVelocity": 4.954948550189996e-8, - "velocityX": -0.07093873724696509, - "velocityY": -3.782275273670803, - "timestamp": 0.6132757047170588 - }, - { - "x": 1.2644718237504313, - "y": 3.076455070529721, - "heading": 0.0000024276011016837486, - "angularVelocity": 0.00003127510359229755, - "velocityX": 0.5668640529240702, - "velocityY": -3.740217233088862, - "timestamp": 0.6899351678066912 - }, - { - "x": 1.3174690376414906, - "y": 2.8354946463151376, - "heading": 0.053981349204806524, - "angularVelocity": 0.7041390511774281, - "velocityX": 0.6913329647129592, - "velocityY": -3.1432573944960427, - "timestamp": 0.7665946308963235 - }, - { - "x": 1.3769500323372545, - "y": 2.642804109504741, - "heading": 0.127260696547334, - "angularVelocity": 0.9559073908050616, - "velocityX": 0.7759119657049679, - "velocityY": -2.5135910042194918, - "timestamp": 0.8432540939859559 - }, - { - "x": 1.4418762753700956, - "y": 2.498725528039788, - "heading": 0.2108122800980793, - "angularVelocity": 1.0899056709157284, - "velocityX": 0.8469436181274491, - "velocityY": -1.879462438922806, - "timestamp": 0.9199135570755882 - }, - { - "x": 1.5117489112481097, - "y": 2.403367219851462, - "heading": 0.3004574317472213, - "angularVelocity": 1.1693944626813566, - "velocityX": 0.9114678483505286, - "velocityY": -1.2439208982827146, - "timestamp": 0.9965730201652205 - }, - { - "x": 1.5862760543823242, - "y": 2.3567769527435303, - "heading": 0.393778947091327, - "angularVelocity": 1.2173515386481575, - "velocityX": 0.9721845174818851, - "velocityY": -0.607756240784745, - "timestamp": 1.073232483254853 - }, - { - "x": 1.6648735689075516, - "y": 2.3587409751636828, - "heading": 0.4887400714151268, - "angularVelocity": 1.2445985149202732, - "velocityX": 1.0301304934108237, - "velocityY": 0.025741264173079863, - "timestamp": 1.1495310833137506 - }, - { - "x": 1.747708145966923, - "y": 2.4090584282818597, - "heading": 0.5850785373582581, - "angularVelocity": 1.2626505056287294, - "velocityX": 1.0856631313736873, - "velocityY": 0.6594806861375633, - "timestamp": 1.2258296833726483 - }, - { - "x": 1.8345057249855814, - "y": 2.507753992923127, - "heading": 0.6817510390114973, - "angularVelocity": 1.2670285113830946, - "velocityX": 1.1376038217169926, - "velocityY": 1.2935435848767918, - "timestamp": 1.302128283431546 - }, - { - "x": 1.9248152988345435, - "y": 2.6548610401322725, - "heading": 0.7770428443944881, - "angularVelocity": 1.248932553276624, - "velocityX": 1.1836334320583706, - "velocityY": 1.9280438578897694, - "timestamp": 1.3784268834904436 - }, - { - "x": 2.017756729148212, - "y": 2.8504188772247816, - "heading": 0.8676164076432323, - "angularVelocity": 1.187093382825206, - "velocityX": 1.2181275965997216, - "velocityY": 2.5630593083169373, - "timestamp": 1.4547254835493413 - }, - { - "x": 2.110868096637194, - "y": 3.094357922878678, - "heading": 0.9442027964608976, - "angularVelocity": 1.003771874694233, - "velocityX": 1.2203548612570354, - "velocityY": 3.197162798080057, - "timestamp": 1.531024083608239 - }, - { - "x": 2.1793890973429906, - "y": 3.3747396477963205, - "heading": 0.9442028150467319, - "angularVelocity": 2.435933862401011e-7, - "velocityX": 0.898063668965131, - "velocityY": 3.6747951430459493, - "timestamp": 1.6073226836671366 - }, - { - "x": 2.2112713834031283, - "y": 3.6616064511690984, - "heading": 0.9442028192329045, - "angularVelocity": 5.486565302861873e-8, - "velocityX": 0.41786200579730515, - "velocityY": 3.7597911777062087, - "timestamp": 1.6836212837260343 - }, - { - "x": 2.243152894403425, - "y": 3.9484733406807293, - "heading": 0.9442028234190755, - "angularVelocity": 5.486563331370058e-8, - "velocityX": 0.41785184755271976, - "velocityY": 3.7597923066765064, - "timestamp": 1.759919883784932 - }, - { - "x": 2.275034438637313, - "y": 4.235340226498878, - "heading": 0.9442028276052465, - "angularVelocity": 5.4865633459900915e-8, - "velocityX": 0.41785228312547945, - "velocityY": 3.7597922582682526, - "timestamp": 1.8362184838438296 - }, - { - "x": 2.3114043136255, - "y": 4.521672682225187, - "heading": 0.9442028318036487, - "angularVelocity": 5.5025940885755316e-8, - "velocityX": 0.4766781429818065, - "velocityY": 3.752787803515112, - "timestamp": 1.9125170839027272 - }, - { - "x": 2.395240306854248, - "y": 4.7978620529174805, - "heading": 0.944202836290881, - "angularVelocity": 5.881146304501388e-8, - "velocityX": 1.098788092625971, - "velocityY": 3.6198484700779985, - "timestamp": 1.988815683961625 - }, - { - "x": 2.5138560537660655, - "y": 5.041648695718218, - "heading": 0.9442028403664345, - "angularVelocity": 5.686795868350027e-8, - "velocityX": 1.6550967466461282, - "velocityY": 3.401660318130064, - "timestamp": 2.0604826406831447 - }, - { - "x": 2.669379769810612, - "y": 5.263716246351053, - "heading": 0.9442028441855257, - "angularVelocity": 5.328942931675909e-8, - "velocityX": 2.170089580458573, - "velocityY": 3.098604444663902, - "timestamp": 2.1321495974046645 - }, - { - "x": 2.829793194306078, - "y": 5.482277808796993, - "heading": 0.9442028479893113, - "angularVelocity": 5.307586429308322e-8, - "velocityX": 2.2383177943328927, - "velocityY": 3.049683877260457, - "timestamp": 2.2038165541261843 - }, - { - "x": 2.9902066606790894, - "y": 5.70083934050687, - "heading": 0.9442028517930969, - "angularVelocity": 5.307586336486897e-8, - "velocityX": 2.2383183786683687, - "velocityY": 3.049683448386907, - "timestamp": 2.275483510847704 - }, - { - "x": 3.1506201270524525, - "y": 5.919400872216489, - "heading": 0.9442028555968824, - "angularVelocity": 5.307586343565819e-8, - "velocityX": 2.2383183786732843, - "velocityY": 3.0496834483832997, - "timestamp": 2.347150467569224 - }, - { - "x": 3.3110335934258157, - "y": 6.137962403926107, - "heading": 0.9442028594006682, - "angularVelocity": 5.3075864810896055e-8, - "velocityX": 2.2383183786732843, - "velocityY": 3.0496834483832993, - "timestamp": 2.4188174242907436 - }, - { - "x": 3.4714470597991793, - "y": 6.3565239356357255, - "heading": 0.9442028632044539, - "angularVelocity": 5.307586438143788e-8, - "velocityX": 2.2383183786732848, - "velocityY": 3.0496834483832993, - "timestamp": 2.4904843810122634 - }, - { - "x": 3.631860526174069, - "y": 6.575085467344224, - "heading": 0.9442028670082396, - "angularVelocity": 5.307586459415007e-8, - "velocityX": 2.2383183786945806, - "velocityY": 3.0496834483676687, - "timestamp": 2.5621513377337832 - }, - { - "x": 3.7922741739851236, - "y": 6.793646865887341, - "heading": 0.9442028708120256, - "angularVelocity": 5.307586821274769e-8, - "velocityX": 2.2383209103517525, - "velocityY": 3.0496815902535848, - "timestamp": 2.633818294455303 - }, - { - "x": 3.9681792086390733, - "y": 6.999945295226872, - "heading": 0.9442028746807025, - "angularVelocity": 5.3981320792667595e-8, - "velocityX": 2.4544789216804146, - "velocityY": 2.878571084595562, - "timestamp": 2.705485251176823 - }, - { - "x": 4.17437520362896, - "y": 7.175970389393745, - "heading": 0.9442028789510033, - "angularVelocity": 5.958535155683375e-8, - "velocityX": 2.8771417738737903, - "velocityY": 2.4561541639177027, - "timestamp": 2.7771522078983426 - }, - { - "x": 4.405717344151184, - "y": 7.3173296257195375, - "heading": 0.9442028842473172, - "angularVelocity": 7.390175547435562e-8, - "velocityX": 3.228016803073774, - "velocityY": 1.9724464773225439, - "timestamp": 2.8488191646198624 - }, - { - "x": 4.656433083655096, - "y": 7.420495670437542, - "heading": 0.9442028919014063, - "angularVelocity": 1.0680080940487753e-7, - "velocityX": 3.498344997097253, - "velocityY": 1.4395203792296967, - "timestamp": 2.920486121341382 - }, - { - "x": 4.920266408469536, - "y": 7.482894237840949, - "heading": 0.9442029059804885, - "angularVelocity": 1.964515145326873e-7, - "velocityX": 3.6813803304029413, - "velocityY": 0.8706741608391041, - "timestamp": 2.992153078062902 - }, - { - "x": 5.190633947862971, - "y": 7.502968306176006, - "heading": 0.9442029492980424, - "angularVelocity": 6.044285388451107e-7, - "velocityX": 3.772555048542354, - "velocityY": 0.2801021454427761, - "timestamp": 3.0638200347844218 - }, - { - "x": 5.460788726806641, - "y": 7.480216979980469, - "heading": 0.9442042125234751, - "angularVelocity": 0.00001762633003205625, - "velocityX": 3.7695863100958116, - "velocityY": -0.31745908067434947, - "timestamp": 3.1354869915059416 - }, - { - "x": 5.6861880959283475, - "y": 7.430105467011311, - "heading": 0.94453500375596, - "angularVelocity": 0.00541646085327845, - "velocityX": 3.6907473331858487, - "velocityY": -0.8205388221514872, - "timestamp": 3.196558462612879 - }, - { - "x": 5.902931349588688, - "y": 7.350124119825956, - "heading": 0.944535163065715, - "angularVelocity": 0.0000026085789673009567, - "velocityX": 3.5490098687948497, - "velocityY": -1.3096351821180778, - "timestamp": 3.2576299337198167 - }, - { - "x": 6.106969137969694, - "y": 7.241757655990003, - "heading": 0.9445351882967145, - "angularVelocity": 4.1313888378756724e-7, - "velocityX": 3.340967307365667, - "velocityY": -1.7744203942000976, - "timestamp": 3.3187014048267542 - }, - { - "x": 6.294604237914379, - "y": 7.106969701935095, - "heading": 0.9445352004387211, - "angularVelocity": 1.988163445738462e-7, - "velocityX": 3.0723854615542603, - "velocityY": -2.207052681257527, - "timestamp": 3.379772875933692 - }, - { - "x": 6.462436717197839, - "y": 6.94820261304716, - "heading": 0.9445352081247251, - "angularVelocity": 1.2585260943351558e-7, - "velocityX": 2.7481322496649945, - "velocityY": -2.599693212071642, - "timestamp": 3.4408443470406294 - }, - { - "x": 6.607425485391203, - "y": 6.7683332434118935, - "heading": 0.944535213822349, - "angularVelocity": 9.329435992491954e-8, - "velocityX": 2.3740834397043513, - "velocityY": -2.945227393005009, - "timestamp": 3.501915818147567 - }, - { - "x": 6.726943413314801, - "y": 6.570620826362678, - "heading": 0.9445352185350564, - "angularVelocity": 7.716708691487089e-8, - "velocityX": 1.9570173398037076, - "velocityY": -3.237394047754587, - "timestamp": 3.5629872892545045 - }, - { - "x": 6.818825069558712, - "y": 6.358647957429321, - "heading": 0.9445352227854475, - "angularVelocity": 6.959699896596626e-8, - "velocityX": 1.5044939081788649, - "velocityY": -3.470898360417538, - "timestamp": 3.624058760361442 - }, - { - "x": 6.885266799619373, - "y": 6.137378364635986, - "heading": 0.944535226910566, - "angularVelocity": 6.754575173659361e-8, - "velocityX": 1.0879340034945393, - "velocityY": -3.623125311749637, - "timestamp": 3.6851302314683796 - }, - { - "x": 6.951706477654193, - "y": 5.916108155680916, - "heading": 0.9445352310356804, - "angularVelocity": 6.754568510636454e-8, - "velocityX": 1.087900403094797, - "velocityY": -3.6231354009406083, - "timestamp": 3.746201702575317 - }, - { - "x": 7.007755279541016, - "y": 5.691980361938477, - "heading": 0.9445352351916124, - "angularVelocity": 6.805030176915139e-8, - "velocityX": 0.917757520343414, - "velocityY": -3.669926230366784, - "timestamp": 3.8072731736822547 - }, - { - "x": 7.033287944264436, - "y": 5.499297549749691, - "heading": 0.9445352391429122, - "angularVelocity": 7.690359033060341e-8, - "velocityX": 0.49693866281360616, - "velocityY": -3.750158476346985, - "timestamp": 3.858653085599064 - }, - { - "x": 7.048377702483028, - "y": 5.30551704932481, - "heading": 0.9445352430586353, - "angularVelocity": 7.621117031703296e-8, - "velocityX": 0.29368984211229937, - "velocityY": -3.7715226281165486, - "timestamp": 3.9100329975158736 - }, - { - "x": 7.063467124411225, - "y": 5.111736522713165, - "heading": 0.9445352469743582, - "angularVelocity": 7.621116311616718e-8, - "velocityX": 0.2936832969396488, - "velocityY": -3.7715231377858505, - "timestamp": 3.961412909432683 - }, - { - "x": 7.0785565463316305, - "y": 4.917955996100912, - "heading": 0.9445352508900811, - "angularVelocity": 7.621116383171076e-8, - "velocityX": 0.29368329678799265, - "velocityY": -3.7715231377976592, - "timestamp": 4.0127928213494926 - }, - { - "x": 7.093645968252035, - "y": 4.72417546948866, - "heading": 0.9445352548058039, - "angularVelocity": 7.621116339702062e-8, - "velocityX": 0.29368329678798916, - "velocityY": -3.77152313779766, - "timestamp": 4.064172733266302 - }, - { - "x": 7.1087353901724395, - "y": 4.530394942876407, - "heading": 0.9445352587215268, - "angularVelocity": 7.621116320689e-8, - "velocityX": 0.2936832967879892, - "velocityY": -3.77152313779766, - "timestamp": 4.1155526451831115 - }, - { - "x": 7.1238248120928445, - "y": 4.336614416264154, - "heading": 0.9445352626372497, - "angularVelocity": 7.621116316535163e-8, - "velocityX": 0.29368329678798916, - "velocityY": -3.77152313779766, - "timestamp": 4.166932557099921 - }, - { - "x": 7.138914234013249, - "y": 4.142833889651901, - "heading": 0.9445352665529726, - "angularVelocity": 7.621116395669404e-8, - "velocityX": 0.2936832967879891, - "velocityY": -3.7715231377976606, - "timestamp": 4.21831246901673 - }, - { - "x": 7.1540036559336535, - "y": 3.9490533630396487, - "heading": 0.9445352704686956, - "angularVelocity": 7.621116465419702e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.7715231377976597, - "timestamp": 4.26969238093354 - }, - { - "x": 7.1690930778540585, - "y": 3.755272836427396, - "heading": 0.9445352743844184, - "angularVelocity": 7.621116300190572e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.7715231377976597, - "timestamp": 4.321072292850349 - }, - { - "x": 7.184182499774463, - "y": 3.561492309815143, - "heading": 0.9445352783001413, - "angularVelocity": 7.621116375934217e-8, - "velocityX": 0.29368329678798927, - "velocityY": -3.7715231377976597, - "timestamp": 4.372452204767159 - }, - { - "x": 7.199271921694868, - "y": 3.367711783202891, - "heading": 0.9445352822158641, - "angularVelocity": 7.621116358329164e-8, - "velocityX": 0.2936832967879893, - "velocityY": -3.7715231377976597, - "timestamp": 4.423832116683968 - }, - { - "x": 7.2143613436152725, - "y": 3.173931256590638, - "heading": 0.944535286131587, - "angularVelocity": 7.621116395825444e-8, - "velocityX": 0.2936832967879889, - "velocityY": -3.77152313779766, - "timestamp": 4.475212028600778 - }, - { - "x": 7.2294507655356774, - "y": 2.980150729978386, - "heading": 0.94453529004731, - "angularVelocity": 7.621116318952045e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.77152313779766, - "timestamp": 4.526591940517587 - }, - { - "x": 7.244540187456082, - "y": 2.786370203366133, - "heading": 0.9445352939630328, - "angularVelocity": 7.621116338095618e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.77152313779766, - "timestamp": 4.577971852434397 - }, - { - "x": 7.2596296093764865, - "y": 2.5925896767538807, - "heading": 0.9445352978787557, - "angularVelocity": 7.621116372521823e-8, - "velocityX": 0.2936832967879889, - "velocityY": -3.77152313779766, - "timestamp": 4.629351764351206 - }, - { - "x": 7.2747190312968915, - "y": 2.398809150141628, - "heading": 0.9445353017944785, - "angularVelocity": 7.621116368417063e-8, - "velocityX": 0.2936832967879889, - "velocityY": -3.77152313779766, - "timestamp": 4.680731676268016 - }, - { - "x": 7.289808453217296, - "y": 2.2050286235293757, - "heading": 0.9445353057102015, - "angularVelocity": 7.621116284744578e-8, - "velocityX": 0.29368329678798905, - "velocityY": -3.77152313779766, - "timestamp": 4.732111588184825 - }, - { - "x": 7.3048978751377005, - "y": 2.011248096917123, - "heading": 0.9445353096259244, - "angularVelocity": 7.621116384489279e-8, - "velocityX": 0.29368329678798877, - "velocityY": -3.77152313779766, - "timestamp": 4.7834915001016345 - }, - { - "x": 7.3199872970581055, - "y": 1.8174675703048704, - "heading": 0.9445353135416472, - "angularVelocity": 7.621116322392135e-8, - "velocityX": 0.2936832967879892, - "velocityY": -3.77152313779766, - "timestamp": 4.834871412018444 - } - ], - "constraints": [], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - } - }, - "splitTrajectoriesAtStopPoints": false, - "usesObstacles": true -} \ No newline at end of file diff --git a/Crescendo2024.chor b/Crescendo2024.chor index 558b1600..761b12bf 100644 --- a/Crescendo2024.chor +++ b/Crescendo2024.chor @@ -1,5 +1,5 @@ { - "version": "v0.2.2", + "version": "v0.3", "robotConfiguration": { "mass": 74.08797700309194, "rotationalInertia": 6, @@ -13,57 +13,206 @@ "wheelRadius": 0.050799972568014815 }, "paths": { - "Test1": { + "FrontWing2": { "waypoints": [ { - "x": 1.4869295358657837, - "y": 5.564249038696289, + "x": 1.2263859510421753, + "y": 5.594110488891602, "heading": 3.141, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 11 }, { - "x": 2.6648950576782227, - "y": 6.813176155090332, - "heading": 3.8960730372009595, + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 26 + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -3.740256292064187e-22, + "velocityX": -5.996486579954623e-19, + "velocityY": 3.7218569679078715e-21, + "timestamp": 0 + }, + { + "x": 1.3163279724628432, + "y": 5.593551858929642, + "heading": 3.140264869702983, + "angularVelocity": -0.007887547277553473, + "velocityX": 0.9650288514473437, + "velocityY": -0.0059937949143052385, + "timestamp": 0.0932013807522681 + }, + { + "x": 1.467723714747352, + "y": 5.592611539861448, + "heading": 3.1402648694385524, + "angularVelocity": -2.83720041018159e-9, + "velocityX": 1.6243937703766675, + "velocityY": -0.010089110918776865, + "timestamp": 0.1864027615045362 + }, + { + "x": 1.6191194570316485, + "y": 5.591671220793256, + "heading": 3.1402648691741217, + "angularVelocity": -2.837195579018617e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777356, + "timestamp": 0.2796041422568043 + }, + { + "x": 1.7705151993162385, + "y": 5.590730901725062, + "heading": 3.1402648689096915, + "angularVelocity": -2.83719540339334e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777356, + "timestamp": 0.3728055230090724 + }, + { + "x": 1.9219109416013613, + "y": 5.589790582656871, + "heading": 3.1402648686452608, + "angularVelocity": -2.837195245417577e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777356, + "timestamp": 0.46600690376134046 + }, + { + "x": 2.0733066838851757, + "y": 5.588850263588676, + "heading": 3.14026486838083, + "angularVelocity": -2.837195862058808e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777355, + "timestamp": 0.5592082845136086 }, { - "x": 2.1113932132720947, - "y": 6.231289386749268, - "heading": 0, + "x": 2.224702426169452, + "y": 5.58790994452049, + "heading": 3.1402648681163994, + "angularVelocity": -2.8371952366306506e-9, + "velocityX": 1.6243937703767442, + "velocityY": -0.010089110918777355, + "timestamp": 0.6524096652658767 + }, + { + "x": 2.3760981684546945, + "y": 5.586969625452291, + "heading": 3.140264867851969, + "angularVelocity": -2.8371952652077662e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777358, + "timestamp": 0.7456110460181449 + }, + { + "x": 2.5274939107386767, + "y": 5.5860293063840984, + "heading": 3.1402648675875384, + "angularVelocity": -2.837200309142864e-9, + "velocityX": 1.624393770376668, + "velocityY": -0.010089110918776867, + "timestamp": 0.838812426770413 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": -0.007887547280595224, + "velocityX": 0.9650288514473356, + "velocityY": -0.0059937949145238275, + "timestamp": 0.9320138075226811 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": 3.1865843751901534e-21, + "velocityX": -4.755464983211591e-19, + "velocityY": 2.948079481483161e-21, + "timestamp": 1.0252151882749492 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 1.0252151882749492, + "isStopPoint": true, + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 23 + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 2.59393310546875, - "y": 5.550056457519531, - "heading": 3.14, + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "FrontWing1": { + "waypoints": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 23 + "controlIntervalCount": 16 }, { - "x": 1.941084861755371, - "y": 4.939785003662109, - "heading": 0, + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 21 + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 2.5655484199523926, - "y": 4.244359970092773, - "heading": 2.649783348569472, + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -72,1032 +221,282 @@ ], "trajectory": [ { - "x": 1.4869295358657837, - "y": 5.564249038696289, + "x": 1.2263859510421753, + "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": 3.7405663636389993e-19, - "velocityX": -1.1249906625590385e-18, - "velocityY": 2.3803295269896554e-19, + "angularVelocity": 4.442897870116303e-27, + "velocityX": 5.36651543276403e-26, + "velocityY": -1.1808751253669755e-25, "timestamp": 0 }, { - "x": 1.5092825889004946, - "y": 5.579306288055266, - "heading": 3.4062283948642453, - "angularVelocity": 3.352121601432042, - "velocityX": 0.282511802606821, - "velocityY": 0.19030289294706518, - "timestamp": 0.07912254577845217 - }, - { - "x": 1.5673643497428493, - "y": 5.60987650843196, - "heading": 3.626081158643754, - "angularVelocity": 2.77863612218835, - "velocityX": 0.7340734587204384, - "velocityY": 0.38636547997701703, - "timestamp": 0.15824509155690433 - }, - { - "x": 1.639918967505268, - "y": 5.662681063987355, - "heading": 3.7707265189248944, - "angularVelocity": 1.828118128126927, - "velocityX": 0.9169904361466855, - "velocityY": 0.6673768523986875, - "timestamp": 0.23736763733535649 - }, - { - "x": 1.719103738696596, - "y": 5.732106202864848, - "heading": 3.861411095653706, - "angularVelocity": 1.1461281463659418, - "velocityX": 1.000786443513203, - "velocityY": 0.8774381333973873, - "timestamp": 0.31649018311380867 - }, - { - "x": 1.8011465793982657, - "y": 5.8124573886259565, - "heading": 3.9165727578512475, - "angularVelocity": 0.6971674338183805, - "velocityX": 1.0369085055907419, - "velocityY": 1.0155283171258929, - "timestamp": 0.39561272889226085 - }, - { - "x": 1.8844011608490543, - "y": 5.899562457377884, - "heading": 3.9495869202931786, - "angularVelocity": 0.41725354154267424, - "velocityX": 1.0522232396807132, - "velocityY": 1.1008880957373124, - "timestamp": 0.474735274670713 - }, - { - "x": 1.968168332271066, - "y": 5.990724119399961, - "heading": 3.96923386729424, - "angularVelocity": 0.24831034956931133, - "velocityX": 1.0587016709063517, - "velocityY": 1.1521578473642025, - "timestamp": 0.5538578204491652 - }, - { - "x": 2.05213613980173, - "y": 6.084290626509761, - "heading": 3.9810005346780404, - "angularVelocity": 0.1487144690307097, - "velocityX": 1.0612374349756033, - "velocityY": 1.1825517769839073, - "timestamp": 0.6329803662276173 - }, - { - "x": 2.1361397889017213, - "y": 6.179275610733387, - "heading": 3.988261371091814, - "angularVelocity": 0.0917669716303672, - "velocityX": 1.0616904230458737, - "velocityY": 1.2004793739775566, - "timestamp": 0.7121029120060695 - }, - { - "x": 2.2200564987148903, - "y": 6.2750981229467735, - "heading": 3.9931304198236632, - "angularVelocity": 0.061538069635463585, - "velocityX": 1.0605916302053886, - "velocityY": 1.2110645742073904, - "timestamp": 0.7912254577845216 - }, - { - "x": 2.3037490732526433, - "y": 6.3714204686821265, - "heading": 3.997036412999259, - "angularVelocity": 0.049366373859271756, - "velocityX": 1.0577588690345867, - "velocityY": 1.217381781484405, - "timestamp": 0.8703480035629737 - }, - { - "x": 2.3870186605553965, - "y": 6.4680502574631, - "heading": 4.001128510533661, - "angularVelocity": 0.05171847662561717, - "velocityX": 1.0524128929813872, - "velocityY": 1.221267438127472, - "timestamp": 0.9494705493414258 - }, - { - "x": 2.4696215654523987, - "y": 6.564870888609008, - "heading": 4.006432047635611, - "angularVelocity": 0.06702940419537926, - "velocityX": 1.0439869456209716, - "velocityY": 1.2236794227654333, - "timestamp": 1.028593095119878 - }, - { - "x": 2.5522006651677756, - "y": 6.661687676467445, - "heading": 4.0117852212202445, - "angularVelocity": 0.06765674097018534, - "velocityX": 1.043686080913052, - "velocityY": 1.223630848905317, - "timestamp": 1.1077156408983302 - }, - { - "x": 2.626171983754375, - "y": 6.756004010665758, - "heading": 4.047305777483365, - "angularVelocity": 0.44893090728627943, - "velocityX": 0.9348955832857486, - "velocityY": 1.1920285586159498, - "timestamp": 1.1868381866767823 - }, - { - "x": 2.664895057678222, - "y": 6.813176155090332, - "heading": 3.896073037200959, - "angularVelocity": -1.9113735382816162, - "velocityX": 0.4894063195625013, - "velocityY": 0.7225771600506798, - "timestamp": 1.2659607324552344 - }, - { - "x": 2.67401621750823, - "y": 6.8287147997218245, - "heading": 3.8216991351700837, - "angularVelocity": -2.7569472556880648, - "velocityX": 0.338109953564034, - "velocityY": 0.5759980652369127, - "timestamp": 1.2929376366660208 - }, - { - "x": 2.6786727556459358, - "y": 6.8400421309827735, - "heading": 3.72670670460024, - "angularVelocity": -3.521250245306635, - "velocityX": 0.17261202773019518, - "velocityY": 0.41988996114756666, - "timestamp": 1.3199145408768072 - }, - { - "x": 2.678518823316929, - "y": 6.846619952079936, - "heading": 3.614568928899329, - "angularVelocity": -4.15680668266135, - "velocityX": -0.00570607834774554, - "velocityY": 0.24383157703223057, - "timestamp": 1.3468914450875935 - }, - { - "x": 2.6741686792707893, - "y": 6.846169187557711, - "heading": 3.4948207380964975, - "angularVelocity": -4.438915224191635, - "velocityX": -0.16125438308832984, - "velocityY": -0.0167092754120811, - "timestamp": 1.3738683492983799 - }, - { - "x": 2.663571596898345, - "y": 6.842490941859769, - "heading": 3.391106479438399, - "angularVelocity": -3.844557472114035, - "velocityX": -0.3928205508549501, - "velocityY": -0.1363479541318252, - "timestamp": 1.4008452535091662 - }, - { - "x": 2.648321534336774, - "y": 6.83512618854535, - "heading": 3.3066791856345255, - "angularVelocity": -3.129613878011262, - "velocityX": -0.5653006898944218, - "velocityY": -0.2730021672197161, - "timestamp": 1.4278221577199526 - }, - { - "x": 2.630755187717048, - "y": 6.821913643765559, - "heading": 3.2403542901418882, - "angularVelocity": -2.4585806797690686, - "velocityX": -0.6511624344467275, - "velocityY": -0.4897724615306165, - "timestamp": 1.454799061930739 - }, - { - "x": 2.610177790375068, - "y": 6.803317677931451, - "heading": 3.192043949934185, - "angularVelocity": -1.7908037123240415, - "velocityX": -0.7627783077407181, - "velocityY": -0.6893291271973806, - "timestamp": 1.4817759661415253 - }, - { - "x": 2.585880089062531, - "y": 6.779956801667491, - "heading": 3.1617447456020207, - "angularVelocity": -1.1231534980960616, - "velocityX": -0.9006853092812849, - "velocityY": -0.8659583798581432, - "timestamp": 1.5087528703523116 - }, - { - "x": 2.557481302008236, - "y": 6.752192795634918, - "heading": 3.1494506971418432, - "angularVelocity": -0.4557249550980476, - "velocityX": -1.0527074134380248, - "velocityY": -1.029176877214801, - "timestamp": 1.535729774563098 - }, - { - "x": 2.5262145192081267, - "y": 6.721488912807054, - "heading": 3.1494498734965517, - "angularVelocity": -0.00003053149779678828, - "velocityX": -1.1590204181995478, - "velocityY": -1.1381544223143767, - "timestamp": 1.5627066787738844 - }, - { - "x": 2.494951526671565, - "y": 6.690781099940809, - "heading": 3.1494491983381225, - "angularVelocity": -0.000025027276073123234, - "velocityX": -1.1588799178840683, - "velocityY": -1.1383001039076166, - "timestamp": 1.5896835829846707 - }, - { - "x": 2.46368853251831, - "y": 6.660073288720487, - "heading": 3.149448523179691, - "angularVelocity": -0.000025027276154336604, - "velocityX": -1.1588799778128431, - "velocityY": -1.138300042895271, - "timestamp": 1.616660487195457 - }, - { - "x": 2.4324255384011386, - "y": 6.629365477463429, - "heading": 3.1494478480212726, - "angularVelocity": -0.000025027275679943262, - "velocityX": -1.1588799764752862, - "velocityY": -1.138300044257102, - "timestamp": 1.6436373914062434 - }, - { - "x": 2.401162544284488, - "y": 6.5986576662058365, - "heading": 3.1494471728628666, - "angularVelocity": -0.000025027275214919653, - "velocityX": -1.1588799764559639, - "velocityY": -1.1383000442768603, - "timestamp": 1.6706142956170298 - }, - { - "x": 2.3698995501679407, - "y": 6.567949854948137, - "heading": 3.1494464977044734, - "angularVelocity": -0.000025027274749328804, - "velocityX": -1.1588799764521394, - "velocityY": -1.1383000442808404, - "timestamp": 1.6975911998278161 - }, - { - "x": 2.3386365560514903, - "y": 6.537242043690337, - "heading": 3.1494458225460926, - "angularVelocity": -0.000025027274284044715, - "velocityX": -1.1588799764485511, - "velocityY": -1.1383000442845796, - "timestamp": 1.7245681040386025 - }, - { - "x": 2.307373561935169, - "y": 6.5065342324324025, - "heading": 3.1494451473877243, - "angularVelocity": -0.00002502727381983999, - "velocityX": -1.1588799764437645, - "velocityY": -1.1383000442895392, - "timestamp": 1.7515450082493889 - }, - { - "x": 2.2761105678205396, - "y": 6.475826421172744, - "heading": 3.1494444722293684, - "angularVelocity": -0.00002502727335405626, - "velocityX": -1.158879976381056, - "velocityY": -1.1383000443534677, - "timestamp": 1.7785219124601752 - }, - { - "x": 2.244847573837531, - "y": 6.445118609779082, - "heading": 3.1494437970710263, - "angularVelocity": -0.000025027272855365495, - "velocityX": -1.1588799715020328, - "velocityY": -1.138300049320807, - "timestamp": 1.8054988166709616 - }, - { - "x": 2.2135845753549397, - "y": 6.414410802966355, - "heading": 3.149443121912666, - "angularVelocity": -0.000025027273518628322, - "velocityX": -1.1588801382959264, - "velocityY": -1.1382998795112838, - "timestamp": 1.832475720881748 - }, - { - "x": 2.182332647777799, - "y": 6.3836917291800725, - "heading": 3.14944244669543, - "angularVelocity": -0.000025029455959603075, - "velocityX": -1.1584697537175366, - "velocityY": -1.1387175320883667, - "timestamp": 1.8594526250925343 - }, - { - "x": 2.153094421241092, - "y": 6.351050761951557, - "heading": 3.149440476891046, - "angularVelocity": -0.00007301817765136917, - "velocityX": -1.0838243820804343, - "velocityY": -1.2099597112211005, - "timestamp": 1.8864295293033206 - }, - { - "x": 2.1314747036692956, - "y": 6.314801767366594, - "heading": 3.1444623769659996, - "angularVelocity": -0.18453191983123554, - "velocityX": -0.801415811201077, - "velocityY": -1.3437047595123932, - "timestamp": 1.913406433514107 - }, - { - "x": 2.1175303161219627, - "y": 6.274679449481932, - "heading": 3.139999999999996, - "angularVelocity": -0.16541471664709217, - "velocityX": -0.5169009549193061, - "velocityY": -1.487283995641412, - "timestamp": 1.9403833377248934 - }, - { - "x": 2.111393213272095, - "y": 6.231289386749268, - "heading": 3.139999999999996, - "angularVelocity": 1.2396255317011e-17, - "velocityX": -0.22749470442979314, - "velocityY": -1.6084151981870152, - "timestamp": 1.9673602419356797 - }, - { - "x": 2.1135515859889713, - "y": 6.18871804241084, - "heading": 3.139999999999996, - "angularVelocity": 4.342266970142451e-18, - "velocityX": 0.08225287430098827, - "velocityY": -1.6223404824025023, - "timestamp": 1.9936009385096722 - }, - { - "x": 2.1237593069102005, - "y": 6.1473322849843255, - "heading": 3.1399999999999966, - "angularVelocity": 4.3422681619637e-18, - "velocityX": 0.3890034280317459, - "velocityY": -1.5771592537498764, - "timestamp": 2.0198416350836648 - }, - { - "x": 2.141644452417822, - "y": 6.1086399046863455, - "heading": 3.1399999999999966, - "angularVelocity": 4.3422691347276405e-18, - "velocityX": 0.6815804396497286, - "velocityY": -1.4745180330437861, - "timestamp": 2.0460823316576575 - }, - { - "x": 2.1665552441015765, - "y": 6.0740504370872355, - "heading": 3.1399999999999966, - "angularVelocity": 4.3422696264537505e-18, - "velocityX": 0.9493189943912999, - "velocityY": -1.3181611814907124, - "timestamp": 2.0723230282316503 - }, - { - "x": 2.1935012145011026, - "y": 6.041021760141006, - "heading": 3.139999999999997, - "angularVelocity": 4.34227316559682e-18, - "velocityX": 1.0268770999862573, - "velocityY": -1.2586814093559167, - "timestamp": 2.098563724805643 - }, - { - "x": 2.220442534619127, - "y": 6.00798928987496, - "heading": 3.139999999999997, - "angularVelocity": 4.342268965346379e-18, - "velocityX": 1.0266998835971468, - "velocityY": -1.2588259680112164, - "timestamp": 2.1248044213796358 - }, - { - "x": 2.247383856273639, - "y": 5.974956820862076, - "heading": 3.1399999999999975, - "angularVelocity": 4.342269673161453e-18, - "velocityX": 1.0266999421507437, - "velocityY": -1.2588259202547785, - "timestamp": 2.1510451179536285 - }, - { - "x": 2.274325177898134, - "y": 5.941924351824711, - "heading": 3.1399999999999975, - "angularVelocity": 4.342269691700593e-18, - "velocityX": 1.0266999410068467, - "velocityY": -1.2588259211877415, - "timestamp": 2.1772858145276213 - }, - { - "x": 2.3012664995222347, - "y": 5.908891882787024, - "heading": 3.1399999999999975, - "angularVelocity": 4.3422697108078704e-18, - "velocityX": 1.0266999409917994, - "velocityY": -1.258825921200014, - "timestamp": 2.203526511101614 - }, - { - "x": 2.328207821146328, - "y": 5.87585941374933, - "heading": 3.139999999999998, - "angularVelocity": 4.3422696965810506e-18, - "velocityX": 1.0266999409915183, - "velocityY": -1.2588259212002433, - "timestamp": 2.2297672076756068 - }, - { - "x": 2.355149142770421, - "y": 5.8428269447116365, - "heading": 3.139999999999998, - "angularVelocity": 4.342269684490725e-18, - "velocityX": 1.0266999409915205, - "velocityY": -1.2588259212002417, - "timestamp": 2.2560079042495995 - }, - { - "x": 2.3820904643945138, - "y": 5.809794475673942, - "heading": 3.139999999999998, - "angularVelocity": 4.342269693460261e-18, - "velocityX": 1.026699940991488, - "velocityY": -1.258825921200268, - "timestamp": 2.2822486008235923 - }, - { - "x": 2.4090317860186072, - "y": 5.7767620066362495, - "heading": 3.1399999999999983, - "angularVelocity": 4.342269692405428e-18, - "velocityX": 1.026699940991538, - "velocityY": -1.258825921200227, - "timestamp": 2.308489297397585 - }, - { - "x": 2.4359731076427, - "y": 5.743729537598556, - "heading": 3.1399999999999983, - "angularVelocity": 4.342269716594298e-18, - "velocityX": 1.0266999409914905, - "velocityY": -1.258825921200266, - "timestamp": 2.3347299939715778 - }, - { - "x": 2.4629144292667973, - "y": 5.710697068560865, - "heading": 3.139999999999999, - "angularVelocity": 4.342269681682682e-18, - "velocityX": 1.026699940991683, - "velocityY": -1.258825921200109, - "timestamp": 2.3609706905455705 - }, - { - "x": 2.4898557508911936, - "y": 5.677664599523419, - "heading": 3.139999999999999, - "angularVelocity": 4.3422696993715505e-18, - "velocityX": 1.0266999410030797, - "velocityY": -1.2588259211908142, - "timestamp": 2.3872113871195633 - }, - { - "x": 2.5167970725019893, - "y": 5.644632130474532, - "heading": 3.139999999999999, - "angularVelocity": 4.342269692409483e-18, - "velocityX": 1.026699940484769, - "velocityY": -1.2588259216268345, - "timestamp": 2.413452083693556 - }, - { - "x": 2.5425090771755396, - "y": 5.613106913889164, - "heading": 3.1399999999999992, - "angularVelocity": 4.342265374488911e-18, - "velocityX": 0.9798522154718704, - "velocityY": -1.2013864226688296, - "timestamp": 2.4396927802675488 - }, - { - "x": 2.563078686478754, - "y": 5.587886733807481, - "heading": 3.1399999999999992, - "angularVelocity": 4.342265374369648e-18, - "velocityX": 0.7838819844288143, - "velocityY": -0.9611093977847911, - "timestamp": 2.4659334768415415 - }, - { - "x": 2.578505895291019, - "y": 5.568971596499493, - "heading": 3.1399999999999992, - "angularVelocity": 4.342265374398685e-18, - "velocityX": 0.5879115582455986, - "velocityY": -0.7208321339584491, - "timestamp": 2.4921741734155343 - }, - { - "x": 2.5887907019054897, - "y": 5.5563615040551815, - "heading": 3.1399999999999997, - "angularVelocity": 4.342265374398075e-18, - "velocityX": 0.39194106701667647, - "velocityY": -0.48055479048560223, - "timestamp": 2.518414869989527 - }, - { - "x": 2.5939331054687496, - "y": 5.55005645751953, - "heading": 3.1399999999999997, - "angularVelocity": 4.342265374359544e-18, - "velocityX": 0.19597054326510124, - "velocityY": -0.2402774071896897, - "timestamp": 2.5446555665635198 - }, - { - "x": 2.5939331054687496, - "y": 5.55005645751953, - "heading": 3.14, - "angularVelocity": 3.0510058962998796e-17, - "velocityX": 1.3525026943280928e-16, - "velocityY": 8.359694800915289e-17, - "timestamp": 2.5708962631375125 - }, - { - "x": 2.5906368423225947, - "y": 5.5485199070890205, - "heading": 3.101591851410996, - "angularVelocity": -1.2805791583146104, - "velocityX": -0.10990183178303778, - "velocityY": -0.05123065102809662, - "timestamp": 2.60088905845952 - }, - { - "x": 2.584621501308477, - "y": 5.544975862182894, - "heading": 3.0243371709692783, - "angularVelocity": -2.5757746022741834, - "velocityX": -0.20055953270102006, - "velocityY": -0.11816320779855842, - "timestamp": 2.6308818537815273 - }, - { - "x": 2.5759613994637824, - "y": 5.536844199254011, - "heading": 2.9135350884263085, - "angularVelocity": -3.6942899570776517, - "velocityX": -0.28873940397378167, - "velocityY": -0.2711205421688905, - "timestamp": 2.6608746491035347 - }, - { - "x": 2.56275074361348, - "y": 5.521892690270415, - "heading": 2.8248688434153846, - "angularVelocity": -2.956251461684603, - "velocityX": -0.44046097433337356, - "velocityY": -0.49850335131840534, - "timestamp": 2.690867444425542 - }, - { - "x": 2.543962903792877, - "y": 5.501410245111836, - "heading": 2.7589030524712514, - "angularVelocity": -2.199387894182165, - "velocityX": -0.6264117638620101, - "velocityY": -0.6829121773583486, - "timestamp": 2.7208602397475494 - }, - { - "x": 2.518778715441927, - "y": 5.477002144640871, - "heading": 2.7154691851313926, - "angularVelocity": -1.448143358205586, - "velocityX": -0.839674597873837, - "velocityY": -0.8137987876364632, - "timestamp": 2.750853035069557 - }, - { - "x": 2.486733800013198, - "y": 5.449761748396881, - "heading": 2.6943182531197865, - "angularVelocity": -0.705200425130341, - "velocityX": -1.0684204351295292, - "velocityY": -0.9082313252703665, - "timestamp": 2.780845830391564 - }, - { - "x": 2.4478196867061426, - "y": 5.4204460978412365, - "heading": 2.694317441639593, - "angularVelocity": -0.000027055837428819432, - "velocityX": -1.2974487002285997, - "velocityY": -0.9774230858209427, - "timestamp": 2.8108386257135716 - }, - { - "x": 2.408901839144878, - "y": 5.391135398464328, - "heading": 2.6943166423252785, - "angularVelocity": -0.000026650210696695328, - "velocityX": -1.2975732052797133, - "velocityY": -0.9772580068715555, - "timestamp": 2.840831421035579 - }, - { - "x": 2.3699840051756724, - "y": 5.361824681040097, - "heading": 2.6943158430110494, - "angularVelocity": -0.00002665020784043696, - "velocityX": -1.2975727521022344, - "velocityY": -0.9772586085935405, - "timestamp": 2.8708242163575863 - }, - { - "x": 2.3310661711945366, - "y": 5.332513963631788, - "heading": 2.6943150436964616, - "angularVelocity": -0.000026650219800690006, - "velocityX": -1.297572752499997, - "velocityY": -0.9772586080626477, - "timestamp": 2.9008170116795937 - }, - { - "x": 2.292148337213513, - "y": 5.303203246223414, - "heading": 2.694314244381513, - "angularVelocity": -0.000026650231834296552, - "velocityX": -1.2975727524962546, - "velocityY": -0.9772586080648237, - "timestamp": 2.930809807001601 - }, - { - "x": 2.25323050323266, - "y": 5.273892528814898, - "heading": 2.6943134450662036, - "angularVelocity": -0.00002665024386857193, - "velocityX": -1.297572752490578, - "velocityY": -0.9772586080695672, - "timestamp": 2.9608026023236085 - }, - { - "x": 2.214312669251978, - "y": 5.2445818114062375, - "heading": 2.694312645750533, - "angularVelocity": -0.00002665025590264624, - "velocityX": -1.2975727524848706, - "velocityY": -0.9772586080743522, - "timestamp": 2.990795397645616 - }, - { - "x": 2.1753948352714723, - "y": 5.215271093997427, - "heading": 2.694311846434502, - "angularVelocity": -0.000026650267936849635, - "velocityX": -1.2975727524790048, - "velocityY": -0.9772586080793467, - "timestamp": 3.0207881929676232 - }, - { - "x": 2.136477001291495, - "y": 5.185960376588, - "heading": 2.6943110471181098, - "angularVelocity": -0.000026650279973527825, - "velocityX": -1.2975727524613627, - "velocityY": -0.9772586080999772, - "timestamp": 3.0507809882896306 - }, - { - "x": 2.097559167357152, - "y": 5.1566496591180675, - "heading": 2.694310247801348, - "angularVelocity": -0.000026650292283470146, - "velocityX": -1.2975727509398636, - "velocityY": -0.9772586101172457, - "timestamp": 3.080773783611638 - }, - { - "x": 2.058641302557044, - "y": 5.127338982627939, - "heading": 2.694309448489961, - "angularVelocity": -0.000026650113093683626, - "velocityX": -1.2975737800459084, - "velocityY": -0.9772572437956306, - "timestamp": 3.1107665789336454 - }, - { - "x": 2.0197569370912083, - "y": 5.0979839038638834, - "heading": 2.6943086027536207, - "angularVelocity": -0.000028197983267341566, - "velocityX": -1.2964568673313885, - "velocityY": -0.9787376751348899, - "timestamp": 3.1407593742556528 - }, - { - "x": 1.9877778714785685, - "y": 5.067888184962114, - "heading": 2.678621118987198, - "angularVelocity": -0.5230417371285907, - "velocityX": -1.0662249139900737, - "velocityY": -1.0034316101155079, - "timestamp": 3.17075216957766 - }, - { - "x": 1.9640654183560038, - "y": 5.031205594348394, - "heading": 2.6596531558314034, - "angularVelocity": -0.6324173173088112, - "velocityX": -0.7906049725553251, - "velocityY": -1.223046742388833, - "timestamp": 3.2007449648996675 - }, - { - "x": 1.9481708008996033, - "y": 4.987987936601904, - "heading": 2.649783348569472, - "angularVelocity": -0.32907260414160233, - "velocityX": -0.5299478519964009, - "velocityY": -1.4409346405525878, - "timestamp": 3.230737760221675 - }, - { - "x": 1.9410848617553718, - "y": 4.939785003662109, - "heading": 2.649783348569472, - "angularVelocity": 6.681257540282107e-18, - "velocityX": -0.23625470944103766, - "velocityY": -1.6071503980304298, - "timestamp": 3.2607305555436823 - }, - { - "x": 1.9456173734407256, - "y": 4.888006870907468, - "heading": 2.649783348569472, - "angularVelocity": -1.484655388526889e-19, - "velocityX": 0.14165581617647935, - "velocityY": -1.6182360167593688, - "timestamp": 3.292727206352592 - }, - { - "x": 1.9619962278910381, - "y": 4.838678856610239, - "heading": 2.649783348569472, - "angularVelocity": -1.4846556369937314e-19, - "velocityX": 0.5118927774045035, - "velocityY": -1.5416618005383362, - "timestamp": 3.324723857161502 - }, - { - "x": 1.9893341653194017, - "y": 4.794473003544163, - "heading": 2.649783348569472, - "angularVelocity": -1.4846558519027202e-19, - "velocityX": 0.8543999680324345, - "velocityY": -1.381577507287736, - "timestamp": 3.3567205079704117 - }, - { - "x": 2.026135553103086, - "y": 4.757769076744815, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559448429508e-19, - "velocityX": 1.1501637469359431, - "velocityY": -1.1471177723711978, - "timestamp": 3.3887171587793214 - }, - { - "x": 2.0637808893572873, - "y": 4.721931260837698, - "heading": 2.649783348569472, - "angularVelocity": -1.4846636373679086e-19, - "velocityX": 1.1765398972283652, - "velocityY": -1.120048973905845, - "timestamp": 3.4207138095882312 - }, - { - "x": 2.1014301321789435, - "y": 4.6860975489840255, - "heading": 2.649783348569472, - "angularVelocity": -1.484648310808098e-19, - "velocityX": 1.1766619902345172, - "velocityY": -1.1199207088167735, - "timestamp": 3.452710460397141 - }, - { - "x": 2.139079370280558, - "y": 4.650263832171171, - "heading": 2.649783348569472, - "angularVelocity": -1.48465589810743e-19, - "velocityX": 1.1766618427177973, - "velocityY": -1.1199208638074638, - "timestamp": 3.484707111206051 - }, - { - "x": 2.17672860838618, - "y": 4.614430115362526, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559463647802e-19, - "velocityX": 1.1766618428430395, - "velocityY": -1.1199208636758753, - "timestamp": 3.5167037620149606 - }, - { - "x": 2.2143778464918284, - "y": 4.57859639855391, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559409367017e-19, - "velocityX": 1.176661842843871, - "velocityY": -1.1199208636750018, - "timestamp": 3.5487004128238704 - }, - { - "x": 2.2520270845974713, - "y": 4.5427626817452875, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559515966518e-19, - "velocityX": 1.1766618428436975, - "velocityY": -1.1199208636751843, - "timestamp": 3.58069706363278 - }, - { - "x": 2.289676322703114, - "y": 4.506928964936664, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559610313848e-19, - "velocityX": 1.1766618428436995, - "velocityY": -1.1199208636751825, - "timestamp": 3.61269371444169 - }, - { - "x": 2.3273255608087573, - "y": 4.471095248128042, - "heading": 2.649783348569472, - "angularVelocity": -1.484655947429445e-19, - "velocityX": 1.1766618428436957, - "velocityY": -1.1199208636751863, - "timestamp": 3.6446903652505998 - }, - { - "x": 2.3649747989143997, - "y": 4.435261531319418, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559574023035e-19, - "velocityX": 1.1766618428436864, - "velocityY": -1.1199208636751963, - "timestamp": 3.6766870160595095 - }, - { - "x": 2.4026240370200447, - "y": 4.399427814510799, - "heading": 2.649783348569472, - "angularVelocity": -1.484655924580371e-19, - "velocityX": 1.1766618428437539, - "velocityY": -1.1199208636751252, - "timestamp": 3.7086836668684193 - }, - { - "x": 2.4402732751256884, - "y": 4.363594097702177, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559654067578e-19, - "velocityX": 1.1766618428437225, - "velocityY": -1.1199208636751585, - "timestamp": 3.740680317677329 - }, - { - "x": 2.4779225132411047, - "y": 4.327760380902407, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559492892923e-19, - "velocityX": 1.1766618431491565, - "velocityY": -1.1199208633985085, - "timestamp": 3.772676968486239 - }, - { - "x": 2.5129728702731393, - "y": 4.294400221963306, - "heading": 2.649783348569472, - "angularVelocity": -1.484655203554499e-19, - "velocityX": 1.0954383082580312, - "velocityY": -1.0426140891540008, - "timestamp": 3.8046736192951487 - }, - { - "x": 2.53926064365096, - "y": 4.2693800974206075, - "heading": 2.649783348569472, - "angularVelocity": -1.484655203554444e-19, - "velocityX": 0.8215789063304317, - "velocityY": -0.7819607337071065, - "timestamp": 3.8366702701040585 - }, - { - "x": 2.5567858275270727, - "y": 4.252700012844841, - "heading": 2.649783348569472, - "angularVelocity": -1.4846552035544855e-19, - "velocityX": 0.5477193216495216, - "velocityY": -0.5213072041629537, - "timestamp": 3.8686669209129683 - }, - { - "x": 2.5655484199523926, - "y": 4.244359970092773, - "heading": 2.649783348569472, - "angularVelocity": -1.4846552035527083e-19, - "velocityX": 0.2738596760533411, - "velocityY": -0.260653616588761, - "timestamp": 3.900663571721878 - }, - { - "x": 2.5655484199523926, - "y": 4.244359970092773, - "heading": 2.649783348569472, - "angularVelocity": -4.948850678520552e-20, - "velocityX": 6.0427487543509175e-18, - "velocityY": -9.335003878092332e-18, - "timestamp": 3.932660222530788 + "x": 1.2664719468759371, + "y": 5.574445984484986, + "heading": 2.936921539881242, + "angularVelocity": -2.425035961622917, + "velocityX": 0.4763363139734096, + "velocityY": -0.2336705712390479, + "timestamp": 0.08415481805151542 + }, + { + "x": 1.353066220057362, + "y": 5.511048868833387, + "heading": 2.8414585534340224, + "angularVelocity": -1.1343733924869426, + "velocityX": 1.028987706068311, + "velocityY": -0.753339109031048, + "timestamp": 0.16830963610303085 + }, + { + "x": 1.4537891107085041, + "y": 5.418622426082042, + "heading": 2.841458332012736, + "angularVelocity": -0.0000026311183569063204, + "velocityX": 1.196876102678809, + "velocityY": -1.0982905660228062, + "timestamp": 0.25246445415454627 + }, + { + "x": 1.554511990655445, + "y": 5.326195971665319, + "heading": 2.841458110592174, + "angularVelocity": -0.000002631109743829199, + "velocityX": 1.1968759754822746, + "velocityY": -1.0982907046408665, + "timestamp": 0.3366192722060617 + }, + { + "x": 1.6552348706023827, + "y": 5.233769517248598, + "heading": 2.8414578891715823, + "angularVelocity": -0.0000026311100983002847, + "velocityX": 1.196875975482236, + "velocityY": -1.098290704640839, + "timestamp": 0.4207740902575771 + }, + { + "x": 1.7559577505493258, + "y": 5.141343062831889, + "heading": 2.841457667750961, + "angularVelocity": -0.0000026311104525941368, + "velocityX": 1.1968759754823024, + "velocityY": -1.0982907046406973, + "timestamp": 0.5049289083090925 + }, + { + "x": 1.8566806304962746, + "y": 5.048916608415192, + "heading": 2.8414574463303097, + "angularVelocity": -0.0000026311108067721062, + "velocityX": 1.1968759754823688, + "velocityY": -1.0982907046405554, + "timestamp": 0.5890837263606079 + }, + { + "x": 1.957403510443229, + "y": 4.956490153998508, + "heading": 2.841457224909629, + "angularVelocity": -0.000002631111160911123, + "velocityX": 1.1968759754824354, + "velocityY": -1.0982907046404133, + "timestamp": 0.6732385444121234 + }, + { + "x": 2.058126390390189, + "y": 4.864063699581835, + "heading": 2.841457003488918, + "angularVelocity": -0.000002631111515369215, + "velocityX": 1.1968759754825018, + "velocityY": -1.0982907046402715, + "timestamp": 0.7573933624636389 + }, + { + "x": 2.158849270337155, + "y": 4.7716372451651745, + "heading": 2.8414567820681773, + "angularVelocity": -0.0000026311118694940932, + "velocityX": 1.1968759754825686, + "velocityY": -1.0982907046401296, + "timestamp": 0.8415481805151543 + }, + { + "x": 2.2595721502841255, + "y": 4.679210790748526, + "heading": 2.8414565606474067, + "angularVelocity": -0.000002631112223792794, + "velocityX": 1.196875975482635, + "velocityY": -1.0982907046399877, + "timestamp": 0.9257029985666698 + }, + { + "x": 2.3602950302311068, + "y": 4.586784336331894, + "heading": 2.8414563392266063, + "angularVelocity": -0.00000263111257823472, + "velocityX": 1.1968759754827565, + "velocityY": -1.098290704639786, + "timestamp": 1.0098578166181853 + }, + { + "x": 2.4610179157680534, + "y": 4.494357888007175, + "heading": 2.841456117805399, + "angularVelocity": -0.0000026311174148596253, + "velocityX": 1.1968760419075375, + "velocityY": -1.0982906322504282, + "timestamp": 1.0940126346697008 + }, + { + "x": 2.552458083373214, + "y": 4.421585489236337, + "heading": 2.775799514245796, + "angularVelocity": -0.7801882896284313, + "velocityX": 1.0865707956160633, + "velocityY": -0.8647442945725389, + "timestamp": 1.1781674527212163 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -2.1007177339153333, + "velocityX": 0.4933178798082817, + "velocityY": -0.38057445433049264, + "timestamp": 1.2623222707727317 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -1.7281844842695858e-24, + "velocityX": -1.1064710362675692e-24, + "velocityY": 7.502253930064392e-24, + "timestamp": 1.3464770888242472 + }, + { + "x": 2.5399319472382507, + "y": 4.43667284651752, + "heading": 2.699289039214539, + "angularVelocity": 1.1496592855145864, + "velocityX": -0.6195856870550529, + "velocityY": 0.5401708775667837, + "timestamp": 1.4336986203748725 + }, + { + "x": 2.4355875318293467, + "y": 4.532520831999065, + "heading": 2.6992890864875387, + "angularVelocity": 5.419877275969795e-7, + "velocityX": -1.19631487264518, + "velocityY": 1.0989028027547665, + "timestamp": 1.5209201519254978 + }, + { + "x": 2.331243168301696, + "y": 4.628368873961615, + "heading": 2.6992891337586675, + "angularVelocity": 5.419662778006892e-7, + "velocityX": -1.1963142778235591, + "velocityY": 1.0989034503128068, + "timestamp": 1.608141683476123 + }, + { + "x": 2.2268988047740805, + "y": 4.724216915924203, + "heading": 2.6992891810297954, + "angularVelocity": 5.419662665035014e-7, + "velocityX": -1.1963142778231501, + "velocityY": 1.098903450313253, + "timestamp": 1.6953632150267484 + }, + { + "x": 2.122554441246435, + "y": 4.82006495788676, + "heading": 2.699289228300922, + "angularVelocity": 5.419662547194978e-7, + "velocityX": -1.1963142778234912, + "velocityY": 1.0989034503128838, + "timestamp": 1.7825847465773736 + }, + { + "x": 2.0182100346281864, + "y": 4.915912952938407, + "heading": 2.699289275573439, + "angularVelocity": 5.419821924710352e-7, + "velocityX": -1.1963147718597973, + "velocityY": 1.0989029124765504, + "timestamp": 1.869806278127999 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 1.1079128329091787, + "velocityX": -0.6246895020546673, + "velocityY": 0.5396514365108126, + "timestamp": 1.9570278096786242 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 3.9745579613293646e-24, + "velocityX": 5.1209291302608005e-24, + "velocityY": -1.0346712550534795e-24, + "timestamp": 2.0442493412292495 } ], - "constraints": [ + "trajectoryWaypoints": [ { - "scope": [ - 2, - 3 - ], - "type": "ZeroAngularVelocity" + "timestamp": 0, + "isStopPoint": true, + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 }, { - "scope": [ - 4, - 5 - ], - "type": "ZeroAngularVelocity" + "timestamp": 1.3464770888242472, + "isStopPoint": true, + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "scope": [ - 3 - ], - "type": "WptZeroVelocity" - }, + "timestamp": 2.0442493412292495, + "isStopPoint": true, + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ { "scope": [ - 3 + "first" ], "type": "StopPoint" }, { "scope": [ - "first" + "last" ], "type": "StopPoint" }, { "scope": [ - "last" + 1 ], "type": "StopPoint" } @@ -1105,1333 +504,976 @@ "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, - "Test2": { + "FrontWing3Contested5": { "waypoints": [ { - "x": 0.3137660622596741, - "y": 6.865104675292969, - "heading": 3.135568639860519, + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 27 + "controlIntervalCount": 17 }, { - "x": 1.7816849946975708, - "y": 6.948645114898682, - "heading": 0, + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, "isInitialGuess": false, "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 24 + "headingConstrained": true, + "controlIntervalCount": 38 }, { - "x": 2.6290202140808105, - "y": 6.972513675689697, - "heading": 3.286405336491994, + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 36 }, { - "x": 7.999455451965332, - "y": 7.473754405975342, - "heading": 3.153293227446569, + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 47 - }, - { - "x": 2.4533867835998535, - "y": 5.739410400390625, - "heading": 5.008312945270705, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, "controlIntervalCount": 40 } ], "trajectory": [ { - "x": 0.3137660622596741, - "y": 6.865104675292969, - "heading": 3.135568639860519, - "angularVelocity": 1.3323616061473399e-26, - "velocityX": -9.030177506128314e-25, - "velocityY": 7.578270673418084e-24, + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -9.716627282364513e-28, + "velocityX": -2.1006296109192272e-27, + "velocityY": 1.6816897032610772e-27, "timestamp": 0 }, { - "x": 0.3285749308509426, - "y": 6.865947459797136, - "heading": 3.135568639860519, - "angularVelocity": 3.3337145202478786e-22, - "velocityX": 0.41797046933441173, - "velocityY": 0.023787032249159984, - "timestamp": 0.03543041836149472 - }, - { - "x": 0.35819266531481686, - "y": 6.867633028650745, - "heading": 3.135568639860519, - "angularVelocity": 3.3337147019213317e-22, - "velocityX": 0.8359408619363751, - "velocityY": 0.04757406013141577, - "timestamp": 0.07086083672298944 - }, - { - "x": 0.4026192574953566, - "y": 6.87016138138964, - "heading": 3.135568639860519, - "angularVelocity": 3.3337143389947597e-22, - "velocityX": 1.2539110243423486, - "velocityY": 0.07136107491303612, - "timestamp": 0.10629125508448417 - }, - { - "x": 0.4600803298886451, - "y": 6.873431536777097, - "heading": 3.135568639860519, - "angularVelocity": 3.331660178480041e-22, - "velocityX": 1.621800561512317, - "velocityY": 0.09229796143220789, - "timestamp": 0.14172167344597889 - }, - { - "x": 0.5175414022820589, - "y": 6.8767016921645565, - "heading": 3.135568639860519, - "angularVelocity": 3.3316629368551667e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233462, - "timestamp": 0.1771520918074736 - }, - { - "x": 0.5750024746754727, - "y": 6.879971847552017, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614681891394e-22, - "velocityX": 1.6218005615158566, - "velocityY": 0.09229796143233254, - "timestamp": 0.2125825101689683 - }, - { - "x": 0.6324635470688867, - "y": 6.883242002939478, - "heading": 3.135568639860519, - "angularVelocity": 3.331683104806914e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233347, - "timestamp": 0.24801292853046303 - }, - { - "x": 0.6899246194623005, - "y": 6.886512158326939, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614878052174e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233253, - "timestamp": 0.28344334689195777 - }, - { - "x": 0.7473856918557144, - "y": 6.8897823137144, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614758461554e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233254, - "timestamp": 0.3188737652534525 - }, - { - "x": 0.8048467642491284, - "y": 6.89305246910186, - "heading": 3.135568639860519, - "angularVelocity": 3.331638534198991e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233299, - "timestamp": 0.35430418361494725 - }, - { - "x": 0.8623078366425422, - "y": 6.89632262448932, - "heading": 3.135568639860519, - "angularVelocity": 3.3317076614902997e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233253, - "timestamp": 0.389734601976442 - }, - { - "x": 0.9197689090359561, - "y": 6.899592779876781, - "heading": 3.135568639860519, - "angularVelocity": 3.331661479116866e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233396, - "timestamp": 0.42516502033793674 - }, - { - "x": 0.97722998142937, - "y": 6.902862935264242, - "heading": 3.135568639860519, - "angularVelocity": 3.33166145502926e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233347, - "timestamp": 0.4605954386994315 - }, - { - "x": 1.0346910538227838, - "y": 6.906133090651703, - "heading": 3.135568639860519, - "angularVelocity": 3.3316600659908063e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233301, - "timestamp": 0.4960258570609262 - }, - { - "x": 1.0921521262161977, - "y": 6.9094032460391634, - "heading": 3.135568639860519, - "angularVelocity": 3.331661460674642e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.092297961432333, - "timestamp": 0.5314562754224209 - }, - { - "x": 1.1496131986096116, - "y": 6.912673401426624, - "heading": 3.135568639860519, - "angularVelocity": 3.331661475383932e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.092297961432333, - "timestamp": 0.5668866937839157 - }, - { - "x": 1.2070742710030253, - "y": 6.915943556814084, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614753921174e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233254, - "timestamp": 0.6023171121454104 - }, - { - "x": 1.2645353433964392, - "y": 6.919213712201545, - "heading": 3.135568639860519, - "angularVelocity": 3.331662731932369e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.092297961432333, - "timestamp": 0.6377475305069051 - }, - { - "x": 1.3219964157898532, - "y": 6.922483867589006, - "heading": 3.135568639860519, - "angularVelocity": 3.3316627143346654e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233393, - "timestamp": 0.6731779488683999 - }, - { - "x": 1.379457488183267, - "y": 6.925754022976466, - "heading": 3.135568639860519, - "angularVelocity": 3.331661483573468e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233157, - "timestamp": 0.7086083672298946 - }, - { - "x": 1.436918560576681, - "y": 6.929024178363926, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614890219236e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233392, - "timestamp": 0.7440387855913894 - }, - { - "x": 1.494379632970095, - "y": 6.932294333751387, - "heading": 3.135568639860519, - "angularVelocity": 3.331662743669949e-22, - "velocityX": 1.6218005615158566, - "velocityY": 0.09229796143233207, - "timestamp": 0.7794692039528841 - }, - { - "x": 1.5518407053635086, - "y": 6.935564489138847, - "heading": 3.135568639860519, - "angularVelocity": 3.331638909213181e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233347, - "timestamp": 0.8148996223143788 - }, - { - "x": 1.6093017777569225, - "y": 6.938834644526308, - "heading": 3.135568639860519, - "angularVelocity": 3.3316600948450887e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233622, - "timestamp": 0.8503300406758736 - }, - { - "x": 1.666762850150336, - "y": 6.942104799913777, - "heading": 3.135568639860519, - "angularVelocity": 3.331671581625241e-22, - "velocityX": 1.621800561515844, - "velocityY": 0.09229796143255317, - "timestamp": 0.8857604590373683 - }, - { - "x": 1.7242239225437261, - "y": 6.945374955301653, - "heading": 3.135568639860519, - "angularVelocity": 3.3334679678699274e-22, - "velocityX": 1.6218005615151887, - "velocityY": 0.09229796144406777, - "timestamp": 0.9211908773988631 - }, - { - "x": 1.7816849946975708, - "y": 6.948645114898682, - "heading": 3.135568639860519, - "angularVelocity": 1.7645788213167404e-22, - "velocityX": 1.6218005547541783, - "velocityY": 0.09229808024460076, - "timestamp": 0.9566212957603578 - }, - { - "x": 1.8178857407766151, - "y": 6.94853231479887, - "heading": 3.1384797599068888, - "angularVelocity": 0.12817174361754557, - "velocityX": 1.5938582646199322, - "velocityY": -0.004966399613447973, - "timestamp": 0.9793339464913666 - }, - { - "x": 1.8540603970824046, - "y": 6.948528771707492, - "heading": 3.1414999940123267, - "angularVelocity": 0.13297585302602363, - "velocityX": 1.592709575567121, - "velocityY": -0.00015599638369279018, - "timestamp": 1.0020465972223755 - }, - { - "x": 1.8901908310793394, - "y": 6.948640868874459, - "heading": 3.1447030512216227, - "angularVelocity": 0.14102524831780386, - "velocityX": 1.5907625413183106, - "velocityY": 0.0049354506567203535, - "timestamp": 1.0247592479533845 - }, - { - "x": 1.926257669084107, - "y": 6.9488776015696425, - "heading": 3.148167194864803, - "angularVelocity": 0.15252044704983372, - "velocityX": 1.5879625162168578, - "velocityY": 0.010422944375284186, - "timestamp": 1.0474718986843934 - }, - { - "x": 1.9622398024006038, - "y": 6.94925072993569, - "heading": 3.151976954699645, - "angularVelocity": 0.16773734954856945, - "velocityX": 1.584233110553301, - "velocityY": 0.01642821749276163, - "timestamp": 1.0701845494154023 - }, - { - "x": 1.9981171224268564, - "y": 6.949774504499785, - "heading": 3.156211459419019, - "angularVelocity": 0.1864381559653407, - "velocityX": 1.5796183567984214, - "velocityY": 0.023060917472754097, - "timestamp": 1.0928972001464112 - }, - { - "x": 2.033847488660219, - "y": 6.950468453992633, - "heading": 3.161038496208879, - "angularVelocity": 0.2125263513725333, - "velocityX": 1.5731482272380108, - "velocityY": 0.03055343478252191, - "timestamp": 1.1156098508774202 - }, - { - "x": 2.0694053227389695, - "y": 6.951354952814641, - "heading": 3.166554769842392, - "angularVelocity": 0.24287229609803831, - "velocityX": 1.565551925218666, - "velocityY": 0.039031059496573456, - "timestamp": 1.138322501608429 - }, - { - "x": 2.104784911670157, - "y": 6.952454847185295, - "heading": 3.1727732170956275, - "angularVelocity": 0.27378782542301205, - "velocityX": 1.5577040896809682, - "velocityY": 0.04842650836667849, - "timestamp": 1.161035152339438 - }, - { - "x": 2.1400047240253706, - "y": 6.9537191803371154, - "heading": 3.1796054213345153, - "angularVelocity": 0.3008105183231903, - "velocityX": 1.550669394441415, - "velocityY": 0.05566647269821489, - "timestamp": 1.183747803070447 - }, - { - "x": 2.175095855891208, - "y": 6.955084615506324, - "heading": 3.18691368664007, - "angularVelocity": 0.3217706903570311, - "velocityX": 1.5450038078527182, - "velocityY": 0.060117825320333544, - "timestamp": 1.2064604538014558 - }, - { - "x": 2.2101380445057046, - "y": 6.956471002562802, - "heading": 3.194369840668752, - "angularVelocity": 0.3282819833311086, - "velocityX": 1.5428489184071512, - "velocityY": 0.06104030185188484, - "timestamp": 1.2291731045324648 - }, - { - "x": 2.2451610724837057, - "y": 6.957849512193169, - "heading": 3.2018529792356185, - "angularVelocity": 0.32947006738627316, - "velocityX": 1.5420053076493287, - "velocityY": 0.060693471963871995, - "timestamp": 1.2518857552634737 - }, - { - "x": 2.2801629678458126, - "y": 6.959221595789265, - "heading": 3.2093707720743723, - "angularVelocity": 0.3309958369804056, - "velocityX": 1.5410748739388525, - "velocityY": 0.060410544429463274, - "timestamp": 1.2745984059944826 - }, - { - "x": 2.315144049729391, - "y": 6.960586594052822, - "heading": 3.2169217131634897, - "angularVelocity": 0.3324552989672944, - "velocityX": 1.540158491312511, - "velocityY": 0.06009858909567994, - "timestamp": 1.2973110567254915 - }, - { - "x": 2.3501040543802802, - "y": 6.961944386116353, - "heading": 3.2245066187504112, - "angularVelocity": 0.3339506989629348, - "velocityX": 1.539230496031878, - "velocityY": 0.05978131216881497, - "timestamp": 1.3200237074565004 - }, - { - "x": 2.3850426980471595, - "y": 6.963294864074251, - "heading": 3.232126373384074, - "angularVelocity": 0.3354850441678861, - "velocityX": 1.5382900076554706, - "velocityY": 0.05945928433854099, - "timestamp": 1.3427363581875094 - }, - { - "x": 2.419959710539657, - "y": 6.964637896582436, - "heading": 3.239781798872735, - "angularVelocity": 0.3370555722150803, - "velocityX": 1.5373376232491214, - "velocityY": 0.05913147364836824, - "timestamp": 1.3654490089185183 - }, - { - "x": 2.454854803951486, - "y": 6.965973356645481, - "heading": 3.247473776388813, - "angularVelocity": 0.3386648968090985, - "velocityX": 1.5363725628109135, - "velocityY": 0.05879807156202029, - "timestamp": 1.3881616596495272 - }, - { - "x": 2.48972762858419, - "y": 6.967301155248667, - "heading": 3.2552034126870217, - "angularVelocity": 0.3403229499609042, - "velocityX": 1.5353921057348447, - "velocityY": 0.05846075030657316, - "timestamp": 1.4108743103805361 - }, - { - "x": 2.5245780070203394, - "y": 6.968621054178632, - "heading": 3.262971149366171, - "angularVelocity": 0.34200044596925644, - "velocityX": 1.5344038372663136, - "velocityY": 0.05811294091547401, - "timestamp": 1.433586961111545 - }, - { - "x": 2.559405532428149, - "y": 6.969932985582836, - "heading": 3.2707782920481354, - "angularVelocity": 0.3437354263236719, - "velocityX": 1.5333976566751537, - "velocityY": 0.05776214409057404, - "timestamp": 1.456299611842554 - }, - { - "x": 2.594207801112396, - "y": 6.97123839692077, - "heading": 3.2786335848605472, - "angularVelocity": 0.34585539598366793, - "velocityX": 1.5322856454060756, - "velocityY": 0.05747507648462563, - "timestamp": 1.4790122625735629 - }, - { - "x": 2.6290202140808105, - "y": 6.972513675689697, - "heading": 3.286405336491994, - "angularVelocity": 0.3421772175995684, - "velocityX": 1.5327322812606092, - "velocityY": 0.05614838990089804, - "timestamp": 1.5017249133045718 - }, - { - "x": 2.7840510289336455, - "y": 6.98699854094784, - "heading": 3.286405287600397, - "angularVelocity": -5.100684883763116e-7, - "velocityX": 1.6173808639626774, - "velocityY": 0.1511154018498607, - "timestamp": 1.5975779175703702 - }, - { - "x": 2.93908160001489, - "y": 7.001486015045406, - "heading": 3.2864052387079807, - "angularVelocity": -5.100770354356606e-7, - "velocityX": 1.6173783207811374, - "velocityY": 0.15114261893547631, - "timestamp": 1.6934309218361685 - }, - { - "x": 3.094112171101734, - "y": 7.015973489083046, - "heading": 3.286405189815564, - "angularVelocity": -5.100770390190521e-7, - "velocityX": 1.617378320839561, - "velocityY": 0.15114261831028078, - "timestamp": 1.7892839261019668 - }, - { - "x": 3.2491427421885786, - "y": 7.030460963120681, - "heading": 3.286405140923147, - "angularVelocity": -5.100770430673946e-7, - "velocityX": 1.617378320839565, - "velocityY": 0.15114261831023715, - "timestamp": 1.8851369303677652 - }, - { - "x": 3.404173313275423, - "y": 7.0449484371583155, - "heading": 3.28640509203073, - "angularVelocity": -5.100770460466358e-7, - "velocityX": 1.6173783208395647, - "velocityY": 0.15114261831023496, - "timestamp": 1.9809899346335635 - }, - { - "x": 3.5592038843622675, - "y": 7.05943591119595, - "heading": 3.286405043138312, - "angularVelocity": -5.100770488976204e-7, - "velocityX": 1.6173783208395647, - "velocityY": 0.15114261831023276, - "timestamp": 2.076842938899362 - }, - { - "x": 3.714234455449112, - "y": 7.073923385233584, - "heading": 3.2864049942458946, - "angularVelocity": -5.100770524050277e-7, - "velocityX": 1.6173783208395642, - "velocityY": 0.15114261831023054, - "timestamp": 2.1726959431651602 - }, - { - "x": 3.8692650265359565, - "y": 7.088410859271219, - "heading": 3.2864049453534765, - "angularVelocity": -5.100770565197187e-7, - "velocityX": 1.6173783208395647, - "velocityY": 0.15114261831022827, - "timestamp": 2.2685489474309586 - }, - { - "x": 4.0242955976228005, - "y": 7.102898333308853, - "heading": 3.2864048964610575, - "angularVelocity": -5.100770606946053e-7, - "velocityX": 1.6173783208395642, - "velocityY": 0.15114261831022607, - "timestamp": 2.364401951696757 - }, - { - "x": 4.179326168709644, - "y": 7.117385807346487, - "heading": 3.286404847568639, - "angularVelocity": -5.100770634020749e-7, - "velocityX": 1.617378320839564, - "velocityY": 0.151142618310224, - "timestamp": 2.4602549559625553 - }, - { - "x": 4.334356739796489, - "y": 7.13187328138412, - "heading": 3.2864047986762195, - "angularVelocity": -5.100770666030285e-7, - "velocityX": 1.617378320839564, - "velocityY": 0.15114261831022163, - "timestamp": 2.5561079602283536 - }, - { - "x": 4.489387310883332, - "y": 7.146360755421753, - "heading": 3.2864047497838, - "angularVelocity": -5.100770706092546e-7, - "velocityX": 1.617378320839564, - "velocityY": 0.15114261831021944, - "timestamp": 2.651960964494152 - }, - { - "x": 4.644417881970177, - "y": 7.160848229459387, - "heading": 3.28640470089138, - "angularVelocity": -5.100770740673254e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.15114261831021727, - "timestamp": 2.7478139687599503 - }, - { - "x": 4.79944845305702, - "y": 7.175335703497019, - "heading": 3.28640465199896, - "angularVelocity": -5.100770777885376e-7, - "velocityX": 1.6173783208395638, - "velocityY": 0.15114261831021514, - "timestamp": 2.8436669730257487 - }, - { - "x": 4.954479024143865, - "y": 7.1898231775346515, - "heading": 3.286404603106539, - "angularVelocity": -5.100770810299081e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.15114261831021283, - "timestamp": 2.939519977291547 - }, - { - "x": 5.109509595230708, - "y": 7.2043106515722855, - "heading": 3.2864045542141187, - "angularVelocity": -5.100770846564483e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.15114261831021056, - "timestamp": 3.0353729815573454 - }, - { - "x": 5.264540166317553, - "y": 7.218798125609918, - "heading": 3.286404505321697, - "angularVelocity": -5.100770889061609e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.1511426183102084, - "timestamp": 3.1312259858231437 - }, - { - "x": 5.419570737404396, - "y": 7.233285599647549, - "heading": 3.2864044564292754, - "angularVelocity": -5.100770922378371e-7, - "velocityX": 1.6173783208395631, - "velocityY": 0.15114261831020614, - "timestamp": 3.227078990088942 - }, - { - "x": 5.574601308491241, - "y": 7.247773073685181, - "heading": 3.2864044075368533, - "angularVelocity": -5.100770952675197e-7, - "velocityX": 1.6173783208395631, - "velocityY": 0.15114261831020392, - "timestamp": 3.3229319943547404 - }, - { - "x": 5.7296318795780845, - "y": 7.262260547722812, - "heading": 3.286404358644431, - "angularVelocity": -5.100770991840634e-7, - "velocityX": 1.617378320839563, - "velocityY": 0.1511426183102018, - "timestamp": 3.4187849986205388 - }, - { - "x": 5.884662450664929, - "y": 7.276748021760444, - "heading": 3.286404309752008, - "angularVelocity": -5.100771023487481e-7, - "velocityX": 1.617378320839563, - "velocityY": 0.15114261831019962, - "timestamp": 3.514638002886337 - }, - { - "x": 6.039693021751773, - "y": 7.291235495798076, - "heading": 3.286404260859585, - "angularVelocity": -5.100771061559551e-7, - "velocityX": 1.6173783208395627, - "velocityY": 0.15114261831019746, - "timestamp": 3.6104910071521354 - }, - { - "x": 6.194723592838616, - "y": 7.305722969835706, - "heading": 3.2864042119671617, - "angularVelocity": -5.100771091660107e-7, - "velocityX": 1.6173783208395627, - "velocityY": 0.1511426183101951, - "timestamp": 3.706344011417934 - }, - { - "x": 6.349754163925461, - "y": 7.320210443873337, - "heading": 3.2864041630747383, - "angularVelocity": -5.100771129976334e-7, - "velocityX": 1.6173783208395625, - "velocityY": 0.15114261831019293, - "timestamp": 3.802197015683732 - }, - { - "x": 6.504784735012304, - "y": 7.334697917910969, - "heading": 3.286404114182314, - "angularVelocity": -5.100771163404491e-7, - "velocityX": 1.6173783208395625, - "velocityY": 0.1511426183101907, - "timestamp": 3.8980500199495305 - }, - { - "x": 6.659815306099149, - "y": 7.349185391948599, - "heading": 3.28640406528989, - "angularVelocity": -5.100771202032421e-7, - "velocityX": 1.6173783208395622, - "velocityY": 0.1511426183101885, - "timestamp": 3.993903024215329 - }, - { - "x": 6.814845877185992, - "y": 7.363672865986229, - "heading": 3.2864040163974653, - "angularVelocity": -5.100771233944824e-7, - "velocityX": 1.6173783208395625, - "velocityY": 0.15114261831018627, - "timestamp": 4.089756028481127 - }, - { - "x": 6.969876448272837, - "y": 7.378160340023859, - "heading": 3.2864039675050396, - "angularVelocity": -5.100771270486632e-7, - "velocityX": 1.6173783208395622, - "velocityY": 0.15114261831018408, - "timestamp": 4.1856090327469255 - }, - { - "x": 7.12490701935968, - "y": 7.392647814061489, - "heading": 3.286403918612615, - "angularVelocity": -5.100771304657537e-7, - "velocityX": 1.617378320839562, - "velocityY": 0.15114261831018191, - "timestamp": 4.281462037012724 - }, - { - "x": 7.279937590446525, - "y": 7.407135288099118, - "heading": 3.286403869720189, - "angularVelocity": -5.10077134427039e-7, - "velocityX": 1.6173783208395618, - "velocityY": 0.1511426183101796, - "timestamp": 4.377315041278522 - }, - { - "x": 7.4349681615333685, - "y": 7.421622762136748, - "heading": 3.286403820827763, - "angularVelocity": -5.100771387503622e-7, - "velocityX": 1.6173783208395618, - "velocityY": 0.15114261831017736, - "timestamp": 4.473168045544321 - }, - { - "x": 7.589998732620213, - "y": 7.436110236174376, - "heading": 3.2864037719353365, - "angularVelocity": -5.100771410554518e-7, - "velocityX": 1.6173783208395616, - "velocityY": 0.15114261831017525, - "timestamp": 4.569021049810119 - }, - { - "x": 7.745029303707056, - "y": 7.450597710212007, - "heading": 3.28640372304291, - "angularVelocity": -5.100771448287922e-7, - "velocityX": 1.6173783208395598, - "velocityY": 0.1511426183101905, - "timestamp": 4.664874054075917 - }, - { - "x": 7.90005987484092, - "y": 7.465085183746354, - "heading": 3.2864036741504377, - "angularVelocity": -5.1007762076643e-7, - "velocityX": 1.6173783213301043, - "velocityY": 0.15114261305961757, - "timestamp": 4.760727058341716 - }, - { - "x": 7.999455451965332, - "y": 7.473754405975342, - "heading": 3.153293227446569, - "angularVelocity": -1.3886935284235506, - "velocityX": 1.0369583915052876, - "velocityY": 0.09044288486721058, - "timestamp": 4.856580062607514 - }, - { - "x": 7.999455451965332, - "y": 7.473754405975342, - "heading": 3.153293227446569, - "angularVelocity": 5.239369635456734e-24, - "velocityX": 2.4359888438060422e-22, - "velocityY": -8.774946070326848e-22, - "timestamp": 4.952433066873312 - }, - { - "x": 7.928671024084623, - "y": 7.451618987983874, - "heading": 3.153293227446569, - "angularVelocity": 2.2670564289793535e-36, - "velocityX": -0.8934591439715088, - "velocityY": -0.2793989045646846, - "timestamp": 5.031658209240836 - }, - { - "x": 7.805841482889084, - "y": 7.413208234129291, - "heading": 3.153293227446569, - "angularVelocity": 3.3814188338755363e-34, - "velocityX": -1.5503858689926344, - "velocityY": -0.4848303544397123, - "timestamp": 5.1108833516083605 - }, - { - "x": 7.683011941693537, - "y": 7.374797480274705, - "heading": 3.153293227446569, - "angularVelocity": 6.0567834914455396e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.190108493975885 - }, - { - "x": 7.56018240049799, - "y": 7.336386726420119, - "heading": 3.153293227446569, - "angularVelocity": -6.892446902508999e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.269333636343409 - }, - { - "x": 7.437352859302442, - "y": 7.297975972565533, - "heading": 3.153293227446569, - "angularVelocity": -4.625140327997389e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.348558778710933 - }, - { - "x": 7.314523318106895, - "y": 7.2595652187109465, - "heading": 3.153293227446569, - "angularVelocity": 2.407409636834091e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.427783921078457 - }, - { - "x": 7.191693776911348, - "y": 7.22115446485636, - "heading": 3.153293227446569, - "angularVelocity": -8.071167864078362e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.507009063445981 - }, - { - "x": 7.0688642357158, - "y": 7.182743711001775, - "heading": 3.153293227446569, - "angularVelocity": 6.706721989227974e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.586234205813505 - }, - { - "x": 6.946034694520253, - "y": 7.1443329571471885, - "heading": 3.153293227446569, - "angularVelocity": -3.932169261853096e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.665459348181029 - }, - { - "x": 6.823205153324706, - "y": 7.105922203292602, - "heading": 3.153293227446569, - "angularVelocity": -5.711604766985455e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.744684490548553 - }, - { - "x": 6.700375612129158, - "y": 7.067511449438016, - "heading": 3.153293227446569, - "angularVelocity": 9.147104299173202e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.823909632916077 - }, - { - "x": 6.577546070933611, - "y": 7.0291006955834305, - "heading": 3.153293227446569, - "angularVelocity": 4.987399043745874e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.903134775283601 - }, - { - "x": 6.4547165297380635, - "y": 6.990689941728844, - "heading": 3.153293227446569, - "angularVelocity": 1.0726945561042014e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.9823599176511255 - }, - { - "x": 6.331886988542516, - "y": 6.952279187874258, - "heading": 3.153293227446569, - "angularVelocity": 1.8749531736864053e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.06158506001865 - }, - { - "x": 6.209057447346969, - "y": 6.913868434019672, - "heading": 3.153293227446569, - "angularVelocity": -8.352063684746622e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.140810202386174 - }, - { - "x": 6.0862279061514215, - "y": 6.875457680165086, - "heading": 3.153293227446569, - "angularVelocity": 6.429027209138877e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.220035344753698 - }, - { - "x": 5.963398364955874, - "y": 6.8370469263105, - "heading": 3.153293227446569, - "angularVelocity": -7.349253697576567e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.299260487121222 - }, - { - "x": 5.840568823760327, - "y": 6.798636172455914, - "heading": 3.153293227446569, - "angularVelocity": -5.316547406258321e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.378485629488746 - }, - { - "x": 5.7177392825647795, - "y": 6.760225418601328, - "heading": 3.153293227446569, - "angularVelocity": 3.261106516242251e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.45771077185627 - }, - { - "x": 5.594909741369232, - "y": 6.721814664746741, - "heading": 3.153293227446569, - "angularVelocity": -2.0479976334874182e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.536935914223794 - }, - { - "x": 5.472080200173685, - "y": 6.683403910892156, - "heading": 3.153293227446569, - "angularVelocity": -7.212902306224552e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.616161056591318 - }, - { - "x": 5.3492506589781375, - "y": 6.64499315703757, - "heading": 3.153293227446569, - "angularVelocity": 2.340395125909778e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.695386198958842 - }, - { - "x": 5.22642111778259, - "y": 6.606582403182983, - "heading": 3.153293227446569, - "angularVelocity": -2.132648663400479e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.774611341326366 - }, - { - "x": 5.103591576587043, - "y": 6.568171649328397, - "heading": 3.153293227446569, - "angularVelocity": -2.6423732915608732e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.8538364836938905 - }, - { - "x": 4.980762035391495, - "y": 6.529760895473811, - "heading": 3.153293227446569, - "angularVelocity": -1.5836433593674017e-28, - "velocityX": -1.5503858689927383, - "velocityY": -0.4848303544397451, - "timestamp": 6.933061626061415 - }, - { - "x": 4.857932494195948, - "y": 6.491350141619225, - "heading": 3.153293227446569, - "angularVelocity": 3.462450644614712e-28, - "velocityX": -1.5503858689927386, - "velocityY": -0.4848303544397447, - "timestamp": 7.012286768428939 - }, - { - "x": 4.735102953000401, - "y": 6.452939387764639, - "heading": 3.153293227446569, - "angularVelocity": 9.425461275139336e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.091511910796463 - }, - { - "x": 4.612273411804853, - "y": 6.414528633910053, - "heading": 3.153293227446569, - "angularVelocity": 2.8373243283513945e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.170737053163987 - }, - { - "x": 4.489443870609306, - "y": 6.3761178800554665, - "heading": 3.153293227446569, - "angularVelocity": 4.424546575494848e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.249962195531511 - }, - { - "x": 4.366614329413759, - "y": 6.337707126200881, - "heading": 3.153293227446569, - "angularVelocity": 2.6048393166300173e-30, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.329187337899035 - }, - { - "x": 4.243784788218211, - "y": 6.299296372346295, - "heading": 3.153293227446569, - "angularVelocity": -8.141761105066643e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.408412480266559 - }, - { - "x": 4.120955247022664, - "y": 6.2608856184917085, - "heading": 3.153293227446569, - "angularVelocity": -1.5769317536066289e-37, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.487637622634083 - }, - { - "x": 3.998125705827117, - "y": 6.222474864637123, - "heading": 3.153293227446569, - "angularVelocity": -1.9751877406999063e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.566862765001607 - }, - { - "x": 3.8752961646315702, - "y": 6.184064110782536, - "heading": 3.153293227446569, - "angularVelocity": 6.296656188647329e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.6460879073691315 - }, - { - "x": 3.7524666234360233, - "y": 6.145653356927951, - "heading": 3.153293227446569, - "angularVelocity": 3.349857109653225e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.725313049736656 - }, - { - "x": 3.6296370822404764, - "y": 6.107242603073365, - "heading": 3.153293227446569, - "angularVelocity": 1.1073851910178502e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.80453819210418 - }, - { - "x": 3.5068075410449295, - "y": 6.068831849218778, - "heading": 3.153293227446569, - "angularVelocity": -1.0776722091283702e-30, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.883763334471704 - }, - { - "x": 3.3839779998493826, - "y": 6.030421095364192, - "heading": 3.153293227446569, - "angularVelocity": -2.2657744543933122e-30, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.962988476839228 - }, - { - "x": 3.2611484586538357, - "y": 5.992010341509607, - "heading": 3.153293227446569, - "angularVelocity": 6.322058172960319e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.042213619206752 - }, - { - "x": 3.138318917458289, - "y": 5.95359958765502, - "heading": 3.153293227446569, - "angularVelocity": -1.267303885279781e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.121438761574275 - }, - { - "x": 3.015489376262742, - "y": 5.915188833800434, - "heading": 3.153293227446569, - "angularVelocity": 4.539047579816208e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.200663903941798 - }, - { - "x": 2.892659835067195, - "y": 5.876778079945848, - "heading": 3.153293227446569, - "angularVelocity": -2.4352191603126546e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.279889046309322 - }, - { - "x": 2.769830293871648, - "y": 5.838367326091261, - "heading": 3.153293227446569, - "angularVelocity": 1.9777124768596482e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.359114188676845 - }, - { - "x": 2.6470007526761012, - "y": 5.799956572236676, - "heading": 3.153293227446569, - "angularVelocity": 1.6826010379363495e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.438339331044368 - }, - { - "x": 2.5241712114805623, - "y": 5.761545818382093, - "heading": 3.153293227446569, - "angularVelocity": -1.9705474475500694e-33, - "velocityX": -1.5503858689926344, - "velocityY": -0.4848303544397123, - "timestamp": 8.517564473411891 - }, - { - "x": 2.4533867835998535, - "y": 5.739410400390625, - "heading": 3.153293227446569, - "angularVelocity": 2.748235734113482e-34, - "velocityX": -0.8934591439715088, - "velocityY": -0.2793989045646846, - "timestamp": 8.596789615779414 - }, - { - "x": 2.4533867835998535, - "y": 5.739410400390625, - "heading": 3.153293227446569, - "angularVelocity": -7.888608661057978e-31, - "velocityX": -1.038819230645035e-25, - "velocityY": -4.640517458430809e-27, - "timestamp": 8.676014758146938 + "x": 1.2629372172985613, + "y": 5.611868166522511, + "heading": 3.3611684637424437, + "angularVelocity": 2.6151040269567423, + "velocityX": 0.4341464801664794, + "velocityY": 0.21092109874697887, + "timestamp": 0.08419109200740245 + }, + { + "x": 1.3467908643065862, + "y": 5.672396849103953, + "heading": 3.4705725495976436, + "angularVelocity": 1.2994734151397, + "velocityX": 0.9959919156370148, + "velocityY": 0.7189440252909496, + "timestamp": 0.1683821840148049 + }, + { + "x": 1.4464012359618026, + "y": 5.766107214252463, + "heading": 3.4705728088664487, + "angularVelocity": 0.0000030795277620559296, + "velocityX": 1.1831462127425316, + "velocityY": 1.1130674625323809, + "timestamp": 0.25257327602220736 + }, + { + "x": 1.5460115925515412, + "y": 5.859817595415609, + "heading": 3.4705730681338594, + "angularVelocity": 0.0000030795111997424536, + "velocityX": 1.1831460337986872, + "velocityY": 1.1130676527500878, + "timestamp": 0.3367643680296098 + }, + { + "x": 1.6456219491412756, + "y": 5.9535279765787505, + "heading": 3.4705733274013144, + "angularVelocity": 0.000003079511724050569, + "velocityX": 1.183146033798638, + "velocityY": 1.1130676527500365, + "timestamp": 0.42095546003701223 + }, + { + "x": 1.7452323057310184, + "y": 6.0472383577418745, + "heading": 3.4705735866688134, + "angularVelocity": 0.000003079512247611481, + "velocityX": 1.1831460337987365, + "velocityY": 1.113067652749828, + "timestamp": 0.5051465520444147 + }, + { + "x": 1.8448426623207694, + "y": 6.1409487389049815, + "heading": 3.4705738459363564, + "angularVelocity": 0.0000030795127711126187, + "velocityX": 1.1831460337988353, + "velocityY": 1.1130676527496197, + "timestamp": 0.5893376440518172 + }, + { + "x": 1.9444530189105287, + "y": 6.234659120068071, + "heading": 3.470574105203944, + "angularVelocity": 0.000003079513294842049, + "velocityX": 1.183146033798934, + "velocityY": 1.1130676527494117, + "timestamp": 0.6735287360592197 + }, + { + "x": 2.0440633755002966, + "y": 6.328369501231142, + "heading": 3.470574364471575, + "angularVelocity": 0.0000030795138187518754, + "velocityX": 1.1831460337990323, + "velocityY": 1.1130676527492032, + "timestamp": 0.7577198280666222 + }, + { + "x": 2.1436737320900723, + "y": 6.422079882394196, + "heading": 3.4705746237392505, + "angularVelocity": 0.0000030795143424110884, + "velocityX": 1.1831460337991306, + "velocityY": 1.113067652748995, + "timestamp": 0.8419109200740247 + }, + { + "x": 2.2432840886798564, + "y": 6.515790263557233, + "heading": 3.47057488300697, + "angularVelocity": 0.0000030795148666763967, + "velocityX": 1.1831460337992294, + "velocityY": 1.1130676527487866, + "timestamp": 0.9261020120814272 + }, + { + "x": 2.342894445269649, + "y": 6.609500644720252, + "heading": 3.4705751422747335, + "angularVelocity": 0.0000030795153898968674, + "velocityX": 1.1831460337993278, + "velocityY": 1.1130676527485786, + "timestamp": 1.0102931040888297 + }, + { + "x": 2.4425048018594557, + "y": 6.703211025883247, + "heading": 3.470575401542541, + "angularVelocity": 0.000003079515913944498, + "velocityX": 1.1831460337994937, + "velocityY": 1.1130676527482986, + "timestamp": 1.0944841960962322 + }, + { + "x": 2.5421151652954292, + "y": 6.796921399768766, + "heading": 3.4705756608109883, + "angularVelocity": 0.0000030795235106850793, + "velocityX": 1.183146115116498, + "velocityY": 1.1130675663083123, + "timestamp": 1.1786752881036346 + }, + { + "x": 2.632005364209705, + "y": 6.868564037074326, + "heading": 3.5437695190039196, + "angularVelocity": 0.8693777031244078, + "velocityX": 1.0676925167614182, + "velocityY": 0.8509527029208858, + "timestamp": 1.2628663801110371 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 2.207199391525135, + "velocityX": 0.46866490416300804, + "velocityY": 0.3760569894002088, + "timestamp": 1.3470574721184396 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": -1.0211590332128658e-23, + "velocityX": -1.1480030002254912e-23, + "velocityY": -4.344519998520169e-23, + "timestamp": 1.4312485641258421 + }, + { + "x": 2.720827723501713, + "y": 6.91216585910853, + "heading": 3.486294732820543, + "angularVelocity": -2.69475141570759, + "velocityX": 0.5467552354822697, + "velocityY": 0.13225779017707837, + "timestamp": 1.5215356725454756 + }, + { + "x": 2.8392095989605575, + "y": 6.942185818091124, + "heading": 3.389947764111763, + "angularVelocity": -1.0671176693463396, + "velocityX": 1.311171412297561, + "velocityY": 0.33249441150564496, + "timestamp": 1.6118227809651091 + }, + { + "x": 2.985101373007063, + "y": 6.9572215101019514, + "heading": 3.3899472630700718, + "angularVelocity": -0.000005549426712336868, + "velocityX": 1.615864951266793, + "velocityY": 0.16653199192676998, + "timestamp": 1.7021098893847426 + }, + { + "x": 3.1309931487591207, + "y": 6.972257185567622, + "heading": 3.3899467620302377, + "angularVelocity": -0.000005549406141075235, + "velocityX": 1.6158649701561478, + "velocityY": 0.16653180870033996, + "timestamp": 1.7923969978043761 + }, + { + "x": 3.276884924511046, + "y": 6.987292861034595, + "heading": 3.389946260989866, + "angularVelocity": -0.000005549412101088006, + "velocityX": 1.615864970155008, + "velocityY": 0.16653180870111933, + "timestamp": 1.8826841062240096 + }, + { + "x": 3.4227767002625384, + "y": 7.002328536502587, + "heading": 3.3899457599489553, + "angularVelocity": -0.000005549418061933105, + "velocityX": 1.6158649701538554, + "velocityY": 0.16653180870202178, + "timestamp": 1.9729712146436431 + }, + { + "x": 3.5686684760143703, + "y": 7.017364211969376, + "heading": 3.389945258907507, + "angularVelocity": -0.000005549424021960657, + "velocityX": 1.6158649701527033, + "velocityY": 0.16653180870292417, + "timestamp": 2.0632583230632764 + }, + { + "x": 3.7145602517660743, + "y": 7.032399887436731, + "heading": 3.3899447578655204, + "angularVelocity": -0.000005549429982841055, + "velocityX": 1.6158649701515504, + "velocityY": 0.16653180870382667, + "timestamp": 2.1535454314829097 + }, + { + "x": 3.8604520275173986, + "y": 7.04743556290717, + "heading": 3.3899442568229956, + "angularVelocity": -0.000005549435944010941, + "velocityX": 1.615864970150398, + "velocityY": 0.1665318087047291, + "timestamp": 2.243832539902543 + }, + { + "x": 4.00634380326868, + "y": 7.062471238372739, + "heading": 3.389943755779933, + "angularVelocity": -0.000005549441903913974, + "velocityX": 1.6158649701492453, + "velocityY": 0.1665318087056315, + "timestamp": 2.334119648322176 + }, + { + "x": 4.1522355790200765, + "y": 7.077506913841867, + "heading": 3.389943254736332, + "angularVelocity": -0.0000055494478640805644, + "velocityX": 1.615864970148093, + "velocityY": 0.16653180870653383, + "timestamp": 2.4244067567418095 + }, + { + "x": 4.298127354771182, + "y": 7.0925425893096525, + "heading": 3.389942753692193, + "angularVelocity": -0.000005549453823846852, + "velocityX": 1.6158649701469403, + "velocityY": 0.16653180870743617, + "timestamp": 2.5146938651614428 + }, + { + "x": 4.444019130522188, + "y": 7.10757826477934, + "heading": 3.389942252647516, + "angularVelocity": -0.000005549459785648991, + "velocityX": 1.6158649701457874, + "velocityY": 0.16653180870833867, + "timestamp": 2.604980973581076 + }, + { + "x": 4.5899109062731025, + "y": 7.122613940245511, + "heading": 3.3899417516023003, + "angularVelocity": -0.000005549465746228024, + "velocityX": 1.6158649701446353, + "velocityY": 0.1665318087092411, + "timestamp": 2.6952680820007093 + }, + { + "x": 4.735802682023898, + "y": 7.137649615713241, + "heading": 3.389941250556547, + "angularVelocity": -0.00000554947170730299, + "velocityX": 1.6158649701434824, + "velocityY": 0.16653180871014361, + "timestamp": 2.7855551904203426 + }, + { + "x": 4.881694457774774, + "y": 7.152685291182225, + "heading": 3.389940749510255, + "angularVelocity": -0.000005549477667769927, + "velocityX": 1.61586497014233, + "velocityY": 0.1665318087110459, + "timestamp": 2.875842298839976 + }, + { + "x": 5.027586233525358, + "y": 7.167720966650761, + "heading": 3.3899402484634247, + "angularVelocity": -0.000005549483629157811, + "velocityX": 1.6158649701411771, + "velocityY": 0.16653180871194842, + "timestamp": 2.966129407259609 + }, + { + "x": 5.173478009275799, + "y": 7.182756642119004, + "heading": 3.3899397474160566, + "angularVelocity": -0.000005549489589198961, + "velocityX": 1.6158649701400245, + "velocityY": 0.1665318087128507, + "timestamp": 3.0564165156792424 + }, + { + "x": 5.319369785026301, + "y": 7.197792317586818, + "heading": 3.38993924636815, + "angularVelocity": -0.0000055494955497599425, + "velocityX": 1.615864970138872, + "velocityY": 0.16653180871375314, + "timestamp": 3.1467036240988757 + }, + { + "x": 5.4652615607764785, + "y": 7.212827993058087, + "heading": 3.3899387453197054, + "angularVelocity": -0.000005549501510730972, + "velocityX": 1.6158649701377192, + "velocityY": 0.16653180871465564, + "timestamp": 3.236990732518509 + }, + { + "x": 5.611153336526806, + "y": 7.227863668525338, + "heading": 3.3899382442707227, + "angularVelocity": -0.000005549507472025305, + "velocityX": 1.6158649701365668, + "velocityY": 0.16653180871555806, + "timestamp": 3.3272778409381423 + }, + { + "x": 5.757045112276785, + "y": 7.242899343993592, + "heading": 3.3899377432212017, + "angularVelocity": -0.000005549513433122579, + "velocityX": 1.6158649701354142, + "velocityY": 0.16653180871646053, + "timestamp": 3.4175649493577755 + }, + { + "x": 5.902936888026919, + "y": 7.257935019462863, + "heading": 3.3899372421711425, + "angularVelocity": -0.000005549519393768537, + "velocityX": 1.6158649701342616, + "velocityY": 0.1665318087173629, + "timestamp": 3.507852057777409 + }, + { + "x": 6.04882866377676, + "y": 7.272970694932001, + "heading": 3.389936741120545, + "angularVelocity": -0.000005549525355144424, + "velocityX": 1.6158649701331087, + "velocityY": 0.16653180871826534, + "timestamp": 3.598139166197042 + }, + { + "x": 6.194720439526491, + "y": 7.288006370401304, + "heading": 3.3899362400694093, + "angularVelocity": -0.000005549531315809206, + "velocityX": 1.615864970131956, + "velocityY": 0.16653180871916778, + "timestamp": 3.6884262746166754 + }, + { + "x": 6.34061221527629, + "y": 7.30304204587104, + "heading": 3.3899357390177354, + "angularVelocity": -0.000005549537277178968, + "velocityX": 1.6158649701308037, + "velocityY": 0.1665318087200703, + "timestamp": 3.7787133830363087 + }, + { + "x": 6.486503991025816, + "y": 7.318077721337987, + "heading": 3.389935237965523, + "angularVelocity": -0.0000055495432383876534, + "velocityX": 1.6158649701296508, + "velocityY": 0.16653180872097262, + "timestamp": 3.869000491455942 + }, + { + "x": 6.632395766775363, + "y": 7.333113396808981, + "heading": 3.389934736912773, + "angularVelocity": -0.000005549549199340779, + "velocityX": 1.615864970128498, + "velocityY": 0.16653180872187498, + "timestamp": 3.959287599875575 + }, + { + "x": 6.778287542524652, + "y": 7.3481490722785985, + "heading": 3.389934235859484, + "angularVelocity": -0.00000554955516023689, + "velocityX": 1.6158649701273455, + "velocityY": 0.16653180872277748, + "timestamp": 4.049574708295209 + }, + { + "x": 6.924179318273876, + "y": 7.363184747747038, + "heading": 3.3899337348056577, + "angularVelocity": -0.000005549561121255593, + "velocityX": 1.6158649701261931, + "velocityY": 0.16653180872368004, + "timestamp": 4.139861816714842 + }, + { + "x": 7.070071094023151, + "y": 7.378220423218671, + "heading": 3.389933233751293, + "angularVelocity": -0.0000055495670825865805, + "velocityX": 1.6158649701250403, + "velocityY": 0.1665318087245824, + "timestamp": 4.2301489251344755 + }, + { + "x": 7.215962869772269, + "y": 7.393256098685687, + "heading": 3.38993273269639, + "angularVelocity": -0.000005549573043968301, + "velocityX": 1.6158649701238874, + "velocityY": 0.16653180872548476, + "timestamp": 4.320436033554109 + }, + { + "x": 7.361854645521179, + "y": 7.4082917741551615, + "heading": 3.389932231640949, + "angularVelocity": -0.00000554957900552128, + "velocityX": 1.6158649701227348, + "velocityY": 0.16653180872638726, + "timestamp": 4.410723141973742 + }, + { + "x": 7.507746421269845, + "y": 7.423327449627381, + "heading": 3.3899317305849697, + "angularVelocity": -0.0000055495849662374065, + "velocityX": 1.615864970121582, + "velocityY": 0.16653180872728968, + "timestamp": 4.501010250393375 + }, + { + "x": 7.653638197018768, + "y": 7.4383631250955204, + "heading": 3.389931229528452, + "angularVelocity": -0.000005549590927964199, + "velocityX": 1.615864970120429, + "velocityY": 0.16653180872819684, + "timestamp": 4.591297358813009 + }, + { + "x": 7.79952997270157, + "y": 7.45339880119461, + "heading": 3.389930728467707, + "angularVelocity": -0.000005549637747689064, + "velocityX": 1.6158649693899627, + "velocityY": 0.16653181569922934, + "timestamp": 4.681584467232642 + }, + { + "x": 7.908273186523536, + "y": 7.465055949439882, + "heading": 3.2483837688222263, + "angularVelocity": -1.567742750023664, + "velocityX": 1.2044157324947478, + "velocityY": 0.1291119900382222, + "timestamp": 4.771871575652275 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "angularVelocity": -1.3108321526994196, + "velocityX": 0.3122914448573039, + "velocityY": -0.13619534121251922, + "timestamp": 4.862158684071908 + }, + { + "x": 7.897313989149588, + "y": 7.432003540468781, + "heading": 3.2231598958654137, + "angularVelocity": 0.9773255115620281, + "velocityX": -0.41091321048106677, + "velocityY": -0.21782102937809103, + "timestamp": 4.957446661958681 + }, + { + "x": 7.764258804459074, + "y": 7.410354349811778, + "heading": 3.302887541742508, + "angularVelocity": 0.8367020441112922, + "velocityX": -1.3963480770719, + "velocityY": -0.2271975031683705, + "timestamp": 5.052734639845453 + }, + { + "x": 7.611104944022113, + "y": 7.387923020288099, + "heading": 3.3028890943761, + "angularVelocity": 0.000016294118377845236, + "velocityX": -1.607273696395563, + "velocityY": -0.23540566212575834, + "timestamp": 5.1480226177322255 + }, + { + "x": 7.457951083368431, + "y": 7.365491692124674, + "heading": 3.3028906469716444, + "angularVelocity": 0.000016293719092580313, + "velocityX": -1.6072736986022782, + "velocityY": -0.23540564768491373, + "timestamp": 5.243310595618998 + }, + { + "x": 7.3047972227130815, + "y": 7.3430603639785605, + "heading": 3.3028921995394867, + "angularVelocity": 0.0000162934283709874, + "velocityX": -1.6072736986590501, + "velocityY": -0.23540564774414025, + "timestamp": 5.33859857350577 + }, + { + "x": 7.15164336205228, + "y": 7.320629035813856, + "heading": 3.3028937520796284, + "angularVelocity": 0.000016293137669547086, + "velocityX": -1.6072736987158178, + "velocityY": -0.23540564780336143, + "timestamp": 5.4338865513925425 + }, + { + "x": 6.998489501387815, + "y": 7.298197707651154, + "heading": 3.3028953045920724, + "angularVelocity": 0.00001629284698917995, + "velocityX": -1.6072736987725829, + "velocityY": -0.23540564786256962, + "timestamp": 5.529174529279315 + }, + { + "x": 6.845335640713116, + "y": 7.275766379466654, + "heading": 3.3028968570768193, + "angularVelocity": 0.000016292556328916256, + "velocityX": -1.607273698829345, + "velocityY": -0.23540564792176438, + "timestamp": 5.624462507166087 + }, + { + "x": 6.692181780035948, + "y": 7.253335051289657, + "heading": 3.3028984095338725, + "angularVelocity": 0.0000162922656907247, + "velocityX": -1.6072736988861036, + "velocityY": -0.2354056479809454, + "timestamp": 5.7197504850528595 + }, + { + "x": 6.539027919353228, + "y": 7.230903723107964, + "heading": 3.302899961963233, + "angularVelocity": 0.00001629197507403925, + "velocityX": -1.6072736989428598, + "velocityY": -0.23540564804011319, + "timestamp": 5.815038462939632 + }, + { + "x": 6.385874058664987, + "y": 7.2084723949147005, + "heading": 3.3029015143649034, + "angularVelocity": 0.000016291684475842535, + "velocityX": -1.607273698999613, + "velocityY": -0.2354056480992679, + "timestamp": 5.910326440826404 + }, + { + "x": 6.232720197971391, + "y": 7.186041066717774, + "heading": 3.3029030667388852, + "angularVelocity": 0.000016291393901831136, + "velocityX": -1.6072736990563632, + "velocityY": -0.2354056481584089, + "timestamp": 6.005614418713177 + }, + { + "x": 6.079566337272002, + "y": 7.163609738524357, + "heading": 3.302904619085181, + "angularVelocity": 0.000016291103348699848, + "velocityX": -1.6072736991131096, + "velocityY": -0.2354056482175359, + "timestamp": 6.100902396599949 + }, + { + "x": 5.9264124765671236, + "y": 7.141178410308786, + "heading": 3.3029061714037917, + "angularVelocity": 0.000016290812813611415, + "velocityX": -1.6072736991698542, + "velocityY": -0.23540564827665061, + "timestamp": 6.196190374486721 + }, + { + "x": 5.773258615858539, + "y": 7.118747082103052, + "heading": 3.302907723694721, + "angularVelocity": 0.000016290522303354113, + "velocityX": -1.6072736992265952, + "velocityY": -0.23540564833575084, + "timestamp": 6.291478352373494 + }, + { + "x": 5.620104755142444, + "y": 7.096315753876903, + "heading": 3.30290927595797, + "angularVelocity": 0.000016290231812342035, + "velocityX": -1.6072736992833332, + "velocityY": -0.2354056483948381, + "timestamp": 6.386766330260266 + }, + { + "x": 5.466950894422645, + "y": 7.07388442565226, + "heading": 3.30291082819354, + "angularVelocity": 0.000016289941343623743, + "velocityX": -1.6072736993400683, + "velocityY": -0.23540564845391188, + "timestamp": 6.482054308147038 + }, + { + "x": 5.313797033694609, + "y": 7.0514530974310325, + "heading": 3.3029123804014344, + "angularVelocity": 0.000016289650893731917, + "velocityX": -1.607273699396801, + "velocityY": -0.23540564851297285, + "timestamp": 6.577342286033811 + }, + { + "x": 5.160643172963514, + "y": 7.02902176918985, + "heading": 3.3029139325816548, + "angularVelocity": 0.000016289360468842725, + "velocityX": -1.6072736994535295, + "velocityY": -0.23540564857201896, + "timestamp": 6.672630263920583 + }, + { + "x": 5.007489312227579, + "y": 7.006590440945802, + "heading": 3.3029154847342026, + "angularVelocity": 0.000016289070060669773, + "velocityX": -1.6072736995102561, + "velocityY": -0.23540564863105312, + "timestamp": 6.767918241807355 + }, + { + "x": 4.854335451483483, + "y": 6.984159112697374, + "heading": 3.30291703685908, + "angularVelocity": 0.00001628877967680068, + "velocityX": -1.6072736995669794, + "velocityY": -0.23540564869007324, + "timestamp": 6.863206219694128 + }, + { + "x": 4.701181590737123, + "y": 6.961727784450269, + "heading": 3.3029185889562895, + "angularVelocity": 0.00001628848931160111, + "velocityX": -1.6072736996236994, + "velocityY": -0.23540564874908013, + "timestamp": 6.9584941975809 + }, + { + "x": 4.548027729985049, + "y": 6.939296456188679, + "heading": 3.3029201410258326, + "angularVelocity": 0.000016288198966670516, + "velocityX": -1.6072736996804171, + "velocityY": -0.23540564880807416, + "timestamp": 7.053782175467672 + }, + { + "x": 4.394873869225821, + "y": 6.916865127932724, + "heading": 3.3029216930677117, + "angularVelocity": 0.000016287908648086418, + "velocityX": -1.607273699737131, + "velocityY": -0.2354056488670535, + "timestamp": 7.149070153354445 + }, + { + "x": 4.2417200084613995, + "y": 6.8944337996586516, + "heading": 3.302923245081929, + "angularVelocity": 0.000016287618349487135, + "velocityX": -1.607273699793842, + "velocityY": -0.23540564892601998, + "timestamp": 7.244358131241217 + }, + { + "x": 4.088566147690897, + "y": 6.872002471380619, + "heading": 3.3029247970684854, + "angularVelocity": 0.00001628732806480796, + "velocityX": -1.607273699850551, + "velocityY": -0.23540564898497424, + "timestamp": 7.339646109127989 + }, + { + "x": 3.935412286918391, + "y": 6.849571143103449, + "heading": 3.3029263490273832, + "angularVelocity": 0.00001628703780483265, + "velocityX": -1.6072736999072568, + "velocityY": -0.2354056490439145, + "timestamp": 7.434934087014762 + }, + { + "x": 3.7822584261353844, + "y": 6.827139814806336, + "heading": 3.302927900958626, + "angularVelocity": 0.000016286747572595648, + "velocityX": -1.6072736999639583, + "velocityY": -0.2354056491028402, + "timestamp": 7.530222064901534 + }, + { + "x": 3.629104565353085, + "y": 6.804708486525145, + "heading": 3.3029294528622137, + "angularVelocity": 0.000016286457354449532, + "velocityX": -1.607273700020658, + "velocityY": -0.2354056491617539, + "timestamp": 7.6255100427883065 + }, + { + "x": 3.475950704559541, + "y": 6.782277158221499, + "heading": 3.3029310047381495, + "angularVelocity": 0.000016286167156294597, + "velocityX": -1.607273700077355, + "velocityY": -0.2354056492206542, + "timestamp": 7.720798020675079 + }, + { + "x": 3.3227968437635895, + "y": 6.759845829932118, + "heading": 3.3029325565864354, + "angularVelocity": 0.000016285876982629207, + "velocityX": -1.6072737001340485, + "velocityY": -0.23540564927954047, + "timestamp": 7.816085998561851 + }, + { + "x": 3.16964298296173, + "y": 6.737414501611197, + "heading": 3.3029341084070736, + "angularVelocity": 0.00001628558683240146, + "velocityX": -1.6072737001907338, + "velocityY": -0.23540564933844627, + "timestamp": 7.9113739764486235 + }, + { + "x": 3.0164891229666777, + "y": 6.714983167772551, + "heading": 3.302935660207411, + "angularVelocity": 0.00001628537379077082, + "velocityX": -1.607273691741332, + "velocityY": -0.23540570734761523, + "timestamp": 8.006661954335396 + }, + { + "x": 2.8768170840789846, + "y": 6.685183755439867, + "heading": 3.351795711849992, + "angularVelocity": 0.512761974030336, + "velocityX": -1.4657886753698728, + "velocityY": -0.3127300316833431, + "timestamp": 8.101949932222169 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 2.3762078855126445, + "velocityX": -0.6771226407787638, + "velocityY": -0.18518169637911341, + "timestamp": 8.197237910108942 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": -9.273365059762218e-23, + "velocityX": 5.842590739873529e-23, + "velocityY": -3.6959137012591573e-22, + "timestamp": 8.292525887995716 } ], - "constraints": [ + "trajectoryWaypoints": [ { - "scope": [ - "first" - ], - "type": "StopPoint" + "timestamp": 0, + "isStopPoint": true, + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 }, { - "scope": [ - "last" - ], - "type": "StopPoint" + "timestamp": 1.4312485641258421, + "isStopPoint": true, + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 38 }, { - "scope": [ - 0, - 1 - ], - "type": "ZeroAngularVelocity" + "timestamp": 4.862158684071908, + "isStopPoint": false, + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 36 }, + { + "timestamp": 8.292525887995716, + "isStopPoint": true, + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ { "scope": [ - 3 + "first" ], "type": "StopPoint" }, { "scope": [ - 3, - 4 + "last" ], - "type": "ZeroAngularVelocity" + "type": "StopPoint" }, { "scope": [ - 3 + 1 ], - "type": "WptZeroVelocity" + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, - "FrontWing2": { + "AmpWing3": { "waypoints": [ { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, + "x": 0.875186562538147, + "y": 6.525386810302734, + "heading": -2.143762813465651, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 10 + "controlIntervalCount": 18 }, { - "x": 2.617435932159424, - "y": 5.585470676422119, - "heading": 3.139529737290238, + "x": 2.6077051162719727, + "y": 7.001829624176025, + "heading": -3.122903212305612, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.0338082313537598, + "y": 6.676982402801514, + "heading": -2.588935989400422, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -2440,103 +1482,291 @@ ], "trajectory": [ { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, - "angularVelocity": -1.8084842834156656e-35, - "velocityX": 0, - "velocityY": 0, + "x": 0.875186562538147, + "y": 6.525386810302734, + "heading": -2.143762813465651, + "angularVelocity": -5.355945535245033e-29, + "velocityX": 2.43610406010345e-28, + "velocityY": -8.419270817143307e-28, "timestamp": 0 }, { - "x": 1.347146156455349, - "y": 5.593360447165824, - "heading": 3.140264869371664, - "angularVelocity": -0.007271662750997594, - "velocityX": 1.1945189788836181, - "velocityY": -0.007419158267666702, - "timestamp": 0.10109525888490682 - }, - { - "x": 1.5113646664969056, - "y": 5.59234048587755, - "heading": 3.1402648691641017, - "angularVelocity": -2.053134778232203e-9, - "velocityX": 1.6243937831794193, - "velocityY": -0.010089110998122406, - "timestamp": 0.20219051776981364 - }, - { - "x": 1.6755831765384634, - "y": 5.591320524589276, - "heading": 3.14026486895654, - "angularVelocity": -2.053134775032819e-9, - "velocityX": 1.624393783179429, - "velocityY": -0.010089110998122467, - "timestamp": 0.30328577665472045 - }, - { - "x": 1.8398016865800209, - "y": 5.5903005633010014, - "heading": 3.140264868748978, - "angularVelocity": -2.0531347455807303e-9, - "velocityX": 1.624393783179429, - "velocityY": -0.010089110998122468, - "timestamp": 0.4043810355396273 - }, - { - "x": 2.0040201966215783, - "y": 5.589280602012727, - "heading": 3.1402648685414154, - "angularVelocity": -2.0531349299385344e-9, - "velocityX": 1.624393783179429, - "velocityY": -0.01008911099812247, - "timestamp": 0.5054762944245341 - }, - { - "x": 2.168238706663136, - "y": 5.588260640724453, - "heading": 3.1402648683338534, - "angularVelocity": -2.053134785130435e-9, - "velocityX": 1.6243937831794293, - "velocityY": -0.010089110998122468, - "timestamp": 0.6065715533094409 - }, - { - "x": 2.3324572167046935, - "y": 5.587240679436179, - "heading": 3.1402648681262915, - "angularVelocity": -2.0531346533039746e-9, - "velocityX": 1.6243937831794288, - "velocityY": -0.010089110998122467, - "timestamp": 0.7076668121943477 - }, + "x": 0.8966416033945684, + "y": 6.537208607893962, + "heading": -2.364630778366916, + "angularVelocity": -2.838662406313846, + "velocityX": 0.27574672466546707, + "velocityY": 0.15193734597170733, + "timestamp": 0.07780705603103885 + }, + { + "x": 0.9545363231559146, + "y": 6.574628260062144, + "heading": -2.5677938220576824, + "angularVelocity": -2.611113362388633, + "velocityX": 0.7440805849054467, + "velocityY": 0.48092877531896794, + "timestamp": 0.1556141120620777 + }, + { + "x": 1.0367704734566419, + "y": 6.613390261480878, + "heading": -2.7011163551758717, + "angularVelocity": -1.7135018328543843, + "velocityX": 1.0568983649493484, + "velocityY": 0.49818105704026555, + "timestamp": 0.23342116809311655 + }, + { + "x": 1.1598322354696735, + "y": 6.642210050914376, + "heading": -2.701117699890818, + "angularVelocity": -0.000017282686365776702, + "velocityX": 1.5816272750885192, + "velocityY": 0.37040071818167397, + "timestamp": 0.3112282241241554 + }, + { + "x": 1.2828941160720575, + "y": 6.671029334799358, + "heading": -2.7011190439216617, + "angularVelocity": -0.00001727389406811502, + "velocityX": 1.5816287992350209, + "velocityY": 0.37039422071804506, + "timestamp": 0.3890352801551943 + }, + { + "x": 1.4059559966738584, + "y": 6.699848618684591, + "heading": -2.7011203879554953, + "angularVelocity": -0.000017273932497369342, + "velocityX": 1.5816287992275255, + "velocityY": 0.3703942207212755, + "timestamp": 0.46684233618623316 + }, + { + "x": 1.5290178772749528, + "y": 6.728667902570602, + "heading": -2.7011217319923193, + "angularVelocity": -0.00001727397092835146, + "velocityX": 1.5816287992184466, + "velocityY": 0.3703942207312666, + "timestamp": 0.544649392217272 + }, + { + "x": 1.6520797578753408, + "y": 6.75748718645739, + "heading": -2.7011230760321334, + "angularVelocity": -0.000017274009359396956, + "velocityX": 1.5816287992093674, + "velocityY": 0.37039422074125794, + "timestamp": 0.6224564482483109 + }, + { + "x": 1.7751416384750223, + "y": 6.786306470344956, + "heading": -2.701124420074938, + "angularVelocity": -0.000017274047791505146, + "velocityX": 1.5816287992002884, + "velocityY": 0.37039422075124934, + "timestamp": 0.7002635042793498 + }, + { + "x": 1.898203519073992, + "y": 6.81512575423332, + "heading": -2.7011257641207327, + "angularVelocity": -0.000017274086223033065, + "velocityX": 1.581628799191142, + "velocityY": 0.3703942207615267, + "timestamp": 0.7780705603103887 + }, + { + "x": 2.021265394654385, + "y": 6.843945059515562, + "heading": -2.7011271081971797, + "angularVelocity": -0.000017274480179424693, + "velocityX": 1.581628734690864, + "velocityY": 0.37039449572214483, + "timestamp": 0.8558776163414276 + }, + { + "x": 2.1197062132467392, + "y": 6.880631402807114, + "heading": -2.784861574958977, + "angularVelocity": -1.0761808894092335, + "velocityX": 1.2651914056879914, + "velocityY": 0.4715040661211618, + "timestamp": 0.9336846723724664 + }, + { + "x": 2.228161206604935, + "y": 6.914841049698727, + "heading": -2.8360417692393636, + "angularVelocity": -0.6577834568110343, + "velocityX": 1.393896632137462, + "velocityY": 0.43967280908258105, + "timestamp": 1.0114917284035052 + }, + { + "x": 2.342602667471001, + "y": 6.947096322596154, + "heading": -2.866867411656533, + "angularVelocity": -0.39618055211949504, + "velocityX": 1.4708365372468657, + "velocityY": 0.4145545988086245, + "timestamp": 1.089298784434544 + }, + { + "x": 2.460575781873871, + "y": 6.9780078455456644, + "heading": -2.885328334561652, + "angularVelocity": -0.23726540813669628, + "velocityX": 1.5162264249634116, + "velocityY": 0.39728431489785143, + "timestamp": 1.167105840465583 + }, + { + "x": 2.563184602851941, + "y": 6.995518176666575, + "heading": -2.9725697719134425, + "angularVelocity": -1.121253544369907, + "velocityX": 1.3187598427723222, + "velocityY": 0.22504811278202433, + "timestamp": 1.2449128964966218 + }, + { + "x": 2.6077051162719727, + "y": 7.001829624176025, + "heading": -3.122903212305612, + "angularVelocity": -1.9321311981293672, + "velocityX": 0.5721912084974853, + "velocityY": 0.08111664714486487, + "timestamp": 1.3227199525276607 + }, + { + "x": 2.6077051162719727, + "y": 7.001829624176025, + "heading": -3.122903212305612, + "angularVelocity": -5.850460431476152e-29, + "velocityX": -4.4073165345897844e-27, + "velocityY": 1.309519926897454e-26, + "timestamp": 1.4005270085586996 + }, + { + "x": 2.581564908929706, + "y": 6.99268752896088, + "heading": -2.9753776937459495, + "angularVelocity": 2.140746378487904, + "velocityX": -0.37932118285182687, + "velocityY": -0.1326611654355847, + "timestamp": 1.4694401315900536 + }, + { + "x": 2.510121983919206, + "y": 6.965703523715206, + "heading": -2.8481100670568136, + "angularVelocity": 1.846783618139497, + "velocityX": -1.0367100178872335, + "velocityY": -0.3915655546969944, + "timestamp": 1.5383532546214076 + }, + { + "x": 2.4183443604796597, + "y": 6.911820320186112, + "heading": -2.82871535814287, + "angularVelocity": 0.2814370915263766, + "velocityX": -1.3317873200694863, + "velocityY": -0.7819004735074551, + "timestamp": 1.6072663776527616 + }, + { + "x": 2.324659626384285, + "y": 6.850545155910229, + "heading": -2.828714867091888, + "angularVelocity": 0.000007125652715720525, + "velocityX": -1.3594614490588457, + "velocityY": -0.8891653952180393, + "timestamp": 1.6761795006841156 + }, + { + "x": 2.2309748959640565, + "y": 6.7892699860148875, + "heading": -2.8287143760416544, + "angularVelocity": 0.000007125641845880462, + "velocityX": -1.359461395728709, + "velocityY": -0.8891654767621404, + "timestamp": 1.7450926237154696 + }, + { + "x": 2.136919509671741, + "y": 6.728565387611802, + "heading": -2.828713709965057, + "angularVelocity": 0.000009665453660450363, + "velocityX": -1.3648399920799144, + "velocityY": -0.8808858999970965, + "timestamp": 1.8140057467468236 + }, + { + "x": 2.062260898631176, + "y": 6.694685917301612, + "heading": -2.7155906116912156, + "angularVelocity": 1.6415320231876958, + "velocityX": -1.0833729158754979, + "velocityY": -0.491625815518109, + "timestamp": 1.8829188697781776 + }, + { + "x": 2.03380823135376, + "y": 6.676982402801514, + "heading": -2.588935989400422, + "angularVelocity": 1.8378882964449172, + "velocityX": -0.4128773450663434, + "velocityY": -0.256896128362137, + "timestamp": 1.9518319928095316 + }, + { + "x": 2.03380823135376, + "y": 6.676982402801514, + "heading": -2.588935989400422, + "angularVelocity": -3.9702743259995845e-28, + "velocityX": -1.31352865617337e-26, + "velocityY": 2.3602776479280726e-26, + "timestamp": 2.0207451158408856 + } + ], + "trajectoryWaypoints": [ { - "x": 2.49667572674625, - "y": 5.586220718147905, - "heading": 3.140264867918729, - "angularVelocity": -2.0531352108819935e-9, - "velocityX": 1.624393783179419, - "velocityY": -0.010089110998122406, - "timestamp": 0.8087620710792545 + "timestamp": 0, + "isStopPoint": true, + "x": 0.875186562538147, + "y": 6.525386810302734, + "heading": -2.143762813465651, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 }, { - "x": 2.617435932159424, - "y": 5.585470676422119, - "heading": 3.139529737290238, - "angularVelocity": -0.0072716627525318345, - "velocityX": 1.1945189788836168, - "velocityY": -0.007419158267749976, - "timestamp": 0.9098573299641612 + "timestamp": 1.4005270085586996, + "isStopPoint": true, + "x": 2.6077051162719727, + "y": 7.001829624176025, + "heading": -3.122903212305612, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 }, { - "x": 2.617435932159424, - "y": 5.585470676422119, - "heading": 3.139529737290238, - "angularVelocity": 0, - "velocityX": -2.8085786967275447e-37, - "velocityY": 1.7440158343416895e-35, - "timestamp": 1.010952588849068 + "timestamp": 2.0207451158408856, + "isStopPoint": true, + "x": 2.0338082313537598, + "y": 6.676982402801514, + "heading": -2.588935989400422, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -2551,37 +1781,45 @@ "last" ], "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" } ], "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, - "FrontWing1": { + "SourceWing1": { "waypoints": [ { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, + "x": 0.7885606288909912, + "y": 4.587131977081299, + "heading": 2.068139227089701, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 16 + "controlIntervalCount": 19 }, { - "x": 2.593973159790039, - "y": 4.3895583152771, - "heading": 2.599013995570559, + "x": 2.6077051162719727, + "y": 4.099861145019531, + "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 8 }, { - "x": 1.9637236595153809, - "y": 4.962982177734375, - "heading": 2.79592312968437, + "x": 2.152919054031372, + "y": 4.283941268920898, + "heading": 2.6349608130221878, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -2590,229 +1828,291 @@ ], "trajectory": [ { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, - "angularVelocity": -1.6112520136935844e-31, - "velocityX": 0, - "velocityY": 0, + "x": 0.7885606288909912, + "y": 4.587131977081299, + "heading": 2.068139227089701, + "angularVelocity": -4.000873155660283e-28, + "velocityX": -1.1169568989245428e-27, + "velocityY": -1.0444692137856757e-26, "timestamp": 0 }, { - "x": 1.2701999983580183, - "y": 5.571793741558495, - "heading": 2.9123192627847865, - "angularVelocity": -2.75479375784366, - "velocityX": 0.5278042458729727, - "velocityY": -0.26883784352486495, - "timestamp": 0.08301192659671743 - }, - { - "x": 1.3598509429602765, - "y": 5.50482600027521, - "heading": 2.837562269615421, - "angularVelocity": -0.9005572600735416, - "velocityX": 1.0799766765779808, - "velocityY": -0.806724335029887, - "timestamp": 0.16602385319343485 - }, - { - "x": 1.459326562321469, - "y": 5.413786422039427, - "heading": 2.837562034760696, - "angularVelocity": -0.000002829168462845694, - "velocityX": 1.1983292454403298, - "velocityY": -1.0967047985533822, - "timestamp": 0.2490357797901523 - }, - { - "x": 1.558802177818681, - "y": 5.322746839581507, - "heading": 2.837561799906192, - "angularVelocity": -0.000002829165803381977, - "velocityX": 1.198329198893034, - "velocityY": -1.0967048494152307, - "timestamp": 0.3320477063868697 - }, - { - "x": 1.6582777933158976, - "y": 5.231707257123597, - "heading": 2.8375615650516535, - "angularVelocity": -0.0000028291662173205513, - "velocityX": 1.1983291988930937, - "velocityY": -1.0967048494150837, - "timestamp": 0.4150596329835871 - }, - { - "x": 1.7577534088131206, - "y": 5.140667674665702, - "heading": 2.837561330197081, - "angularVelocity": -0.0000028291666305712727, - "velocityX": 1.19832919889317, - "velocityY": -1.0967048494149187, - "timestamp": 0.4980715595803045 - }, - { - "x": 1.8572290243103502, - "y": 5.04962809220782, - "heading": 2.837561095342474, - "angularVelocity": -0.000002829167043337366, - "velocityX": 1.1983291988932463, - "velocityY": -1.0967048494147538, - "timestamp": 0.581083486177022 - }, - { - "x": 1.9567046398075858, - "y": 4.958588509749952, - "heading": 2.8375608604878324, - "angularVelocity": -0.000002829167457373439, - "velocityX": 1.198329198893323, - "velocityY": -1.0967048494145886, - "timestamp": 0.6640954127737394 - }, - { - "x": 2.056180255304828, - "y": 4.867548927292098, - "heading": 2.8375606256331567, - "angularVelocity": -0.00000282916787026248, - "velocityX": 1.1983291988933988, - "velocityY": -1.096704849414424, - "timestamp": 0.7471073393704568 - }, - { - "x": 2.1556558708020765, - "y": 4.776509344834258, - "heading": 2.8375603907784472, - "angularVelocity": -0.0000028291682836294088, - "velocityX": 1.1983291988934754, - "velocityY": -1.0967048494142588, - "timestamp": 0.8301192659671742 - }, - { - "x": 2.2551314862993315, - "y": 4.685469762376432, - "heading": 2.8375601559237027, - "angularVelocity": -0.000002829168697323748, - "velocityX": 1.1983291988935516, - "velocityY": -1.096704849414094, - "timestamp": 0.9131311925638916 - }, - { - "x": 2.354607101796593, - "y": 4.594430179918619, - "heading": 2.8375599210689244, - "angularVelocity": -0.000002829169110584532, - "velocityX": 1.1983291988936342, - "velocityY": -1.096704849413922, - "timestamp": 0.996143119160609 - }, - { - "x": 2.45408271880603, - "y": 4.503390599113157, - "heading": 2.837559686214013, - "angularVelocity": -0.000002829170713915412, - "velocityX": 1.1983292171099962, - "velocityY": -1.0967048295089454, - "timestamp": 1.0791550457573265 - }, - { - "x": 2.5484359060007082, - "y": 4.424785420118879, - "heading": 2.7982046114916295, - "angularVelocity": -0.47408940300319863, - "velocityX": 1.13662206218943, - "velocityY": -0.9469142834878584, - "timestamp": 1.162166972354044 - }, - { - "x": 2.593973159790039, - "y": 4.3895583152771, - "heading": 2.599013995570559, - "angularVelocity": -2.3995421391526537, - "velocityX": 0.5485627867734783, - "velocityY": -0.4243619716588108, - "timestamp": 1.2451788989507615 - }, + "x": 0.8077628028564999, + "y": 4.577523858678819, + "heading": 2.29346899836082, + "angularVelocity": 2.9086898452542567, + "velocityX": 0.24787300899124248, + "velocityY": -0.12402727021663285, + "timestamp": 0.07746778902493198 + }, + { + "x": 0.8565084293998517, + "y": 4.542709239371979, + "heading": 2.521391906348417, + "angularVelocity": 2.942163586393862, + "velocityX": 0.6292373534458789, + "velocityY": -0.44940767956648386, + "timestamp": 0.15493557804986396 + }, + { + "x": 0.9320047259554965, + "y": 4.504567603535149, + "heading": 2.6737831770723717, + "angularVelocity": 1.96715657748938, + "velocityX": 0.9745508101612039, + "velocityY": -0.492354775022096, + "timestamp": 0.23240336707479595 + }, + { + "x": 1.0259716426537533, + "y": 4.468035910816095, + "heading": 2.7708019811352167, + "angularVelocity": 1.252376055700012, + "velocityX": 1.2129804900978245, + "velocityY": -0.4715726778686935, + "timestamp": 0.30987115609972793 + }, + { + "x": 1.1315666128977582, + "y": 4.434097705174351, + "heading": 2.8306190294366744, + "angularVelocity": 0.7721538081099244, + "velocityX": 1.3630822768159876, + "velocityY": -0.43809441406442845, + "timestamp": 0.38733894512465994 + }, + { + "x": 1.2441769248956835, + "y": 4.4023522435792275, + "heading": 2.866815836119521, + "angularVelocity": 0.46724977101382253, + "velocityX": 1.4536404538624839, + "velocityY": -0.4097891781177288, + "timestamp": 0.46480673414959195 + }, + { + "x": 1.360949666856534, + "y": 4.372150081143396, + "heading": 2.8884951991985233, + "angularVelocity": 0.2798500299527736, + "velocityX": 1.5073715595945123, + "velocityY": -0.3898673605633771, + "timestamp": 0.542274523174524 + }, + { + "x": 1.483714050289093, + "y": 4.344503608621978, + "heading": 2.888502422435885, + "angularVelocity": 0.00009324181640315531, + "velocityX": 1.584715208446818, + "velocityY": -0.356877004873872, + "timestamp": 0.619742312199456 + }, + { + "x": 1.6064784345242633, + "y": 4.316857140064094, + "heading": 2.88850964608229, + "angularVelocity": 0.00009324709657913404, + "velocityX": 1.5847152188073954, + "velocityY": -0.3568769537102325, + "timestamp": 0.697210101224388 + }, + { + "x": 1.729242818618949, + "y": 4.289210671426577, + "heading": 2.8885168702684654, + "angularVelocity": 0.00009325406426226353, + "velocityX": 1.5847152169939396, + "velocityY": -0.3568769547381692, + "timestamp": 0.77467789024932 + }, + { + "x": 1.8520072025730983, + "y": 4.261564202709402, + "heading": 2.888524094994609, + "angularVelocity": 0.00009326103448260538, + "velocityX": 1.5847152151798087, + "velocityY": -0.3568769557664531, + "timestamp": 0.852145679274252 + }, + { + "x": 1.9747715863866588, + "y": 4.233917733912545, + "heading": 2.8885313202609173, + "angularVelocity": 0.00009326800724451237, + "velocityX": 1.5847152133650135, + "velocityY": -0.35687695679502923, + "timestamp": 0.929613468299184 + }, + { + "x": 2.09753597005958, + "y": 4.206271265035981, + "heading": 2.8885385460675863, + "angularVelocity": 0.00009327498254687209, + "velocityX": 1.584715211549548, + "velocityY": -0.35687695782393064, + "timestamp": 1.007081257324116 + }, + { + "x": 2.2203003530348537, + "y": 4.178624793690813, + "heading": 2.888545772490986, + "angularVelocity": 0.00009328294366610178, + "velocityX": 1.5847152025439089, + "velocityY": -0.3568769896901373, + "timestamp": 1.084549046349048 + }, + { + "x": 2.339535767958966, + "y": 4.149427051618625, + "heading": 2.9014383733628053, + "angularVelocity": 0.16642531088204846, + "velocityX": 1.5391611975105928, + "velocityY": -0.37690170895145225, + "timestamp": 1.16201683537398 + }, + { + "x": 2.4602156964640742, + "y": 4.1208550801313235, + "heading": 2.9091068110983676, + "angularVelocity": 0.0989887259218709, + "velocityX": 1.5578078324433486, + "velocityY": -0.36882389244523134, + "timestamp": 1.239484624398912 + }, + { + "x": 2.563006485689252, + "y": 4.105677497528831, + "heading": 2.9949971538769358, + "angularVelocity": 1.1087233011248803, + "velocityX": 1.3268842511059193, + "velocityY": -0.1959212053619926, + "timestamp": 1.316952413423844 + }, + { + "x": 2.6077051162719727, + "y": 4.099861145019531, + "heading": 3.141592653589793, + "angularVelocity": 1.8923413402914784, + "velocityX": 0.5769963380306965, + "velocityY": -0.07508091533924856, + "timestamp": 1.394420202448776 + }, + { + "x": 2.6077051162719727, + "y": 4.099861145019531, + "heading": 3.141592653589793, + "angularVelocity": 9.529530648984287e-27, + "velocityX": -6.64992161175884e-27, + "velocityY": -1.7807748804280264e-26, + "timestamp": 1.471887991473708 + }, + { + "x": 2.582723056141482, + "y": 4.106470038143139, + "heading": 3.0254564505751103, + "angularVelocity": -1.8328957625774476, + "velocityX": -0.39427423116151483, + "velocityY": 0.10430349785119646, + "timestamp": 1.5352501342566507 + }, + { + "x": 2.5176865762372205, + "y": 4.122909182686089, + "heading": 2.894011234282021, + "angularVelocity": -2.0745071192332674, + "velocityX": -1.0264248816056334, + "velocityY": 0.25944742114017705, + "timestamp": 1.5986122770395934 + }, + { + "x": 2.4297948421318796, + "y": 4.1567674114258075, + "heading": 2.8608182151959642, + "angularVelocity": -0.5238620038429632, + "velocityX": -1.3871332351626442, + "velocityY": 0.5343605385269978, + "timestamp": 1.6619744198225361 + }, + { + "x": 2.336899878456132, + "y": 4.201089714349744, + "heading": 2.8608175146596406, + "angularVelocity": -0.00001105607059469762, + "velocityX": -1.4660956778872583, + "velocityY": 0.6995076393765645, + "timestamp": 1.7253365626054789 + }, + { + "x": 2.2457294091191753, + "y": 4.241992503114134, + "heading": 2.8495777745026225, + "angularVelocity": -0.17738888969588018, + "velocityX": -1.4388792003022337, + "velocityY": 0.6455398597315226, + "timestamp": 1.7886987053884216 + }, + { + "x": 2.180218037779224, + "y": 4.270414402297769, + "heading": 2.7322527229675986, + "angularVelocity": -1.8516585200872955, + "velocityX": -1.033919758117567, + "velocityY": 0.44856278426375645, + "timestamp": 1.8520608481713643 + }, + { + "x": 2.152919054031372, + "y": 4.283941268920898, + "heading": 2.634960813022188, + "angularVelocity": -1.5354895789856624, + "velocityX": -0.4308406021142514, + "velocityY": 0.21348499323117995, + "timestamp": 1.915422990954307 + }, + { + "x": 2.152919054031372, + "y": 4.283941268920898, + "heading": 2.634960813022188, + "angularVelocity": 1.4290307836372668e-27, + "velocityX": -1.821728341071586e-27, + "velocityY": 7.158150670810205e-27, + "timestamp": 1.9787851337372497 + } + ], + "trajectoryWaypoints": [ { - "x": 2.593973159790039, - "y": 4.3895583152771, - "heading": 2.599013995570559, - "angularVelocity": 0, - "velocityX": -4.934633813021939e-37, - "velocityY": -2.767548277117811e-37, - "timestamp": 1.328190825547479 - }, - { - "x": 2.534366599021267, - "y": 4.442118048365455, - "heading": 2.6991662321397754, - "angularVelocity": 1.173988745719896, - "velocityX": -0.6987106220563037, - "velocityY": 0.6161074104533542, - "timestamp": 1.4135001923828505 - }, - { - "x": 2.4322324251030567, - "y": 4.53578051923557, - "heading": 2.6991662757513564, - "angularVelocity": 5.112167893680353e-7, - "velocityX": -1.1972210990067154, - "velocityY": 1.097915438182342, - "timestamp": 1.498809559218222 - }, - { - "x": 2.330098253355979, - "y": 4.629442992473233, - "heading": 2.6991663193628423, - "angularVelocity": 5.112156854714172e-7, - "velocityX": -1.1972210735566082, - "velocityY": 1.0979154659348471, - "timestamp": 1.5841189260535935 - }, - { - "x": 2.227964081608902, - "y": 4.723105465710898, - "heading": 2.6991663629743283, - "angularVelocity": 5.112156767559466e-7, - "velocityX": -1.1972210735566007, - "velocityY": 1.0979154659348562, - "timestamp": 1.669428292888965 - }, - { - "x": 2.1258299098618236, - "y": 4.816767938948561, - "heading": 2.6991664065858134, - "angularVelocity": 5.112156675644069e-7, - "velocityX": -1.1972210735566096, - "velocityY": 1.0979154659348476, - "timestamp": 1.7547376597243365 - }, - { - "x": 2.023695735807304, - "y": 4.910430409670039, - "heading": 2.6991664501973878, - "angularVelocity": 5.112167177081662e-7, - "velocityX": -1.1972211006045375, - "velocityY": 1.0979154364400177, - "timestamp": 1.840047026559708 + "timestamp": 0, + "isStopPoint": true, + "x": 0.7885606288909912, + "y": 4.587131977081299, + "heading": 2.068139227089701, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 19 }, { - "x": 1.9637236595153809, - "y": 4.962982177734375, - "heading": 2.79592312968437, - "angularVelocity": 1.1341858822337967, - "velocityX": -0.702995210451579, - "velocityY": 0.6160140441054905, - "timestamp": 1.9253563933950795 + "timestamp": 1.471887991473708, + "isStopPoint": true, + "x": 2.6077051162719727, + "y": 4.099861145019531, + "heading": 3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 }, { - "x": 1.9637236595153809, - "y": 4.962982177734375, - "heading": 2.79592312968437, - "angularVelocity": -1.527841446263418e-36, - "velocityX": 0, - "velocityY": 0, - "timestamp": 2.010665760230451 + "timestamp": 1.9787851337372497, + "isStopPoint": true, + "x": 2.152919054031372, + "y": 4.283941268920898, + "heading": 2.6349608130221878, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -2838,875 +2138,1142 @@ "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false }, - "FrontWing3Contested5": { + "FrontWing3": { "waypoints": [ { - "x": 1.2263859510421753, - "y": 5.594110488891602, + "x": 1.2650032043457031, + "y": 5.615814685821533, "heading": 3.141, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { - "x": 2.6714627742767334, - "y": 6.900224685668945, - "heading": 3.729596046054495, + "x": 2.6726744174957275, + "y": 6.893547058105469, + "heading": 3.6456400220863263, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 38 + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2650032043457031, + "y": 5.615814685821533, + "heading": 3.141, + "angularVelocity": -9.961906884794862e-27, + "velocityX": 6.399458285179746e-26, + "velocityY": 1.0314201039068904e-25, + "timestamp": 0 }, { - "x": 7.936469078063965, - "y": 7.452759265899658, - "heading": 3.1300325241315123, + "x": 1.310864623992567, + "y": 5.640347561603675, + "heading": 3.338413018068366, + "angularVelocity": 2.2732552294766206, + "velocityX": 0.5281045447942572, + "velocityY": 0.28250157315633995, + "timestamp": 0.08684155457274238 + }, + { + "x": 1.4026758531664765, + "y": 5.712962256076443, + "heading": 3.4150278612810783, + "angularVelocity": 0.8822371224197347, + "velocityX": 1.0572269189055614, + "velocityY": 0.8361745115000444, + "timestamp": 0.17368310914548477 + }, + { + "x": 1.505326244994725, + "y": 5.809724596960637, + "heading": 3.4150280522977696, + "angularVelocity": 0.0000021996000891052408, + "velocityX": 1.1820423106574365, + "velocityY": 1.1142400819545595, + "timestamp": 0.26052466371822713 + }, + { + "x": 1.6079766300540685, + "y": 5.906486945025769, + "heading": 3.4150282433141586, + "angularVelocity": 0.0000021995966111503638, + "velocityX": 1.182042232711982, + "velocityY": 1.1142401646446862, + "timestamp": 0.34736621829096953 + }, + { + "x": 1.7106270151134115, + "y": 6.003249293090899, + "heading": 3.4150284343305666, + "angularVelocity": 0.0000021995968268735563, + "velocityX": 1.182042232711972, + "velocityY": 1.1142401646446563, + "timestamp": 0.4342077728637119 + }, + { + "x": 1.8132774001727587, + "y": 6.100011641156021, + "heading": 3.4150286253469933, + "angularVelocity": 0.0000021995970422363537, + "velocityX": 1.1820422327120208, + "velocityY": 1.1142401646445645, + "timestamp": 0.5210493274364543 + }, + { + "x": 1.9159277852321102, + "y": 6.1967739892211355, + "heading": 3.4150288163634386, + "angularVelocity": 0.000002199597258109715, + "velocityX": 1.1820422327120694, + "velocityY": 1.1142401646444724, + "timestamp": 0.6078908820091966 + }, + { + "x": 2.018578170291466, + "y": 6.293536337286241, + "heading": 3.4150290073799026, + "angularVelocity": 0.0000021995974740409558, + "velocityX": 1.1820422327121183, + "velocityY": 1.1142401646443802, + "timestamp": 0.694732436581939 + }, + { + "x": 2.121228555350826, + "y": 6.390298685351339, + "heading": 3.415029198396385, + "angularVelocity": 0.000002199597689726547, + "velocityX": 1.182042232712167, + "velocityY": 1.1142401646442885, + "timestamp": 0.7815739911546813 + }, + { + "x": 2.22387894041019, + "y": 6.487061033416429, + "heading": 3.4150293894128865, + "angularVelocity": 0.0000021995979051884085, + "velocityX": 1.1820422327122158, + "velocityY": 1.1142401646441964, + "timestamp": 0.8684155457274236 + }, + { + "x": 2.3265293254695587, + "y": 6.583823381481512, + "heading": 3.415029580429407, + "angularVelocity": 0.000002199598120574345, + "velocityX": 1.1820422327122646, + "velocityY": 1.1142401646441045, + "timestamp": 0.955257100300166 + }, + { + "x": 2.4291797105289343, + "y": 6.680585729546584, + "heading": 3.415029771445946, + "angularVelocity": 0.0000021995983364688408, + "velocityX": 1.1820422327123468, + "velocityY": 1.114240164643977, + "timestamp": 1.0420986548729083 + }, + { + "x": 2.5318300994211365, + "y": 6.777348073545511, + "heading": 3.4150299624626883, + "angularVelocity": 0.0000021996006785706527, + "velocityX": 1.1820422768482035, + "velocityY": 1.1142401178214183, + "timestamp": 1.1289402094456507 + }, + { + "x": 2.6271100513248182, + "y": 6.858057139717029, + "heading": 3.467307103119733, + "angularVelocity": 0.6019830127897501, + "velocityX": 1.0971700399935989, + "velocityY": 0.9293830190926906, + "timestamp": 1.215781764018393 + }, + { + "x": 2.6726744174957275, + "y": 6.893547058105469, + "heading": 3.6456400220863263, + "angularVelocity": 2.0535436041418818, + "velocityX": 0.5246839073193058, + "velocityY": 0.40867437902337306, + "timestamp": 1.3026233185911353 + }, + { + "x": 2.6726744174957275, + "y": 6.893547058105469, + "heading": 3.6456400220863263, + "angularVelocity": 9.465973134511326e-26, + "velocityX": -9.092672379053801e-26, + "velocityY": -8.734024503832287e-26, + "timestamp": 1.3894648731638777 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 1.2650032043457031, + "y": 5.615814685821533, + "heading": 3.141, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 36 + "controlIntervalCount": 16 }, { - "x": 2.812295436859131, - "y": 6.667538166046143, - "heading": 3.5782197562990956, + "timestamp": 1.3894648731638777, + "isStopPoint": true, + "x": 2.6726744174957275, + "y": 6.893547058105469, + "heading": 3.6456400220863263, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, "controlIntervalCount": 40 } ], - "trajectory": [ + "constraints": [ { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, - "angularVelocity": 0, - "velocityX": -1.009927502555409e-33, - "velocityY": 9.510006388327946e-34, - "timestamp": 0 + "scope": [ + "first" + ], + "type": "StopPoint" }, { - "x": 1.2629372157532939, - "y": 5.6118681655215035, - "heading": 3.3611684682063676, - "angularVelocity": 2.615104090958444, - "velocityX": 0.4341464636351002, - "velocityY": 0.21092108774289103, - "timestamp": 0.08419109165389853 - }, - { - "x": 1.3467908645165432, - "y": 5.672396843669079, - "heading": 3.470572560445472, - "angularVelocity": 1.2994734964223353, - "velocityX": 0.9959919406671149, - "velocityY": 0.718943975645346, - "timestamp": 0.16838218330779706 - }, - { - "x": 1.4464012333437837, - "y": 5.766107211983687, - "heading": 3.4705728197139094, - "angularVelocity": 0.000003079523404990731, - "velocityX": 1.1831461841203954, - "velocityY": 1.1130675048120657, - "timestamp": 0.25257327496169557 - }, - { - "x": 1.5460115900456228, - "y": 5.859817593187631, - "heading": 3.4705730789812166, - "angularVelocity": 0.000003079509987095674, - "velocityX": 1.1831460400980154, - "velocityY": 1.1130676579082395, - "timestamp": 0.3367643666155941 - }, - { - "x": 1.645621946747464, - "y": 5.953527974391563, - "heading": 3.470573338248568, - "angularVelocity": 0.000003079510511134596, - "velocityX": 1.1831460400980387, - "velocityY": 1.1130676579081111, - "timestamp": 0.42095545826949265 - }, - { - "x": 1.7452323034493133, - "y": 6.047238355595477, - "heading": 3.470573597515964, - "angularVelocity": 0.000003079511034719833, - "velocityX": 1.183146040098137, - "velocityY": 1.1130676579079029, - "timestamp": 0.5051465499233911 - }, - { - "x": 1.844842660151171, - "y": 6.140948736799373, - "heading": 3.470573856783404, - "angularVelocity": 0.0000030795115586857372, - "velocityX": 1.1831460400982357, - "velocityY": 1.1130676579076944, - "timestamp": 0.5893376415772896 - }, - { - "x": 1.944453016853037, - "y": 6.234659118003253, - "heading": 3.470574116050888, - "angularVelocity": 0.0000030795120819840736, - "velocityX": 1.1831460400983345, - "velocityY": 1.1130676579074863, - "timestamp": 0.6735287332311881 - }, - { - "x": 2.044063373554911, - "y": 6.328369499207114, - "heading": 3.470574375318416, - "angularVelocity": 0.0000030795126059487745, - "velocityX": 1.1831460400984328, - "velocityY": 1.1130676579072778, - "timestamp": 0.7577198248850866 - }, - { - "x": 2.1436737302567934, - "y": 6.4220798804109585, - "heading": 3.470574634585988, - "angularVelocity": 0.000003079513129853114, - "velocityX": 1.183146040098531, - "velocityY": 1.1130676579070695, - "timestamp": 0.8419109165389851 - }, - { - "x": 2.243284086958684, - "y": 6.515790261614786, - "heading": 3.470574893853604, - "angularVelocity": 0.0000030795136536386766, - "velocityX": 1.1831460400986302, - "velocityY": 1.1130676579068612, - "timestamp": 0.9261020081928836 - }, - { - "x": 2.342894443660583, - "y": 6.6095006428185945, - "heading": 3.4705751531212647, - "angularVelocity": 0.0000030795141771132826, - "velocityX": 1.1831460400987281, - "velocityY": 1.1130676579066532, - "timestamp": 1.010293099846782 - }, - { - "x": 2.442504800362493, - "y": 6.703211024022384, - "heading": 3.470575412388969, - "angularVelocity": 0.0000030795147010529834, - "velocityX": 1.1831460400988572, - "velocityY": 1.1130676579064127, - "timestamp": 1.0944841915006807 - }, - { - "x": 2.5421151619283058, - "y": 6.79692140005584, - "heading": 3.470575671657143, - "angularVelocity": 0.0000030795202773893193, - "velocityX": 1.1831460978710306, - "velocityY": 1.1130675964945436, - "timestamp": 1.1786752831545793 - }, - { - "x": 2.632005363421341, - "y": 6.868564036243049, - "heading": 3.5437695268680747, - "angularVelocity": 0.8693776713553536, - "velocityX": 1.0676925518743172, - "velocityY": 0.8509526932103911, - "timestamp": 1.2628663748084779 + "scope": [ + "last" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "SourceContested1": { + "waypoints": [ + { + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 57 }, { - "x": 2.6714627742767334, - "y": 6.900224685668945, - "heading": 3.729596046054495, - "angularVelocity": 2.207199307384385, - "velocityX": 0.46866491549483613, - "velocityY": 0.37605700085290744, - "timestamp": 1.3470574664623765 + "x": 7.956855773925781, + "y": 0.7431064248085022, + "heading": 3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 36 }, { - "x": 2.6714627742767334, - "y": 6.900224685668945, - "heading": 3.729596046054495, - "angularVelocity": 0, - "velocityX": -1.5194658088032264e-34, - "velocityY": -2.7343286363371283e-33, - "timestamp": 1.431248558116275 - }, - { - "x": 2.7208277247721226, - "y": 6.912165859423971, - "heading": 3.486294740771461, - "angularVelocity": -2.694751338131124, - "velocityX": 0.5467552516805291, - "velocityY": 0.13225779416916014, - "timestamp": 1.5215356661845725 - }, - { - "x": 2.839209601133149, - "y": 6.942185820162622, - "heading": 3.3899477762404344, - "angularVelocity": -1.0671176272269702, - "velocityX": 1.311171427392232, - "velocityY": 0.33249443227201864, - "timestamp": 1.61182277425287 - }, - { - "x": 2.985101375777118, - "y": 6.957221507964457, - "heading": 3.3899472751996993, - "angularVelocity": -0.0000055494161427999685, - "velocityX": 1.615864964171956, - "velocityY": 0.16653194596133747, - "timestamp": 1.7021098823211673 - }, - { - "x": 3.130993151670898, - "y": 6.97225718364292, - "heading": 3.3899467741601925, - "angularVelocity": -0.000005549402536979108, - "velocityX": 1.6158649780145908, - "velocityY": 0.1665318116855625, - "timestamp": 1.7923969903894648 - }, - { - "x": 3.2768849275645753, - "y": 6.98729285932146, - "heading": 3.3899462731201484, - "angularVelocity": -0.00000554940849560343, - "velocityX": 1.6158649780134444, - "velocityY": 0.1665318116864077, - "timestamp": 1.8826840984577622 - }, - { - "x": 3.4227767034581484, - "y": 7.002328535000082, - "heading": 3.3899457720795656, - "angularVelocity": -0.0000055494144557147385, - "velocityX": 1.615864978012292, - "velocityY": 0.1665318116873102, - "timestamp": 1.9729712065260596 - }, - { - "x": 3.5686684793516172, - "y": 7.017364210678785, - "heading": 3.3899452710384446, - "angularVelocity": -0.0000055494204159770735, - "velocityX": 1.6158649780111394, - "velocityY": 0.1665318116882126, - "timestamp": 2.063258314594357 - }, - { - "x": 3.7145602552449817, - "y": 7.03239988635757, - "heading": 3.3899447699967857, - "angularVelocity": -0.000005549426375721003, - "velocityX": 1.6158649780099874, - "velocityY": 0.1665318116891151, - "timestamp": 2.1535454226626545 - }, - { - "x": 3.8604520311382426, - "y": 7.047435562036436, - "heading": 3.3899442689545887, - "angularVelocity": -0.000005549432334941507, - "velocityX": 1.615864978008835, - "velocityY": 0.16653181169001743, - "timestamp": 2.243832530730952 - }, - { - "x": 4.006343807031399, - "y": 7.062471237715384, - "heading": 3.389943767911854, - "angularVelocity": -0.000005549438294605419, - "velocityX": 1.6158649780076826, - "velocityY": 0.16653181169091993, - "timestamp": 2.3341196387992493 - }, - { - "x": 4.152235582924452, - "y": 7.077506913394413, - "heading": 3.3899432668685807, - "angularVelocity": -0.000005549444254929096, - "velocityX": 1.6158649780065304, - "velocityY": 0.16653181169182238, - "timestamp": 2.4244067468675468 - }, - { - "x": 4.298127358817401, - "y": 7.092542589073523, - "heading": 3.3899427658247694, - "angularVelocity": -0.000005549450214439604, - "velocityX": 1.615864978005378, - "velocityY": 0.1665318116927247, - "timestamp": 2.514693854935844 - }, - { - "x": 4.444019134710246, - "y": 7.107578264752715, - "heading": 3.38994226478042, - "angularVelocity": -0.000005549456174760312, - "velocityX": 1.6158649780042256, - "velocityY": 0.16653181169362713, - "timestamp": 2.6049809630041416 - }, - { - "x": 4.5899109106029865, - "y": 7.122613940431989, - "heading": 3.389941763735532, - "angularVelocity": -0.000005549462134716656, - "velocityX": 1.6158649780030732, - "velocityY": 0.16653181169452963, - "timestamp": 2.695268071072439 - }, - { - "x": 4.735802686495623, - "y": 7.137649616111343, - "heading": 3.389941262690107, - "angularVelocity": -0.0000055494680950242725, - "velocityX": 1.6158649780019205, - "velocityY": 0.1665318116954319, - "timestamp": 2.7855551791407365 - }, - { - "x": 4.881694462388156, - "y": 7.15268529179078, - "heading": 3.389940761644143, - "angularVelocity": -0.000005549474055578583, - "velocityX": 1.6158649780007681, - "velocityY": 0.16653181169633444, - "timestamp": 2.875842287209034 - }, - { - "x": 5.027586238280584, - "y": 7.167720967470297, - "heading": 3.3899402605976414, - "angularVelocity": -0.000005549480015574405, - "velocityX": 1.6158649779996155, - "velocityY": 0.1665318116972368, - "timestamp": 2.9661293952773313 - }, - { - "x": 5.173478014172908, - "y": 7.182756643149896, - "heading": 3.389939759550601, - "angularVelocity": -0.000005549485976020103, - "velocityX": 1.6158649779984633, - "velocityY": 0.16653181169813924, - "timestamp": 3.0564165033456288 - }, - { - "x": 5.319369790065129, - "y": 7.197792318829577, - "heading": 3.389939258503023, - "angularVelocity": -0.000005549491936617687, - "velocityX": 1.6158649779973107, - "velocityY": 0.16653181169904166, - "timestamp": 3.146703611413926 - }, - { - "x": 5.4652615659572445, - "y": 7.212827994509339, - "heading": 3.3899387574549067, - "angularVelocity": -0.00000554949789662324, - "velocityX": 1.6158649779961582, - "velocityY": 0.1665318116999441, - "timestamp": 3.2369907194822236 - }, - { - "x": 5.611153341849257, - "y": 7.2278636701891825, - "heading": 3.389938256406252, - "angularVelocity": -0.000005549503857497513, - "velocityX": 1.6158649779950054, - "velocityY": 0.16653181170084652, - "timestamp": 3.327277827550521 - }, - { - "x": 5.757045117741165, - "y": 7.242899345869108, - "heading": 3.3899377553570593, - "angularVelocity": -0.000005549509818264508, - "velocityX": 1.615864977993853, - "velocityY": 0.16653181170174894, - "timestamp": 3.4175649356188185 - }, - { - "x": 5.902936893632969, - "y": 7.257935021549114, - "heading": 3.3899372543073283, - "angularVelocity": -0.000005549515778536107, - "velocityX": 1.6158649779927006, - "velocityY": 0.1665318117026513, - "timestamp": 3.507852043687116 - }, - { - "x": 6.048828669524668, - "y": 7.272970697229202, - "heading": 3.3899367532570595, - "angularVelocity": -0.000005549521739714091, - "velocityX": 1.6158649779915477, - "velocityY": 0.16653181170355377, - "timestamp": 3.5981391517554133 - }, - { - "x": 6.194720445416264, - "y": 7.288006372909372, - "heading": 3.3899362522062524, - "angularVelocity": -0.000005549527700309559, - "velocityX": 1.6158649779903953, - "velocityY": 0.16653181170445627, - "timestamp": 3.6884262598237108 - }, - { - "x": 6.340612221307755, - "y": 7.303042048589623, - "heading": 3.389935751154907, - "angularVelocity": -0.000005549533660762894, - "velocityX": 1.6158649779892427, - "velocityY": 0.16653181170535863, - "timestamp": 3.778713367892008 - }, - { - "x": 6.486503997199144, - "y": 7.318077724269955, - "heading": 3.3899352501030235, - "angularVelocity": -0.000005549539621393899, - "velocityX": 1.6158649779880898, - "velocityY": 0.16653181170626094, - "timestamp": 3.8690004759603056 - }, - { - "x": 6.632395773090427, - "y": 7.333113399950369, - "heading": 3.3899347490506018, - "angularVelocity": -0.00000554954558299688, - "velocityX": 1.6158649779869374, - "velocityY": 0.1665318117071635, - "timestamp": 3.959287584028603 - }, - { - "x": 6.778287548981607, - "y": 7.3481490756308645, - "heading": 3.3899342479976413, - "angularVelocity": -0.000005549551543569729, - "velocityX": 1.615864977985785, - "velocityY": 0.16653181170806597, - "timestamp": 4.0495746920969005 - }, - { - "x": 6.924179324872682, - "y": 7.363184751311442, - "heading": 3.3899337469441435, - "angularVelocity": -0.000005549557505439446, - "velocityX": 1.615864977984632, - "velocityY": 0.16653181170896839, - "timestamp": 4.139861800165198 - }, - { - "x": 7.070071100763654, - "y": 7.378220426992101, - "heading": 3.389933245890107, - "angularVelocity": -0.00000554956346606953, - "velocityX": 1.6158649779834795, - "velocityY": 0.1665318117098707, - "timestamp": 4.230148908233495 - }, - { - "x": 7.215962876654522, - "y": 7.393256102672841, - "heading": 3.3899327448355323, - "angularVelocity": -0.000005549569427174084, - "velocityX": 1.6158649779823266, - "velocityY": 0.16653181171077316, - "timestamp": 4.320436016301793 - }, - { - "x": 7.361854652545285, - "y": 7.408291778353663, - "heading": 3.3899322437804194, - "angularVelocity": -0.000005549575389475234, - "velocityX": 1.615864977981174, - "velocityY": 0.16653181171167572, - "timestamp": 4.41072312437009 - }, - { - "x": 7.5077464284359445, - "y": 7.423327454034565, - "heading": 3.389931742724768, - "angularVelocity": -0.000005549581350385169, - "velocityX": 1.6158649779800212, - "velocityY": 0.1665318117125781, - "timestamp": 4.501010232438388 - }, - { - "x": 7.6536382043265, - "y": 7.43836312971555, - "heading": 3.3899312416685787, - "angularVelocity": -0.000005549587311867374, - "velocityX": 1.6158649779788683, - "velocityY": 0.166531811713483, - "timestamp": 4.591297340506685 - }, - { - "x": 7.799529980161501, - "y": 7.453398805927109, - "heading": 3.3899307406089574, - "angularVelocity": -0.000005549625325450788, - "velocityX": 1.6158649773635676, - "velocityY": 0.16653181759000288, - "timestamp": 4.6815844485749825 - }, - { - "x": 7.908273189210625, - "y": 7.465055953097054, - "heading": 3.2483837637195303, - "angularVelocity": -1.5677429471143751, - "velocityX": 1.204415684317477, - "velocityY": 0.12911197865732152, - "timestamp": 4.77187155664328 + "x": 3.387338399887085, + "y": 2.2590601444244385, + "heading": 2.3477629859714533, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "angularVelocity": -1.8256752618245288e-22, + "velocityX": -7.65916010942737e-22, + "velocityY": 5.834863940365417e-22, + "timestamp": 0 }, { - "x": 7.936469078063965, - "y": 7.452759265899658, - "heading": 3.1300325241315123, - "angularVelocity": -1.3108321012839574, - "velocityX": 0.3122914163117384, - "velocityY": -0.13619538226979142, - "timestamp": 4.862158664711577 - }, - { - "x": 7.897313987574637, - "y": 7.4320035366195905, - "heading": 3.223159902402127, - "angularVelocity": 0.9773255855261127, - "velocityX": -0.4109132292719665, - "velocityY": -0.21782107097140077, - "timestamp": 4.957446642075316 - }, - { - "x": 7.764258802337476, - "y": 7.410354345348057, - "heading": 3.302887545584371, - "angularVelocity": 0.8367020204228218, - "velocityX": -1.396348090475833, - "velocityY": -0.22719751085588358, - "timestamp": 5.052734619439054 - }, - { - "x": 7.611104941895525, - "y": 7.387923016344696, - "heading": 3.302889098211225, - "angularVelocity": 0.000016294047758499195, - "velocityX": -1.607273705236954, - "velocityY": -0.23540565792192894, - "timestamp": 5.148022596802792 - }, - { - "x": 7.457951081309142, - "y": 7.365491688274562, - "heading": 3.302890650804134, - "angularVelocity": 0.000016293691520657764, - "velocityX": -1.6072737067526932, - "velocityY": -0.2354056481281831, - "timestamp": 5.2433105741665305 - }, - { - "x": 7.30479722071746, - "y": 7.343060360198914, - "heading": 3.3028922033698884, - "angularVelocity": 0.000016293406553711894, - "velocityX": -1.6072737068083087, - "velocityY": -0.23540564818605267, - "timestamp": 5.338598551530269 - }, - { - "x": 7.151643360120445, - "y": 7.320629032117711, - "heading": 3.302893755908325, - "angularVelocity": 0.00001629311986139254, - "velocityX": -1.6072737068642717, - "velocityY": -0.23540564824432697, - "timestamp": 5.433886528894007 - }, - { - "x": 6.998489499518071, - "y": 7.298197704030925, - "heading": 3.302895308419307, - "angularVelocity": 0.00001629283173826444, - "velocityX": -1.6072737069205232, - "velocityY": -0.2354056483029315, - "timestamp": 5.529174506257745 - }, - { - "x": 6.845335638910315, - "y": 7.275766375938532, - "heading": 3.3028968609027314, - "angularVelocity": 0.000016292542536041963, - "velocityX": -1.6072737069769925, - "velocityY": -0.23540564836178338, - "timestamp": 5.624462483621484 - }, - { - "x": 6.692181778297163, - "y": 7.253335047840514, - "heading": 3.302898413358527, - "angularVelocity": 0.000016292252586671315, - "velocityX": -1.6072737070336132, - "velocityY": -0.23540564842080344, - "timestamp": 5.719750460985222 - }, - { - "x": 6.539027917678609, - "y": 7.230903719736863, - "heading": 3.3028999657866516, - "angularVelocity": 0.000016291962193735624, - "velocityX": -1.607273707090324, - "velocityY": -0.23540564847991918, - "timestamp": 5.81503843834896 - }, - { - "x": 6.385874057054646, - "y": 7.208472391627575, - "heading": 3.302901518187087, - "angularVelocity": 0.000016291671606816483, - "velocityX": -1.607273707147075, - "velocityY": -0.2354056485390708, - "timestamp": 5.9103264157126985 - }, - { - "x": 6.232720196425276, - "y": 7.186041063512652, - "heading": 3.3029030705598315, - "angularVelocity": 0.00001629138100746966, - "velocityX": -1.60727370720383, - "velocityY": -0.23540564859821628, - "timestamp": 6.005614393076437 - }, - { - "x": 6.079566335790499, - "y": 7.1636097353920976, - "heading": 3.3029046229048977, - "angularVelocity": 0.000016291090533089062, - "velocityX": -1.6072737072605614, - "velocityY": -0.23540564865732222, - "timestamp": 6.100902370440175 - }, - { - "x": 5.926412475150321, - "y": 7.141178407265915, - "heading": 3.302906175222303, - "angularVelocity": 0.000016290800243766995, - "velocityX": -1.6072737073172572, - "velocityY": -0.23540564871637523, - "timestamp": 6.196190347803913 - }, - { - "x": 5.773258614504743, - "y": 7.118747079134112, - "heading": 3.3029077275120664, - "angularVelocity": 0.00001629051016274816, - "velocityX": -1.6072737073739125, - "velocityY": -0.23540564877536937, - "timestamp": 6.291478325167652 - }, - { - "x": 5.62010475385377, - "y": 7.096315750996691, - "heading": 3.302909279774206, - "angularVelocity": 0.000016290220263808567, - "velocityX": -1.6072737074305328, - "velocityY": -0.23540564883431206, - "timestamp": 6.38676630253139 - }, - { - "x": 5.466950893197405, - "y": 7.073884422853658, - "heading": 3.302910832008734, - "angularVelocity": 0.00001628993049423592, - "velocityX": -1.6072737074871284, - "velocityY": -0.23540564889321514, - "timestamp": 6.482054279895128 - }, - { - "x": 5.313797032535647, - "y": 7.051453094705014, - "heading": 3.3029123842156554, - "angularVelocity": 0.000016289640769266148, - "velocityX": -1.6072737075437156, - "velocityY": -0.23540564895209914, - "timestamp": 6.5773422572588665 - }, - { - "x": 5.160643171868496, - "y": 7.02902176655076, - "heading": 3.3029139363949653, - "angularVelocity": 0.000016289351008016845, - "velocityX": -1.6072737076003119, - "velocityY": -0.23540564901098462, - "timestamp": 6.672630234622605 - }, - { - "x": 5.0074893111959495, - "y": 7.006590438390892, - "heading": 3.302915488546652, - "angularVelocity": 0.000016289061112899286, - "velocityX": -1.6072737076569361, - "velocityY": -0.23540564906989445, - "timestamp": 6.767918211986343 - }, - { - "x": 4.854335450518003, - "y": 6.984159110225406, - "heading": 3.3029170406706947, - "angularVelocity": 0.000016288771004663956, - "velocityX": -1.607273707713604, - "velocityY": -0.23540564912884793, - "timestamp": 6.863206189350081 - }, - { - "x": 4.701181589834651, - "y": 6.961727782054296, - "heading": 3.3029185927670666, - "angularVelocity": 0.00001628848060846621, - "velocityX": -1.6072737077703305, - "velocityY": -0.235405649187863, - "timestamp": 6.95849416671382 - }, - { - "x": 4.548027729145889, - "y": 6.939296453877556, - "heading": 3.3029201448357353, - "angularVelocity": 0.000016288189883493963, - "velocityX": -1.607273707827124, - "velocityY": -0.23540564924694996, - "timestamp": 7.053782144077558 - }, - { - "x": 4.3948738684517075, - "y": 6.916865125695178, - "heading": 3.3029216968766675, - "angularVelocity": 0.00001628789879930085, - "velocityX": -1.6072737078839905, - "velocityY": -0.23540564930611646, - "timestamp": 7.149070121441296 - }, - { - "x": 4.2417200077521, - "y": 6.894433797507156, - "heading": 3.302923248889828, - "angularVelocity": 0.00001628760735122893, - "velocityX": -1.6072737079409307, - "velocityY": -0.23540564936536426, - "timestamp": 7.2443580988050345 - }, - { - "x": 4.088566147047059, - "y": 6.872002469313479, - "heading": 3.3029248008751826, - "angularVelocity": 0.000016287315540072175, - "velocityX": -1.6072737079979444, - "velocityY": -0.23540564942469375, - "timestamp": 7.339646076168773 - }, - { - "x": 3.9354122863365792, - "y": 6.849571141114141, - "heading": 3.3029263528326944, - "angularVelocity": 0.000016287023346622306, - "velocityX": -1.6072737080550357, - "velocityY": -0.23540564948410944, - "timestamp": 7.434934053532511 - }, - { - "x": 3.7822584256206495, - "y": 6.827139812909132, - "heading": 3.3029279047623197, - "angularVelocity": 0.00001628673068839883, - "velocityX": -1.6072737081122201, - "velocityY": -0.23540564954363288, - "timestamp": 7.530222030896249 - }, - { - "x": 3.629104564899259, - "y": 6.804708484698436, - "heading": 3.3029294566639957, - "angularVelocity": 0.000016286437378238427, - "velocityX": -1.607273708169536, - "velocityY": -0.2354056496033085, - "timestamp": 7.625510008259988 - }, - { - "x": 3.4759507041723876, - "y": 6.7822771564820306, - "heading": 3.302931008537628, - "angularVelocity": 0.00001628614306837342, - "velocityX": -1.6072737082270525, - "velocityY": -0.2354056496632214, - "timestamp": 7.720797985623726 - }, - { - "x": 3.3227968434400057, - "y": 6.759845828259881, - "heading": 3.3029325603830677, - "angularVelocity": 0.000016285847206848686, - "velocityX": -1.6072737082848803, - "velocityY": -0.23540564972350414, - "timestamp": 7.816085962987464 - }, - { - "x": 3.1696429827020713, - "y": 6.737414500031936, - "heading": 3.3029341122001052, - "angularVelocity": 0.00001628554913981109, - "velocityX": -1.6072737083431483, - "velocityY": -0.2354056497843278, - "timestamp": 7.9113739403512024 - }, - { - "x": 3.016489122487879, - "y": 6.714983168191867, - "heading": 3.3029356639933174, - "angularVelocity": 0.000016285299102484297, - "velocityX": -1.6072737028467396, - "velocityY": -0.23540568769178183, - "timestamp": 8.006661917714942 - }, - { - "x": 2.8768170839321456, - "y": 6.685183755290752, - "heading": 3.3517957161483998, - "angularVelocity": 0.512761982223329, - "velocityX": -1.465788679956645, - "velocityY": -0.31273003925103915, - "timestamp": 8.10194989507868 + "x": 0.8644346902480902, + "y": 4.538605095268569, + "heading": 2.3610664104627634, + "angularVelocity": 2.6730073050757213, + "velocityX": 0.4641971643197831, + "velocityY": -0.4033128817553312, + "timestamp": 0.09347181429718082 + }, + { + "x": 0.9651901704673621, + "y": 4.45361330197089, + "heading": 2.440857637900237, + "angularVelocity": 0.8536394424076145, + "velocityX": 1.0779236604837226, + "velocityY": -0.9092772397405119, + "timestamp": 0.18694362859436164 + }, + { + "x": 1.0866380874820318, + "y": 4.362483747012655, + "heading": 2.440865267791502, + "angularVelocity": 0.00008162772192147527, + "velocityX": 1.299299879090211, + "velocityY": -0.9749415440733835, + "timestamp": 0.28041544289154247 + }, + { + "x": 1.2080860091215304, + "y": 4.271354198213206, + "heading": 2.4408728977229917, + "angularVelocity": 0.00008162815226084984, + "velocityX": 1.2992999285685376, + "velocityY": -0.9749414781841598, + "timestamp": 0.3738872571887233 + }, + { + "x": 1.3295339322232647, + "y": 4.1802246514640125, + "heading": 2.440880527950737, + "angularVelocity": 0.0000816313217297415, + "velocityX": 1.299299944212136, + "velocityY": -0.9749414562496848, + "timestamp": 0.4673590714859041 + }, + { + "x": 1.4509818569281794, + "y": 4.089095106952003, + "heading": 2.440888158472548, + "angularVelocity": 0.00008163446776368671, + "velocityX": 1.2992999613636238, + "velocityY": -0.9749414323153683, + "timestamp": 0.5608308857830849 + }, + { + "x": 1.5724297833961638, + "y": 3.997965564889279, + "heading": 2.440895789286055, + "angularVelocity": 0.00008163758844700525, + "velocityX": 1.2992999802256675, + "velocityY": -0.9749414061119055, + "timestamp": 0.6543027000802657 + }, + { + "x": 1.6938777118095218, + "y": 3.906836025517733, + "heading": 2.4409034203886897, + "angularVelocity": 0.00008164068165665073, + "velocityX": 1.299300001038079, + "velocityY": -0.9749413773205733, + "timestamp": 0.7477745143774466 + }, + { + "x": 1.8153256423773063, + "y": 3.8157064891148127, + "heading": 2.4409110517776624, + "angularVelocity": 0.00008164374501723712, + "velocityX": 1.299300024087019, + "velocityY": -0.9749413455609843, + "timestamp": 0.8412463286746275 + }, + { + "x": 1.9367735753408017, + "y": 3.7245769560008215, + "heading": 2.440918683449934, + "angularVelocity": 0.00008164677586345695, + "velocityX": 1.2993000497173246, + "velocityY": -0.9749413103746718, + "timestamp": 0.9347181429718083 + }, + { + "x": 2.0582215109806032, + "y": 3.6334474265483334, + "heading": 2.4409263154021836, + "angularVelocity": 0.0000816497711865655, + "velocityX": 1.2993000783495519, + "velocityY": -0.9749412712023965, + "timestamp": 1.0281899572689892 + }, + { + "x": 2.1796694496259765, + "y": 3.542317901194656, + "heading": 2.440933947630772, + "angularVelocity": 0.00008165272757387169, + "velocityX": 1.2993001105043924, + "velocityY": -0.9749412273516357, + "timestamp": 1.12166177156617 + }, + { + "x": 2.3011173916676397, + "y": 3.451188380458844, + "heading": 2.440941580131694, + "angularVelocity": 0.00008165564110490276, + "velocityX": 1.2993001468393033, + "velocityY": -0.9749411779478068, + "timestamp": 1.215133585863351 + }, + { + "x": 2.422565337575994, + "y": 3.3600588649659713, + "heading": 2.440949212900517, + "angularVelocity": 0.00008165850722838425, + "velocityX": 1.2993001882067592, + "velocityY": -0.97494112185668, + "timestamp": 1.3086054001605318 + }, + { + "x": 2.5440132879286748, + "y": 3.268929355483818, + "heading": 2.44095684593231, + "angularVelocity": 0.00008166132058271907, + "velocityX": 1.2993002357539973, + "velocityY": -0.9749410575515263, + "timestamp": 1.4020772144577127 + }, + { + "x": 2.6654612434555944, + "y": 3.1777998529828566, + "heading": 2.4409644792215364, + "angularVelocity": 0.00008166407471316297, + "velocityX": 1.2993002911101328, + "velocityY": -0.9749409828638569, + "timestamp": 1.4955490287548936 + }, + { + "x": 2.7869092051206428, + "y": 3.0866703587450828, + "heading": 2.4409721127619197, + "angularVelocity": 0.00008166676169345355, + "velocityX": 1.2993003567783705, + "velocityY": -0.9749408944608698, + "timestamp": 1.5890208430520745 + }, + { + "x": 2.908357174291307, + "y": 2.995540874590004, + "heading": 2.440979746546289, + "angularVelocity": 0.00008166937195400168, + "velocityX": 1.2993004370765409, + "velocityY": -0.9749407865920395, + "timestamp": 1.6824926573492553 + }, + { + "x": 3.0298051531585033, + "y": 2.9044114034341826, + "heading": 2.440987380566619, + "angularVelocity": 0.00008167189636088749, + "velocityX": 1.299300540814034, + "velocityY": -0.9749406475206273, + "timestamp": 1.7759644716464362 + }, + { + "x": 3.1512531460668667, + "y": 2.813281951064894, + "heading": 2.4409950148158184, + "angularVelocity": 0.00008167434490013359, + "velocityX": 1.2993006910322324, + "velocityY": -0.9749404465345578, + "timestamp": 1.869436285943617 + }, + { + "x": 3.272701165942649, + "y": 2.722152534713991, + "heading": 2.4410026493051027, + "angularVelocity": 0.00008167691342827456, + "velocityX": 1.2993009795407924, + "velocityY": -0.9749400611950203, + "timestamp": 1.962908100240798 + }, + { + "x": 3.3941493332630563, + "y": 2.6310233151653177, + "heading": 2.441010284564204, + "angularVelocity": 0.00008168514924485609, + "velocityX": 1.2993025569641734, + "velocityY": -0.9749379557236404, + "timestamp": 2.056379914537979 + }, + { + "x": 3.506643194922811, + "y": 2.543133701847389, + "heading": 2.478368219245545, + "angularVelocity": 0.39967058478790624, + "velocityX": 1.2035057038916106, + "velocityY": -0.9402793128471436, + "timestamp": 2.1498517288351597 + }, + { + "x": 3.621033917156409, + "y": 2.462292474945906, + "heading": 2.526376588848021, + "angularVelocity": 0.513613327862023, + "velocityX": 1.2237991002283153, + "velocityY": -0.8648727695009685, + "timestamp": 2.2433235431323406 + }, + { + "x": 3.7386117914711243, + "y": 2.3880497601699076, + "heading": 2.5783804352338895, + "angularVelocity": 0.5563585854932618, + "velocityX": 1.2578965669896234, + "velocityY": -0.7942791667652267, + "timestamp": 2.3367953574295215 + }, + { + "x": 3.8565091125978217, + "y": 2.322003920666466, + "heading": 2.644821825276836, + "angularVelocity": 0.7108173789342068, + "velocityX": 1.2613141406655402, + "velocityY": -0.7065856162100131, + "timestamp": 2.4302671717267024 + }, + { + "x": 3.9764532017194134, + "y": 2.262287896924173, + "heading": 2.7144845491695992, + "angularVelocity": 0.7452805363473517, + "velocityX": 1.2832113083869967, + "velocityY": -0.6388666379410853, + "timestamp": 2.5237389860238832 + }, + { + "x": 4.100272385460963, + "y": 2.211007382543893, + "heading": 2.7840189996546836, + "angularVelocity": 0.7439082145555573, + "velocityX": 1.3246686680102626, + "velocityY": -0.5486200815278979, + "timestamp": 2.617210800321064 + }, + { + "x": 4.2303737393158425, + "y": 2.1552421719955963, + "heading": 2.8233254460416966, + "angularVelocity": 0.42051656622437983, + "velocityX": 1.3918779134985075, + "velocityY": -0.5965992098002822, + "timestamp": 2.710682614618245 + }, + { + "x": 4.368649457253469, + "y": 2.0958799166815227, + "heading": 2.828594846354344, + "angularVelocity": 0.056374216679845754, + "velocityX": 1.479330630065645, + "velocityY": -0.6350818774667083, + "timestamp": 2.804154428915426 + }, + { + "x": 4.510357504555393, + "y": 2.041351647410644, + "heading": 2.8285983828965033, + "angularVelocity": 0.00003783538584099394, + "velocityX": 1.5160511044685778, + "velocityY": -0.5833659021265345, + "timestamp": 2.8976262432126068 + }, + { + "x": 4.652065594721857, + "y": 1.986823489488553, + "heading": 2.8286019193610827, + "angularVelocity": 0.00003783455586217891, + "velocityX": 1.516051563051105, + "velocityY": -0.5833647108713029, + "timestamp": 2.9910980575097876 + }, + { + "x": 4.793773697805799, + "y": 1.9322953651089438, + "heading": 2.8286054557754516, + "angularVelocity": 0.000037834018690956495, + "velocityX": 1.5160517012476136, + "velocityY": -0.5833643520200054, + "timestamp": 3.0845698718069685 + }, + { + "x": 4.935481808422067, + "y": 1.8777672602773423, + "heading": 2.8286089921399973, + "angularVelocity": 0.0000378334856573034, + "velocityX": 1.5160517818315487, + "velocityY": -0.5833641428873607, + "timestamp": 3.1780416861041494 + }, + { + "x": 5.0771899244715755, + "y": 1.8232391695375483, + "heading": 2.82861252845304, + "angularVelocity": 0.00003783293466201255, + "velocityX": 1.516051839958576, + "velocityY": -0.5833639921273963, + "timestamp": 3.2715135004013303 + }, + { + "x": 5.218898044817724, + "y": 1.76871108993463, + "heading": 2.8286160647128784, + "angularVelocity": 0.0000378323654513879, + "velocityX": 1.5160518859258105, + "velocityY": -0.5833638729805077, + "timestamp": 3.364985314698511 + }, + { + "x": 5.360606168722747, + "y": 1.7141830195502608, + "heading": 2.828619600918001, + "angularVelocity": 0.00003783178008651746, + "velocityX": 1.5160519240001233, + "velocityY": -0.5833637743566714, + "timestamp": 3.458457128995692 + }, + { + "x": 5.5023142956547595, + "y": 1.6596549570012744, + "heading": 2.828623137067082, + "angularVelocity": 0.0000378311805264838, + "velocityX": 1.516051956384105, + "velocityY": -0.5833636905305171, + "timestamp": 3.551928943292873 + }, + { + "x": 5.644022425205511, + "y": 1.605126901225908, + "heading": 2.828626673158945, + "angularVelocity": 0.00003783056838692872, + "velocityX": 1.5160519844004554, + "velocityY": -0.5833636180635361, + "timestamp": 3.6454007575900538 + }, + { + "x": 5.785730557049162, + "y": 1.5505988513766498, + "heading": 2.828630209192539, + "angularVelocity": 0.000037829944994143904, + "velocityX": 1.516052008930833, + "velocityY": -0.5833635546635878, + "timestamp": 3.7388725718872347 + }, + { + "x": 5.927438690918852, + "y": 1.4960708067593358, + "heading": 2.8286337451669143, + "angularVelocity": 0.00003782931145939862, + "velocityX": 1.5160520306062335, + "velocityY": -0.5833634986900924, + "timestamp": 3.8323443861844155 + }, + { + "x": 6.069146826592036, + "y": 1.4415427667950262, + "heading": 2.828637281081213, + "angularVelocity": 0.00003782866873129857, + "velocityX": 1.5160520499007515, + "velocityY": -0.583363448910333, + "timestamp": 3.9258162004815964 + }, + { + "x": 6.210854963880611, + "y": 1.387014730994346, + "heading": 2.828640816934652, + "angularVelocity": 0.000037828017628644405, + "velocityX": 1.516052067182881, + "velocityY": -0.5833634043661104, + "timestamp": 4.019288014778777 + }, + { + "x": 6.35256310262391, + "y": 1.332486698939259, + "heading": 2.8286443527265157, + "angularVelocity": 0.00003782735887192692, + "velocityX": 1.5160520827461144, + "velocityY": -0.5833633642942074, + "timestamp": 4.112759829075958 + }, + { + "x": 6.494271242683516, + "y": 1.2779586702695989, + "heading": 2.828647888456148, + "angularVelocity": 0.00003782669309301873, + "velocityX": 1.516052096828514, + "velocityY": -0.583363328075519, + "timestamp": 4.206231643373139 + }, + { + "x": 6.635979383939319, + "y": 1.223430644672807, + "heading": 2.828651424122945, + "angularVelocity": 0.0000378260208545108, + "velocityX": 1.5160521096259227, + "velocityY": -0.5833632952007055, + "timestamp": 4.2997034576703195 + }, + { + "x": 6.7776875262864404, + "y": 1.1689026218759404, + "heading": 2.8286549597263497, + "angularVelocity": 0.0000378253426576497, + "velocityX": 1.516052121301291, + "velocityY": -0.5833632652459484, + "timestamp": 4.3931752719675 + }, + { + "x": 6.9193956696327925, + "y": 1.114374601639329, + "heading": 2.8286584952658473, + "angularVelocity": 0.00003782465895599354, + "velocityX": 1.516052131991471, + "velocityY": -0.5833632378552868, + "timestamp": 4.486647086264681 + }, + { + "x": 7.061103813897117, + "y": 1.0598465837514708, + "heading": 2.828662030740962, + "angularVelocity": 0.000037823970152435987, + "velocityX": 1.5160521418123194, + "velocityY": -0.5833632127273561, + "timestamp": 4.580118900561862 + }, + { + "x": 7.202811959007387, + "y": 1.0053185680248766, + "heading": 2.8286655661512494, + "angularVelocity": 0.00003782327661332265, + "velocityX": 1.5160521508625917, + "velocityY": -0.5833631896052619, + "timestamp": 4.673590714859043 + }, + { + "x": 7.344520104899491, + "y": 0.9507905542926514, + "heading": 2.828669101496299, + "angularVelocity": 0.000037822578668805064, + "velocityX": 1.5160521592269784, + "velocityY": -0.5833631682686824, + "timestamp": 4.767062529156224 + }, + { + "x": 7.486228251516145, + "y": 0.8962625424056586, + "heading": 2.828672636775727, + "angularVelocity": 0.000037821876620878625, + "velocityX": 1.5160521669785085, + "velocityY": -0.5833631485276237, + "timestamp": 4.860534343453405 + }, + { + "x": 7.627936398805981, + "y": 0.8417345322301597, + "heading": 2.828676171989175, + "angularVelocity": 0.00003782117074336596, + "velocityX": 1.516052174180496, + "velocityY": -0.5833631302173573, + "timestamp": 4.954006157750586 + }, + { + "x": 7.769644550650757, + "y": 0.7872065338976291, + "heading": 2.8286797071997065, + "angularVelocity": 0.000037821139539143325, + "velocityX": 1.5160522229111162, + "velocityY": -0.5833630035164005, + "timestamp": 5.0474779720477665 + }, + { + "x": 7.89860842703643, + "y": 0.754016059353999, + "heading": 2.902617724341904, + "angularVelocity": 0.7910193858773472, + "velocityX": 1.3797087106456418, + "velocityY": -0.3550853783377481, + "timestamp": 5.140949786344947 + }, + { + "x": 7.956855773925781, + "y": 0.7431064248085022, + "heading": 3.141592653589793, + "angularVelocity": 2.5566523025657903, + "velocityX": 0.6231541275550934, + "velocityY": -0.1167157675019664, + "timestamp": 5.234421600642128 + }, + { + "x": 7.956855773925781, + "y": 0.7431064248085022, + "heading": 3.141592653589793, + "angularVelocity": -3.8315823290727504e-21, + "velocityX": 3.876602036733102e-21, + "velocityY": -3.3558838471561212e-21, + "timestamp": 5.327893414939309 + }, + { + "x": 7.9018904258286256, + "y": 0.7580071883715975, + "heading": 2.929746437641678, + "angularVelocity": -2.3660710929042184, + "velocityX": -0.6138977779803758, + "velocityY": 0.166423864457769, + "timestamp": 5.417428432117549 + }, + { + "x": 7.77927283258943, + "y": 0.7936488654478715, + "heading": 2.8587142949291526, + "angularVelocity": -0.7933448269867456, + "velocityX": -1.3694931559023147, + "velocityY": 0.39807528048295576, + "timestamp": 5.506963449295789 + }, + { + "x": 7.643021297144732, + "y": 0.8351292518640833, + "heading": 2.8460714923728077, + "angularVelocity": -0.14120511677768, + "velocityX": -1.5217681275858608, + "velocityY": 0.4632867421428616, + "timestamp": 5.596498466474029 + }, + { + "x": 7.50584253248777, + "y": 0.8779834119507215, + "heading": 2.8388066609223235, + "angularVelocity": -0.08113955499692087, + "velocityX": -1.532124178676125, + "velocityY": 0.4786301654617149, + "timestamp": 5.686033483652269 + }, + { + "x": 7.368255234864718, + "y": 0.9216599007543135, + "heading": 2.8342113751430746, + "angularVelocity": -0.051323894539503874, + "velocityX": -1.536687007600088, + "velocityY": 0.487814602376678, + "timestamp": 5.775568500830508 + }, + { + "x": 7.230614869935536, + "y": 0.9659066250673984, + "heading": 2.8305583662971365, + "angularVelocity": -0.040799778243928046, + "velocityX": -1.5372797064993953, + "velocityY": 0.4941834570155044, + "timestamp": 5.865103518008748 + }, + { + "x": 7.093252862402945, + "y": 1.010681152263652, + "heading": 2.826475143484932, + "angularVelocity": -0.045604758237506364, + "velocityX": -1.5341707843663217, + "velocityY": 0.5000783895212758, + "timestamp": 5.954638535186988 + }, + { + "x": 6.956119170941034, + "y": 1.0557271520364861, + "heading": 2.821845381251112, + "angularVelocity": -0.051708955665952475, + "velocityX": -1.5316207645207218, + "velocityY": 0.5031104163766442, + "timestamp": 6.044173552365228 + }, + { + "x": 6.818992484234107, + "y": 1.100769539311748, + "heading": 2.817204104711705, + "angularVelocity": -0.0518375568093923, + "velocityX": -1.5315425297115504, + "velocityY": 0.5030700690613045, + "timestamp": 6.133708569543468 + }, + { + "x": 6.681872127416794, + "y": 1.1458071900530102, + "heading": 2.8125526096190336, + "angularVelocity": -0.05195168593547435, + "velocityX": -1.5314718323484902, + "velocityY": 0.5030171675915902, + "timestamp": 6.223243586721708 + }, + { + "x": 6.544758118625867, + "y": 1.1908399522969457, + "heading": 2.8078906701771453, + "angularVelocity": -0.052068336934676634, + "velocityX": -1.5314009324192217, + "velocityY": 0.5029625688716561, + "timestamp": 6.312778603899948 + }, + { + "x": 6.407650518790602, + "y": 1.2358677646327403, + "heading": 2.8032180092193197, + "angularVelocity": -0.05218808355755985, + "velocityX": -1.5313293519821691, + "velocityY": 0.5029072842657369, + "timestamp": 6.4023136210781875 + }, + { + "x": 6.270549391355044, + "y": 1.280890584680893, + "heading": 2.7985343668363263, + "angularVelocity": -0.05231073305843581, + "velocityX": -1.531257062950314, + "velocityY": 0.5028515263310278, + "timestamp": 6.491848638256427 + }, + { + "x": 6.1334547976207645, + "y": 1.3259083749515967, + "heading": 2.793839500454244, + "angularVelocity": -0.05243609182244443, + "velocityX": -1.5311840892526187, + "velocityY": 0.5027953496796241, + "timestamp": 6.581383655434667 + }, + { + "x": 5.996366796772198, + "y": 1.3709210989414258, + "heading": 2.7891331795005336, + "angularVelocity": -0.05256402580837642, + "velocityX": -1.5311104545349274, + "velocityY": 0.5027387653282189, + "timestamp": 6.670918672612907 + }, + { + "x": 5.8592854465592445, + "y": 1.4159287198651518, + "heading": 2.784415180924841, + "angularVelocity": -0.052694451002343774, + "velocityX": -1.5310361748193169, + "velocityY": 0.5026817701294257, + "timestamp": 6.760453689791147 + }, + { + "x": 5.722210803960778, + "y": 1.4609312002186288, + "heading": 2.7796852859248706, + "angularVelocity": -0.05282731995856272, + "velocityX": -1.530961258717216, + "velocityY": 0.5026243560537855, + "timestamp": 6.849988706969387 + }, + { + "x": 5.585142925732609, + "y": 1.5059285016384842, + "heading": 2.7749432775568774, + "angularVelocity": -0.052962611919236485, + "velocityX": -1.5308857087200383, + "velocityY": 0.5025665135047443, + "timestamp": 6.939523724147627 + }, + { + "x": 5.448081868853608, + "y": 1.5509205848763696, + "heading": 2.7701889389309033, + "angularVelocity": -0.05310032628362197, + "velocityX": -1.5308095223363887, + "velocityY": 0.502508232598188, + "timestamp": 7.0290587413258665 + }, + { + "x": 5.311027690899557, + "y": 1.5959074098195543, + "heading": 2.7654220517793227, + "angularVelocity": -0.05324047843863795, + "velocityX": -1.5307326928995175, + "velocityY": 0.502449503679975, + "timestamp": 7.118593758504106 + }, + { + "x": 5.173980450370318, + "y": 1.6408889355303347, + "heading": 2.7606423952580355, + "angularVelocity": -0.05338309716043176, + "velocityX": -1.5306552100885358, + "velocityY": 0.5023903175361529, + "timestamp": 7.208128775682346 + }, + { + "x": 5.0369402069873335, + "y": 1.6858651202909567, + "heading": 2.7558497448937236, + "angularVelocity": -0.05352822298309391, + "velocityX": -1.5305770602598503, + "velocityY": 0.5023306654544649, + "timestamp": 7.297663792860586 + }, + { + "x": 4.899907021964721, + "y": 1.7308359216420997, + "heading": 2.751043871646217, + "angularVelocity": -0.05367590691292534, + "velocityX": -1.5304982267420264, + "velocityY": 0.5022705391525029, + "timestamp": 7.387198810038826 + }, + { + "x": 4.762880958231051, + "y": 1.7758012963934744, + "heading": 2.7462245411420234, + "angularVelocity": -0.05382620851683086, + "velocityX": -1.5304186903866774, + "velocityY": 0.5022099304662124, + "timestamp": 7.476733827217066 + }, + { + "x": 4.625862080529766, + "y": 1.8207612005630536, + "heading": 2.7413915132887583, + "angularVelocity": -0.053979191668033985, + "velocityX": -1.5303384309238324, + "velocityY": 0.5021488305528153, + "timestamp": 7.566268844395306 + }, + { + "x": 4.488850455262, + "y": 1.8657155891757824, + "heading": 2.736544542679806, + "angularVelocity": -0.054134915720223636, + "velocityX": -1.5302574298390257, + "velocityY": 0.5020872283213718, + "timestamp": 7.6558038615735455 + }, + { + "x": 4.351846149940031, + "y": 1.910664415870897, + "heading": 2.7316833801907157, + "angularVelocity": -0.05429342219718387, + "velocityX": -1.5301756747221047, + "velocityY": 0.5020251082951581, + "timestamp": 7.745338878751785 + }, + { + "x": 4.214849232355917, + "y": 1.955607632403858, + "heading": 2.7268077754532425, + "angularVelocity": -0.05445472499064026, + "velocityX": -1.530093162448295, + "velocityY": 0.5019624494346333, + "timestamp": 7.834873895930025 + }, + { + "x": 4.077859769965423, + "y": 2.0005451883147503, + "heading": 2.721917478680163, + "angularVelocity": -0.05461881761126215, + "velocityX": -1.5300098967735323, + "velocityY": 0.5018992269966714, + "timestamp": 7.924408913108265 + }, + { + "x": 3.940877826047202, + "y": 2.045477029542233, + "heading": 2.717012252912238, + "angularVelocity": -0.05478555678567875, + "velocityX": -1.5299259243512187, + "velocityY": 0.5018354007576298, + "timestamp": 8.013943930286505 + }, + { + "x": 3.8039034641003533, + "y": 2.0904031002081847, + "heading": 2.712091859898031, + "angularVelocity": -0.05495495694618896, + "velocityX": -1.5298412427192656, + "velocityY": 0.5017709504262021, + "timestamp": 8.103478947464744 + }, + { + "x": 3.66693674900291, + "y": 2.135323343068443, + "heading": 2.707156056414113, + "angularVelocity": -0.05512707362408464, + "velocityX": -1.5297558364765886, + "velocityY": 0.501705860745354, + "timestamp": 8.193013964642983 + }, + { + "x": 3.5299779535424887, + "y": 2.1802377886608806, + "heading": 2.7022039564715556, + "angularVelocity": -0.05530908574796876, + "velocityX": -1.5296673835196157, + "velocityY": 0.5016411121363245, + "timestamp": 8.282548981821222 + }, + { + "x": 3.426502516345769, + "y": 2.233179068985383, + "heading": 2.594355857349509, + "angularVelocity": -1.2045354155386023, + "velocityX": -1.1556979655315056, + "velocityY": 0.5912913404496402, + "timestamp": 8.37208399899946 + }, + { + "x": 3.387338399887085, + "y": 2.2590601444244385, + "heading": 2.3477629859714533, + "angularVelocity": -2.7541500426269785, + "velocityX": -0.43741675260662705, + "velocityY": 0.28906093118330495, + "timestamp": 8.4616190161777 + }, + { + "x": 3.387338399887085, + "y": 2.2590601444244385, + "heading": 2.3477629859714533, + "angularVelocity": -2.400686558954734e-26, + "velocityX": 2.2132770883951947e-26, + "velocityY": 2.6443990744659133e-26, + "timestamp": 8.551154033355939 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 57 }, { - "x": 2.812295436859131, - "y": 6.667538166046143, - "heading": 3.5782197562990956, - "angularVelocity": 2.3762078534459548, - "velocityX": -0.6771226429407677, - "velocityY": -0.18518169587388544, - "timestamp": 8.19723787244242 + "timestamp": 5.327893414939309, + "isStopPoint": true, + "x": 7.956855773925781, + "y": 0.7431064248085022, + "heading": 3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 36 }, { - "x": 2.812295436859131, - "y": 6.667538166046143, - "heading": 3.5782197562990956, - "angularVelocity": 4.7042853211430965e-33, - "velocityX": -2.4138813228334137e-34, - "velocityY": 0, - "timestamp": 8.29252584980616 + "timestamp": 8.551154033355939, + "isStopPoint": true, + "x": 3.387338399887085, + "y": 2.2590601444244385, + "heading": 2.3477629859714533, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 } ], "constraints": [ @@ -3732,9 +3299,17 @@ "usesControlIntervalGuessing": true, "defaultControlIntervalCount": 40, "usesDefaultFieldObstacles": true, - "circleObstacles": [] + "circleObstacles": [ + { + "x": 4.773353099822998, + "y": 4.013234615325928, + "radius": 1.6623732676337921 + } + ], + "eventMarkers": [], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": true, - "usesObstacles": false + "usesObstacles": true } \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing3.1.traj b/src/main/deploy/choreo/AmpWing3.1.traj new file mode 100644 index 00000000..7fc5db33 --- /dev/null +++ b/src/main/deploy/choreo/AmpWing3.1.traj @@ -0,0 +1,176 @@ +{ + "samples": [ + { + "x": 0.875186562538147, + "y": 6.525386810302734, + "heading": -2.143762813465651, + "angularVelocity": -5.355945535245033e-29, + "velocityX": 2.43610406010345e-28, + "velocityY": -8.419270817143307e-28, + "timestamp": 0 + }, + { + "x": 0.8966416033945684, + "y": 6.537208607893962, + "heading": -2.364630778366916, + "angularVelocity": -2.838662406313846, + "velocityX": 0.27574672466546707, + "velocityY": 0.15193734597170733, + "timestamp": 0.07780705603103885 + }, + { + "x": 0.9545363231559146, + "y": 6.574628260062144, + "heading": -2.5677938220576824, + "angularVelocity": -2.611113362388633, + "velocityX": 0.7440805849054467, + "velocityY": 0.48092877531896794, + "timestamp": 0.1556141120620777 + }, + { + "x": 1.0367704734566419, + "y": 6.613390261480878, + "heading": -2.7011163551758717, + "angularVelocity": -1.7135018328543843, + "velocityX": 1.0568983649493484, + "velocityY": 0.49818105704026555, + "timestamp": 0.23342116809311655 + }, + { + "x": 1.1598322354696735, + "y": 6.642210050914376, + "heading": -2.701117699890818, + "angularVelocity": -0.000017282686365776702, + "velocityX": 1.5816272750885192, + "velocityY": 0.37040071818167397, + "timestamp": 0.3112282241241554 + }, + { + "x": 1.2828941160720575, + "y": 6.671029334799358, + "heading": -2.7011190439216617, + "angularVelocity": -0.00001727389406811502, + "velocityX": 1.5816287992350209, + "velocityY": 0.37039422071804506, + "timestamp": 0.3890352801551943 + }, + { + "x": 1.4059559966738584, + "y": 6.699848618684591, + "heading": -2.7011203879554953, + "angularVelocity": -0.000017273932497369342, + "velocityX": 1.5816287992275255, + "velocityY": 0.3703942207212755, + "timestamp": 0.46684233618623316 + }, + { + "x": 1.5290178772749528, + "y": 6.728667902570602, + "heading": -2.7011217319923193, + "angularVelocity": -0.00001727397092835146, + "velocityX": 1.5816287992184466, + "velocityY": 0.3703942207312666, + "timestamp": 0.544649392217272 + }, + { + "x": 1.6520797578753408, + "y": 6.75748718645739, + "heading": -2.7011230760321334, + "angularVelocity": -0.000017274009359396956, + "velocityX": 1.5816287992093674, + "velocityY": 0.37039422074125794, + "timestamp": 0.6224564482483109 + }, + { + "x": 1.7751416384750223, + "y": 6.786306470344956, + "heading": -2.701124420074938, + "angularVelocity": -0.000017274047791505146, + "velocityX": 1.5816287992002884, + "velocityY": 0.37039422075124934, + "timestamp": 0.7002635042793498 + }, + { + "x": 1.898203519073992, + "y": 6.81512575423332, + "heading": -2.7011257641207327, + "angularVelocity": -0.000017274086223033065, + "velocityX": 1.581628799191142, + "velocityY": 0.3703942207615267, + "timestamp": 0.7780705603103887 + }, + { + "x": 2.021265394654385, + "y": 6.843945059515562, + "heading": -2.7011271081971797, + "angularVelocity": -0.000017274480179424693, + "velocityX": 1.581628734690864, + "velocityY": 0.37039449572214483, + "timestamp": 0.8558776163414276 + }, + { + "x": 2.1197062132467392, + "y": 6.880631402807114, + "heading": -2.784861574958977, + "angularVelocity": -1.0761808894092335, + "velocityX": 1.2651914056879914, + "velocityY": 0.4715040661211618, + "timestamp": 0.9336846723724664 + }, + { + "x": 2.228161206604935, + "y": 6.914841049698727, + "heading": -2.8360417692393636, + "angularVelocity": -0.6577834568110343, + "velocityX": 1.393896632137462, + "velocityY": 0.43967280908258105, + "timestamp": 1.0114917284035052 + }, + { + "x": 2.342602667471001, + "y": 6.947096322596154, + "heading": -2.866867411656533, + "angularVelocity": -0.39618055211949504, + "velocityX": 1.4708365372468657, + "velocityY": 0.4145545988086245, + "timestamp": 1.089298784434544 + }, + { + "x": 2.460575781873871, + "y": 6.9780078455456644, + "heading": -2.885328334561652, + "angularVelocity": -0.23726540813669628, + "velocityX": 1.5162264249634116, + "velocityY": 0.39728431489785143, + "timestamp": 1.167105840465583 + }, + { + "x": 2.563184602851941, + "y": 6.995518176666575, + "heading": -2.9725697719134425, + "angularVelocity": -1.121253544369907, + "velocityX": 1.3187598427723222, + "velocityY": 0.22504811278202433, + "timestamp": 1.2449128964966218 + }, + { + "x": 2.6077051162719727, + "y": 7.001829624176025, + "heading": -3.122903212305612, + "angularVelocity": -1.9321311981293672, + "velocityX": 0.5721912084974853, + "velocityY": 0.08111664714486487, + "timestamp": 1.3227199525276607 + }, + { + "x": 2.6077051162719727, + "y": 7.001829624176025, + "heading": -3.122903212305612, + "angularVelocity": -5.850460431476152e-29, + "velocityX": -4.4073165345897844e-27, + "velocityY": 1.309519926897454e-26, + "timestamp": 1.4005270085586996 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing3.2.traj b/src/main/deploy/choreo/AmpWing3.2.traj new file mode 100644 index 00000000..70c64142 --- /dev/null +++ b/src/main/deploy/choreo/AmpWing3.2.traj @@ -0,0 +1,95 @@ +{ + "samples": [ + { + "x": 2.6077051162719727, + "y": 7.001829624176025, + "heading": -3.122903212305612, + "angularVelocity": -5.850460431476152e-29, + "velocityX": -4.4073165345897844e-27, + "velocityY": 1.309519926897454e-26, + "timestamp": 0 + }, + { + "x": 2.581564908929706, + "y": 6.99268752896088, + "heading": -2.9753776937459495, + "angularVelocity": 2.140746378487904, + "velocityX": -0.37932118285182687, + "velocityY": -0.1326611654355847, + "timestamp": 0.068913123031354 + }, + { + "x": 2.510121983919206, + "y": 6.965703523715206, + "heading": -2.8481100670568136, + "angularVelocity": 1.846783618139497, + "velocityX": -1.0367100178872335, + "velocityY": -0.3915655546969944, + "timestamp": 0.137826246062708 + }, + { + "x": 2.4183443604796597, + "y": 6.911820320186112, + "heading": -2.82871535814287, + "angularVelocity": 0.2814370915263766, + "velocityX": -1.3317873200694863, + "velocityY": -0.7819004735074551, + "timestamp": 0.206739369094062 + }, + { + "x": 2.324659626384285, + "y": 6.850545155910229, + "heading": -2.828714867091888, + "angularVelocity": 0.000007125652715720525, + "velocityX": -1.3594614490588457, + "velocityY": -0.8891653952180393, + "timestamp": 0.275652492125416 + }, + { + "x": 2.2309748959640565, + "y": 6.7892699860148875, + "heading": -2.8287143760416544, + "angularVelocity": 0.000007125641845880462, + "velocityX": -1.359461395728709, + "velocityY": -0.8891654767621404, + "timestamp": 0.34456561515677 + }, + { + "x": 2.136919509671741, + "y": 6.728565387611802, + "heading": -2.828713709965057, + "angularVelocity": 0.000009665453660450363, + "velocityX": -1.3648399920799144, + "velocityY": -0.8808858999970965, + "timestamp": 0.413478738188124 + }, + { + "x": 2.062260898631176, + "y": 6.694685917301612, + "heading": -2.7155906116912156, + "angularVelocity": 1.6415320231876958, + "velocityX": -1.0833729158754979, + "velocityY": -0.491625815518109, + "timestamp": 0.48239186121947797 + }, + { + "x": 2.03380823135376, + "y": 6.676982402801514, + "heading": -2.588935989400422, + "angularVelocity": 1.8378882964449172, + "velocityX": -0.4128773450663434, + "velocityY": -0.256896128362137, + "timestamp": 0.551304984250832 + }, + { + "x": 2.03380823135376, + "y": 6.676982402801514, + "heading": -2.588935989400422, + "angularVelocity": -3.9702743259995845e-28, + "velocityX": -1.31352865617337e-26, + "velocityY": 2.3602776479280726e-26, + "timestamp": 0.620218107282186 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing3.traj b/src/main/deploy/choreo/AmpWing3.traj new file mode 100644 index 00000000..e637d35f --- /dev/null +++ b/src/main/deploy/choreo/AmpWing3.traj @@ -0,0 +1,257 @@ +{ + "samples": [ + { + "x": 0.875186562538147, + "y": 6.525386810302734, + "heading": -2.143762813465651, + "angularVelocity": -5.355945535245033e-29, + "velocityX": 2.43610406010345e-28, + "velocityY": -8.419270817143307e-28, + "timestamp": 0 + }, + { + "x": 0.8966416033945684, + "y": 6.537208607893962, + "heading": -2.364630778366916, + "angularVelocity": -2.838662406313846, + "velocityX": 0.27574672466546707, + "velocityY": 0.15193734597170733, + "timestamp": 0.07780705603103885 + }, + { + "x": 0.9545363231559146, + "y": 6.574628260062144, + "heading": -2.5677938220576824, + "angularVelocity": -2.611113362388633, + "velocityX": 0.7440805849054467, + "velocityY": 0.48092877531896794, + "timestamp": 0.1556141120620777 + }, + { + "x": 1.0367704734566419, + "y": 6.613390261480878, + "heading": -2.7011163551758717, + "angularVelocity": -1.7135018328543843, + "velocityX": 1.0568983649493484, + "velocityY": 0.49818105704026555, + "timestamp": 0.23342116809311655 + }, + { + "x": 1.1598322354696735, + "y": 6.642210050914376, + "heading": -2.701117699890818, + "angularVelocity": -0.000017282686365776702, + "velocityX": 1.5816272750885192, + "velocityY": 0.37040071818167397, + "timestamp": 0.3112282241241554 + }, + { + "x": 1.2828941160720575, + "y": 6.671029334799358, + "heading": -2.7011190439216617, + "angularVelocity": -0.00001727389406811502, + "velocityX": 1.5816287992350209, + "velocityY": 0.37039422071804506, + "timestamp": 0.3890352801551943 + }, + { + "x": 1.4059559966738584, + "y": 6.699848618684591, + "heading": -2.7011203879554953, + "angularVelocity": -0.000017273932497369342, + "velocityX": 1.5816287992275255, + "velocityY": 0.3703942207212755, + "timestamp": 0.46684233618623316 + }, + { + "x": 1.5290178772749528, + "y": 6.728667902570602, + "heading": -2.7011217319923193, + "angularVelocity": -0.00001727397092835146, + "velocityX": 1.5816287992184466, + "velocityY": 0.3703942207312666, + "timestamp": 0.544649392217272 + }, + { + "x": 1.6520797578753408, + "y": 6.75748718645739, + "heading": -2.7011230760321334, + "angularVelocity": -0.000017274009359396956, + "velocityX": 1.5816287992093674, + "velocityY": 0.37039422074125794, + "timestamp": 0.6224564482483109 + }, + { + "x": 1.7751416384750223, + "y": 6.786306470344956, + "heading": -2.701124420074938, + "angularVelocity": -0.000017274047791505146, + "velocityX": 1.5816287992002884, + "velocityY": 0.37039422075124934, + "timestamp": 0.7002635042793498 + }, + { + "x": 1.898203519073992, + "y": 6.81512575423332, + "heading": -2.7011257641207327, + "angularVelocity": -0.000017274086223033065, + "velocityX": 1.581628799191142, + "velocityY": 0.3703942207615267, + "timestamp": 0.7780705603103887 + }, + { + "x": 2.021265394654385, + "y": 6.843945059515562, + "heading": -2.7011271081971797, + "angularVelocity": -0.000017274480179424693, + "velocityX": 1.581628734690864, + "velocityY": 0.37039449572214483, + "timestamp": 0.8558776163414276 + }, + { + "x": 2.1197062132467392, + "y": 6.880631402807114, + "heading": -2.784861574958977, + "angularVelocity": -1.0761808894092335, + "velocityX": 1.2651914056879914, + "velocityY": 0.4715040661211618, + "timestamp": 0.9336846723724664 + }, + { + "x": 2.228161206604935, + "y": 6.914841049698727, + "heading": -2.8360417692393636, + "angularVelocity": -0.6577834568110343, + "velocityX": 1.393896632137462, + "velocityY": 0.43967280908258105, + "timestamp": 1.0114917284035052 + }, + { + "x": 2.342602667471001, + "y": 6.947096322596154, + "heading": -2.866867411656533, + "angularVelocity": -0.39618055211949504, + "velocityX": 1.4708365372468657, + "velocityY": 0.4145545988086245, + "timestamp": 1.089298784434544 + }, + { + "x": 2.460575781873871, + "y": 6.9780078455456644, + "heading": -2.885328334561652, + "angularVelocity": -0.23726540813669628, + "velocityX": 1.5162264249634116, + "velocityY": 0.39728431489785143, + "timestamp": 1.167105840465583 + }, + { + "x": 2.563184602851941, + "y": 6.995518176666575, + "heading": -2.9725697719134425, + "angularVelocity": -1.121253544369907, + "velocityX": 1.3187598427723222, + "velocityY": 0.22504811278202433, + "timestamp": 1.2449128964966218 + }, + { + "x": 2.6077051162719727, + "y": 7.001829624176025, + "heading": -3.122903212305612, + "angularVelocity": -1.9321311981293672, + "velocityX": 0.5721912084974853, + "velocityY": 0.08111664714486487, + "timestamp": 1.3227199525276607 + }, + { + "x": 2.6077051162719727, + "y": 7.001829624176025, + "heading": -3.122903212305612, + "angularVelocity": -5.850460431476152e-29, + "velocityX": -4.4073165345897844e-27, + "velocityY": 1.309519926897454e-26, + "timestamp": 1.4005270085586996 + }, + { + "x": 2.581564908929706, + "y": 6.99268752896088, + "heading": -2.9753776937459495, + "angularVelocity": 2.140746378487904, + "velocityX": -0.37932118285182687, + "velocityY": -0.1326611654355847, + "timestamp": 1.4694401315900536 + }, + { + "x": 2.510121983919206, + "y": 6.965703523715206, + "heading": -2.8481100670568136, + "angularVelocity": 1.846783618139497, + "velocityX": -1.0367100178872335, + "velocityY": -0.3915655546969944, + "timestamp": 1.5383532546214076 + }, + { + "x": 2.4183443604796597, + "y": 6.911820320186112, + "heading": -2.82871535814287, + "angularVelocity": 0.2814370915263766, + "velocityX": -1.3317873200694863, + "velocityY": -0.7819004735074551, + "timestamp": 1.6072663776527616 + }, + { + "x": 2.324659626384285, + "y": 6.850545155910229, + "heading": -2.828714867091888, + "angularVelocity": 0.000007125652715720525, + "velocityX": -1.3594614490588457, + "velocityY": -0.8891653952180393, + "timestamp": 1.6761795006841156 + }, + { + "x": 2.2309748959640565, + "y": 6.7892699860148875, + "heading": -2.8287143760416544, + "angularVelocity": 0.000007125641845880462, + "velocityX": -1.359461395728709, + "velocityY": -0.8891654767621404, + "timestamp": 1.7450926237154696 + }, + { + "x": 2.136919509671741, + "y": 6.728565387611802, + "heading": -2.828713709965057, + "angularVelocity": 0.000009665453660450363, + "velocityX": -1.3648399920799144, + "velocityY": -0.8808858999970965, + "timestamp": 1.8140057467468236 + }, + { + "x": 2.062260898631176, + "y": 6.694685917301612, + "heading": -2.7155906116912156, + "angularVelocity": 1.6415320231876958, + "velocityX": -1.0833729158754979, + "velocityY": -0.491625815518109, + "timestamp": 1.8829188697781776 + }, + { + "x": 2.03380823135376, + "y": 6.676982402801514, + "heading": -2.588935989400422, + "angularVelocity": 1.8378882964449172, + "velocityX": -0.4128773450663434, + "velocityY": -0.256896128362137, + "timestamp": 1.9518319928095316 + }, + { + "x": 2.03380823135376, + "y": 6.676982402801514, + "heading": -2.588935989400422, + "angularVelocity": -3.9702743259995845e-28, + "velocityX": -1.31352865617337e-26, + "velocityY": 2.3602776479280726e-26, + "timestamp": 2.0207451158408856 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing1.1.traj b/src/main/deploy/choreo/FrontWing1.1.traj index 090e2a15..990bbd1d 100644 --- a/src/main/deploy/choreo/FrontWing1.1.traj +++ b/src/main/deploy/choreo/FrontWing1.1.traj @@ -1,157 +1,158 @@ { - "samples": [ - { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, - "angularVelocity": -1.6112520136935844e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.2701999983580183, - "y": 5.571793741558495, - "heading": 2.9123192627847865, - "angularVelocity": -2.75479375784366, - "velocityX": 0.5278042458729727, - "velocityY": -0.26883784352486495, - "timestamp": 0.08301192659671743 - }, - { - "x": 1.3598509429602765, - "y": 5.50482600027521, - "heading": 2.837562269615421, - "angularVelocity": -0.9005572600735416, - "velocityX": 1.0799766765779808, - "velocityY": -0.806724335029887, - "timestamp": 0.16602385319343485 - }, - { - "x": 1.459326562321469, - "y": 5.413786422039427, - "heading": 2.837562034760696, - "angularVelocity": -0.000002829168462845694, - "velocityX": 1.1983292454403298, - "velocityY": -1.0967047985533822, - "timestamp": 0.2490357797901523 - }, - { - "x": 1.558802177818681, - "y": 5.322746839581507, - "heading": 2.837561799906192, - "angularVelocity": -0.000002829165803381977, - "velocityX": 1.198329198893034, - "velocityY": -1.0967048494152307, - "timestamp": 0.3320477063868697 - }, - { - "x": 1.6582777933158976, - "y": 5.231707257123597, - "heading": 2.8375615650516535, - "angularVelocity": -0.0000028291662173205513, - "velocityX": 1.1983291988930937, - "velocityY": -1.0967048494150837, - "timestamp": 0.4150596329835871 - }, - { - "x": 1.7577534088131206, - "y": 5.140667674665702, - "heading": 2.837561330197081, - "angularVelocity": -0.0000028291666305712727, - "velocityX": 1.19832919889317, - "velocityY": -1.0967048494149187, - "timestamp": 0.4980715595803045 - }, - { - "x": 1.8572290243103502, - "y": 5.04962809220782, - "heading": 2.837561095342474, - "angularVelocity": -0.000002829167043337366, - "velocityX": 1.1983291988932463, - "velocityY": -1.0967048494147538, - "timestamp": 0.581083486177022 - }, - { - "x": 1.9567046398075858, - "y": 4.958588509749952, - "heading": 2.8375608604878324, - "angularVelocity": -0.000002829167457373439, - "velocityX": 1.198329198893323, - "velocityY": -1.0967048494145886, - "timestamp": 0.6640954127737394 - }, - { - "x": 2.056180255304828, - "y": 4.867548927292098, - "heading": 2.8375606256331567, - "angularVelocity": -0.00000282916787026248, - "velocityX": 1.1983291988933988, - "velocityY": -1.096704849414424, - "timestamp": 0.7471073393704568 - }, - { - "x": 2.1556558708020765, - "y": 4.776509344834258, - "heading": 2.8375603907784472, - "angularVelocity": -0.0000028291682836294088, - "velocityX": 1.1983291988934754, - "velocityY": -1.0967048494142588, - "timestamp": 0.8301192659671742 - }, - { - "x": 2.2551314862993315, - "y": 4.685469762376432, - "heading": 2.8375601559237027, - "angularVelocity": -0.000002829168697323748, - "velocityX": 1.1983291988935516, - "velocityY": -1.096704849414094, - "timestamp": 0.9131311925638916 - }, - { - "x": 2.354607101796593, - "y": 4.594430179918619, - "heading": 2.8375599210689244, - "angularVelocity": -0.000002829169110584532, - "velocityX": 1.1983291988936342, - "velocityY": -1.096704849413922, - "timestamp": 0.996143119160609 - }, - { - "x": 2.45408271880603, - "y": 4.503390599113157, - "heading": 2.837559686214013, - "angularVelocity": -0.000002829170713915412, - "velocityX": 1.1983292171099962, - "velocityY": -1.0967048295089454, - "timestamp": 1.0791550457573265 - }, - { - "x": 2.5484359060007082, - "y": 4.424785420118879, - "heading": 2.7982046114916295, - "angularVelocity": -0.47408940300319863, - "velocityX": 1.13662206218943, - "velocityY": -0.9469142834878584, - "timestamp": 1.162166972354044 - }, - { - "x": 2.593973159790039, - "y": 4.3895583152771, - "heading": 2.599013995570559, - "angularVelocity": -2.3995421391526537, - "velocityX": 0.5485627867734783, - "velocityY": -0.4243619716588108, - "timestamp": 1.2451788989507615 - }, - { - "x": 2.593973159790039, - "y": 4.3895583152771, - "heading": 2.599013995570559, - "angularVelocity": 0, - "velocityX": -4.934633813021939e-37, - "velocityY": -2.767548277117811e-37, - "timestamp": 1.328190825547479 - } - ] + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": 4.442897870116303e-27, + "velocityX": 5.36651543276403e-26, + "velocityY": -1.1808751253669755e-25, + "timestamp": 0 + }, + { + "x": 1.2664719468759371, + "y": 5.574445984484986, + "heading": 2.936921539881242, + "angularVelocity": -2.425035961622917, + "velocityX": 0.4763363139734096, + "velocityY": -0.2336705712390479, + "timestamp": 0.08415481805151542 + }, + { + "x": 1.353066220057362, + "y": 5.511048868833387, + "heading": 2.8414585534340224, + "angularVelocity": -1.1343733924869426, + "velocityX": 1.028987706068311, + "velocityY": -0.753339109031048, + "timestamp": 0.16830963610303085 + }, + { + "x": 1.4537891107085041, + "y": 5.418622426082042, + "heading": 2.841458332012736, + "angularVelocity": -0.0000026311183569063204, + "velocityX": 1.196876102678809, + "velocityY": -1.0982905660228062, + "timestamp": 0.25246445415454627 + }, + { + "x": 1.554511990655445, + "y": 5.326195971665319, + "heading": 2.841458110592174, + "angularVelocity": -0.000002631109743829199, + "velocityX": 1.1968759754822746, + "velocityY": -1.0982907046408665, + "timestamp": 0.3366192722060617 + }, + { + "x": 1.6552348706023827, + "y": 5.233769517248598, + "heading": 2.8414578891715823, + "angularVelocity": -0.0000026311100983002847, + "velocityX": 1.196875975482236, + "velocityY": -1.098290704640839, + "timestamp": 0.4207740902575771 + }, + { + "x": 1.7559577505493258, + "y": 5.141343062831889, + "heading": 2.841457667750961, + "angularVelocity": -0.0000026311104525941368, + "velocityX": 1.1968759754823024, + "velocityY": -1.0982907046406973, + "timestamp": 0.5049289083090925 + }, + { + "x": 1.8566806304962746, + "y": 5.048916608415192, + "heading": 2.8414574463303097, + "angularVelocity": -0.0000026311108067721062, + "velocityX": 1.1968759754823688, + "velocityY": -1.0982907046405554, + "timestamp": 0.5890837263606079 + }, + { + "x": 1.957403510443229, + "y": 4.956490153998508, + "heading": 2.841457224909629, + "angularVelocity": -0.000002631111160911123, + "velocityX": 1.1968759754824354, + "velocityY": -1.0982907046404133, + "timestamp": 0.6732385444121234 + }, + { + "x": 2.058126390390189, + "y": 4.864063699581835, + "heading": 2.841457003488918, + "angularVelocity": -0.000002631111515369215, + "velocityX": 1.1968759754825018, + "velocityY": -1.0982907046402715, + "timestamp": 0.7573933624636389 + }, + { + "x": 2.158849270337155, + "y": 4.7716372451651745, + "heading": 2.8414567820681773, + "angularVelocity": -0.0000026311118694940932, + "velocityX": 1.1968759754825686, + "velocityY": -1.0982907046401296, + "timestamp": 0.8415481805151543 + }, + { + "x": 2.2595721502841255, + "y": 4.679210790748526, + "heading": 2.8414565606474067, + "angularVelocity": -0.000002631112223792794, + "velocityX": 1.196875975482635, + "velocityY": -1.0982907046399877, + "timestamp": 0.9257029985666698 + }, + { + "x": 2.3602950302311068, + "y": 4.586784336331894, + "heading": 2.8414563392266063, + "angularVelocity": -0.00000263111257823472, + "velocityX": 1.1968759754827565, + "velocityY": -1.098290704639786, + "timestamp": 1.0098578166181853 + }, + { + "x": 2.4610179157680534, + "y": 4.494357888007175, + "heading": 2.841456117805399, + "angularVelocity": -0.0000026311174148596253, + "velocityX": 1.1968760419075375, + "velocityY": -1.0982906322504282, + "timestamp": 1.0940126346697008 + }, + { + "x": 2.552458083373214, + "y": 4.421585489236337, + "heading": 2.775799514245796, + "angularVelocity": -0.7801882896284313, + "velocityX": 1.0865707956160633, + "velocityY": -0.8647442945725389, + "timestamp": 1.1781674527212163 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -2.1007177339153333, + "velocityX": 0.4933178798082817, + "velocityY": -0.38057445433049264, + "timestamp": 1.2623222707727317 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -1.7281844842695858e-24, + "velocityX": -1.1064710362675692e-24, + "velocityY": 7.502253930064392e-24, + "timestamp": 1.3464770888242472 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing1.2.traj b/src/main/deploy/choreo/FrontWing1.2.traj index d3d054b7..b50ede37 100644 --- a/src/main/deploy/choreo/FrontWing1.2.traj +++ b/src/main/deploy/choreo/FrontWing1.2.traj @@ -1,85 +1,86 @@ { - "samples": [ - { - "x": 2.593973159790039, - "y": 4.3895583152771, - "heading": 2.599013995570559, - "angularVelocity": 0, - "velocityX": -4.934633813021939e-37, - "velocityY": -2.767548277117811e-37, - "timestamp": 0 - }, - { - "x": 2.534366599021267, - "y": 4.442118048365455, - "heading": 2.6991662321397754, - "angularVelocity": 1.173988745719896, - "velocityX": -0.6987106220563037, - "velocityY": 0.6161074104533542, - "timestamp": 0.08530936683537149 - }, - { - "x": 2.4322324251030567, - "y": 4.53578051923557, - "heading": 2.6991662757513564, - "angularVelocity": 5.112167893680353e-7, - "velocityX": -1.1972210990067154, - "velocityY": 1.097915438182342, - "timestamp": 0.17061873367074298 - }, - { - "x": 2.330098253355979, - "y": 4.629442992473233, - "heading": 2.6991663193628423, - "angularVelocity": 5.112156854714172e-7, - "velocityX": -1.1972210735566082, - "velocityY": 1.0979154659348471, - "timestamp": 0.2559281005061145 - }, - { - "x": 2.227964081608902, - "y": 4.723105465710898, - "heading": 2.6991663629743283, - "angularVelocity": 5.112156767559466e-7, - "velocityX": -1.1972210735566007, - "velocityY": 1.0979154659348562, - "timestamp": 0.34123746734148597 - }, - { - "x": 2.1258299098618236, - "y": 4.816767938948561, - "heading": 2.6991664065858134, - "angularVelocity": 5.112156675644069e-7, - "velocityX": -1.1972210735566096, - "velocityY": 1.0979154659348476, - "timestamp": 0.42654683417685746 - }, - { - "x": 2.023695735807304, - "y": 4.910430409670039, - "heading": 2.6991664501973878, - "angularVelocity": 5.112167177081662e-7, - "velocityX": -1.1972211006045375, - "velocityY": 1.0979154364400177, - "timestamp": 0.511856201012229 - }, - { - "x": 1.9637236595153809, - "y": 4.962982177734375, - "heading": 2.79592312968437, - "angularVelocity": 1.1341858822337967, - "velocityX": -0.702995210451579, - "velocityY": 0.6160140441054905, - "timestamp": 0.5971655678476004 - }, - { - "x": 1.9637236595153809, - "y": 4.962982177734375, - "heading": 2.79592312968437, - "angularVelocity": -1.527841446263418e-36, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0.6824749346829719 - } - ] + "samples": [ + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -1.7281844842695858e-24, + "velocityX": -1.1064710362675692e-24, + "velocityY": 7.502253930064392e-24, + "timestamp": 0 + }, + { + "x": 2.5399319472382507, + "y": 4.43667284651752, + "heading": 2.699289039214539, + "angularVelocity": 1.1496592855145864, + "velocityX": -0.6195856870550529, + "velocityY": 0.5401708775667837, + "timestamp": 0.08722153155062529 + }, + { + "x": 2.4355875318293467, + "y": 4.532520831999065, + "heading": 2.6992890864875387, + "angularVelocity": 5.419877275969795e-7, + "velocityX": -1.19631487264518, + "velocityY": 1.0989028027547665, + "timestamp": 0.17444306310125057 + }, + { + "x": 2.331243168301696, + "y": 4.628368873961615, + "heading": 2.6992891337586675, + "angularVelocity": 5.419662778006892e-7, + "velocityX": -1.1963142778235591, + "velocityY": 1.0989034503128068, + "timestamp": 0.26166459465187586 + }, + { + "x": 2.2268988047740805, + "y": 4.724216915924203, + "heading": 2.6992891810297954, + "angularVelocity": 5.419662665035014e-7, + "velocityX": -1.1963142778231501, + "velocityY": 1.098903450313253, + "timestamp": 0.34888612620250115 + }, + { + "x": 2.122554441246435, + "y": 4.82006495788676, + "heading": 2.699289228300922, + "angularVelocity": 5.419662547194978e-7, + "velocityX": -1.1963142778234912, + "velocityY": 1.0989034503128838, + "timestamp": 0.43610765775312643 + }, + { + "x": 2.0182100346281864, + "y": 4.915912952938407, + "heading": 2.699289275573439, + "angularVelocity": 5.419821924710352e-7, + "velocityX": -1.1963147718597973, + "velocityY": 1.0989029124765504, + "timestamp": 0.5233291893037517 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 1.1079128329091787, + "velocityX": -0.6246895020546673, + "velocityY": 0.5396514365108126, + "timestamp": 0.610550720854377 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 3.9745579613293646e-24, + "velocityX": 5.1209291302608005e-24, + "velocityY": -1.0346712550534795e-24, + "timestamp": 0.6977722524050023 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing1.traj b/src/main/deploy/choreo/FrontWing1.traj index e9811392..c2bb5840 100644 --- a/src/main/deploy/choreo/FrontWing1.traj +++ b/src/main/deploy/choreo/FrontWing1.traj @@ -1,229 +1,230 @@ { - "samples": [ - { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, - "angularVelocity": -1.6112520136935844e-31, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.2701999983580183, - "y": 5.571793741558495, - "heading": 2.9123192627847865, - "angularVelocity": -2.75479375784366, - "velocityX": 0.5278042458729727, - "velocityY": -0.26883784352486495, - "timestamp": 0.08301192659671743 - }, - { - "x": 1.3598509429602765, - "y": 5.50482600027521, - "heading": 2.837562269615421, - "angularVelocity": -0.9005572600735416, - "velocityX": 1.0799766765779808, - "velocityY": -0.806724335029887, - "timestamp": 0.16602385319343485 - }, - { - "x": 1.459326562321469, - "y": 5.413786422039427, - "heading": 2.837562034760696, - "angularVelocity": -0.000002829168462845694, - "velocityX": 1.1983292454403298, - "velocityY": -1.0967047985533822, - "timestamp": 0.2490357797901523 - }, - { - "x": 1.558802177818681, - "y": 5.322746839581507, - "heading": 2.837561799906192, - "angularVelocity": -0.000002829165803381977, - "velocityX": 1.198329198893034, - "velocityY": -1.0967048494152307, - "timestamp": 0.3320477063868697 - }, - { - "x": 1.6582777933158976, - "y": 5.231707257123597, - "heading": 2.8375615650516535, - "angularVelocity": -0.0000028291662173205513, - "velocityX": 1.1983291988930937, - "velocityY": -1.0967048494150837, - "timestamp": 0.4150596329835871 - }, - { - "x": 1.7577534088131206, - "y": 5.140667674665702, - "heading": 2.837561330197081, - "angularVelocity": -0.0000028291666305712727, - "velocityX": 1.19832919889317, - "velocityY": -1.0967048494149187, - "timestamp": 0.4980715595803045 - }, - { - "x": 1.8572290243103502, - "y": 5.04962809220782, - "heading": 2.837561095342474, - "angularVelocity": -0.000002829167043337366, - "velocityX": 1.1983291988932463, - "velocityY": -1.0967048494147538, - "timestamp": 0.581083486177022 - }, - { - "x": 1.9567046398075858, - "y": 4.958588509749952, - "heading": 2.8375608604878324, - "angularVelocity": -0.000002829167457373439, - "velocityX": 1.198329198893323, - "velocityY": -1.0967048494145886, - "timestamp": 0.6640954127737394 - }, - { - "x": 2.056180255304828, - "y": 4.867548927292098, - "heading": 2.8375606256331567, - "angularVelocity": -0.00000282916787026248, - "velocityX": 1.1983291988933988, - "velocityY": -1.096704849414424, - "timestamp": 0.7471073393704568 - }, - { - "x": 2.1556558708020765, - "y": 4.776509344834258, - "heading": 2.8375603907784472, - "angularVelocity": -0.0000028291682836294088, - "velocityX": 1.1983291988934754, - "velocityY": -1.0967048494142588, - "timestamp": 0.8301192659671742 - }, - { - "x": 2.2551314862993315, - "y": 4.685469762376432, - "heading": 2.8375601559237027, - "angularVelocity": -0.000002829168697323748, - "velocityX": 1.1983291988935516, - "velocityY": -1.096704849414094, - "timestamp": 0.9131311925638916 - }, - { - "x": 2.354607101796593, - "y": 4.594430179918619, - "heading": 2.8375599210689244, - "angularVelocity": -0.000002829169110584532, - "velocityX": 1.1983291988936342, - "velocityY": -1.096704849413922, - "timestamp": 0.996143119160609 - }, - { - "x": 2.45408271880603, - "y": 4.503390599113157, - "heading": 2.837559686214013, - "angularVelocity": -0.000002829170713915412, - "velocityX": 1.1983292171099962, - "velocityY": -1.0967048295089454, - "timestamp": 1.0791550457573265 - }, - { - "x": 2.5484359060007082, - "y": 4.424785420118879, - "heading": 2.7982046114916295, - "angularVelocity": -0.47408940300319863, - "velocityX": 1.13662206218943, - "velocityY": -0.9469142834878584, - "timestamp": 1.162166972354044 - }, - { - "x": 2.593973159790039, - "y": 4.3895583152771, - "heading": 2.599013995570559, - "angularVelocity": -2.3995421391526537, - "velocityX": 0.5485627867734783, - "velocityY": -0.4243619716588108, - "timestamp": 1.2451788989507615 - }, - { - "x": 2.593973159790039, - "y": 4.3895583152771, - "heading": 2.599013995570559, - "angularVelocity": 0, - "velocityX": -4.934633813021939e-37, - "velocityY": -2.767548277117811e-37, - "timestamp": 1.328190825547479 - }, - { - "x": 2.534366599021267, - "y": 4.442118048365455, - "heading": 2.6991662321397754, - "angularVelocity": 1.173988745719896, - "velocityX": -0.6987106220563037, - "velocityY": 0.6161074104533542, - "timestamp": 1.4135001923828505 - }, - { - "x": 2.4322324251030567, - "y": 4.53578051923557, - "heading": 2.6991662757513564, - "angularVelocity": 5.112167893680353e-7, - "velocityX": -1.1972210990067154, - "velocityY": 1.097915438182342, - "timestamp": 1.498809559218222 - }, - { - "x": 2.330098253355979, - "y": 4.629442992473233, - "heading": 2.6991663193628423, - "angularVelocity": 5.112156854714172e-7, - "velocityX": -1.1972210735566082, - "velocityY": 1.0979154659348471, - "timestamp": 1.5841189260535935 - }, - { - "x": 2.227964081608902, - "y": 4.723105465710898, - "heading": 2.6991663629743283, - "angularVelocity": 5.112156767559466e-7, - "velocityX": -1.1972210735566007, - "velocityY": 1.0979154659348562, - "timestamp": 1.669428292888965 - }, - { - "x": 2.1258299098618236, - "y": 4.816767938948561, - "heading": 2.6991664065858134, - "angularVelocity": 5.112156675644069e-7, - "velocityX": -1.1972210735566096, - "velocityY": 1.0979154659348476, - "timestamp": 1.7547376597243365 - }, - { - "x": 2.023695735807304, - "y": 4.910430409670039, - "heading": 2.6991664501973878, - "angularVelocity": 5.112167177081662e-7, - "velocityX": -1.1972211006045375, - "velocityY": 1.0979154364400177, - "timestamp": 1.840047026559708 - }, - { - "x": 1.9637236595153809, - "y": 4.962982177734375, - "heading": 2.79592312968437, - "angularVelocity": 1.1341858822337967, - "velocityX": -0.702995210451579, - "velocityY": 0.6160140441054905, - "timestamp": 1.9253563933950795 - }, - { - "x": 1.9637236595153809, - "y": 4.962982177734375, - "heading": 2.79592312968437, - "angularVelocity": -1.527841446263418e-36, - "velocityX": 0, - "velocityY": 0, - "timestamp": 2.010665760230451 - } - ] + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": 4.442897870116303e-27, + "velocityX": 5.36651543276403e-26, + "velocityY": -1.1808751253669755e-25, + "timestamp": 0 + }, + { + "x": 1.2664719468759371, + "y": 5.574445984484986, + "heading": 2.936921539881242, + "angularVelocity": -2.425035961622917, + "velocityX": 0.4763363139734096, + "velocityY": -0.2336705712390479, + "timestamp": 0.08415481805151542 + }, + { + "x": 1.353066220057362, + "y": 5.511048868833387, + "heading": 2.8414585534340224, + "angularVelocity": -1.1343733924869426, + "velocityX": 1.028987706068311, + "velocityY": -0.753339109031048, + "timestamp": 0.16830963610303085 + }, + { + "x": 1.4537891107085041, + "y": 5.418622426082042, + "heading": 2.841458332012736, + "angularVelocity": -0.0000026311183569063204, + "velocityX": 1.196876102678809, + "velocityY": -1.0982905660228062, + "timestamp": 0.25246445415454627 + }, + { + "x": 1.554511990655445, + "y": 5.326195971665319, + "heading": 2.841458110592174, + "angularVelocity": -0.000002631109743829199, + "velocityX": 1.1968759754822746, + "velocityY": -1.0982907046408665, + "timestamp": 0.3366192722060617 + }, + { + "x": 1.6552348706023827, + "y": 5.233769517248598, + "heading": 2.8414578891715823, + "angularVelocity": -0.0000026311100983002847, + "velocityX": 1.196875975482236, + "velocityY": -1.098290704640839, + "timestamp": 0.4207740902575771 + }, + { + "x": 1.7559577505493258, + "y": 5.141343062831889, + "heading": 2.841457667750961, + "angularVelocity": -0.0000026311104525941368, + "velocityX": 1.1968759754823024, + "velocityY": -1.0982907046406973, + "timestamp": 0.5049289083090925 + }, + { + "x": 1.8566806304962746, + "y": 5.048916608415192, + "heading": 2.8414574463303097, + "angularVelocity": -0.0000026311108067721062, + "velocityX": 1.1968759754823688, + "velocityY": -1.0982907046405554, + "timestamp": 0.5890837263606079 + }, + { + "x": 1.957403510443229, + "y": 4.956490153998508, + "heading": 2.841457224909629, + "angularVelocity": -0.000002631111160911123, + "velocityX": 1.1968759754824354, + "velocityY": -1.0982907046404133, + "timestamp": 0.6732385444121234 + }, + { + "x": 2.058126390390189, + "y": 4.864063699581835, + "heading": 2.841457003488918, + "angularVelocity": -0.000002631111515369215, + "velocityX": 1.1968759754825018, + "velocityY": -1.0982907046402715, + "timestamp": 0.7573933624636389 + }, + { + "x": 2.158849270337155, + "y": 4.7716372451651745, + "heading": 2.8414567820681773, + "angularVelocity": -0.0000026311118694940932, + "velocityX": 1.1968759754825686, + "velocityY": -1.0982907046401296, + "timestamp": 0.8415481805151543 + }, + { + "x": 2.2595721502841255, + "y": 4.679210790748526, + "heading": 2.8414565606474067, + "angularVelocity": -0.000002631112223792794, + "velocityX": 1.196875975482635, + "velocityY": -1.0982907046399877, + "timestamp": 0.9257029985666698 + }, + { + "x": 2.3602950302311068, + "y": 4.586784336331894, + "heading": 2.8414563392266063, + "angularVelocity": -0.00000263111257823472, + "velocityX": 1.1968759754827565, + "velocityY": -1.098290704639786, + "timestamp": 1.0098578166181853 + }, + { + "x": 2.4610179157680534, + "y": 4.494357888007175, + "heading": 2.841456117805399, + "angularVelocity": -0.0000026311174148596253, + "velocityX": 1.1968760419075375, + "velocityY": -1.0982906322504282, + "timestamp": 1.0940126346697008 + }, + { + "x": 2.552458083373214, + "y": 4.421585489236337, + "heading": 2.775799514245796, + "angularVelocity": -0.7801882896284313, + "velocityX": 1.0865707956160633, + "velocityY": -0.8647442945725389, + "timestamp": 1.1781674527212163 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -2.1007177339153333, + "velocityX": 0.4933178798082817, + "velocityY": -0.38057445433049264, + "timestamp": 1.2623222707727317 + }, + { + "x": 2.593973159790039, + "y": 4.3895583152771, + "heading": 2.599013995570559, + "angularVelocity": -1.7281844842695858e-24, + "velocityX": -1.1064710362675692e-24, + "velocityY": 7.502253930064392e-24, + "timestamp": 1.3464770888242472 + }, + { + "x": 2.5399319472382507, + "y": 4.43667284651752, + "heading": 2.699289039214539, + "angularVelocity": 1.1496592855145864, + "velocityX": -0.6195856870550529, + "velocityY": 0.5401708775667837, + "timestamp": 1.4336986203748725 + }, + { + "x": 2.4355875318293467, + "y": 4.532520831999065, + "heading": 2.6992890864875387, + "angularVelocity": 5.419877275969795e-7, + "velocityX": -1.19631487264518, + "velocityY": 1.0989028027547665, + "timestamp": 1.5209201519254978 + }, + { + "x": 2.331243168301696, + "y": 4.628368873961615, + "heading": 2.6992891337586675, + "angularVelocity": 5.419662778006892e-7, + "velocityX": -1.1963142778235591, + "velocityY": 1.0989034503128068, + "timestamp": 1.608141683476123 + }, + { + "x": 2.2268988047740805, + "y": 4.724216915924203, + "heading": 2.6992891810297954, + "angularVelocity": 5.419662665035014e-7, + "velocityX": -1.1963142778231501, + "velocityY": 1.098903450313253, + "timestamp": 1.6953632150267484 + }, + { + "x": 2.122554441246435, + "y": 4.82006495788676, + "heading": 2.699289228300922, + "angularVelocity": 5.419662547194978e-7, + "velocityX": -1.1963142778234912, + "velocityY": 1.0989034503128838, + "timestamp": 1.7825847465773736 + }, + { + "x": 2.0182100346281864, + "y": 4.915912952938407, + "heading": 2.699289275573439, + "angularVelocity": 5.419821924710352e-7, + "velocityX": -1.1963147718597973, + "velocityY": 1.0989029124765504, + "timestamp": 1.869806278127999 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 1.1079128329091787, + "velocityX": -0.6246895020546673, + "velocityY": 0.5396514365108126, + "timestamp": 1.9570278096786242 + }, + { + "x": 1.9637236595153809, + "y": 4.962982177734375, + "heading": 2.79592312968437, + "angularVelocity": 3.9745579613293646e-24, + "velocityX": 5.1209291302608005e-24, + "velocityY": -1.0346712550534795e-24, + "timestamp": 2.0442493412292495 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing2.1.traj b/src/main/deploy/choreo/FrontWing2.1.traj index 73d19d55..217c3a7b 100644 --- a/src/main/deploy/choreo/FrontWing2.1.traj +++ b/src/main/deploy/choreo/FrontWing2.1.traj @@ -1,103 +1,113 @@ { - "samples": [ - { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, - "angularVelocity": -1.8084842834156656e-35, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.347146156455349, - "y": 5.593360447165824, - "heading": 3.140264869371664, - "angularVelocity": -0.007271662750997594, - "velocityX": 1.1945189788836181, - "velocityY": -0.007419158267666702, - "timestamp": 0.10109525888490682 - }, - { - "x": 1.5113646664969056, - "y": 5.59234048587755, - "heading": 3.1402648691641017, - "angularVelocity": -2.053134778232203e-9, - "velocityX": 1.6243937831794193, - "velocityY": -0.010089110998122406, - "timestamp": 0.20219051776981364 - }, - { - "x": 1.6755831765384634, - "y": 5.591320524589276, - "heading": 3.14026486895654, - "angularVelocity": -2.053134775032819e-9, - "velocityX": 1.624393783179429, - "velocityY": -0.010089110998122467, - "timestamp": 0.30328577665472045 - }, - { - "x": 1.8398016865800209, - "y": 5.5903005633010014, - "heading": 3.140264868748978, - "angularVelocity": -2.0531347455807303e-9, - "velocityX": 1.624393783179429, - "velocityY": -0.010089110998122468, - "timestamp": 0.4043810355396273 - }, - { - "x": 2.0040201966215783, - "y": 5.589280602012727, - "heading": 3.1402648685414154, - "angularVelocity": -2.0531349299385344e-9, - "velocityX": 1.624393783179429, - "velocityY": -0.01008911099812247, - "timestamp": 0.5054762944245341 - }, - { - "x": 2.168238706663136, - "y": 5.588260640724453, - "heading": 3.1402648683338534, - "angularVelocity": -2.053134785130435e-9, - "velocityX": 1.6243937831794293, - "velocityY": -0.010089110998122468, - "timestamp": 0.6065715533094409 - }, - { - "x": 2.3324572167046935, - "y": 5.587240679436179, - "heading": 3.1402648681262915, - "angularVelocity": -2.0531346533039746e-9, - "velocityX": 1.6243937831794288, - "velocityY": -0.010089110998122467, - "timestamp": 0.7076668121943477 - }, - { - "x": 2.49667572674625, - "y": 5.586220718147905, - "heading": 3.140264867918729, - "angularVelocity": -2.0531352108819935e-9, - "velocityX": 1.624393783179419, - "velocityY": -0.010089110998122406, - "timestamp": 0.8087620710792545 - }, - { - "x": 2.617435932159424, - "y": 5.585470676422119, - "heading": 3.139529737290238, - "angularVelocity": -0.0072716627525318345, - "velocityX": 1.1945189788836168, - "velocityY": -0.007419158267749976, - "timestamp": 0.9098573299641612 - }, - { - "x": 2.617435932159424, - "y": 5.585470676422119, - "heading": 3.139529737290238, - "angularVelocity": 0, - "velocityX": -2.8085786967275447e-37, - "velocityY": 1.7440158343416895e-35, - "timestamp": 1.010952588849068 - } - ] + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -3.740256292064187e-22, + "velocityX": -5.996486579954623e-19, + "velocityY": 3.7218569679078715e-21, + "timestamp": 0 + }, + { + "x": 1.3163279724628432, + "y": 5.593551858929642, + "heading": 3.140264869702983, + "angularVelocity": -0.007887547277553473, + "velocityX": 0.9650288514473437, + "velocityY": -0.0059937949143052385, + "timestamp": 0.0932013807522681 + }, + { + "x": 1.467723714747352, + "y": 5.592611539861448, + "heading": 3.1402648694385524, + "angularVelocity": -2.83720041018159e-9, + "velocityX": 1.6243937703766675, + "velocityY": -0.010089110918776865, + "timestamp": 0.1864027615045362 + }, + { + "x": 1.6191194570316485, + "y": 5.591671220793256, + "heading": 3.1402648691741217, + "angularVelocity": -2.837195579018617e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777356, + "timestamp": 0.2796041422568043 + }, + { + "x": 1.7705151993162385, + "y": 5.590730901725062, + "heading": 3.1402648689096915, + "angularVelocity": -2.83719540339334e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777356, + "timestamp": 0.3728055230090724 + }, + { + "x": 1.9219109416013613, + "y": 5.589790582656871, + "heading": 3.1402648686452608, + "angularVelocity": -2.837195245417577e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777356, + "timestamp": 0.46600690376134046 + }, + { + "x": 2.0733066838851757, + "y": 5.588850263588676, + "heading": 3.14026486838083, + "angularVelocity": -2.837195862058808e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777355, + "timestamp": 0.5592082845136086 + }, + { + "x": 2.224702426169452, + "y": 5.58790994452049, + "heading": 3.1402648681163994, + "angularVelocity": -2.8371952366306506e-9, + "velocityX": 1.6243937703767442, + "velocityY": -0.010089110918777355, + "timestamp": 0.6524096652658767 + }, + { + "x": 2.3760981684546945, + "y": 5.586969625452291, + "heading": 3.140264867851969, + "angularVelocity": -2.8371952652077662e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777358, + "timestamp": 0.7456110460181449 + }, + { + "x": 2.5274939107386767, + "y": 5.5860293063840984, + "heading": 3.1402648675875384, + "angularVelocity": -2.837200309142864e-9, + "velocityX": 1.624393770376668, + "velocityY": -0.010089110918776867, + "timestamp": 0.838812426770413 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": -0.007887547280595224, + "velocityX": 0.9650288514473356, + "velocityY": -0.0059937949145238275, + "timestamp": 0.9320138075226811 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": 3.1865843751901534e-21, + "velocityX": -4.755464983211591e-19, + "velocityY": 2.948079481483161e-21, + "timestamp": 1.0252151882749492 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing2.traj b/src/main/deploy/choreo/FrontWing2.traj index 73d19d55..217c3a7b 100644 --- a/src/main/deploy/choreo/FrontWing2.traj +++ b/src/main/deploy/choreo/FrontWing2.traj @@ -1,103 +1,113 @@ { - "samples": [ - { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, - "angularVelocity": -1.8084842834156656e-35, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 1.347146156455349, - "y": 5.593360447165824, - "heading": 3.140264869371664, - "angularVelocity": -0.007271662750997594, - "velocityX": 1.1945189788836181, - "velocityY": -0.007419158267666702, - "timestamp": 0.10109525888490682 - }, - { - "x": 1.5113646664969056, - "y": 5.59234048587755, - "heading": 3.1402648691641017, - "angularVelocity": -2.053134778232203e-9, - "velocityX": 1.6243937831794193, - "velocityY": -0.010089110998122406, - "timestamp": 0.20219051776981364 - }, - { - "x": 1.6755831765384634, - "y": 5.591320524589276, - "heading": 3.14026486895654, - "angularVelocity": -2.053134775032819e-9, - "velocityX": 1.624393783179429, - "velocityY": -0.010089110998122467, - "timestamp": 0.30328577665472045 - }, - { - "x": 1.8398016865800209, - "y": 5.5903005633010014, - "heading": 3.140264868748978, - "angularVelocity": -2.0531347455807303e-9, - "velocityX": 1.624393783179429, - "velocityY": -0.010089110998122468, - "timestamp": 0.4043810355396273 - }, - { - "x": 2.0040201966215783, - "y": 5.589280602012727, - "heading": 3.1402648685414154, - "angularVelocity": -2.0531349299385344e-9, - "velocityX": 1.624393783179429, - "velocityY": -0.01008911099812247, - "timestamp": 0.5054762944245341 - }, - { - "x": 2.168238706663136, - "y": 5.588260640724453, - "heading": 3.1402648683338534, - "angularVelocity": -2.053134785130435e-9, - "velocityX": 1.6243937831794293, - "velocityY": -0.010089110998122468, - "timestamp": 0.6065715533094409 - }, - { - "x": 2.3324572167046935, - "y": 5.587240679436179, - "heading": 3.1402648681262915, - "angularVelocity": -2.0531346533039746e-9, - "velocityX": 1.6243937831794288, - "velocityY": -0.010089110998122467, - "timestamp": 0.7076668121943477 - }, - { - "x": 2.49667572674625, - "y": 5.586220718147905, - "heading": 3.140264867918729, - "angularVelocity": -2.0531352108819935e-9, - "velocityX": 1.624393783179419, - "velocityY": -0.010089110998122406, - "timestamp": 0.8087620710792545 - }, - { - "x": 2.617435932159424, - "y": 5.585470676422119, - "heading": 3.139529737290238, - "angularVelocity": -0.0072716627525318345, - "velocityX": 1.1945189788836168, - "velocityY": -0.007419158267749976, - "timestamp": 0.9098573299641612 - }, - { - "x": 2.617435932159424, - "y": 5.585470676422119, - "heading": 3.139529737290238, - "angularVelocity": 0, - "velocityX": -2.8085786967275447e-37, - "velocityY": 1.7440158343416895e-35, - "timestamp": 1.010952588849068 - } - ] + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -3.740256292064187e-22, + "velocityX": -5.996486579954623e-19, + "velocityY": 3.7218569679078715e-21, + "timestamp": 0 + }, + { + "x": 1.3163279724628432, + "y": 5.593551858929642, + "heading": 3.140264869702983, + "angularVelocity": -0.007887547277553473, + "velocityX": 0.9650288514473437, + "velocityY": -0.0059937949143052385, + "timestamp": 0.0932013807522681 + }, + { + "x": 1.467723714747352, + "y": 5.592611539861448, + "heading": 3.1402648694385524, + "angularVelocity": -2.83720041018159e-9, + "velocityX": 1.6243937703766675, + "velocityY": -0.010089110918776865, + "timestamp": 0.1864027615045362 + }, + { + "x": 1.6191194570316485, + "y": 5.591671220793256, + "heading": 3.1402648691741217, + "angularVelocity": -2.837195579018617e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777356, + "timestamp": 0.2796041422568043 + }, + { + "x": 1.7705151993162385, + "y": 5.590730901725062, + "heading": 3.1402648689096915, + "angularVelocity": -2.83719540339334e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777356, + "timestamp": 0.3728055230090724 + }, + { + "x": 1.9219109416013613, + "y": 5.589790582656871, + "heading": 3.1402648686452608, + "angularVelocity": -2.837195245417577e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777356, + "timestamp": 0.46600690376134046 + }, + { + "x": 2.0733066838851757, + "y": 5.588850263588676, + "heading": 3.14026486838083, + "angularVelocity": -2.837195862058808e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777355, + "timestamp": 0.5592082845136086 + }, + { + "x": 2.224702426169452, + "y": 5.58790994452049, + "heading": 3.1402648681163994, + "angularVelocity": -2.8371952366306506e-9, + "velocityX": 1.6243937703767442, + "velocityY": -0.010089110918777355, + "timestamp": 0.6524096652658767 + }, + { + "x": 2.3760981684546945, + "y": 5.586969625452291, + "heading": 3.140264867851969, + "angularVelocity": -2.8371952652077662e-9, + "velocityX": 1.6243937703767444, + "velocityY": -0.010089110918777358, + "timestamp": 0.7456110460181449 + }, + { + "x": 2.5274939107386767, + "y": 5.5860293063840984, + "heading": 3.1402648675875384, + "angularVelocity": -2.837200309142864e-9, + "velocityX": 1.624393770376668, + "velocityY": -0.010089110918776867, + "timestamp": 0.838812426770413 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": -0.007887547280595224, + "velocityX": 0.9650288514473356, + "velocityY": -0.0059937949145238275, + "timestamp": 0.9320138075226811 + }, + { + "x": 2.617435932159424, + "y": 5.585470676422119, + "heading": 3.139529737290238, + "angularVelocity": 3.1865843751901534e-21, + "velocityX": -4.755464983211591e-19, + "velocityY": 2.948079481483161e-21, + "timestamp": 1.0252151882749492 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3.1.traj b/src/main/deploy/choreo/FrontWing3.1.traj new file mode 100644 index 00000000..261ddd63 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing3.1.traj @@ -0,0 +1,158 @@ +{ + "samples": [ + { + "x": 1.2650032043457031, + "y": 5.615814685821533, + "heading": 3.141, + "angularVelocity": -9.961906884794862e-27, + "velocityX": 6.399458285179746e-26, + "velocityY": 1.0314201039068904e-25, + "timestamp": 0 + }, + { + "x": 1.310864623992567, + "y": 5.640347561603675, + "heading": 3.338413018068366, + "angularVelocity": 2.2732552294766206, + "velocityX": 0.5281045447942572, + "velocityY": 0.28250157315633995, + "timestamp": 0.08684155457274238 + }, + { + "x": 1.4026758531664765, + "y": 5.712962256076443, + "heading": 3.4150278612810783, + "angularVelocity": 0.8822371224197347, + "velocityX": 1.0572269189055614, + "velocityY": 0.8361745115000444, + "timestamp": 0.17368310914548477 + }, + { + "x": 1.505326244994725, + "y": 5.809724596960637, + "heading": 3.4150280522977696, + "angularVelocity": 0.0000021996000891052408, + "velocityX": 1.1820423106574365, + "velocityY": 1.1142400819545595, + "timestamp": 0.26052466371822713 + }, + { + "x": 1.6079766300540685, + "y": 5.906486945025769, + "heading": 3.4150282433141586, + "angularVelocity": 0.0000021995966111503638, + "velocityX": 1.182042232711982, + "velocityY": 1.1142401646446862, + "timestamp": 0.34736621829096953 + }, + { + "x": 1.7106270151134115, + "y": 6.003249293090899, + "heading": 3.4150284343305666, + "angularVelocity": 0.0000021995968268735563, + "velocityX": 1.182042232711972, + "velocityY": 1.1142401646446563, + "timestamp": 0.4342077728637119 + }, + { + "x": 1.8132774001727587, + "y": 6.100011641156021, + "heading": 3.4150286253469933, + "angularVelocity": 0.0000021995970422363537, + "velocityX": 1.1820422327120208, + "velocityY": 1.1142401646445645, + "timestamp": 0.5210493274364543 + }, + { + "x": 1.9159277852321102, + "y": 6.1967739892211355, + "heading": 3.4150288163634386, + "angularVelocity": 0.000002199597258109715, + "velocityX": 1.1820422327120694, + "velocityY": 1.1142401646444724, + "timestamp": 0.6078908820091966 + }, + { + "x": 2.018578170291466, + "y": 6.293536337286241, + "heading": 3.4150290073799026, + "angularVelocity": 0.0000021995974740409558, + "velocityX": 1.1820422327121183, + "velocityY": 1.1142401646443802, + "timestamp": 0.694732436581939 + }, + { + "x": 2.121228555350826, + "y": 6.390298685351339, + "heading": 3.415029198396385, + "angularVelocity": 0.000002199597689726547, + "velocityX": 1.182042232712167, + "velocityY": 1.1142401646442885, + "timestamp": 0.7815739911546813 + }, + { + "x": 2.22387894041019, + "y": 6.487061033416429, + "heading": 3.4150293894128865, + "angularVelocity": 0.0000021995979051884085, + "velocityX": 1.1820422327122158, + "velocityY": 1.1142401646441964, + "timestamp": 0.8684155457274236 + }, + { + "x": 2.3265293254695587, + "y": 6.583823381481512, + "heading": 3.415029580429407, + "angularVelocity": 0.000002199598120574345, + "velocityX": 1.1820422327122646, + "velocityY": 1.1142401646441045, + "timestamp": 0.955257100300166 + }, + { + "x": 2.4291797105289343, + "y": 6.680585729546584, + "heading": 3.415029771445946, + "angularVelocity": 0.0000021995983364688408, + "velocityX": 1.1820422327123468, + "velocityY": 1.114240164643977, + "timestamp": 1.0420986548729083 + }, + { + "x": 2.5318300994211365, + "y": 6.777348073545511, + "heading": 3.4150299624626883, + "angularVelocity": 0.0000021996006785706527, + "velocityX": 1.1820422768482035, + "velocityY": 1.1142401178214183, + "timestamp": 1.1289402094456507 + }, + { + "x": 2.6271100513248182, + "y": 6.858057139717029, + "heading": 3.467307103119733, + "angularVelocity": 0.6019830127897501, + "velocityX": 1.0971700399935989, + "velocityY": 0.9293830190926906, + "timestamp": 1.215781764018393 + }, + { + "x": 2.6726744174957275, + "y": 6.893547058105469, + "heading": 3.6456400220863263, + "angularVelocity": 2.0535436041418818, + "velocityX": 0.5246839073193058, + "velocityY": 0.40867437902337306, + "timestamp": 1.3026233185911353 + }, + { + "x": 2.6726744174957275, + "y": 6.893547058105469, + "heading": 3.6456400220863263, + "angularVelocity": 9.465973134511326e-26, + "velocityX": -9.092672379053801e-26, + "velocityY": -8.734024503832287e-26, + "timestamp": 1.3894648731638777 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3.traj b/src/main/deploy/choreo/FrontWing3.traj new file mode 100644 index 00000000..261ddd63 --- /dev/null +++ b/src/main/deploy/choreo/FrontWing3.traj @@ -0,0 +1,158 @@ +{ + "samples": [ + { + "x": 1.2650032043457031, + "y": 5.615814685821533, + "heading": 3.141, + "angularVelocity": -9.961906884794862e-27, + "velocityX": 6.399458285179746e-26, + "velocityY": 1.0314201039068904e-25, + "timestamp": 0 + }, + { + "x": 1.310864623992567, + "y": 5.640347561603675, + "heading": 3.338413018068366, + "angularVelocity": 2.2732552294766206, + "velocityX": 0.5281045447942572, + "velocityY": 0.28250157315633995, + "timestamp": 0.08684155457274238 + }, + { + "x": 1.4026758531664765, + "y": 5.712962256076443, + "heading": 3.4150278612810783, + "angularVelocity": 0.8822371224197347, + "velocityX": 1.0572269189055614, + "velocityY": 0.8361745115000444, + "timestamp": 0.17368310914548477 + }, + { + "x": 1.505326244994725, + "y": 5.809724596960637, + "heading": 3.4150280522977696, + "angularVelocity": 0.0000021996000891052408, + "velocityX": 1.1820423106574365, + "velocityY": 1.1142400819545595, + "timestamp": 0.26052466371822713 + }, + { + "x": 1.6079766300540685, + "y": 5.906486945025769, + "heading": 3.4150282433141586, + "angularVelocity": 0.0000021995966111503638, + "velocityX": 1.182042232711982, + "velocityY": 1.1142401646446862, + "timestamp": 0.34736621829096953 + }, + { + "x": 1.7106270151134115, + "y": 6.003249293090899, + "heading": 3.4150284343305666, + "angularVelocity": 0.0000021995968268735563, + "velocityX": 1.182042232711972, + "velocityY": 1.1142401646446563, + "timestamp": 0.4342077728637119 + }, + { + "x": 1.8132774001727587, + "y": 6.100011641156021, + "heading": 3.4150286253469933, + "angularVelocity": 0.0000021995970422363537, + "velocityX": 1.1820422327120208, + "velocityY": 1.1142401646445645, + "timestamp": 0.5210493274364543 + }, + { + "x": 1.9159277852321102, + "y": 6.1967739892211355, + "heading": 3.4150288163634386, + "angularVelocity": 0.000002199597258109715, + "velocityX": 1.1820422327120694, + "velocityY": 1.1142401646444724, + "timestamp": 0.6078908820091966 + }, + { + "x": 2.018578170291466, + "y": 6.293536337286241, + "heading": 3.4150290073799026, + "angularVelocity": 0.0000021995974740409558, + "velocityX": 1.1820422327121183, + "velocityY": 1.1142401646443802, + "timestamp": 0.694732436581939 + }, + { + "x": 2.121228555350826, + "y": 6.390298685351339, + "heading": 3.415029198396385, + "angularVelocity": 0.000002199597689726547, + "velocityX": 1.182042232712167, + "velocityY": 1.1142401646442885, + "timestamp": 0.7815739911546813 + }, + { + "x": 2.22387894041019, + "y": 6.487061033416429, + "heading": 3.4150293894128865, + "angularVelocity": 0.0000021995979051884085, + "velocityX": 1.1820422327122158, + "velocityY": 1.1142401646441964, + "timestamp": 0.8684155457274236 + }, + { + "x": 2.3265293254695587, + "y": 6.583823381481512, + "heading": 3.415029580429407, + "angularVelocity": 0.000002199598120574345, + "velocityX": 1.1820422327122646, + "velocityY": 1.1142401646441045, + "timestamp": 0.955257100300166 + }, + { + "x": 2.4291797105289343, + "y": 6.680585729546584, + "heading": 3.415029771445946, + "angularVelocity": 0.0000021995983364688408, + "velocityX": 1.1820422327123468, + "velocityY": 1.114240164643977, + "timestamp": 1.0420986548729083 + }, + { + "x": 2.5318300994211365, + "y": 6.777348073545511, + "heading": 3.4150299624626883, + "angularVelocity": 0.0000021996006785706527, + "velocityX": 1.1820422768482035, + "velocityY": 1.1142401178214183, + "timestamp": 1.1289402094456507 + }, + { + "x": 2.6271100513248182, + "y": 6.858057139717029, + "heading": 3.467307103119733, + "angularVelocity": 0.6019830127897501, + "velocityX": 1.0971700399935989, + "velocityY": 0.9293830190926906, + "timestamp": 1.215781764018393 + }, + { + "x": 2.6726744174957275, + "y": 6.893547058105469, + "heading": 3.6456400220863263, + "angularVelocity": 2.0535436041418818, + "velocityX": 0.5246839073193058, + "velocityY": 0.40867437902337306, + "timestamp": 1.3026233185911353 + }, + { + "x": 2.6726744174957275, + "y": 6.893547058105469, + "heading": 3.6456400220863263, + "angularVelocity": 9.465973134511326e-26, + "velocityX": -9.092672379053801e-26, + "velocityY": -8.734024503832287e-26, + "timestamp": 1.3894648731638777 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3Contested5.1.traj b/src/main/deploy/choreo/FrontWing3Contested5.1.traj index 0cd984e3..b82d664d 100644 --- a/src/main/deploy/choreo/FrontWing3Contested5.1.traj +++ b/src/main/deploy/choreo/FrontWing3Contested5.1.traj @@ -1,166 +1,167 @@ { - "samples": [ - { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, - "angularVelocity": 0, - "velocityX": -1.009927502555409e-33, - "velocityY": 9.510006388327946e-34, - "timestamp": 0 - }, - { - "x": 1.2629372157532939, - "y": 5.6118681655215035, - "heading": 3.3611684682063676, - "angularVelocity": 2.615104090958444, - "velocityX": 0.4341464636351002, - "velocityY": 0.21092108774289103, - "timestamp": 0.08419109165389853 - }, - { - "x": 1.3467908645165432, - "y": 5.672396843669079, - "heading": 3.470572560445472, - "angularVelocity": 1.2994734964223353, - "velocityX": 0.9959919406671149, - "velocityY": 0.718943975645346, - "timestamp": 0.16838218330779706 - }, - { - "x": 1.4464012333437837, - "y": 5.766107211983687, - "heading": 3.4705728197139094, - "angularVelocity": 0.000003079523404990731, - "velocityX": 1.1831461841203954, - "velocityY": 1.1130675048120657, - "timestamp": 0.25257327496169557 - }, - { - "x": 1.5460115900456228, - "y": 5.859817593187631, - "heading": 3.4705730789812166, - "angularVelocity": 0.000003079509987095674, - "velocityX": 1.1831460400980154, - "velocityY": 1.1130676579082395, - "timestamp": 0.3367643666155941 - }, - { - "x": 1.645621946747464, - "y": 5.953527974391563, - "heading": 3.470573338248568, - "angularVelocity": 0.000003079510511134596, - "velocityX": 1.1831460400980387, - "velocityY": 1.1130676579081111, - "timestamp": 0.42095545826949265 - }, - { - "x": 1.7452323034493133, - "y": 6.047238355595477, - "heading": 3.470573597515964, - "angularVelocity": 0.000003079511034719833, - "velocityX": 1.183146040098137, - "velocityY": 1.1130676579079029, - "timestamp": 0.5051465499233911 - }, - { - "x": 1.844842660151171, - "y": 6.140948736799373, - "heading": 3.470573856783404, - "angularVelocity": 0.0000030795115586857372, - "velocityX": 1.1831460400982357, - "velocityY": 1.1130676579076944, - "timestamp": 0.5893376415772896 - }, - { - "x": 1.944453016853037, - "y": 6.234659118003253, - "heading": 3.470574116050888, - "angularVelocity": 0.0000030795120819840736, - "velocityX": 1.1831460400983345, - "velocityY": 1.1130676579074863, - "timestamp": 0.6735287332311881 - }, - { - "x": 2.044063373554911, - "y": 6.328369499207114, - "heading": 3.470574375318416, - "angularVelocity": 0.0000030795126059487745, - "velocityX": 1.1831460400984328, - "velocityY": 1.1130676579072778, - "timestamp": 0.7577198248850866 - }, - { - "x": 2.1436737302567934, - "y": 6.4220798804109585, - "heading": 3.470574634585988, - "angularVelocity": 0.000003079513129853114, - "velocityX": 1.183146040098531, - "velocityY": 1.1130676579070695, - "timestamp": 0.8419109165389851 - }, - { - "x": 2.243284086958684, - "y": 6.515790261614786, - "heading": 3.470574893853604, - "angularVelocity": 0.0000030795136536386766, - "velocityX": 1.1831460400986302, - "velocityY": 1.1130676579068612, - "timestamp": 0.9261020081928836 - }, - { - "x": 2.342894443660583, - "y": 6.6095006428185945, - "heading": 3.4705751531212647, - "angularVelocity": 0.0000030795141771132826, - "velocityX": 1.1831460400987281, - "velocityY": 1.1130676579066532, - "timestamp": 1.010293099846782 - }, - { - "x": 2.442504800362493, - "y": 6.703211024022384, - "heading": 3.470575412388969, - "angularVelocity": 0.0000030795147010529834, - "velocityX": 1.1831460400988572, - "velocityY": 1.1130676579064127, - "timestamp": 1.0944841915006807 - }, - { - "x": 2.5421151619283058, - "y": 6.79692140005584, - "heading": 3.470575671657143, - "angularVelocity": 0.0000030795202773893193, - "velocityX": 1.1831460978710306, - "velocityY": 1.1130675964945436, - "timestamp": 1.1786752831545793 - }, - { - "x": 2.632005363421341, - "y": 6.868564036243049, - "heading": 3.5437695268680747, - "angularVelocity": 0.8693776713553536, - "velocityX": 1.0676925518743172, - "velocityY": 0.8509526932103911, - "timestamp": 1.2628663748084779 - }, - { - "x": 2.6714627742767334, - "y": 6.900224685668945, - "heading": 3.729596046054495, - "angularVelocity": 2.207199307384385, - "velocityX": 0.46866491549483613, - "velocityY": 0.37605700085290744, - "timestamp": 1.3470574664623765 - }, - { - "x": 2.6714627742767334, - "y": 6.900224685668945, - "heading": 3.729596046054495, - "angularVelocity": 0, - "velocityX": -1.5194658088032264e-34, - "velocityY": -2.7343286363371283e-33, - "timestamp": 1.431248558116275 - } - ] + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -9.716627282364513e-28, + "velocityX": -2.1006296109192272e-27, + "velocityY": 1.6816897032610772e-27, + "timestamp": 0 + }, + { + "x": 1.2629372172985613, + "y": 5.611868166522511, + "heading": 3.3611684637424437, + "angularVelocity": 2.6151040269567423, + "velocityX": 0.4341464801664794, + "velocityY": 0.21092109874697887, + "timestamp": 0.08419109200740245 + }, + { + "x": 1.3467908643065862, + "y": 5.672396849103953, + "heading": 3.4705725495976436, + "angularVelocity": 1.2994734151397, + "velocityX": 0.9959919156370148, + "velocityY": 0.7189440252909496, + "timestamp": 0.1683821840148049 + }, + { + "x": 1.4464012359618026, + "y": 5.766107214252463, + "heading": 3.4705728088664487, + "angularVelocity": 0.0000030795277620559296, + "velocityX": 1.1831462127425316, + "velocityY": 1.1130674625323809, + "timestamp": 0.25257327602220736 + }, + { + "x": 1.5460115925515412, + "y": 5.859817595415609, + "heading": 3.4705730681338594, + "angularVelocity": 0.0000030795111997424536, + "velocityX": 1.1831460337986872, + "velocityY": 1.1130676527500878, + "timestamp": 0.3367643680296098 + }, + { + "x": 1.6456219491412756, + "y": 5.9535279765787505, + "heading": 3.4705733274013144, + "angularVelocity": 0.000003079511724050569, + "velocityX": 1.183146033798638, + "velocityY": 1.1130676527500365, + "timestamp": 0.42095546003701223 + }, + { + "x": 1.7452323057310184, + "y": 6.0472383577418745, + "heading": 3.4705735866688134, + "angularVelocity": 0.000003079512247611481, + "velocityX": 1.1831460337987365, + "velocityY": 1.113067652749828, + "timestamp": 0.5051465520444147 + }, + { + "x": 1.8448426623207694, + "y": 6.1409487389049815, + "heading": 3.4705738459363564, + "angularVelocity": 0.0000030795127711126187, + "velocityX": 1.1831460337988353, + "velocityY": 1.1130676527496197, + "timestamp": 0.5893376440518172 + }, + { + "x": 1.9444530189105287, + "y": 6.234659120068071, + "heading": 3.470574105203944, + "angularVelocity": 0.000003079513294842049, + "velocityX": 1.183146033798934, + "velocityY": 1.1130676527494117, + "timestamp": 0.6735287360592197 + }, + { + "x": 2.0440633755002966, + "y": 6.328369501231142, + "heading": 3.470574364471575, + "angularVelocity": 0.0000030795138187518754, + "velocityX": 1.1831460337990323, + "velocityY": 1.1130676527492032, + "timestamp": 0.7577198280666222 + }, + { + "x": 2.1436737320900723, + "y": 6.422079882394196, + "heading": 3.4705746237392505, + "angularVelocity": 0.0000030795143424110884, + "velocityX": 1.1831460337991306, + "velocityY": 1.113067652748995, + "timestamp": 0.8419109200740247 + }, + { + "x": 2.2432840886798564, + "y": 6.515790263557233, + "heading": 3.47057488300697, + "angularVelocity": 0.0000030795148666763967, + "velocityX": 1.1831460337992294, + "velocityY": 1.1130676527487866, + "timestamp": 0.9261020120814272 + }, + { + "x": 2.342894445269649, + "y": 6.609500644720252, + "heading": 3.4705751422747335, + "angularVelocity": 0.0000030795153898968674, + "velocityX": 1.1831460337993278, + "velocityY": 1.1130676527485786, + "timestamp": 1.0102931040888297 + }, + { + "x": 2.4425048018594557, + "y": 6.703211025883247, + "heading": 3.470575401542541, + "angularVelocity": 0.000003079515913944498, + "velocityX": 1.1831460337994937, + "velocityY": 1.1130676527482986, + "timestamp": 1.0944841960962322 + }, + { + "x": 2.5421151652954292, + "y": 6.796921399768766, + "heading": 3.4705756608109883, + "angularVelocity": 0.0000030795235106850793, + "velocityX": 1.183146115116498, + "velocityY": 1.1130675663083123, + "timestamp": 1.1786752881036346 + }, + { + "x": 2.632005364209705, + "y": 6.868564037074326, + "heading": 3.5437695190039196, + "angularVelocity": 0.8693777031244078, + "velocityX": 1.0676925167614182, + "velocityY": 0.8509527029208858, + "timestamp": 1.2628663801110371 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 2.207199391525135, + "velocityX": 0.46866490416300804, + "velocityY": 0.3760569894002088, + "timestamp": 1.3470574721184396 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": -1.0211590332128658e-23, + "velocityX": -1.1480030002254912e-23, + "velocityY": -4.344519998520169e-23, + "timestamp": 1.4312485641258421 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3Contested5.2.traj b/src/main/deploy/choreo/FrontWing3Contested5.2.traj index 17798e6e..424aea1d 100644 --- a/src/main/deploy/choreo/FrontWing3Contested5.2.traj +++ b/src/main/deploy/choreo/FrontWing3Contested5.2.traj @@ -1,679 +1,680 @@ { - "samples": [ - { - "x": 2.6714627742767334, - "y": 6.900224685668945, - "heading": 3.729596046054495, - "angularVelocity": 0, - "velocityX": -1.5194658088032264e-34, - "velocityY": -2.7343286363371283e-33, - "timestamp": 0 - }, - { - "x": 2.7208277247721226, - "y": 6.912165859423971, - "heading": 3.486294740771461, - "angularVelocity": -2.694751338131124, - "velocityX": 0.5467552516805291, - "velocityY": 0.13225779416916014, - "timestamp": 0.09028710806829743 - }, - { - "x": 2.839209601133149, - "y": 6.942185820162622, - "heading": 3.3899477762404344, - "angularVelocity": -1.0671176272269702, - "velocityX": 1.311171427392232, - "velocityY": 0.33249443227201864, - "timestamp": 0.18057421613659486 - }, - { - "x": 2.985101375777118, - "y": 6.957221507964457, - "heading": 3.3899472751996993, - "angularVelocity": -0.0000055494161427999685, - "velocityX": 1.615864964171956, - "velocityY": 0.16653194596133747, - "timestamp": 0.2708613242048923 - }, - { - "x": 3.130993151670898, - "y": 6.97225718364292, - "heading": 3.3899467741601925, - "angularVelocity": -0.000005549402536979108, - "velocityX": 1.6158649780145908, - "velocityY": 0.1665318116855625, - "timestamp": 0.3611484322731897 - }, - { - "x": 3.2768849275645753, - "y": 6.98729285932146, - "heading": 3.3899462731201484, - "angularVelocity": -0.00000554940849560343, - "velocityX": 1.6158649780134444, - "velocityY": 0.1665318116864077, - "timestamp": 0.45143554034148714 - }, - { - "x": 3.4227767034581484, - "y": 7.002328535000082, - "heading": 3.3899457720795656, - "angularVelocity": -0.0000055494144557147385, - "velocityX": 1.615864978012292, - "velocityY": 0.1665318116873102, - "timestamp": 0.5417226484097846 - }, - { - "x": 3.5686684793516172, - "y": 7.017364210678785, - "heading": 3.3899452710384446, - "angularVelocity": -0.0000055494204159770735, - "velocityX": 1.6158649780111394, - "velocityY": 0.1665318116882126, - "timestamp": 0.632009756478082 - }, - { - "x": 3.7145602552449817, - "y": 7.03239988635757, - "heading": 3.3899447699967857, - "angularVelocity": -0.000005549426375721003, - "velocityX": 1.6158649780099874, - "velocityY": 0.1665318116891151, - "timestamp": 0.7222968645463794 - }, - { - "x": 3.8604520311382426, - "y": 7.047435562036436, - "heading": 3.3899442689545887, - "angularVelocity": -0.000005549432334941507, - "velocityX": 1.615864978008835, - "velocityY": 0.16653181169001743, - "timestamp": 0.8125839726146769 - }, - { - "x": 4.006343807031399, - "y": 7.062471237715384, - "heading": 3.389943767911854, - "angularVelocity": -0.000005549438294605419, - "velocityX": 1.6158649780076826, - "velocityY": 0.16653181169091993, - "timestamp": 0.9028710806829743 - }, - { - "x": 4.152235582924452, - "y": 7.077506913394413, - "heading": 3.3899432668685807, - "angularVelocity": -0.000005549444254929096, - "velocityX": 1.6158649780065304, - "velocityY": 0.16653181169182238, - "timestamp": 0.9931581887512717 - }, - { - "x": 4.298127358817401, - "y": 7.092542589073523, - "heading": 3.3899427658247694, - "angularVelocity": -0.000005549450214439604, - "velocityX": 1.615864978005378, - "velocityY": 0.1665318116927247, - "timestamp": 1.0834452968195691 - }, - { - "x": 4.444019134710246, - "y": 7.107578264752715, - "heading": 3.38994226478042, - "angularVelocity": -0.000005549456174760312, - "velocityX": 1.6158649780042256, - "velocityY": 0.16653181169362713, - "timestamp": 1.1737324048878666 - }, - { - "x": 4.5899109106029865, - "y": 7.122613940431989, - "heading": 3.389941763735532, - "angularVelocity": -0.000005549462134716656, - "velocityX": 1.6158649780030732, - "velocityY": 0.16653181169452963, - "timestamp": 1.264019512956164 - }, - { - "x": 4.735802686495623, - "y": 7.137649616111343, - "heading": 3.389941262690107, - "angularVelocity": -0.0000055494680950242725, - "velocityX": 1.6158649780019205, - "velocityY": 0.1665318116954319, - "timestamp": 1.3543066210244614 - }, - { - "x": 4.881694462388156, - "y": 7.15268529179078, - "heading": 3.389940761644143, - "angularVelocity": -0.000005549474055578583, - "velocityX": 1.6158649780007681, - "velocityY": 0.16653181169633444, - "timestamp": 1.4445937290927588 - }, - { - "x": 5.027586238280584, - "y": 7.167720967470297, - "heading": 3.3899402605976414, - "angularVelocity": -0.000005549480015574405, - "velocityX": 1.6158649779996155, - "velocityY": 0.1665318116972368, - "timestamp": 1.5348808371610563 - }, - { - "x": 5.173478014172908, - "y": 7.182756643149896, - "heading": 3.389939759550601, - "angularVelocity": -0.000005549485976020103, - "velocityX": 1.6158649779984633, - "velocityY": 0.16653181169813924, - "timestamp": 1.6251679452293537 - }, - { - "x": 5.319369790065129, - "y": 7.197792318829577, - "heading": 3.389939258503023, - "angularVelocity": -0.000005549491936617687, - "velocityX": 1.6158649779973107, - "velocityY": 0.16653181169904166, - "timestamp": 1.7154550532976511 - }, - { - "x": 5.4652615659572445, - "y": 7.212827994509339, - "heading": 3.3899387574549067, - "angularVelocity": -0.00000554949789662324, - "velocityX": 1.6158649779961582, - "velocityY": 0.1665318116999441, - "timestamp": 1.8057421613659486 - }, - { - "x": 5.611153341849257, - "y": 7.2278636701891825, - "heading": 3.389938256406252, - "angularVelocity": -0.000005549503857497513, - "velocityX": 1.6158649779950054, - "velocityY": 0.16653181170084652, - "timestamp": 1.896029269434246 - }, - { - "x": 5.757045117741165, - "y": 7.242899345869108, - "heading": 3.3899377553570593, - "angularVelocity": -0.000005549509818264508, - "velocityX": 1.615864977993853, - "velocityY": 0.16653181170174894, - "timestamp": 1.9863163775025434 - }, - { - "x": 5.902936893632969, - "y": 7.257935021549114, - "heading": 3.3899372543073283, - "angularVelocity": -0.000005549515778536107, - "velocityX": 1.6158649779927006, - "velocityY": 0.1665318117026513, - "timestamp": 2.076603485570841 - }, - { - "x": 6.048828669524668, - "y": 7.272970697229202, - "heading": 3.3899367532570595, - "angularVelocity": -0.000005549521739714091, - "velocityX": 1.6158649779915477, - "velocityY": 0.16653181170355377, - "timestamp": 2.1668905936391383 - }, - { - "x": 6.194720445416264, - "y": 7.288006372909372, - "heading": 3.3899362522062524, - "angularVelocity": -0.000005549527700309559, - "velocityX": 1.6158649779903953, - "velocityY": 0.16653181170445627, - "timestamp": 2.2571777017074357 - }, - { - "x": 6.340612221307755, - "y": 7.303042048589623, - "heading": 3.389935751154907, - "angularVelocity": -0.000005549533660762894, - "velocityX": 1.6158649779892427, - "velocityY": 0.16653181170535863, - "timestamp": 2.347464809775733 - }, - { - "x": 6.486503997199144, - "y": 7.318077724269955, - "heading": 3.3899352501030235, - "angularVelocity": -0.000005549539621393899, - "velocityX": 1.6158649779880898, - "velocityY": 0.16653181170626094, - "timestamp": 2.4377519178440306 - }, - { - "x": 6.632395773090427, - "y": 7.333113399950369, - "heading": 3.3899347490506018, - "angularVelocity": -0.00000554954558299688, - "velocityX": 1.6158649779869374, - "velocityY": 0.1665318117071635, - "timestamp": 2.528039025912328 - }, - { - "x": 6.778287548981607, - "y": 7.3481490756308645, - "heading": 3.3899342479976413, - "angularVelocity": -0.000005549551543569729, - "velocityX": 1.615864977985785, - "velocityY": 0.16653181170806597, - "timestamp": 2.6183261339806254 - }, - { - "x": 6.924179324872682, - "y": 7.363184751311442, - "heading": 3.3899337469441435, - "angularVelocity": -0.000005549557505439446, - "velocityX": 1.615864977984632, - "velocityY": 0.16653181170896839, - "timestamp": 2.708613242048923 - }, - { - "x": 7.070071100763654, - "y": 7.378220426992101, - "heading": 3.389933245890107, - "angularVelocity": -0.00000554956346606953, - "velocityX": 1.6158649779834795, - "velocityY": 0.1665318117098707, - "timestamp": 2.7989003501172203 - }, - { - "x": 7.215962876654522, - "y": 7.393256102672841, - "heading": 3.3899327448355323, - "angularVelocity": -0.000005549569427174084, - "velocityX": 1.6158649779823266, - "velocityY": 0.16653181171077316, - "timestamp": 2.8891874581855177 - }, - { - "x": 7.361854652545285, - "y": 7.408291778353663, - "heading": 3.3899322437804194, - "angularVelocity": -0.000005549575389475234, - "velocityX": 1.615864977981174, - "velocityY": 0.16653181171167572, - "timestamp": 2.979474566253815 - }, - { - "x": 7.5077464284359445, - "y": 7.423327454034565, - "heading": 3.389931742724768, - "angularVelocity": -0.000005549581350385169, - "velocityX": 1.6158649779800212, - "velocityY": 0.1665318117125781, - "timestamp": 3.0697616743221126 - }, - { - "x": 7.6536382043265, - "y": 7.43836312971555, - "heading": 3.3899312416685787, - "angularVelocity": -0.000005549587311867374, - "velocityX": 1.6158649779788683, - "velocityY": 0.166531811713483, - "timestamp": 3.16004878239041 - }, - { - "x": 7.799529980161501, - "y": 7.453398805927109, - "heading": 3.3899307406089574, - "angularVelocity": -0.000005549625325450788, - "velocityX": 1.6158649773635676, - "velocityY": 0.16653181759000288, - "timestamp": 3.2503358904587074 - }, - { - "x": 7.908273189210625, - "y": 7.465055953097054, - "heading": 3.2483837637195303, - "angularVelocity": -1.5677429471143751, - "velocityX": 1.204415684317477, - "velocityY": 0.12911197865732152, - "timestamp": 3.340622998527005 - }, - { - "x": 7.936469078063965, - "y": 7.452759265899658, - "heading": 3.1300325241315123, - "angularVelocity": -1.3108321012839574, - "velocityX": 0.3122914163117384, - "velocityY": -0.13619538226979142, - "timestamp": 3.4309101065953023 - }, - { - "x": 7.897313987574637, - "y": 7.4320035366195905, - "heading": 3.223159902402127, - "angularVelocity": 0.9773255855261127, - "velocityX": -0.4109132292719665, - "velocityY": -0.21782107097140077, - "timestamp": 3.5261980839590406 - }, - { - "x": 7.764258802337476, - "y": 7.410354345348057, - "heading": 3.302887545584371, - "angularVelocity": 0.8367020204228218, - "velocityX": -1.396348090475833, - "velocityY": -0.22719751085588358, - "timestamp": 3.621486061322779 - }, - { - "x": 7.611104941895525, - "y": 7.387923016344696, - "heading": 3.302889098211225, - "angularVelocity": 0.000016294047758499195, - "velocityX": -1.607273705236954, - "velocityY": -0.23540565792192894, - "timestamp": 3.716774038686517 - }, - { - "x": 7.457951081309142, - "y": 7.365491688274562, - "heading": 3.302890650804134, - "angularVelocity": 0.000016293691520657764, - "velocityX": -1.6072737067526932, - "velocityY": -0.2354056481281831, - "timestamp": 3.8120620160502554 - }, - { - "x": 7.30479722071746, - "y": 7.343060360198914, - "heading": 3.3028922033698884, - "angularVelocity": 0.000016293406553711894, - "velocityX": -1.6072737068083087, - "velocityY": -0.23540564818605267, - "timestamp": 3.9073499934139937 - }, - { - "x": 7.151643360120445, - "y": 7.320629032117711, - "heading": 3.302893755908325, - "angularVelocity": 0.00001629311986139254, - "velocityX": -1.6072737068642717, - "velocityY": -0.23540564824432697, - "timestamp": 4.002637970777732 - }, - { - "x": 6.998489499518071, - "y": 7.298197704030925, - "heading": 3.302895308419307, - "angularVelocity": 0.00001629283173826444, - "velocityX": -1.6072737069205232, - "velocityY": -0.2354056483029315, - "timestamp": 4.09792594814147 - }, - { - "x": 6.845335638910315, - "y": 7.275766375938532, - "heading": 3.3028968609027314, - "angularVelocity": 0.000016292542536041963, - "velocityX": -1.6072737069769925, - "velocityY": -0.23540564836178338, - "timestamp": 4.1932139255052086 - }, - { - "x": 6.692181778297163, - "y": 7.253335047840514, - "heading": 3.302898413358527, - "angularVelocity": 0.000016292252586671315, - "velocityX": -1.6072737070336132, - "velocityY": -0.23540564842080344, - "timestamp": 4.288501902868947 - }, - { - "x": 6.539027917678609, - "y": 7.230903719736863, - "heading": 3.3028999657866516, - "angularVelocity": 0.000016291962193735624, - "velocityX": -1.607273707090324, - "velocityY": -0.23540564847991918, - "timestamp": 4.383789880232685 - }, - { - "x": 6.385874057054646, - "y": 7.208472391627575, - "heading": 3.302901518187087, - "angularVelocity": 0.000016291671606816483, - "velocityX": -1.607273707147075, - "velocityY": -0.2354056485390708, - "timestamp": 4.479077857596423 - }, - { - "x": 6.232720196425276, - "y": 7.186041063512652, - "heading": 3.3029030705598315, - "angularVelocity": 0.00001629138100746966, - "velocityX": -1.60727370720383, - "velocityY": -0.23540564859821628, - "timestamp": 4.574365834960162 - }, - { - "x": 6.079566335790499, - "y": 7.1636097353920976, - "heading": 3.3029046229048977, - "angularVelocity": 0.000016291090533089062, - "velocityX": -1.6072737072605614, - "velocityY": -0.23540564865732222, - "timestamp": 4.6696538123239 - }, - { - "x": 5.926412475150321, - "y": 7.141178407265915, - "heading": 3.302906175222303, - "angularVelocity": 0.000016290800243766995, - "velocityX": -1.6072737073172572, - "velocityY": -0.23540564871637523, - "timestamp": 4.764941789687638 - }, - { - "x": 5.773258614504743, - "y": 7.118747079134112, - "heading": 3.3029077275120664, - "angularVelocity": 0.00001629051016274816, - "velocityX": -1.6072737073739125, - "velocityY": -0.23540564877536937, - "timestamp": 4.8602297670513765 - }, - { - "x": 5.62010475385377, - "y": 7.096315750996691, - "heading": 3.302909279774206, - "angularVelocity": 0.000016290220263808567, - "velocityX": -1.6072737074305328, - "velocityY": -0.23540564883431206, - "timestamp": 4.955517744415115 - }, - { - "x": 5.466950893197405, - "y": 7.073884422853658, - "heading": 3.302910832008734, - "angularVelocity": 0.00001628993049423592, - "velocityX": -1.6072737074871284, - "velocityY": -0.23540564889321514, - "timestamp": 5.050805721778853 - }, - { - "x": 5.313797032535647, - "y": 7.051453094705014, - "heading": 3.3029123842156554, - "angularVelocity": 0.000016289640769266148, - "velocityX": -1.6072737075437156, - "velocityY": -0.23540564895209914, - "timestamp": 5.146093699142591 - }, - { - "x": 5.160643171868496, - "y": 7.02902176655076, - "heading": 3.3029139363949653, - "angularVelocity": 0.000016289351008016845, - "velocityX": -1.6072737076003119, - "velocityY": -0.23540564901098462, - "timestamp": 5.24138167650633 - }, - { - "x": 5.0074893111959495, - "y": 7.006590438390892, - "heading": 3.302915488546652, - "angularVelocity": 0.000016289061112899286, - "velocityX": -1.6072737076569361, - "velocityY": -0.23540564906989445, - "timestamp": 5.336669653870068 - }, - { - "x": 4.854335450518003, - "y": 6.984159110225406, - "heading": 3.3029170406706947, - "angularVelocity": 0.000016288771004663956, - "velocityX": -1.607273707713604, - "velocityY": -0.23540564912884793, - "timestamp": 5.431957631233806 - }, - { - "x": 4.701181589834651, - "y": 6.961727782054296, - "heading": 3.3029185927670666, - "angularVelocity": 0.00001628848060846621, - "velocityX": -1.6072737077703305, - "velocityY": -0.235405649187863, - "timestamp": 5.5272456085975445 - }, - { - "x": 4.548027729145889, - "y": 6.939296453877556, - "heading": 3.3029201448357353, - "angularVelocity": 0.000016288189883493963, - "velocityX": -1.607273707827124, - "velocityY": -0.23540564924694996, - "timestamp": 5.622533585961283 - }, - { - "x": 4.3948738684517075, - "y": 6.916865125695178, - "heading": 3.3029216968766675, - "angularVelocity": 0.00001628789879930085, - "velocityX": -1.6072737078839905, - "velocityY": -0.23540564930611646, - "timestamp": 5.717821563325021 - }, - { - "x": 4.2417200077521, - "y": 6.894433797507156, - "heading": 3.302923248889828, - "angularVelocity": 0.00001628760735122893, - "velocityX": -1.6072737079409307, - "velocityY": -0.23540564936536426, - "timestamp": 5.813109540688759 - }, - { - "x": 4.088566147047059, - "y": 6.872002469313479, - "heading": 3.3029248008751826, - "angularVelocity": 0.000016287315540072175, - "velocityX": -1.6072737079979444, - "velocityY": -0.23540564942469375, - "timestamp": 5.908397518052498 - }, - { - "x": 3.9354122863365792, - "y": 6.849571141114141, - "heading": 3.3029263528326944, - "angularVelocity": 0.000016287023346622306, - "velocityX": -1.6072737080550357, - "velocityY": -0.23540564948410944, - "timestamp": 6.003685495416236 - }, - { - "x": 3.7822584256206495, - "y": 6.827139812909132, - "heading": 3.3029279047623197, - "angularVelocity": 0.00001628673068839883, - "velocityX": -1.6072737081122201, - "velocityY": -0.23540564954363288, - "timestamp": 6.098973472779974 - }, - { - "x": 3.629104564899259, - "y": 6.804708484698436, - "heading": 3.3029294566639957, - "angularVelocity": 0.000016286437378238427, - "velocityX": -1.607273708169536, - "velocityY": -0.2354056496033085, - "timestamp": 6.1942614501437125 - }, - { - "x": 3.4759507041723876, - "y": 6.7822771564820306, - "heading": 3.302931008537628, - "angularVelocity": 0.00001628614306837342, - "velocityX": -1.6072737082270525, - "velocityY": -0.2354056496632214, - "timestamp": 6.289549427507451 - }, - { - "x": 3.3227968434400057, - "y": 6.759845828259881, - "heading": 3.3029325603830677, - "angularVelocity": 0.000016285847206848686, - "velocityX": -1.6072737082848803, - "velocityY": -0.23540564972350414, - "timestamp": 6.384837404871189 - }, - { - "x": 3.1696429827020713, - "y": 6.737414500031936, - "heading": 3.3029341122001052, - "angularVelocity": 0.00001628554913981109, - "velocityX": -1.6072737083431483, - "velocityY": -0.2354056497843278, - "timestamp": 6.480125382234927 - }, - { - "x": 3.016489122487879, - "y": 6.714983168191867, - "heading": 3.3029356639933174, - "angularVelocity": 0.000016285299102484297, - "velocityX": -1.6072737028467396, - "velocityY": -0.23540568769178183, - "timestamp": 6.575413359598667 - }, - { - "x": 2.8768170839321456, - "y": 6.685183755290752, - "heading": 3.3517957161483998, - "angularVelocity": 0.512761982223329, - "velocityX": -1.465788679956645, - "velocityY": -0.31273003925103915, - "timestamp": 6.670701336962406 - }, - { - "x": 2.812295436859131, - "y": 6.667538166046143, - "heading": 3.5782197562990956, - "angularVelocity": 2.3762078534459548, - "velocityX": -0.6771226429407677, - "velocityY": -0.18518169587388544, - "timestamp": 6.765989314326145 - }, - { - "x": 2.812295436859131, - "y": 6.667538166046143, - "heading": 3.5782197562990956, - "angularVelocity": 4.7042853211430965e-33, - "velocityX": -2.4138813228334137e-34, - "velocityY": 0, - "timestamp": 6.861277291689884 - } - ] + "samples": [ + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": -1.0211590332128658e-23, + "velocityX": -1.1480030002254912e-23, + "velocityY": -4.344519998520169e-23, + "timestamp": 0 + }, + { + "x": 2.720827723501713, + "y": 6.91216585910853, + "heading": 3.486294732820543, + "angularVelocity": -2.69475141570759, + "velocityX": 0.5467552354822697, + "velocityY": 0.13225779017707837, + "timestamp": 0.0902871084196335 + }, + { + "x": 2.8392095989605575, + "y": 6.942185818091124, + "heading": 3.389947764111763, + "angularVelocity": -1.0671176693463396, + "velocityX": 1.311171412297561, + "velocityY": 0.33249441150564496, + "timestamp": 0.180574216839267 + }, + { + "x": 2.985101373007063, + "y": 6.9572215101019514, + "heading": 3.3899472630700718, + "angularVelocity": -0.000005549426712336868, + "velocityX": 1.615864951266793, + "velocityY": 0.16653199192676998, + "timestamp": 0.2708613252589005 + }, + { + "x": 3.1309931487591207, + "y": 6.972257185567622, + "heading": 3.3899467620302377, + "angularVelocity": -0.000005549406141075235, + "velocityX": 1.6158649701561478, + "velocityY": 0.16653180870033996, + "timestamp": 0.361148433678534 + }, + { + "x": 3.276884924511046, + "y": 6.987292861034595, + "heading": 3.389946260989866, + "angularVelocity": -0.000005549412101088006, + "velocityX": 1.615864970155008, + "velocityY": 0.16653180870111933, + "timestamp": 0.4514355420981675 + }, + { + "x": 3.4227767002625384, + "y": 7.002328536502587, + "heading": 3.3899457599489553, + "angularVelocity": -0.000005549418061933105, + "velocityX": 1.6158649701538554, + "velocityY": 0.16653180870202178, + "timestamp": 0.541722650517801 + }, + { + "x": 3.5686684760143703, + "y": 7.017364211969376, + "heading": 3.389945258907507, + "angularVelocity": -0.000005549424021960657, + "velocityX": 1.6158649701527033, + "velocityY": 0.16653180870292417, + "timestamp": 0.6320097589374343 + }, + { + "x": 3.7145602517660743, + "y": 7.032399887436731, + "heading": 3.3899447578655204, + "angularVelocity": -0.000005549429982841055, + "velocityX": 1.6158649701515504, + "velocityY": 0.16653180870382667, + "timestamp": 0.7222968673570676 + }, + { + "x": 3.8604520275173986, + "y": 7.04743556290717, + "heading": 3.3899442568229956, + "angularVelocity": -0.000005549435944010941, + "velocityX": 1.615864970150398, + "velocityY": 0.1665318087047291, + "timestamp": 0.8125839757767008 + }, + { + "x": 4.00634380326868, + "y": 7.062471238372739, + "heading": 3.389943755779933, + "angularVelocity": -0.000005549441903913974, + "velocityX": 1.6158649701492453, + "velocityY": 0.1665318087056315, + "timestamp": 0.9028710841963341 + }, + { + "x": 4.1522355790200765, + "y": 7.077506913841867, + "heading": 3.389943254736332, + "angularVelocity": -0.0000055494478640805644, + "velocityX": 1.615864970148093, + "velocityY": 0.16653180870653383, + "timestamp": 0.9931581926159674 + }, + { + "x": 4.298127354771182, + "y": 7.0925425893096525, + "heading": 3.389942753692193, + "angularVelocity": -0.000005549453823846852, + "velocityX": 1.6158649701469403, + "velocityY": 0.16653180870743617, + "timestamp": 1.0834453010356007 + }, + { + "x": 4.444019130522188, + "y": 7.10757826477934, + "heading": 3.389942252647516, + "angularVelocity": -0.000005549459785648991, + "velocityX": 1.6158649701457874, + "velocityY": 0.16653180870833867, + "timestamp": 1.173732409455234 + }, + { + "x": 4.5899109062731025, + "y": 7.122613940245511, + "heading": 3.3899417516023003, + "angularVelocity": -0.000005549465746228024, + "velocityX": 1.6158649701446353, + "velocityY": 0.1665318087092411, + "timestamp": 1.2640195178748672 + }, + { + "x": 4.735802682023898, + "y": 7.137649615713241, + "heading": 3.389941250556547, + "angularVelocity": -0.00000554947170730299, + "velocityX": 1.6158649701434824, + "velocityY": 0.16653180871014361, + "timestamp": 1.3543066262945005 + }, + { + "x": 4.881694457774774, + "y": 7.152685291182225, + "heading": 3.389940749510255, + "angularVelocity": -0.000005549477667769927, + "velocityX": 1.61586497014233, + "velocityY": 0.1665318087110459, + "timestamp": 1.4445937347141338 + }, + { + "x": 5.027586233525358, + "y": 7.167720966650761, + "heading": 3.3899402484634247, + "angularVelocity": -0.000005549483629157811, + "velocityX": 1.6158649701411771, + "velocityY": 0.16653180871194842, + "timestamp": 1.534880843133767 + }, + { + "x": 5.173478009275799, + "y": 7.182756642119004, + "heading": 3.3899397474160566, + "angularVelocity": -0.000005549489589198961, + "velocityX": 1.6158649701400245, + "velocityY": 0.1665318087128507, + "timestamp": 1.6251679515534003 + }, + { + "x": 5.319369785026301, + "y": 7.197792317586818, + "heading": 3.38993924636815, + "angularVelocity": -0.0000055494955497599425, + "velocityX": 1.615864970138872, + "velocityY": 0.16653180871375314, + "timestamp": 1.7154550599730336 + }, + { + "x": 5.4652615607764785, + "y": 7.212827993058087, + "heading": 3.3899387453197054, + "angularVelocity": -0.000005549501510730972, + "velocityX": 1.6158649701377192, + "velocityY": 0.16653180871465564, + "timestamp": 1.8057421683926669 + }, + { + "x": 5.611153336526806, + "y": 7.227863668525338, + "heading": 3.3899382442707227, + "angularVelocity": -0.000005549507472025305, + "velocityX": 1.6158649701365668, + "velocityY": 0.16653180871555806, + "timestamp": 1.8960292768123002 + }, + { + "x": 5.757045112276785, + "y": 7.242899343993592, + "heading": 3.3899377432212017, + "angularVelocity": -0.000005549513433122579, + "velocityX": 1.6158649701354142, + "velocityY": 0.16653180871646053, + "timestamp": 1.9863163852319334 + }, + { + "x": 5.902936888026919, + "y": 7.257935019462863, + "heading": 3.3899372421711425, + "angularVelocity": -0.000005549519393768537, + "velocityX": 1.6158649701342616, + "velocityY": 0.1665318087173629, + "timestamp": 2.0766034936515667 + }, + { + "x": 6.04882866377676, + "y": 7.272970694932001, + "heading": 3.389936741120545, + "angularVelocity": -0.000005549525355144424, + "velocityX": 1.6158649701331087, + "velocityY": 0.16653180871826534, + "timestamp": 2.1668906020712 + }, + { + "x": 6.194720439526491, + "y": 7.288006370401304, + "heading": 3.3899362400694093, + "angularVelocity": -0.000005549531315809206, + "velocityX": 1.615864970131956, + "velocityY": 0.16653180871916778, + "timestamp": 2.2571777104908333 + }, + { + "x": 6.34061221527629, + "y": 7.30304204587104, + "heading": 3.3899357390177354, + "angularVelocity": -0.000005549537277178968, + "velocityX": 1.6158649701308037, + "velocityY": 0.1665318087200703, + "timestamp": 2.3474648189104665 + }, + { + "x": 6.486503991025816, + "y": 7.318077721337987, + "heading": 3.389935237965523, + "angularVelocity": -0.0000055495432383876534, + "velocityX": 1.6158649701296508, + "velocityY": 0.16653180872097262, + "timestamp": 2.4377519273301 + }, + { + "x": 6.632395766775363, + "y": 7.333113396808981, + "heading": 3.389934736912773, + "angularVelocity": -0.000005549549199340779, + "velocityX": 1.615864970128498, + "velocityY": 0.16653180872187498, + "timestamp": 2.528039035749733 + }, + { + "x": 6.778287542524652, + "y": 7.3481490722785985, + "heading": 3.389934235859484, + "angularVelocity": -0.00000554955516023689, + "velocityX": 1.6158649701273455, + "velocityY": 0.16653180872277748, + "timestamp": 2.618326144169367 + }, + { + "x": 6.924179318273876, + "y": 7.363184747747038, + "heading": 3.3899337348056577, + "angularVelocity": -0.000005549561121255593, + "velocityX": 1.6158649701261931, + "velocityY": 0.16653180872368004, + "timestamp": 2.708613252589 + }, + { + "x": 7.070071094023151, + "y": 7.378220423218671, + "heading": 3.389933233751293, + "angularVelocity": -0.0000055495670825865805, + "velocityX": 1.6158649701250403, + "velocityY": 0.1665318087245824, + "timestamp": 2.7989003610086334 + }, + { + "x": 7.215962869772269, + "y": 7.393256098685687, + "heading": 3.38993273269639, + "angularVelocity": -0.000005549573043968301, + "velocityX": 1.6158649701238874, + "velocityY": 0.16653180872548476, + "timestamp": 2.8891874694282667 + }, + { + "x": 7.361854645521179, + "y": 7.4082917741551615, + "heading": 3.389932231640949, + "angularVelocity": -0.00000554957900552128, + "velocityX": 1.6158649701227348, + "velocityY": 0.16653180872638726, + "timestamp": 2.9794745778479 + }, + { + "x": 7.507746421269845, + "y": 7.423327449627381, + "heading": 3.3899317305849697, + "angularVelocity": -0.0000055495849662374065, + "velocityX": 1.615864970121582, + "velocityY": 0.16653180872728968, + "timestamp": 3.069761686267533 + }, + { + "x": 7.653638197018768, + "y": 7.4383631250955204, + "heading": 3.389931229528452, + "angularVelocity": -0.000005549590927964199, + "velocityX": 1.615864970120429, + "velocityY": 0.16653180872819684, + "timestamp": 3.1600487946871665 + }, + { + "x": 7.79952997270157, + "y": 7.45339880119461, + "heading": 3.389930728467707, + "angularVelocity": -0.000005549637747689064, + "velocityX": 1.6158649693899627, + "velocityY": 0.16653181569922934, + "timestamp": 3.2503359031067998 + }, + { + "x": 7.908273186523536, + "y": 7.465055949439882, + "heading": 3.2483837688222263, + "angularVelocity": -1.567742750023664, + "velocityX": 1.2044157324947478, + "velocityY": 0.1291119900382222, + "timestamp": 3.340623011526433 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "angularVelocity": -1.3108321526994196, + "velocityX": 0.3122914448573039, + "velocityY": -0.13619534121251922, + "timestamp": 3.4309101199460663 + }, + { + "x": 7.897313989149588, + "y": 7.432003540468781, + "heading": 3.2231598958654137, + "angularVelocity": 0.9773255115620281, + "velocityX": -0.41091321048106677, + "velocityY": -0.21782102937809103, + "timestamp": 3.5261980978328387 + }, + { + "x": 7.764258804459074, + "y": 7.410354349811778, + "heading": 3.302887541742508, + "angularVelocity": 0.8367020441112922, + "velocityX": -1.3963480770719, + "velocityY": -0.2271975031683705, + "timestamp": 3.621486075719611 + }, + { + "x": 7.611104944022113, + "y": 7.387923020288099, + "heading": 3.3028890943761, + "angularVelocity": 0.000016294118377845236, + "velocityX": -1.607273696395563, + "velocityY": -0.23540566212575834, + "timestamp": 3.7167740536063834 + }, + { + "x": 7.457951083368431, + "y": 7.365491692124674, + "heading": 3.3028906469716444, + "angularVelocity": 0.000016293719092580313, + "velocityX": -1.6072736986022782, + "velocityY": -0.23540564768491373, + "timestamp": 3.8120620314931557 + }, + { + "x": 7.3047972227130815, + "y": 7.3430603639785605, + "heading": 3.3028921995394867, + "angularVelocity": 0.0000162934283709874, + "velocityX": -1.6072736986590501, + "velocityY": -0.23540564774414025, + "timestamp": 3.907350009379928 + }, + { + "x": 7.15164336205228, + "y": 7.320629035813856, + "heading": 3.3028937520796284, + "angularVelocity": 0.000016293137669547086, + "velocityX": -1.6072736987158178, + "velocityY": -0.23540564780336143, + "timestamp": 4.0026379872667 + }, + { + "x": 6.998489501387815, + "y": 7.298197707651154, + "heading": 3.3028953045920724, + "angularVelocity": 0.00001629284698917995, + "velocityX": -1.6072736987725829, + "velocityY": -0.23540564786256962, + "timestamp": 4.097925965153473 + }, + { + "x": 6.845335640713116, + "y": 7.275766379466654, + "heading": 3.3028968570768193, + "angularVelocity": 0.000016292556328916256, + "velocityX": -1.607273698829345, + "velocityY": -0.23540564792176438, + "timestamp": 4.193213943040245 + }, + { + "x": 6.692181780035948, + "y": 7.253335051289657, + "heading": 3.3028984095338725, + "angularVelocity": 0.0000162922656907247, + "velocityX": -1.6072736988861036, + "velocityY": -0.2354056479809454, + "timestamp": 4.288501920927017 + }, + { + "x": 6.539027919353228, + "y": 7.230903723107964, + "heading": 3.302899961963233, + "angularVelocity": 0.00001629197507403925, + "velocityX": -1.6072736989428598, + "velocityY": -0.23540564804011319, + "timestamp": 4.38378989881379 + }, + { + "x": 6.385874058664987, + "y": 7.2084723949147005, + "heading": 3.3029015143649034, + "angularVelocity": 0.000016291684475842535, + "velocityX": -1.607273698999613, + "velocityY": -0.2354056480992679, + "timestamp": 4.479077876700562 + }, + { + "x": 6.232720197971391, + "y": 7.186041066717774, + "heading": 3.3029030667388852, + "angularVelocity": 0.000016291393901831136, + "velocityX": -1.6072736990563632, + "velocityY": -0.2354056481584089, + "timestamp": 4.5743658545873345 + }, + { + "x": 6.079566337272002, + "y": 7.163609738524357, + "heading": 3.302904619085181, + "angularVelocity": 0.000016291103348699848, + "velocityX": -1.6072736991131096, + "velocityY": -0.2354056482175359, + "timestamp": 4.669653832474107 + }, + { + "x": 5.9264124765671236, + "y": 7.141178410308786, + "heading": 3.3029061714037917, + "angularVelocity": 0.000016290812813611415, + "velocityX": -1.6072736991698542, + "velocityY": -0.23540564827665061, + "timestamp": 4.764941810360879 + }, + { + "x": 5.773258615858539, + "y": 7.118747082103052, + "heading": 3.302907723694721, + "angularVelocity": 0.000016290522303354113, + "velocityX": -1.6072736992265952, + "velocityY": -0.23540564833575084, + "timestamp": 4.8602297882476515 + }, + { + "x": 5.620104755142444, + "y": 7.096315753876903, + "heading": 3.30290927595797, + "angularVelocity": 0.000016290231812342035, + "velocityX": -1.6072736992833332, + "velocityY": -0.2354056483948381, + "timestamp": 4.955517766134424 + }, + { + "x": 5.466950894422645, + "y": 7.07388442565226, + "heading": 3.30291082819354, + "angularVelocity": 0.000016289941343623743, + "velocityX": -1.6072736993400683, + "velocityY": -0.23540564845391188, + "timestamp": 5.050805744021196 + }, + { + "x": 5.313797033694609, + "y": 7.0514530974310325, + "heading": 3.3029123804014344, + "angularVelocity": 0.000016289650893731917, + "velocityX": -1.607273699396801, + "velocityY": -0.23540564851297285, + "timestamp": 5.1460937219079685 + }, + { + "x": 5.160643172963514, + "y": 7.02902176918985, + "heading": 3.3029139325816548, + "angularVelocity": 0.000016289360468842725, + "velocityX": -1.6072736994535295, + "velocityY": -0.23540564857201896, + "timestamp": 5.241381699794741 + }, + { + "x": 5.007489312227579, + "y": 7.006590440945802, + "heading": 3.3029154847342026, + "angularVelocity": 0.000016289070060669773, + "velocityX": -1.6072736995102561, + "velocityY": -0.23540564863105312, + "timestamp": 5.336669677681513 + }, + { + "x": 4.854335451483483, + "y": 6.984159112697374, + "heading": 3.30291703685908, + "angularVelocity": 0.00001628877967680068, + "velocityX": -1.6072736995669794, + "velocityY": -0.23540564869007324, + "timestamp": 5.431957655568286 + }, + { + "x": 4.701181590737123, + "y": 6.961727784450269, + "heading": 3.3029185889562895, + "angularVelocity": 0.00001628848931160111, + "velocityX": -1.6072736996236994, + "velocityY": -0.23540564874908013, + "timestamp": 5.527245633455058 + }, + { + "x": 4.548027729985049, + "y": 6.939296456188679, + "heading": 3.3029201410258326, + "angularVelocity": 0.000016288198966670516, + "velocityX": -1.6072736996804171, + "velocityY": -0.23540564880807416, + "timestamp": 5.62253361134183 + }, + { + "x": 4.394873869225821, + "y": 6.916865127932724, + "heading": 3.3029216930677117, + "angularVelocity": 0.000016287908648086418, + "velocityX": -1.607273699737131, + "velocityY": -0.2354056488670535, + "timestamp": 5.717821589228603 + }, + { + "x": 4.2417200084613995, + "y": 6.8944337996586516, + "heading": 3.302923245081929, + "angularVelocity": 0.000016287618349487135, + "velocityX": -1.607273699793842, + "velocityY": -0.23540564892601998, + "timestamp": 5.813109567115375 + }, + { + "x": 4.088566147690897, + "y": 6.872002471380619, + "heading": 3.3029247970684854, + "angularVelocity": 0.00001628732806480796, + "velocityX": -1.607273699850551, + "velocityY": -0.23540564898497424, + "timestamp": 5.908397545002147 + }, + { + "x": 3.935412286918391, + "y": 6.849571143103449, + "heading": 3.3029263490273832, + "angularVelocity": 0.00001628703780483265, + "velocityX": -1.6072736999072568, + "velocityY": -0.2354056490439145, + "timestamp": 6.00368552288892 + }, + { + "x": 3.7822584261353844, + "y": 6.827139814806336, + "heading": 3.302927900958626, + "angularVelocity": 0.000016286747572595648, + "velocityX": -1.6072736999639583, + "velocityY": -0.2354056491028402, + "timestamp": 6.098973500775692 + }, + { + "x": 3.629104565353085, + "y": 6.804708486525145, + "heading": 3.3029294528622137, + "angularVelocity": 0.000016286457354449532, + "velocityX": -1.607273700020658, + "velocityY": -0.2354056491617539, + "timestamp": 6.194261478662464 + }, + { + "x": 3.475950704559541, + "y": 6.782277158221499, + "heading": 3.3029310047381495, + "angularVelocity": 0.000016286167156294597, + "velocityX": -1.607273700077355, + "velocityY": -0.2354056492206542, + "timestamp": 6.289549456549237 + }, + { + "x": 3.3227968437635895, + "y": 6.759845829932118, + "heading": 3.3029325565864354, + "angularVelocity": 0.000016285876982629207, + "velocityX": -1.6072737001340485, + "velocityY": -0.23540564927954047, + "timestamp": 6.384837434436009 + }, + { + "x": 3.16964298296173, + "y": 6.737414501611197, + "heading": 3.3029341084070736, + "angularVelocity": 0.00001628558683240146, + "velocityX": -1.6072737001907338, + "velocityY": -0.23540564933844627, + "timestamp": 6.480125412322781 + }, + { + "x": 3.0164891229666777, + "y": 6.714983167772551, + "heading": 3.302935660207411, + "angularVelocity": 0.00001628537379077082, + "velocityX": -1.607273691741332, + "velocityY": -0.23540570734761523, + "timestamp": 6.575413390209554 + }, + { + "x": 2.8768170840789846, + "y": 6.685183755439867, + "heading": 3.351795711849992, + "angularVelocity": 0.512761974030336, + "velocityX": -1.4657886753698728, + "velocityY": -0.3127300316833431, + "timestamp": 6.670701368096327 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 2.3762078855126445, + "velocityX": -0.6771226407787638, + "velocityY": -0.18518169637911341, + "timestamp": 6.7659893459831 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": -9.273365059762218e-23, + "velocityX": 5.842590739873529e-23, + "velocityY": -3.6959137012591573e-22, + "timestamp": 6.861277323869873 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing3Contested5.traj b/src/main/deploy/choreo/FrontWing3Contested5.traj index c412d812..73648914 100644 --- a/src/main/deploy/choreo/FrontWing3Contested5.traj +++ b/src/main/deploy/choreo/FrontWing3Contested5.traj @@ -1,832 +1,833 @@ { - "samples": [ - { - "x": 1.2263859510421753, - "y": 5.594110488891602, - "heading": 3.141, - "angularVelocity": 0, - "velocityX": -1.009927502555409e-33, - "velocityY": 9.510006388327946e-34, - "timestamp": 0 - }, - { - "x": 1.2629372157532939, - "y": 5.6118681655215035, - "heading": 3.3611684682063676, - "angularVelocity": 2.615104090958444, - "velocityX": 0.4341464636351002, - "velocityY": 0.21092108774289103, - "timestamp": 0.08419109165389853 - }, - { - "x": 1.3467908645165432, - "y": 5.672396843669079, - "heading": 3.470572560445472, - "angularVelocity": 1.2994734964223353, - "velocityX": 0.9959919406671149, - "velocityY": 0.718943975645346, - "timestamp": 0.16838218330779706 - }, - { - "x": 1.4464012333437837, - "y": 5.766107211983687, - "heading": 3.4705728197139094, - "angularVelocity": 0.000003079523404990731, - "velocityX": 1.1831461841203954, - "velocityY": 1.1130675048120657, - "timestamp": 0.25257327496169557 - }, - { - "x": 1.5460115900456228, - "y": 5.859817593187631, - "heading": 3.4705730789812166, - "angularVelocity": 0.000003079509987095674, - "velocityX": 1.1831460400980154, - "velocityY": 1.1130676579082395, - "timestamp": 0.3367643666155941 - }, - { - "x": 1.645621946747464, - "y": 5.953527974391563, - "heading": 3.470573338248568, - "angularVelocity": 0.000003079510511134596, - "velocityX": 1.1831460400980387, - "velocityY": 1.1130676579081111, - "timestamp": 0.42095545826949265 - }, - { - "x": 1.7452323034493133, - "y": 6.047238355595477, - "heading": 3.470573597515964, - "angularVelocity": 0.000003079511034719833, - "velocityX": 1.183146040098137, - "velocityY": 1.1130676579079029, - "timestamp": 0.5051465499233911 - }, - { - "x": 1.844842660151171, - "y": 6.140948736799373, - "heading": 3.470573856783404, - "angularVelocity": 0.0000030795115586857372, - "velocityX": 1.1831460400982357, - "velocityY": 1.1130676579076944, - "timestamp": 0.5893376415772896 - }, - { - "x": 1.944453016853037, - "y": 6.234659118003253, - "heading": 3.470574116050888, - "angularVelocity": 0.0000030795120819840736, - "velocityX": 1.1831460400983345, - "velocityY": 1.1130676579074863, - "timestamp": 0.6735287332311881 - }, - { - "x": 2.044063373554911, - "y": 6.328369499207114, - "heading": 3.470574375318416, - "angularVelocity": 0.0000030795126059487745, - "velocityX": 1.1831460400984328, - "velocityY": 1.1130676579072778, - "timestamp": 0.7577198248850866 - }, - { - "x": 2.1436737302567934, - "y": 6.4220798804109585, - "heading": 3.470574634585988, - "angularVelocity": 0.000003079513129853114, - "velocityX": 1.183146040098531, - "velocityY": 1.1130676579070695, - "timestamp": 0.8419109165389851 - }, - { - "x": 2.243284086958684, - "y": 6.515790261614786, - "heading": 3.470574893853604, - "angularVelocity": 0.0000030795136536386766, - "velocityX": 1.1831460400986302, - "velocityY": 1.1130676579068612, - "timestamp": 0.9261020081928836 - }, - { - "x": 2.342894443660583, - "y": 6.6095006428185945, - "heading": 3.4705751531212647, - "angularVelocity": 0.0000030795141771132826, - "velocityX": 1.1831460400987281, - "velocityY": 1.1130676579066532, - "timestamp": 1.010293099846782 - }, - { - "x": 2.442504800362493, - "y": 6.703211024022384, - "heading": 3.470575412388969, - "angularVelocity": 0.0000030795147010529834, - "velocityX": 1.1831460400988572, - "velocityY": 1.1130676579064127, - "timestamp": 1.0944841915006807 - }, - { - "x": 2.5421151619283058, - "y": 6.79692140005584, - "heading": 3.470575671657143, - "angularVelocity": 0.0000030795202773893193, - "velocityX": 1.1831460978710306, - "velocityY": 1.1130675964945436, - "timestamp": 1.1786752831545793 - }, - { - "x": 2.632005363421341, - "y": 6.868564036243049, - "heading": 3.5437695268680747, - "angularVelocity": 0.8693776713553536, - "velocityX": 1.0676925518743172, - "velocityY": 0.8509526932103911, - "timestamp": 1.2628663748084779 - }, - { - "x": 2.6714627742767334, - "y": 6.900224685668945, - "heading": 3.729596046054495, - "angularVelocity": 2.207199307384385, - "velocityX": 0.46866491549483613, - "velocityY": 0.37605700085290744, - "timestamp": 1.3470574664623765 - }, - { - "x": 2.6714627742767334, - "y": 6.900224685668945, - "heading": 3.729596046054495, - "angularVelocity": 0, - "velocityX": -1.5194658088032264e-34, - "velocityY": -2.7343286363371283e-33, - "timestamp": 1.431248558116275 - }, - { - "x": 2.7208277247721226, - "y": 6.912165859423971, - "heading": 3.486294740771461, - "angularVelocity": -2.694751338131124, - "velocityX": 0.5467552516805291, - "velocityY": 0.13225779416916014, - "timestamp": 1.5215356661845725 - }, - { - "x": 2.839209601133149, - "y": 6.942185820162622, - "heading": 3.3899477762404344, - "angularVelocity": -1.0671176272269702, - "velocityX": 1.311171427392232, - "velocityY": 0.33249443227201864, - "timestamp": 1.61182277425287 - }, - { - "x": 2.985101375777118, - "y": 6.957221507964457, - "heading": 3.3899472751996993, - "angularVelocity": -0.0000055494161427999685, - "velocityX": 1.615864964171956, - "velocityY": 0.16653194596133747, - "timestamp": 1.7021098823211673 - }, - { - "x": 3.130993151670898, - "y": 6.97225718364292, - "heading": 3.3899467741601925, - "angularVelocity": -0.000005549402536979108, - "velocityX": 1.6158649780145908, - "velocityY": 0.1665318116855625, - "timestamp": 1.7923969903894648 - }, - { - "x": 3.2768849275645753, - "y": 6.98729285932146, - "heading": 3.3899462731201484, - "angularVelocity": -0.00000554940849560343, - "velocityX": 1.6158649780134444, - "velocityY": 0.1665318116864077, - "timestamp": 1.8826840984577622 - }, - { - "x": 3.4227767034581484, - "y": 7.002328535000082, - "heading": 3.3899457720795656, - "angularVelocity": -0.0000055494144557147385, - "velocityX": 1.615864978012292, - "velocityY": 0.1665318116873102, - "timestamp": 1.9729712065260596 - }, - { - "x": 3.5686684793516172, - "y": 7.017364210678785, - "heading": 3.3899452710384446, - "angularVelocity": -0.0000055494204159770735, - "velocityX": 1.6158649780111394, - "velocityY": 0.1665318116882126, - "timestamp": 2.063258314594357 - }, - { - "x": 3.7145602552449817, - "y": 7.03239988635757, - "heading": 3.3899447699967857, - "angularVelocity": -0.000005549426375721003, - "velocityX": 1.6158649780099874, - "velocityY": 0.1665318116891151, - "timestamp": 2.1535454226626545 - }, - { - "x": 3.8604520311382426, - "y": 7.047435562036436, - "heading": 3.3899442689545887, - "angularVelocity": -0.000005549432334941507, - "velocityX": 1.615864978008835, - "velocityY": 0.16653181169001743, - "timestamp": 2.243832530730952 - }, - { - "x": 4.006343807031399, - "y": 7.062471237715384, - "heading": 3.389943767911854, - "angularVelocity": -0.000005549438294605419, - "velocityX": 1.6158649780076826, - "velocityY": 0.16653181169091993, - "timestamp": 2.3341196387992493 - }, - { - "x": 4.152235582924452, - "y": 7.077506913394413, - "heading": 3.3899432668685807, - "angularVelocity": -0.000005549444254929096, - "velocityX": 1.6158649780065304, - "velocityY": 0.16653181169182238, - "timestamp": 2.4244067468675468 - }, - { - "x": 4.298127358817401, - "y": 7.092542589073523, - "heading": 3.3899427658247694, - "angularVelocity": -0.000005549450214439604, - "velocityX": 1.615864978005378, - "velocityY": 0.1665318116927247, - "timestamp": 2.514693854935844 - }, - { - "x": 4.444019134710246, - "y": 7.107578264752715, - "heading": 3.38994226478042, - "angularVelocity": -0.000005549456174760312, - "velocityX": 1.6158649780042256, - "velocityY": 0.16653181169362713, - "timestamp": 2.6049809630041416 - }, - { - "x": 4.5899109106029865, - "y": 7.122613940431989, - "heading": 3.389941763735532, - "angularVelocity": -0.000005549462134716656, - "velocityX": 1.6158649780030732, - "velocityY": 0.16653181169452963, - "timestamp": 2.695268071072439 - }, - { - "x": 4.735802686495623, - "y": 7.137649616111343, - "heading": 3.389941262690107, - "angularVelocity": -0.0000055494680950242725, - "velocityX": 1.6158649780019205, - "velocityY": 0.1665318116954319, - "timestamp": 2.7855551791407365 - }, - { - "x": 4.881694462388156, - "y": 7.15268529179078, - "heading": 3.389940761644143, - "angularVelocity": -0.000005549474055578583, - "velocityX": 1.6158649780007681, - "velocityY": 0.16653181169633444, - "timestamp": 2.875842287209034 - }, - { - "x": 5.027586238280584, - "y": 7.167720967470297, - "heading": 3.3899402605976414, - "angularVelocity": -0.000005549480015574405, - "velocityX": 1.6158649779996155, - "velocityY": 0.1665318116972368, - "timestamp": 2.9661293952773313 - }, - { - "x": 5.173478014172908, - "y": 7.182756643149896, - "heading": 3.389939759550601, - "angularVelocity": -0.000005549485976020103, - "velocityX": 1.6158649779984633, - "velocityY": 0.16653181169813924, - "timestamp": 3.0564165033456288 - }, - { - "x": 5.319369790065129, - "y": 7.197792318829577, - "heading": 3.389939258503023, - "angularVelocity": -0.000005549491936617687, - "velocityX": 1.6158649779973107, - "velocityY": 0.16653181169904166, - "timestamp": 3.146703611413926 - }, - { - "x": 5.4652615659572445, - "y": 7.212827994509339, - "heading": 3.3899387574549067, - "angularVelocity": -0.00000554949789662324, - "velocityX": 1.6158649779961582, - "velocityY": 0.1665318116999441, - "timestamp": 3.2369907194822236 - }, - { - "x": 5.611153341849257, - "y": 7.2278636701891825, - "heading": 3.389938256406252, - "angularVelocity": -0.000005549503857497513, - "velocityX": 1.6158649779950054, - "velocityY": 0.16653181170084652, - "timestamp": 3.327277827550521 - }, - { - "x": 5.757045117741165, - "y": 7.242899345869108, - "heading": 3.3899377553570593, - "angularVelocity": -0.000005549509818264508, - "velocityX": 1.615864977993853, - "velocityY": 0.16653181170174894, - "timestamp": 3.4175649356188185 - }, - { - "x": 5.902936893632969, - "y": 7.257935021549114, - "heading": 3.3899372543073283, - "angularVelocity": -0.000005549515778536107, - "velocityX": 1.6158649779927006, - "velocityY": 0.1665318117026513, - "timestamp": 3.507852043687116 - }, - { - "x": 6.048828669524668, - "y": 7.272970697229202, - "heading": 3.3899367532570595, - "angularVelocity": -0.000005549521739714091, - "velocityX": 1.6158649779915477, - "velocityY": 0.16653181170355377, - "timestamp": 3.5981391517554133 - }, - { - "x": 6.194720445416264, - "y": 7.288006372909372, - "heading": 3.3899362522062524, - "angularVelocity": -0.000005549527700309559, - "velocityX": 1.6158649779903953, - "velocityY": 0.16653181170445627, - "timestamp": 3.6884262598237108 - }, - { - "x": 6.340612221307755, - "y": 7.303042048589623, - "heading": 3.389935751154907, - "angularVelocity": -0.000005549533660762894, - "velocityX": 1.6158649779892427, - "velocityY": 0.16653181170535863, - "timestamp": 3.778713367892008 - }, - { - "x": 6.486503997199144, - "y": 7.318077724269955, - "heading": 3.3899352501030235, - "angularVelocity": -0.000005549539621393899, - "velocityX": 1.6158649779880898, - "velocityY": 0.16653181170626094, - "timestamp": 3.8690004759603056 - }, - { - "x": 6.632395773090427, - "y": 7.333113399950369, - "heading": 3.3899347490506018, - "angularVelocity": -0.00000554954558299688, - "velocityX": 1.6158649779869374, - "velocityY": 0.1665318117071635, - "timestamp": 3.959287584028603 - }, - { - "x": 6.778287548981607, - "y": 7.3481490756308645, - "heading": 3.3899342479976413, - "angularVelocity": -0.000005549551543569729, - "velocityX": 1.615864977985785, - "velocityY": 0.16653181170806597, - "timestamp": 4.0495746920969005 - }, - { - "x": 6.924179324872682, - "y": 7.363184751311442, - "heading": 3.3899337469441435, - "angularVelocity": -0.000005549557505439446, - "velocityX": 1.615864977984632, - "velocityY": 0.16653181170896839, - "timestamp": 4.139861800165198 - }, - { - "x": 7.070071100763654, - "y": 7.378220426992101, - "heading": 3.389933245890107, - "angularVelocity": -0.00000554956346606953, - "velocityX": 1.6158649779834795, - "velocityY": 0.1665318117098707, - "timestamp": 4.230148908233495 - }, - { - "x": 7.215962876654522, - "y": 7.393256102672841, - "heading": 3.3899327448355323, - "angularVelocity": -0.000005549569427174084, - "velocityX": 1.6158649779823266, - "velocityY": 0.16653181171077316, - "timestamp": 4.320436016301793 - }, - { - "x": 7.361854652545285, - "y": 7.408291778353663, - "heading": 3.3899322437804194, - "angularVelocity": -0.000005549575389475234, - "velocityX": 1.615864977981174, - "velocityY": 0.16653181171167572, - "timestamp": 4.41072312437009 - }, - { - "x": 7.5077464284359445, - "y": 7.423327454034565, - "heading": 3.389931742724768, - "angularVelocity": -0.000005549581350385169, - "velocityX": 1.6158649779800212, - "velocityY": 0.1665318117125781, - "timestamp": 4.501010232438388 - }, - { - "x": 7.6536382043265, - "y": 7.43836312971555, - "heading": 3.3899312416685787, - "angularVelocity": -0.000005549587311867374, - "velocityX": 1.6158649779788683, - "velocityY": 0.166531811713483, - "timestamp": 4.591297340506685 - }, - { - "x": 7.799529980161501, - "y": 7.453398805927109, - "heading": 3.3899307406089574, - "angularVelocity": -0.000005549625325450788, - "velocityX": 1.6158649773635676, - "velocityY": 0.16653181759000288, - "timestamp": 4.6815844485749825 - }, - { - "x": 7.908273189210625, - "y": 7.465055953097054, - "heading": 3.2483837637195303, - "angularVelocity": -1.5677429471143751, - "velocityX": 1.204415684317477, - "velocityY": 0.12911197865732152, - "timestamp": 4.77187155664328 - }, - { - "x": 7.936469078063965, - "y": 7.452759265899658, - "heading": 3.1300325241315123, - "angularVelocity": -1.3108321012839574, - "velocityX": 0.3122914163117384, - "velocityY": -0.13619538226979142, - "timestamp": 4.862158664711577 - }, - { - "x": 7.897313987574637, - "y": 7.4320035366195905, - "heading": 3.223159902402127, - "angularVelocity": 0.9773255855261127, - "velocityX": -0.4109132292719665, - "velocityY": -0.21782107097140077, - "timestamp": 4.957446642075316 - }, - { - "x": 7.764258802337476, - "y": 7.410354345348057, - "heading": 3.302887545584371, - "angularVelocity": 0.8367020204228218, - "velocityX": -1.396348090475833, - "velocityY": -0.22719751085588358, - "timestamp": 5.052734619439054 - }, - { - "x": 7.611104941895525, - "y": 7.387923016344696, - "heading": 3.302889098211225, - "angularVelocity": 0.000016294047758499195, - "velocityX": -1.607273705236954, - "velocityY": -0.23540565792192894, - "timestamp": 5.148022596802792 - }, - { - "x": 7.457951081309142, - "y": 7.365491688274562, - "heading": 3.302890650804134, - "angularVelocity": 0.000016293691520657764, - "velocityX": -1.6072737067526932, - "velocityY": -0.2354056481281831, - "timestamp": 5.2433105741665305 - }, - { - "x": 7.30479722071746, - "y": 7.343060360198914, - "heading": 3.3028922033698884, - "angularVelocity": 0.000016293406553711894, - "velocityX": -1.6072737068083087, - "velocityY": -0.23540564818605267, - "timestamp": 5.338598551530269 - }, - { - "x": 7.151643360120445, - "y": 7.320629032117711, - "heading": 3.302893755908325, - "angularVelocity": 0.00001629311986139254, - "velocityX": -1.6072737068642717, - "velocityY": -0.23540564824432697, - "timestamp": 5.433886528894007 - }, - { - "x": 6.998489499518071, - "y": 7.298197704030925, - "heading": 3.302895308419307, - "angularVelocity": 0.00001629283173826444, - "velocityX": -1.6072737069205232, - "velocityY": -0.2354056483029315, - "timestamp": 5.529174506257745 - }, - { - "x": 6.845335638910315, - "y": 7.275766375938532, - "heading": 3.3028968609027314, - "angularVelocity": 0.000016292542536041963, - "velocityX": -1.6072737069769925, - "velocityY": -0.23540564836178338, - "timestamp": 5.624462483621484 - }, - { - "x": 6.692181778297163, - "y": 7.253335047840514, - "heading": 3.302898413358527, - "angularVelocity": 0.000016292252586671315, - "velocityX": -1.6072737070336132, - "velocityY": -0.23540564842080344, - "timestamp": 5.719750460985222 - }, - { - "x": 6.539027917678609, - "y": 7.230903719736863, - "heading": 3.3028999657866516, - "angularVelocity": 0.000016291962193735624, - "velocityX": -1.607273707090324, - "velocityY": -0.23540564847991918, - "timestamp": 5.81503843834896 - }, - { - "x": 6.385874057054646, - "y": 7.208472391627575, - "heading": 3.302901518187087, - "angularVelocity": 0.000016291671606816483, - "velocityX": -1.607273707147075, - "velocityY": -0.2354056485390708, - "timestamp": 5.9103264157126985 - }, - { - "x": 6.232720196425276, - "y": 7.186041063512652, - "heading": 3.3029030705598315, - "angularVelocity": 0.00001629138100746966, - "velocityX": -1.60727370720383, - "velocityY": -0.23540564859821628, - "timestamp": 6.005614393076437 - }, - { - "x": 6.079566335790499, - "y": 7.1636097353920976, - "heading": 3.3029046229048977, - "angularVelocity": 0.000016291090533089062, - "velocityX": -1.6072737072605614, - "velocityY": -0.23540564865732222, - "timestamp": 6.100902370440175 - }, - { - "x": 5.926412475150321, - "y": 7.141178407265915, - "heading": 3.302906175222303, - "angularVelocity": 0.000016290800243766995, - "velocityX": -1.6072737073172572, - "velocityY": -0.23540564871637523, - "timestamp": 6.196190347803913 - }, - { - "x": 5.773258614504743, - "y": 7.118747079134112, - "heading": 3.3029077275120664, - "angularVelocity": 0.00001629051016274816, - "velocityX": -1.6072737073739125, - "velocityY": -0.23540564877536937, - "timestamp": 6.291478325167652 - }, - { - "x": 5.62010475385377, - "y": 7.096315750996691, - "heading": 3.302909279774206, - "angularVelocity": 0.000016290220263808567, - "velocityX": -1.6072737074305328, - "velocityY": -0.23540564883431206, - "timestamp": 6.38676630253139 - }, - { - "x": 5.466950893197405, - "y": 7.073884422853658, - "heading": 3.302910832008734, - "angularVelocity": 0.00001628993049423592, - "velocityX": -1.6072737074871284, - "velocityY": -0.23540564889321514, - "timestamp": 6.482054279895128 - }, - { - "x": 5.313797032535647, - "y": 7.051453094705014, - "heading": 3.3029123842156554, - "angularVelocity": 0.000016289640769266148, - "velocityX": -1.6072737075437156, - "velocityY": -0.23540564895209914, - "timestamp": 6.5773422572588665 - }, - { - "x": 5.160643171868496, - "y": 7.02902176655076, - "heading": 3.3029139363949653, - "angularVelocity": 0.000016289351008016845, - "velocityX": -1.6072737076003119, - "velocityY": -0.23540564901098462, - "timestamp": 6.672630234622605 - }, - { - "x": 5.0074893111959495, - "y": 7.006590438390892, - "heading": 3.302915488546652, - "angularVelocity": 0.000016289061112899286, - "velocityX": -1.6072737076569361, - "velocityY": -0.23540564906989445, - "timestamp": 6.767918211986343 - }, - { - "x": 4.854335450518003, - "y": 6.984159110225406, - "heading": 3.3029170406706947, - "angularVelocity": 0.000016288771004663956, - "velocityX": -1.607273707713604, - "velocityY": -0.23540564912884793, - "timestamp": 6.863206189350081 - }, - { - "x": 4.701181589834651, - "y": 6.961727782054296, - "heading": 3.3029185927670666, - "angularVelocity": 0.00001628848060846621, - "velocityX": -1.6072737077703305, - "velocityY": -0.235405649187863, - "timestamp": 6.95849416671382 - }, - { - "x": 4.548027729145889, - "y": 6.939296453877556, - "heading": 3.3029201448357353, - "angularVelocity": 0.000016288189883493963, - "velocityX": -1.607273707827124, - "velocityY": -0.23540564924694996, - "timestamp": 7.053782144077558 - }, - { - "x": 4.3948738684517075, - "y": 6.916865125695178, - "heading": 3.3029216968766675, - "angularVelocity": 0.00001628789879930085, - "velocityX": -1.6072737078839905, - "velocityY": -0.23540564930611646, - "timestamp": 7.149070121441296 - }, - { - "x": 4.2417200077521, - "y": 6.894433797507156, - "heading": 3.302923248889828, - "angularVelocity": 0.00001628760735122893, - "velocityX": -1.6072737079409307, - "velocityY": -0.23540564936536426, - "timestamp": 7.2443580988050345 - }, - { - "x": 4.088566147047059, - "y": 6.872002469313479, - "heading": 3.3029248008751826, - "angularVelocity": 0.000016287315540072175, - "velocityX": -1.6072737079979444, - "velocityY": -0.23540564942469375, - "timestamp": 7.339646076168773 - }, - { - "x": 3.9354122863365792, - "y": 6.849571141114141, - "heading": 3.3029263528326944, - "angularVelocity": 0.000016287023346622306, - "velocityX": -1.6072737080550357, - "velocityY": -0.23540564948410944, - "timestamp": 7.434934053532511 - }, - { - "x": 3.7822584256206495, - "y": 6.827139812909132, - "heading": 3.3029279047623197, - "angularVelocity": 0.00001628673068839883, - "velocityX": -1.6072737081122201, - "velocityY": -0.23540564954363288, - "timestamp": 7.530222030896249 - }, - { - "x": 3.629104564899259, - "y": 6.804708484698436, - "heading": 3.3029294566639957, - "angularVelocity": 0.000016286437378238427, - "velocityX": -1.607273708169536, - "velocityY": -0.2354056496033085, - "timestamp": 7.625510008259988 - }, - { - "x": 3.4759507041723876, - "y": 6.7822771564820306, - "heading": 3.302931008537628, - "angularVelocity": 0.00001628614306837342, - "velocityX": -1.6072737082270525, - "velocityY": -0.2354056496632214, - "timestamp": 7.720797985623726 - }, - { - "x": 3.3227968434400057, - "y": 6.759845828259881, - "heading": 3.3029325603830677, - "angularVelocity": 0.000016285847206848686, - "velocityX": -1.6072737082848803, - "velocityY": -0.23540564972350414, - "timestamp": 7.816085962987464 - }, - { - "x": 3.1696429827020713, - "y": 6.737414500031936, - "heading": 3.3029341122001052, - "angularVelocity": 0.00001628554913981109, - "velocityX": -1.6072737083431483, - "velocityY": -0.2354056497843278, - "timestamp": 7.9113739403512024 - }, - { - "x": 3.016489122487879, - "y": 6.714983168191867, - "heading": 3.3029356639933174, - "angularVelocity": 0.000016285299102484297, - "velocityX": -1.6072737028467396, - "velocityY": -0.23540568769178183, - "timestamp": 8.006661917714942 - }, - { - "x": 2.8768170839321456, - "y": 6.685183755290752, - "heading": 3.3517957161483998, - "angularVelocity": 0.512761982223329, - "velocityX": -1.465788679956645, - "velocityY": -0.31273003925103915, - "timestamp": 8.10194989507868 - }, - { - "x": 2.812295436859131, - "y": 6.667538166046143, - "heading": 3.5782197562990956, - "angularVelocity": 2.3762078534459548, - "velocityX": -0.6771226429407677, - "velocityY": -0.18518169587388544, - "timestamp": 8.19723787244242 - }, - { - "x": 2.812295436859131, - "y": 6.667538166046143, - "heading": 3.5782197562990956, - "angularVelocity": 4.7042853211430965e-33, - "velocityX": -2.4138813228334137e-34, - "velocityY": 0, - "timestamp": 8.29252584980616 - } - ] + "samples": [ + { + "x": 1.2263859510421753, + "y": 5.594110488891602, + "heading": 3.141, + "angularVelocity": -9.716627282364513e-28, + "velocityX": -2.1006296109192272e-27, + "velocityY": 1.6816897032610772e-27, + "timestamp": 0 + }, + { + "x": 1.2629372172985613, + "y": 5.611868166522511, + "heading": 3.3611684637424437, + "angularVelocity": 2.6151040269567423, + "velocityX": 0.4341464801664794, + "velocityY": 0.21092109874697887, + "timestamp": 0.08419109200740245 + }, + { + "x": 1.3467908643065862, + "y": 5.672396849103953, + "heading": 3.4705725495976436, + "angularVelocity": 1.2994734151397, + "velocityX": 0.9959919156370148, + "velocityY": 0.7189440252909496, + "timestamp": 0.1683821840148049 + }, + { + "x": 1.4464012359618026, + "y": 5.766107214252463, + "heading": 3.4705728088664487, + "angularVelocity": 0.0000030795277620559296, + "velocityX": 1.1831462127425316, + "velocityY": 1.1130674625323809, + "timestamp": 0.25257327602220736 + }, + { + "x": 1.5460115925515412, + "y": 5.859817595415609, + "heading": 3.4705730681338594, + "angularVelocity": 0.0000030795111997424536, + "velocityX": 1.1831460337986872, + "velocityY": 1.1130676527500878, + "timestamp": 0.3367643680296098 + }, + { + "x": 1.6456219491412756, + "y": 5.9535279765787505, + "heading": 3.4705733274013144, + "angularVelocity": 0.000003079511724050569, + "velocityX": 1.183146033798638, + "velocityY": 1.1130676527500365, + "timestamp": 0.42095546003701223 + }, + { + "x": 1.7452323057310184, + "y": 6.0472383577418745, + "heading": 3.4705735866688134, + "angularVelocity": 0.000003079512247611481, + "velocityX": 1.1831460337987365, + "velocityY": 1.113067652749828, + "timestamp": 0.5051465520444147 + }, + { + "x": 1.8448426623207694, + "y": 6.1409487389049815, + "heading": 3.4705738459363564, + "angularVelocity": 0.0000030795127711126187, + "velocityX": 1.1831460337988353, + "velocityY": 1.1130676527496197, + "timestamp": 0.5893376440518172 + }, + { + "x": 1.9444530189105287, + "y": 6.234659120068071, + "heading": 3.470574105203944, + "angularVelocity": 0.000003079513294842049, + "velocityX": 1.183146033798934, + "velocityY": 1.1130676527494117, + "timestamp": 0.6735287360592197 + }, + { + "x": 2.0440633755002966, + "y": 6.328369501231142, + "heading": 3.470574364471575, + "angularVelocity": 0.0000030795138187518754, + "velocityX": 1.1831460337990323, + "velocityY": 1.1130676527492032, + "timestamp": 0.7577198280666222 + }, + { + "x": 2.1436737320900723, + "y": 6.422079882394196, + "heading": 3.4705746237392505, + "angularVelocity": 0.0000030795143424110884, + "velocityX": 1.1831460337991306, + "velocityY": 1.113067652748995, + "timestamp": 0.8419109200740247 + }, + { + "x": 2.2432840886798564, + "y": 6.515790263557233, + "heading": 3.47057488300697, + "angularVelocity": 0.0000030795148666763967, + "velocityX": 1.1831460337992294, + "velocityY": 1.1130676527487866, + "timestamp": 0.9261020120814272 + }, + { + "x": 2.342894445269649, + "y": 6.609500644720252, + "heading": 3.4705751422747335, + "angularVelocity": 0.0000030795153898968674, + "velocityX": 1.1831460337993278, + "velocityY": 1.1130676527485786, + "timestamp": 1.0102931040888297 + }, + { + "x": 2.4425048018594557, + "y": 6.703211025883247, + "heading": 3.470575401542541, + "angularVelocity": 0.000003079515913944498, + "velocityX": 1.1831460337994937, + "velocityY": 1.1130676527482986, + "timestamp": 1.0944841960962322 + }, + { + "x": 2.5421151652954292, + "y": 6.796921399768766, + "heading": 3.4705756608109883, + "angularVelocity": 0.0000030795235106850793, + "velocityX": 1.183146115116498, + "velocityY": 1.1130675663083123, + "timestamp": 1.1786752881036346 + }, + { + "x": 2.632005364209705, + "y": 6.868564037074326, + "heading": 3.5437695190039196, + "angularVelocity": 0.8693777031244078, + "velocityX": 1.0676925167614182, + "velocityY": 0.8509527029208858, + "timestamp": 1.2628663801110371 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": 2.207199391525135, + "velocityX": 0.46866490416300804, + "velocityY": 0.3760569894002088, + "timestamp": 1.3470574721184396 + }, + { + "x": 2.6714627742767334, + "y": 6.900224685668945, + "heading": 3.729596046054495, + "angularVelocity": -1.0211590332128658e-23, + "velocityX": -1.1480030002254912e-23, + "velocityY": -4.344519998520169e-23, + "timestamp": 1.4312485641258421 + }, + { + "x": 2.720827723501713, + "y": 6.91216585910853, + "heading": 3.486294732820543, + "angularVelocity": -2.69475141570759, + "velocityX": 0.5467552354822697, + "velocityY": 0.13225779017707837, + "timestamp": 1.5215356725454756 + }, + { + "x": 2.8392095989605575, + "y": 6.942185818091124, + "heading": 3.389947764111763, + "angularVelocity": -1.0671176693463396, + "velocityX": 1.311171412297561, + "velocityY": 0.33249441150564496, + "timestamp": 1.6118227809651091 + }, + { + "x": 2.985101373007063, + "y": 6.9572215101019514, + "heading": 3.3899472630700718, + "angularVelocity": -0.000005549426712336868, + "velocityX": 1.615864951266793, + "velocityY": 0.16653199192676998, + "timestamp": 1.7021098893847426 + }, + { + "x": 3.1309931487591207, + "y": 6.972257185567622, + "heading": 3.3899467620302377, + "angularVelocity": -0.000005549406141075235, + "velocityX": 1.6158649701561478, + "velocityY": 0.16653180870033996, + "timestamp": 1.7923969978043761 + }, + { + "x": 3.276884924511046, + "y": 6.987292861034595, + "heading": 3.389946260989866, + "angularVelocity": -0.000005549412101088006, + "velocityX": 1.615864970155008, + "velocityY": 0.16653180870111933, + "timestamp": 1.8826841062240096 + }, + { + "x": 3.4227767002625384, + "y": 7.002328536502587, + "heading": 3.3899457599489553, + "angularVelocity": -0.000005549418061933105, + "velocityX": 1.6158649701538554, + "velocityY": 0.16653180870202178, + "timestamp": 1.9729712146436431 + }, + { + "x": 3.5686684760143703, + "y": 7.017364211969376, + "heading": 3.389945258907507, + "angularVelocity": -0.000005549424021960657, + "velocityX": 1.6158649701527033, + "velocityY": 0.16653180870292417, + "timestamp": 2.0632583230632764 + }, + { + "x": 3.7145602517660743, + "y": 7.032399887436731, + "heading": 3.3899447578655204, + "angularVelocity": -0.000005549429982841055, + "velocityX": 1.6158649701515504, + "velocityY": 0.16653180870382667, + "timestamp": 2.1535454314829097 + }, + { + "x": 3.8604520275173986, + "y": 7.04743556290717, + "heading": 3.3899442568229956, + "angularVelocity": -0.000005549435944010941, + "velocityX": 1.615864970150398, + "velocityY": 0.1665318087047291, + "timestamp": 2.243832539902543 + }, + { + "x": 4.00634380326868, + "y": 7.062471238372739, + "heading": 3.389943755779933, + "angularVelocity": -0.000005549441903913974, + "velocityX": 1.6158649701492453, + "velocityY": 0.1665318087056315, + "timestamp": 2.334119648322176 + }, + { + "x": 4.1522355790200765, + "y": 7.077506913841867, + "heading": 3.389943254736332, + "angularVelocity": -0.0000055494478640805644, + "velocityX": 1.615864970148093, + "velocityY": 0.16653180870653383, + "timestamp": 2.4244067567418095 + }, + { + "x": 4.298127354771182, + "y": 7.0925425893096525, + "heading": 3.389942753692193, + "angularVelocity": -0.000005549453823846852, + "velocityX": 1.6158649701469403, + "velocityY": 0.16653180870743617, + "timestamp": 2.5146938651614428 + }, + { + "x": 4.444019130522188, + "y": 7.10757826477934, + "heading": 3.389942252647516, + "angularVelocity": -0.000005549459785648991, + "velocityX": 1.6158649701457874, + "velocityY": 0.16653180870833867, + "timestamp": 2.604980973581076 + }, + { + "x": 4.5899109062731025, + "y": 7.122613940245511, + "heading": 3.3899417516023003, + "angularVelocity": -0.000005549465746228024, + "velocityX": 1.6158649701446353, + "velocityY": 0.1665318087092411, + "timestamp": 2.6952680820007093 + }, + { + "x": 4.735802682023898, + "y": 7.137649615713241, + "heading": 3.389941250556547, + "angularVelocity": -0.00000554947170730299, + "velocityX": 1.6158649701434824, + "velocityY": 0.16653180871014361, + "timestamp": 2.7855551904203426 + }, + { + "x": 4.881694457774774, + "y": 7.152685291182225, + "heading": 3.389940749510255, + "angularVelocity": -0.000005549477667769927, + "velocityX": 1.61586497014233, + "velocityY": 0.1665318087110459, + "timestamp": 2.875842298839976 + }, + { + "x": 5.027586233525358, + "y": 7.167720966650761, + "heading": 3.3899402484634247, + "angularVelocity": -0.000005549483629157811, + "velocityX": 1.6158649701411771, + "velocityY": 0.16653180871194842, + "timestamp": 2.966129407259609 + }, + { + "x": 5.173478009275799, + "y": 7.182756642119004, + "heading": 3.3899397474160566, + "angularVelocity": -0.000005549489589198961, + "velocityX": 1.6158649701400245, + "velocityY": 0.1665318087128507, + "timestamp": 3.0564165156792424 + }, + { + "x": 5.319369785026301, + "y": 7.197792317586818, + "heading": 3.38993924636815, + "angularVelocity": -0.0000055494955497599425, + "velocityX": 1.615864970138872, + "velocityY": 0.16653180871375314, + "timestamp": 3.1467036240988757 + }, + { + "x": 5.4652615607764785, + "y": 7.212827993058087, + "heading": 3.3899387453197054, + "angularVelocity": -0.000005549501510730972, + "velocityX": 1.6158649701377192, + "velocityY": 0.16653180871465564, + "timestamp": 3.236990732518509 + }, + { + "x": 5.611153336526806, + "y": 7.227863668525338, + "heading": 3.3899382442707227, + "angularVelocity": -0.000005549507472025305, + "velocityX": 1.6158649701365668, + "velocityY": 0.16653180871555806, + "timestamp": 3.3272778409381423 + }, + { + "x": 5.757045112276785, + "y": 7.242899343993592, + "heading": 3.3899377432212017, + "angularVelocity": -0.000005549513433122579, + "velocityX": 1.6158649701354142, + "velocityY": 0.16653180871646053, + "timestamp": 3.4175649493577755 + }, + { + "x": 5.902936888026919, + "y": 7.257935019462863, + "heading": 3.3899372421711425, + "angularVelocity": -0.000005549519393768537, + "velocityX": 1.6158649701342616, + "velocityY": 0.1665318087173629, + "timestamp": 3.507852057777409 + }, + { + "x": 6.04882866377676, + "y": 7.272970694932001, + "heading": 3.389936741120545, + "angularVelocity": -0.000005549525355144424, + "velocityX": 1.6158649701331087, + "velocityY": 0.16653180871826534, + "timestamp": 3.598139166197042 + }, + { + "x": 6.194720439526491, + "y": 7.288006370401304, + "heading": 3.3899362400694093, + "angularVelocity": -0.000005549531315809206, + "velocityX": 1.615864970131956, + "velocityY": 0.16653180871916778, + "timestamp": 3.6884262746166754 + }, + { + "x": 6.34061221527629, + "y": 7.30304204587104, + "heading": 3.3899357390177354, + "angularVelocity": -0.000005549537277178968, + "velocityX": 1.6158649701308037, + "velocityY": 0.1665318087200703, + "timestamp": 3.7787133830363087 + }, + { + "x": 6.486503991025816, + "y": 7.318077721337987, + "heading": 3.389935237965523, + "angularVelocity": -0.0000055495432383876534, + "velocityX": 1.6158649701296508, + "velocityY": 0.16653180872097262, + "timestamp": 3.869000491455942 + }, + { + "x": 6.632395766775363, + "y": 7.333113396808981, + "heading": 3.389934736912773, + "angularVelocity": -0.000005549549199340779, + "velocityX": 1.615864970128498, + "velocityY": 0.16653180872187498, + "timestamp": 3.959287599875575 + }, + { + "x": 6.778287542524652, + "y": 7.3481490722785985, + "heading": 3.389934235859484, + "angularVelocity": -0.00000554955516023689, + "velocityX": 1.6158649701273455, + "velocityY": 0.16653180872277748, + "timestamp": 4.049574708295209 + }, + { + "x": 6.924179318273876, + "y": 7.363184747747038, + "heading": 3.3899337348056577, + "angularVelocity": -0.000005549561121255593, + "velocityX": 1.6158649701261931, + "velocityY": 0.16653180872368004, + "timestamp": 4.139861816714842 + }, + { + "x": 7.070071094023151, + "y": 7.378220423218671, + "heading": 3.389933233751293, + "angularVelocity": -0.0000055495670825865805, + "velocityX": 1.6158649701250403, + "velocityY": 0.1665318087245824, + "timestamp": 4.2301489251344755 + }, + { + "x": 7.215962869772269, + "y": 7.393256098685687, + "heading": 3.38993273269639, + "angularVelocity": -0.000005549573043968301, + "velocityX": 1.6158649701238874, + "velocityY": 0.16653180872548476, + "timestamp": 4.320436033554109 + }, + { + "x": 7.361854645521179, + "y": 7.4082917741551615, + "heading": 3.389932231640949, + "angularVelocity": -0.00000554957900552128, + "velocityX": 1.6158649701227348, + "velocityY": 0.16653180872638726, + "timestamp": 4.410723141973742 + }, + { + "x": 7.507746421269845, + "y": 7.423327449627381, + "heading": 3.3899317305849697, + "angularVelocity": -0.0000055495849662374065, + "velocityX": 1.615864970121582, + "velocityY": 0.16653180872728968, + "timestamp": 4.501010250393375 + }, + { + "x": 7.653638197018768, + "y": 7.4383631250955204, + "heading": 3.389931229528452, + "angularVelocity": -0.000005549590927964199, + "velocityX": 1.615864970120429, + "velocityY": 0.16653180872819684, + "timestamp": 4.591297358813009 + }, + { + "x": 7.79952997270157, + "y": 7.45339880119461, + "heading": 3.389930728467707, + "angularVelocity": -0.000005549637747689064, + "velocityX": 1.6158649693899627, + "velocityY": 0.16653181569922934, + "timestamp": 4.681584467232642 + }, + { + "x": 7.908273186523536, + "y": 7.465055949439882, + "heading": 3.2483837688222263, + "angularVelocity": -1.567742750023664, + "velocityX": 1.2044157324947478, + "velocityY": 0.1291119900382222, + "timestamp": 4.771871575652275 + }, + { + "x": 7.936469078063965, + "y": 7.452759265899658, + "heading": 3.1300325241315123, + "angularVelocity": -1.3108321526994196, + "velocityX": 0.3122914448573039, + "velocityY": -0.13619534121251922, + "timestamp": 4.862158684071908 + }, + { + "x": 7.897313989149588, + "y": 7.432003540468781, + "heading": 3.2231598958654137, + "angularVelocity": 0.9773255115620281, + "velocityX": -0.41091321048106677, + "velocityY": -0.21782102937809103, + "timestamp": 4.957446661958681 + }, + { + "x": 7.764258804459074, + "y": 7.410354349811778, + "heading": 3.302887541742508, + "angularVelocity": 0.8367020441112922, + "velocityX": -1.3963480770719, + "velocityY": -0.2271975031683705, + "timestamp": 5.052734639845453 + }, + { + "x": 7.611104944022113, + "y": 7.387923020288099, + "heading": 3.3028890943761, + "angularVelocity": 0.000016294118377845236, + "velocityX": -1.607273696395563, + "velocityY": -0.23540566212575834, + "timestamp": 5.1480226177322255 + }, + { + "x": 7.457951083368431, + "y": 7.365491692124674, + "heading": 3.3028906469716444, + "angularVelocity": 0.000016293719092580313, + "velocityX": -1.6072736986022782, + "velocityY": -0.23540564768491373, + "timestamp": 5.243310595618998 + }, + { + "x": 7.3047972227130815, + "y": 7.3430603639785605, + "heading": 3.3028921995394867, + "angularVelocity": 0.0000162934283709874, + "velocityX": -1.6072736986590501, + "velocityY": -0.23540564774414025, + "timestamp": 5.33859857350577 + }, + { + "x": 7.15164336205228, + "y": 7.320629035813856, + "heading": 3.3028937520796284, + "angularVelocity": 0.000016293137669547086, + "velocityX": -1.6072736987158178, + "velocityY": -0.23540564780336143, + "timestamp": 5.4338865513925425 + }, + { + "x": 6.998489501387815, + "y": 7.298197707651154, + "heading": 3.3028953045920724, + "angularVelocity": 0.00001629284698917995, + "velocityX": -1.6072736987725829, + "velocityY": -0.23540564786256962, + "timestamp": 5.529174529279315 + }, + { + "x": 6.845335640713116, + "y": 7.275766379466654, + "heading": 3.3028968570768193, + "angularVelocity": 0.000016292556328916256, + "velocityX": -1.607273698829345, + "velocityY": -0.23540564792176438, + "timestamp": 5.624462507166087 + }, + { + "x": 6.692181780035948, + "y": 7.253335051289657, + "heading": 3.3028984095338725, + "angularVelocity": 0.0000162922656907247, + "velocityX": -1.6072736988861036, + "velocityY": -0.2354056479809454, + "timestamp": 5.7197504850528595 + }, + { + "x": 6.539027919353228, + "y": 7.230903723107964, + "heading": 3.302899961963233, + "angularVelocity": 0.00001629197507403925, + "velocityX": -1.6072736989428598, + "velocityY": -0.23540564804011319, + "timestamp": 5.815038462939632 + }, + { + "x": 6.385874058664987, + "y": 7.2084723949147005, + "heading": 3.3029015143649034, + "angularVelocity": 0.000016291684475842535, + "velocityX": -1.607273698999613, + "velocityY": -0.2354056480992679, + "timestamp": 5.910326440826404 + }, + { + "x": 6.232720197971391, + "y": 7.186041066717774, + "heading": 3.3029030667388852, + "angularVelocity": 0.000016291393901831136, + "velocityX": -1.6072736990563632, + "velocityY": -0.2354056481584089, + "timestamp": 6.005614418713177 + }, + { + "x": 6.079566337272002, + "y": 7.163609738524357, + "heading": 3.302904619085181, + "angularVelocity": 0.000016291103348699848, + "velocityX": -1.6072736991131096, + "velocityY": -0.2354056482175359, + "timestamp": 6.100902396599949 + }, + { + "x": 5.9264124765671236, + "y": 7.141178410308786, + "heading": 3.3029061714037917, + "angularVelocity": 0.000016290812813611415, + "velocityX": -1.6072736991698542, + "velocityY": -0.23540564827665061, + "timestamp": 6.196190374486721 + }, + { + "x": 5.773258615858539, + "y": 7.118747082103052, + "heading": 3.302907723694721, + "angularVelocity": 0.000016290522303354113, + "velocityX": -1.6072736992265952, + "velocityY": -0.23540564833575084, + "timestamp": 6.291478352373494 + }, + { + "x": 5.620104755142444, + "y": 7.096315753876903, + "heading": 3.30290927595797, + "angularVelocity": 0.000016290231812342035, + "velocityX": -1.6072736992833332, + "velocityY": -0.2354056483948381, + "timestamp": 6.386766330260266 + }, + { + "x": 5.466950894422645, + "y": 7.07388442565226, + "heading": 3.30291082819354, + "angularVelocity": 0.000016289941343623743, + "velocityX": -1.6072736993400683, + "velocityY": -0.23540564845391188, + "timestamp": 6.482054308147038 + }, + { + "x": 5.313797033694609, + "y": 7.0514530974310325, + "heading": 3.3029123804014344, + "angularVelocity": 0.000016289650893731917, + "velocityX": -1.607273699396801, + "velocityY": -0.23540564851297285, + "timestamp": 6.577342286033811 + }, + { + "x": 5.160643172963514, + "y": 7.02902176918985, + "heading": 3.3029139325816548, + "angularVelocity": 0.000016289360468842725, + "velocityX": -1.6072736994535295, + "velocityY": -0.23540564857201896, + "timestamp": 6.672630263920583 + }, + { + "x": 5.007489312227579, + "y": 7.006590440945802, + "heading": 3.3029154847342026, + "angularVelocity": 0.000016289070060669773, + "velocityX": -1.6072736995102561, + "velocityY": -0.23540564863105312, + "timestamp": 6.767918241807355 + }, + { + "x": 4.854335451483483, + "y": 6.984159112697374, + "heading": 3.30291703685908, + "angularVelocity": 0.00001628877967680068, + "velocityX": -1.6072736995669794, + "velocityY": -0.23540564869007324, + "timestamp": 6.863206219694128 + }, + { + "x": 4.701181590737123, + "y": 6.961727784450269, + "heading": 3.3029185889562895, + "angularVelocity": 0.00001628848931160111, + "velocityX": -1.6072736996236994, + "velocityY": -0.23540564874908013, + "timestamp": 6.9584941975809 + }, + { + "x": 4.548027729985049, + "y": 6.939296456188679, + "heading": 3.3029201410258326, + "angularVelocity": 0.000016288198966670516, + "velocityX": -1.6072736996804171, + "velocityY": -0.23540564880807416, + "timestamp": 7.053782175467672 + }, + { + "x": 4.394873869225821, + "y": 6.916865127932724, + "heading": 3.3029216930677117, + "angularVelocity": 0.000016287908648086418, + "velocityX": -1.607273699737131, + "velocityY": -0.2354056488670535, + "timestamp": 7.149070153354445 + }, + { + "x": 4.2417200084613995, + "y": 6.8944337996586516, + "heading": 3.302923245081929, + "angularVelocity": 0.000016287618349487135, + "velocityX": -1.607273699793842, + "velocityY": -0.23540564892601998, + "timestamp": 7.244358131241217 + }, + { + "x": 4.088566147690897, + "y": 6.872002471380619, + "heading": 3.3029247970684854, + "angularVelocity": 0.00001628732806480796, + "velocityX": -1.607273699850551, + "velocityY": -0.23540564898497424, + "timestamp": 7.339646109127989 + }, + { + "x": 3.935412286918391, + "y": 6.849571143103449, + "heading": 3.3029263490273832, + "angularVelocity": 0.00001628703780483265, + "velocityX": -1.6072736999072568, + "velocityY": -0.2354056490439145, + "timestamp": 7.434934087014762 + }, + { + "x": 3.7822584261353844, + "y": 6.827139814806336, + "heading": 3.302927900958626, + "angularVelocity": 0.000016286747572595648, + "velocityX": -1.6072736999639583, + "velocityY": -0.2354056491028402, + "timestamp": 7.530222064901534 + }, + { + "x": 3.629104565353085, + "y": 6.804708486525145, + "heading": 3.3029294528622137, + "angularVelocity": 0.000016286457354449532, + "velocityX": -1.607273700020658, + "velocityY": -0.2354056491617539, + "timestamp": 7.6255100427883065 + }, + { + "x": 3.475950704559541, + "y": 6.782277158221499, + "heading": 3.3029310047381495, + "angularVelocity": 0.000016286167156294597, + "velocityX": -1.607273700077355, + "velocityY": -0.2354056492206542, + "timestamp": 7.720798020675079 + }, + { + "x": 3.3227968437635895, + "y": 6.759845829932118, + "heading": 3.3029325565864354, + "angularVelocity": 0.000016285876982629207, + "velocityX": -1.6072737001340485, + "velocityY": -0.23540564927954047, + "timestamp": 7.816085998561851 + }, + { + "x": 3.16964298296173, + "y": 6.737414501611197, + "heading": 3.3029341084070736, + "angularVelocity": 0.00001628558683240146, + "velocityX": -1.6072737001907338, + "velocityY": -0.23540564933844627, + "timestamp": 7.9113739764486235 + }, + { + "x": 3.0164891229666777, + "y": 6.714983167772551, + "heading": 3.302935660207411, + "angularVelocity": 0.00001628537379077082, + "velocityX": -1.607273691741332, + "velocityY": -0.23540570734761523, + "timestamp": 8.006661954335396 + }, + { + "x": 2.8768170840789846, + "y": 6.685183755439867, + "heading": 3.351795711849992, + "angularVelocity": 0.512761974030336, + "velocityX": -1.4657886753698728, + "velocityY": -0.3127300316833431, + "timestamp": 8.101949932222169 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": 2.3762078855126445, + "velocityX": -0.6771226407787638, + "velocityY": -0.18518169637911341, + "timestamp": 8.197237910108942 + }, + { + "x": 2.812295436859131, + "y": 6.667538166046143, + "heading": 3.5782197562990956, + "angularVelocity": -9.273365059762218e-23, + "velocityX": 5.842590739873529e-23, + "velocityY": -3.6959137012591573e-22, + "timestamp": 8.292525887995716 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceContested1.1.traj b/src/main/deploy/choreo/SourceContested1.1.traj new file mode 100644 index 00000000..46b0f61d --- /dev/null +++ b/src/main/deploy/choreo/SourceContested1.1.traj @@ -0,0 +1,527 @@ +{ + "samples": [ + { + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "angularVelocity": -1.8256752618245288e-22, + "velocityX": -7.65916010942737e-22, + "velocityY": 5.834863940365417e-22, + "timestamp": 0 + }, + { + "x": 0.8644346902480902, + "y": 4.538605095268569, + "heading": 2.3610664104627634, + "angularVelocity": 2.6730073050757213, + "velocityX": 0.4641971643197831, + "velocityY": -0.4033128817553312, + "timestamp": 0.09347181429718082 + }, + { + "x": 0.9651901704673621, + "y": 4.45361330197089, + "heading": 2.440857637900237, + "angularVelocity": 0.8536394424076145, + "velocityX": 1.0779236604837226, + "velocityY": -0.9092772397405119, + "timestamp": 0.18694362859436164 + }, + { + "x": 1.0866380874820318, + "y": 4.362483747012655, + "heading": 2.440865267791502, + "angularVelocity": 0.00008162772192147527, + "velocityX": 1.299299879090211, + "velocityY": -0.9749415440733835, + "timestamp": 0.28041544289154247 + }, + { + "x": 1.2080860091215304, + "y": 4.271354198213206, + "heading": 2.4408728977229917, + "angularVelocity": 0.00008162815226084984, + "velocityX": 1.2992999285685376, + "velocityY": -0.9749414781841598, + "timestamp": 0.3738872571887233 + }, + { + "x": 1.3295339322232647, + "y": 4.1802246514640125, + "heading": 2.440880527950737, + "angularVelocity": 0.0000816313217297415, + "velocityX": 1.299299944212136, + "velocityY": -0.9749414562496848, + "timestamp": 0.4673590714859041 + }, + { + "x": 1.4509818569281794, + "y": 4.089095106952003, + "heading": 2.440888158472548, + "angularVelocity": 0.00008163446776368671, + "velocityX": 1.2992999613636238, + "velocityY": -0.9749414323153683, + "timestamp": 0.5608308857830849 + }, + { + "x": 1.5724297833961638, + "y": 3.997965564889279, + "heading": 2.440895789286055, + "angularVelocity": 0.00008163758844700525, + "velocityX": 1.2992999802256675, + "velocityY": -0.9749414061119055, + "timestamp": 0.6543027000802657 + }, + { + "x": 1.6938777118095218, + "y": 3.906836025517733, + "heading": 2.4409034203886897, + "angularVelocity": 0.00008164068165665073, + "velocityX": 1.299300001038079, + "velocityY": -0.9749413773205733, + "timestamp": 0.7477745143774466 + }, + { + "x": 1.8153256423773063, + "y": 3.8157064891148127, + "heading": 2.4409110517776624, + "angularVelocity": 0.00008164374501723712, + "velocityX": 1.299300024087019, + "velocityY": -0.9749413455609843, + "timestamp": 0.8412463286746275 + }, + { + "x": 1.9367735753408017, + "y": 3.7245769560008215, + "heading": 2.440918683449934, + "angularVelocity": 0.00008164677586345695, + "velocityX": 1.2993000497173246, + "velocityY": -0.9749413103746718, + "timestamp": 0.9347181429718083 + }, + { + "x": 2.0582215109806032, + "y": 3.6334474265483334, + "heading": 2.4409263154021836, + "angularVelocity": 0.0000816497711865655, + "velocityX": 1.2993000783495519, + "velocityY": -0.9749412712023965, + "timestamp": 1.0281899572689892 + }, + { + "x": 2.1796694496259765, + "y": 3.542317901194656, + "heading": 2.440933947630772, + "angularVelocity": 0.00008165272757387169, + "velocityX": 1.2993001105043924, + "velocityY": -0.9749412273516357, + "timestamp": 1.12166177156617 + }, + { + "x": 2.3011173916676397, + "y": 3.451188380458844, + "heading": 2.440941580131694, + "angularVelocity": 0.00008165564110490276, + "velocityX": 1.2993001468393033, + "velocityY": -0.9749411779478068, + "timestamp": 1.215133585863351 + }, + { + "x": 2.422565337575994, + "y": 3.3600588649659713, + "heading": 2.440949212900517, + "angularVelocity": 0.00008165850722838425, + "velocityX": 1.2993001882067592, + "velocityY": -0.97494112185668, + "timestamp": 1.3086054001605318 + }, + { + "x": 2.5440132879286748, + "y": 3.268929355483818, + "heading": 2.44095684593231, + "angularVelocity": 0.00008166132058271907, + "velocityX": 1.2993002357539973, + "velocityY": -0.9749410575515263, + "timestamp": 1.4020772144577127 + }, + { + "x": 2.6654612434555944, + "y": 3.1777998529828566, + "heading": 2.4409644792215364, + "angularVelocity": 0.00008166407471316297, + "velocityX": 1.2993002911101328, + "velocityY": -0.9749409828638569, + "timestamp": 1.4955490287548936 + }, + { + "x": 2.7869092051206428, + "y": 3.0866703587450828, + "heading": 2.4409721127619197, + "angularVelocity": 0.00008166676169345355, + "velocityX": 1.2993003567783705, + "velocityY": -0.9749408944608698, + "timestamp": 1.5890208430520745 + }, + { + "x": 2.908357174291307, + "y": 2.995540874590004, + "heading": 2.440979746546289, + "angularVelocity": 0.00008166937195400168, + "velocityX": 1.2993004370765409, + "velocityY": -0.9749407865920395, + "timestamp": 1.6824926573492553 + }, + { + "x": 3.0298051531585033, + "y": 2.9044114034341826, + "heading": 2.440987380566619, + "angularVelocity": 0.00008167189636088749, + "velocityX": 1.299300540814034, + "velocityY": -0.9749406475206273, + "timestamp": 1.7759644716464362 + }, + { + "x": 3.1512531460668667, + "y": 2.813281951064894, + "heading": 2.4409950148158184, + "angularVelocity": 0.00008167434490013359, + "velocityX": 1.2993006910322324, + "velocityY": -0.9749404465345578, + "timestamp": 1.869436285943617 + }, + { + "x": 3.272701165942649, + "y": 2.722152534713991, + "heading": 2.4410026493051027, + "angularVelocity": 0.00008167691342827456, + "velocityX": 1.2993009795407924, + "velocityY": -0.9749400611950203, + "timestamp": 1.962908100240798 + }, + { + "x": 3.3941493332630563, + "y": 2.6310233151653177, + "heading": 2.441010284564204, + "angularVelocity": 0.00008168514924485609, + "velocityX": 1.2993025569641734, + "velocityY": -0.9749379557236404, + "timestamp": 2.056379914537979 + }, + { + "x": 3.506643194922811, + "y": 2.543133701847389, + "heading": 2.478368219245545, + "angularVelocity": 0.39967058478790624, + "velocityX": 1.2035057038916106, + "velocityY": -0.9402793128471436, + "timestamp": 2.1498517288351597 + }, + { + "x": 3.621033917156409, + "y": 2.462292474945906, + "heading": 2.526376588848021, + "angularVelocity": 0.513613327862023, + "velocityX": 1.2237991002283153, + "velocityY": -0.8648727695009685, + "timestamp": 2.2433235431323406 + }, + { + "x": 3.7386117914711243, + "y": 2.3880497601699076, + "heading": 2.5783804352338895, + "angularVelocity": 0.5563585854932618, + "velocityX": 1.2578965669896234, + "velocityY": -0.7942791667652267, + "timestamp": 2.3367953574295215 + }, + { + "x": 3.8565091125978217, + "y": 2.322003920666466, + "heading": 2.644821825276836, + "angularVelocity": 0.7108173789342068, + "velocityX": 1.2613141406655402, + "velocityY": -0.7065856162100131, + "timestamp": 2.4302671717267024 + }, + { + "x": 3.9764532017194134, + "y": 2.262287896924173, + "heading": 2.7144845491695992, + "angularVelocity": 0.7452805363473517, + "velocityX": 1.2832113083869967, + "velocityY": -0.6388666379410853, + "timestamp": 2.5237389860238832 + }, + { + "x": 4.100272385460963, + "y": 2.211007382543893, + "heading": 2.7840189996546836, + "angularVelocity": 0.7439082145555573, + "velocityX": 1.3246686680102626, + "velocityY": -0.5486200815278979, + "timestamp": 2.617210800321064 + }, + { + "x": 4.2303737393158425, + "y": 2.1552421719955963, + "heading": 2.8233254460416966, + "angularVelocity": 0.42051656622437983, + "velocityX": 1.3918779134985075, + "velocityY": -0.5965992098002822, + "timestamp": 2.710682614618245 + }, + { + "x": 4.368649457253469, + "y": 2.0958799166815227, + "heading": 2.828594846354344, + "angularVelocity": 0.056374216679845754, + "velocityX": 1.479330630065645, + "velocityY": -0.6350818774667083, + "timestamp": 2.804154428915426 + }, + { + "x": 4.510357504555393, + "y": 2.041351647410644, + "heading": 2.8285983828965033, + "angularVelocity": 0.00003783538584099394, + "velocityX": 1.5160511044685778, + "velocityY": -0.5833659021265345, + "timestamp": 2.8976262432126068 + }, + { + "x": 4.652065594721857, + "y": 1.986823489488553, + "heading": 2.8286019193610827, + "angularVelocity": 0.00003783455586217891, + "velocityX": 1.516051563051105, + "velocityY": -0.5833647108713029, + "timestamp": 2.9910980575097876 + }, + { + "x": 4.793773697805799, + "y": 1.9322953651089438, + "heading": 2.8286054557754516, + "angularVelocity": 0.000037834018690956495, + "velocityX": 1.5160517012476136, + "velocityY": -0.5833643520200054, + "timestamp": 3.0845698718069685 + }, + { + "x": 4.935481808422067, + "y": 1.8777672602773423, + "heading": 2.8286089921399973, + "angularVelocity": 0.0000378334856573034, + "velocityX": 1.5160517818315487, + "velocityY": -0.5833641428873607, + "timestamp": 3.1780416861041494 + }, + { + "x": 5.0771899244715755, + "y": 1.8232391695375483, + "heading": 2.82861252845304, + "angularVelocity": 0.00003783293466201255, + "velocityX": 1.516051839958576, + "velocityY": -0.5833639921273963, + "timestamp": 3.2715135004013303 + }, + { + "x": 5.218898044817724, + "y": 1.76871108993463, + "heading": 2.8286160647128784, + "angularVelocity": 0.0000378323654513879, + "velocityX": 1.5160518859258105, + "velocityY": -0.5833638729805077, + "timestamp": 3.364985314698511 + }, + { + "x": 5.360606168722747, + "y": 1.7141830195502608, + "heading": 2.828619600918001, + "angularVelocity": 0.00003783178008651746, + "velocityX": 1.5160519240001233, + "velocityY": -0.5833637743566714, + "timestamp": 3.458457128995692 + }, + { + "x": 5.5023142956547595, + "y": 1.6596549570012744, + "heading": 2.828623137067082, + "angularVelocity": 0.0000378311805264838, + "velocityX": 1.516051956384105, + "velocityY": -0.5833636905305171, + "timestamp": 3.551928943292873 + }, + { + "x": 5.644022425205511, + "y": 1.605126901225908, + "heading": 2.828626673158945, + "angularVelocity": 0.00003783056838692872, + "velocityX": 1.5160519844004554, + "velocityY": -0.5833636180635361, + "timestamp": 3.6454007575900538 + }, + { + "x": 5.785730557049162, + "y": 1.5505988513766498, + "heading": 2.828630209192539, + "angularVelocity": 0.000037829944994143904, + "velocityX": 1.516052008930833, + "velocityY": -0.5833635546635878, + "timestamp": 3.7388725718872347 + }, + { + "x": 5.927438690918852, + "y": 1.4960708067593358, + "heading": 2.8286337451669143, + "angularVelocity": 0.00003782931145939862, + "velocityX": 1.5160520306062335, + "velocityY": -0.5833634986900924, + "timestamp": 3.8323443861844155 + }, + { + "x": 6.069146826592036, + "y": 1.4415427667950262, + "heading": 2.828637281081213, + "angularVelocity": 0.00003782866873129857, + "velocityX": 1.5160520499007515, + "velocityY": -0.583363448910333, + "timestamp": 3.9258162004815964 + }, + { + "x": 6.210854963880611, + "y": 1.387014730994346, + "heading": 2.828640816934652, + "angularVelocity": 0.000037828017628644405, + "velocityX": 1.516052067182881, + "velocityY": -0.5833634043661104, + "timestamp": 4.019288014778777 + }, + { + "x": 6.35256310262391, + "y": 1.332486698939259, + "heading": 2.8286443527265157, + "angularVelocity": 0.00003782735887192692, + "velocityX": 1.5160520827461144, + "velocityY": -0.5833633642942074, + "timestamp": 4.112759829075958 + }, + { + "x": 6.494271242683516, + "y": 1.2779586702695989, + "heading": 2.828647888456148, + "angularVelocity": 0.00003782669309301873, + "velocityX": 1.516052096828514, + "velocityY": -0.583363328075519, + "timestamp": 4.206231643373139 + }, + { + "x": 6.635979383939319, + "y": 1.223430644672807, + "heading": 2.828651424122945, + "angularVelocity": 0.0000378260208545108, + "velocityX": 1.5160521096259227, + "velocityY": -0.5833632952007055, + "timestamp": 4.2997034576703195 + }, + { + "x": 6.7776875262864404, + "y": 1.1689026218759404, + "heading": 2.8286549597263497, + "angularVelocity": 0.0000378253426576497, + "velocityX": 1.516052121301291, + "velocityY": -0.5833632652459484, + "timestamp": 4.3931752719675 + }, + { + "x": 6.9193956696327925, + "y": 1.114374601639329, + "heading": 2.8286584952658473, + "angularVelocity": 0.00003782465895599354, + "velocityX": 1.516052131991471, + "velocityY": -0.5833632378552868, + "timestamp": 4.486647086264681 + }, + { + "x": 7.061103813897117, + "y": 1.0598465837514708, + "heading": 2.828662030740962, + "angularVelocity": 0.000037823970152435987, + "velocityX": 1.5160521418123194, + "velocityY": -0.5833632127273561, + "timestamp": 4.580118900561862 + }, + { + "x": 7.202811959007387, + "y": 1.0053185680248766, + "heading": 2.8286655661512494, + "angularVelocity": 0.00003782327661332265, + "velocityX": 1.5160521508625917, + "velocityY": -0.5833631896052619, + "timestamp": 4.673590714859043 + }, + { + "x": 7.344520104899491, + "y": 0.9507905542926514, + "heading": 2.828669101496299, + "angularVelocity": 0.000037822578668805064, + "velocityX": 1.5160521592269784, + "velocityY": -0.5833631682686824, + "timestamp": 4.767062529156224 + }, + { + "x": 7.486228251516145, + "y": 0.8962625424056586, + "heading": 2.828672636775727, + "angularVelocity": 0.000037821876620878625, + "velocityX": 1.5160521669785085, + "velocityY": -0.5833631485276237, + "timestamp": 4.860534343453405 + }, + { + "x": 7.627936398805981, + "y": 0.8417345322301597, + "heading": 2.828676171989175, + "angularVelocity": 0.00003782117074336596, + "velocityX": 1.516052174180496, + "velocityY": -0.5833631302173573, + "timestamp": 4.954006157750586 + }, + { + "x": 7.769644550650757, + "y": 0.7872065338976291, + "heading": 2.8286797071997065, + "angularVelocity": 0.000037821139539143325, + "velocityX": 1.5160522229111162, + "velocityY": -0.5833630035164005, + "timestamp": 5.0474779720477665 + }, + { + "x": 7.89860842703643, + "y": 0.754016059353999, + "heading": 2.902617724341904, + "angularVelocity": 0.7910193858773472, + "velocityX": 1.3797087106456418, + "velocityY": -0.3550853783377481, + "timestamp": 5.140949786344947 + }, + { + "x": 7.956855773925781, + "y": 0.7431064248085022, + "heading": 3.141592653589793, + "angularVelocity": 2.5566523025657903, + "velocityX": 0.6231541275550934, + "velocityY": -0.1167157675019664, + "timestamp": 5.234421600642128 + }, + { + "x": 7.956855773925781, + "y": 0.7431064248085022, + "heading": 3.141592653589793, + "angularVelocity": -3.8315823290727504e-21, + "velocityX": 3.876602036733102e-21, + "velocityY": -3.3558838471561212e-21, + "timestamp": 5.327893414939309 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceContested1.2.traj b/src/main/deploy/choreo/SourceContested1.2.traj new file mode 100644 index 00000000..295ce698 --- /dev/null +++ b/src/main/deploy/choreo/SourceContested1.2.traj @@ -0,0 +1,338 @@ +{ + "samples": [ + { + "x": 7.956855773925781, + "y": 0.7431064248085022, + "heading": 3.141592653589793, + "angularVelocity": -3.8315823290727504e-21, + "velocityX": 3.876602036733102e-21, + "velocityY": -3.3558838471561212e-21, + "timestamp": 0 + }, + { + "x": 7.9018904258286256, + "y": 0.7580071883715975, + "heading": 2.929746437641678, + "angularVelocity": -2.3660710929042184, + "velocityX": -0.6138977779803758, + "velocityY": 0.166423864457769, + "timestamp": 0.08953501717823986 + }, + { + "x": 7.77927283258943, + "y": 0.7936488654478715, + "heading": 2.8587142949291526, + "angularVelocity": -0.7933448269867456, + "velocityX": -1.3694931559023147, + "velocityY": 0.39807528048295576, + "timestamp": 0.17907003435647972 + }, + { + "x": 7.643021297144732, + "y": 0.8351292518640833, + "heading": 2.8460714923728077, + "angularVelocity": -0.14120511677768, + "velocityX": -1.5217681275858608, + "velocityY": 0.4632867421428616, + "timestamp": 0.2686050515347196 + }, + { + "x": 7.50584253248777, + "y": 0.8779834119507215, + "heading": 2.8388066609223235, + "angularVelocity": -0.08113955499692087, + "velocityX": -1.532124178676125, + "velocityY": 0.4786301654617149, + "timestamp": 0.35814006871295945 + }, + { + "x": 7.368255234864718, + "y": 0.9216599007543135, + "heading": 2.8342113751430746, + "angularVelocity": -0.051323894539503874, + "velocityX": -1.536687007600088, + "velocityY": 0.487814602376678, + "timestamp": 0.4476750858911993 + }, + { + "x": 7.230614869935536, + "y": 0.9659066250673984, + "heading": 2.8305583662971365, + "angularVelocity": -0.040799778243928046, + "velocityX": -1.5372797064993953, + "velocityY": 0.4941834570155044, + "timestamp": 0.5372101030694392 + }, + { + "x": 7.093252862402945, + "y": 1.010681152263652, + "heading": 2.826475143484932, + "angularVelocity": -0.045604758237506364, + "velocityX": -1.5341707843663217, + "velocityY": 0.5000783895212758, + "timestamp": 0.626745120247679 + }, + { + "x": 6.956119170941034, + "y": 1.0557271520364861, + "heading": 2.821845381251112, + "angularVelocity": -0.051708955665952475, + "velocityX": -1.5316207645207218, + "velocityY": 0.5031104163766442, + "timestamp": 0.7162801374259189 + }, + { + "x": 6.818992484234107, + "y": 1.100769539311748, + "heading": 2.817204104711705, + "angularVelocity": -0.0518375568093923, + "velocityX": -1.5315425297115504, + "velocityY": 0.5030700690613045, + "timestamp": 0.8058151546041588 + }, + { + "x": 6.681872127416794, + "y": 1.1458071900530102, + "heading": 2.8125526096190336, + "angularVelocity": -0.05195168593547435, + "velocityX": -1.5314718323484902, + "velocityY": 0.5030171675915902, + "timestamp": 0.8953501717823986 + }, + { + "x": 6.544758118625867, + "y": 1.1908399522969457, + "heading": 2.8078906701771453, + "angularVelocity": -0.052068336934676634, + "velocityX": -1.5314009324192217, + "velocityY": 0.5029625688716561, + "timestamp": 0.9848851889606385 + }, + { + "x": 6.407650518790602, + "y": 1.2358677646327403, + "heading": 2.8032180092193197, + "angularVelocity": -0.05218808355755985, + "velocityX": -1.5313293519821691, + "velocityY": 0.5029072842657369, + "timestamp": 1.0744202061388783 + }, + { + "x": 6.270549391355044, + "y": 1.280890584680893, + "heading": 2.7985343668363263, + "angularVelocity": -0.05231073305843581, + "velocityX": -1.531257062950314, + "velocityY": 0.5028515263310278, + "timestamp": 1.1639552233171182 + }, + { + "x": 6.1334547976207645, + "y": 1.3259083749515967, + "heading": 2.793839500454244, + "angularVelocity": -0.05243609182244443, + "velocityX": -1.5311840892526187, + "velocityY": 0.5027953496796241, + "timestamp": 1.253490240495358 + }, + { + "x": 5.996366796772198, + "y": 1.3709210989414258, + "heading": 2.7891331795005336, + "angularVelocity": -0.05256402580837642, + "velocityX": -1.5311104545349274, + "velocityY": 0.5027387653282189, + "timestamp": 1.343025257673598 + }, + { + "x": 5.8592854465592445, + "y": 1.4159287198651518, + "heading": 2.784415180924841, + "angularVelocity": -0.052694451002343774, + "velocityX": -1.5310361748193169, + "velocityY": 0.5026817701294257, + "timestamp": 1.4325602748518378 + }, + { + "x": 5.722210803960778, + "y": 1.4609312002186288, + "heading": 2.7796852859248706, + "angularVelocity": -0.05282731995856272, + "velocityX": -1.530961258717216, + "velocityY": 0.5026243560537855, + "timestamp": 1.5220952920300777 + }, + { + "x": 5.585142925732609, + "y": 1.5059285016384842, + "heading": 2.7749432775568774, + "angularVelocity": -0.052962611919236485, + "velocityX": -1.5308857087200383, + "velocityY": 0.5025665135047443, + "timestamp": 1.6116303092083175 + }, + { + "x": 5.448081868853608, + "y": 1.5509205848763696, + "heading": 2.7701889389309033, + "angularVelocity": -0.05310032628362197, + "velocityX": -1.5308095223363887, + "velocityY": 0.502508232598188, + "timestamp": 1.7011653263865574 + }, + { + "x": 5.311027690899557, + "y": 1.5959074098195543, + "heading": 2.7654220517793227, + "angularVelocity": -0.05324047843863795, + "velocityX": -1.5307326928995175, + "velocityY": 0.502449503679975, + "timestamp": 1.7907003435647972 + }, + { + "x": 5.173980450370318, + "y": 1.6408889355303347, + "heading": 2.7606423952580355, + "angularVelocity": -0.05338309716043176, + "velocityX": -1.5306552100885358, + "velocityY": 0.5023903175361529, + "timestamp": 1.880235360743037 + }, + { + "x": 5.0369402069873335, + "y": 1.6858651202909567, + "heading": 2.7558497448937236, + "angularVelocity": -0.05352822298309391, + "velocityX": -1.5305770602598503, + "velocityY": 0.5023306654544649, + "timestamp": 1.969770377921277 + }, + { + "x": 4.899907021964721, + "y": 1.7308359216420997, + "heading": 2.751043871646217, + "angularVelocity": -0.05367590691292534, + "velocityX": -1.5304982267420264, + "velocityY": 0.5022705391525029, + "timestamp": 2.059305395099517 + }, + { + "x": 4.762880958231051, + "y": 1.7758012963934744, + "heading": 2.7462245411420234, + "angularVelocity": -0.05382620851683086, + "velocityX": -1.5304186903866774, + "velocityY": 0.5022099304662124, + "timestamp": 2.1488404122777567 + }, + { + "x": 4.625862080529766, + "y": 1.8207612005630536, + "heading": 2.7413915132887583, + "angularVelocity": -0.053979191668033985, + "velocityX": -1.5303384309238324, + "velocityY": 0.5021488305528153, + "timestamp": 2.2383754294559965 + }, + { + "x": 4.488850455262, + "y": 1.8657155891757824, + "heading": 2.736544542679806, + "angularVelocity": -0.054134915720223636, + "velocityX": -1.5302574298390257, + "velocityY": 0.5020872283213718, + "timestamp": 2.3279104466342364 + }, + { + "x": 4.351846149940031, + "y": 1.910664415870897, + "heading": 2.7316833801907157, + "angularVelocity": -0.05429342219718387, + "velocityX": -1.5301756747221047, + "velocityY": 0.5020251082951581, + "timestamp": 2.4174454638124763 + }, + { + "x": 4.214849232355917, + "y": 1.955607632403858, + "heading": 2.7268077754532425, + "angularVelocity": -0.05445472499064026, + "velocityX": -1.530093162448295, + "velocityY": 0.5019624494346333, + "timestamp": 2.506980480990716 + }, + { + "x": 4.077859769965423, + "y": 2.0005451883147503, + "heading": 2.721917478680163, + "angularVelocity": -0.05461881761126215, + "velocityX": -1.5300098967735323, + "velocityY": 0.5018992269966714, + "timestamp": 2.596515498168956 + }, + { + "x": 3.940877826047202, + "y": 2.045477029542233, + "heading": 2.717012252912238, + "angularVelocity": -0.05478555678567875, + "velocityX": -1.5299259243512187, + "velocityY": 0.5018354007576298, + "timestamp": 2.686050515347196 + }, + { + "x": 3.8039034641003533, + "y": 2.0904031002081847, + "heading": 2.712091859898031, + "angularVelocity": -0.05495495694618896, + "velocityX": -1.5298412427192656, + "velocityY": 0.5017709504262021, + "timestamp": 2.775585532525435 + }, + { + "x": 3.66693674900291, + "y": 2.135323343068443, + "heading": 2.707156056414113, + "angularVelocity": -0.05512707362408464, + "velocityX": -1.5297558364765886, + "velocityY": 0.501705860745354, + "timestamp": 2.865120549703674 + }, + { + "x": 3.5299779535424887, + "y": 2.1802377886608806, + "heading": 2.7022039564715556, + "angularVelocity": -0.05530908574796876, + "velocityX": -1.5296673835196157, + "velocityY": 0.5016411121363245, + "timestamp": 2.9546555668819128 + }, + { + "x": 3.426502516345769, + "y": 2.233179068985383, + "heading": 2.594355857349509, + "angularVelocity": -1.2045354155386023, + "velocityX": -1.1556979655315056, + "velocityY": 0.5912913404496402, + "timestamp": 3.0441905840601517 + }, + { + "x": 3.387338399887085, + "y": 2.2590601444244385, + "heading": 2.3477629859714533, + "angularVelocity": -2.7541500426269785, + "velocityX": -0.43741675260662705, + "velocityY": 0.28906093118330495, + "timestamp": 3.1337256012383907 + }, + { + "x": 3.387338399887085, + "y": 2.2590601444244385, + "heading": 2.3477629859714533, + "angularVelocity": -2.400686558954734e-26, + "velocityX": 2.2132770883951947e-26, + "velocityY": 2.6443990744659133e-26, + "timestamp": 3.2232606184166297 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceContested1.traj b/src/main/deploy/choreo/SourceContested1.traj new file mode 100644 index 00000000..eea3ac49 --- /dev/null +++ b/src/main/deploy/choreo/SourceContested1.traj @@ -0,0 +1,851 @@ +{ + "samples": [ + { + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "angularVelocity": -1.8256752618245288e-22, + "velocityX": -7.65916010942737e-22, + "velocityY": 5.834863940365417e-22, + "timestamp": 0 + }, + { + "x": 0.8644346902480902, + "y": 4.538605095268569, + "heading": 2.3610664104627634, + "angularVelocity": 2.6730073050757213, + "velocityX": 0.4641971643197831, + "velocityY": -0.4033128817553312, + "timestamp": 0.09347181429718082 + }, + { + "x": 0.9651901704673621, + "y": 4.45361330197089, + "heading": 2.440857637900237, + "angularVelocity": 0.8536394424076145, + "velocityX": 1.0779236604837226, + "velocityY": -0.9092772397405119, + "timestamp": 0.18694362859436164 + }, + { + "x": 1.0866380874820318, + "y": 4.362483747012655, + "heading": 2.440865267791502, + "angularVelocity": 0.00008162772192147527, + "velocityX": 1.299299879090211, + "velocityY": -0.9749415440733835, + "timestamp": 0.28041544289154247 + }, + { + "x": 1.2080860091215304, + "y": 4.271354198213206, + "heading": 2.4408728977229917, + "angularVelocity": 0.00008162815226084984, + "velocityX": 1.2992999285685376, + "velocityY": -0.9749414781841598, + "timestamp": 0.3738872571887233 + }, + { + "x": 1.3295339322232647, + "y": 4.1802246514640125, + "heading": 2.440880527950737, + "angularVelocity": 0.0000816313217297415, + "velocityX": 1.299299944212136, + "velocityY": -0.9749414562496848, + "timestamp": 0.4673590714859041 + }, + { + "x": 1.4509818569281794, + "y": 4.089095106952003, + "heading": 2.440888158472548, + "angularVelocity": 0.00008163446776368671, + "velocityX": 1.2992999613636238, + "velocityY": -0.9749414323153683, + "timestamp": 0.5608308857830849 + }, + { + "x": 1.5724297833961638, + "y": 3.997965564889279, + "heading": 2.440895789286055, + "angularVelocity": 0.00008163758844700525, + "velocityX": 1.2992999802256675, + "velocityY": -0.9749414061119055, + "timestamp": 0.6543027000802657 + }, + { + "x": 1.6938777118095218, + "y": 3.906836025517733, + "heading": 2.4409034203886897, + "angularVelocity": 0.00008164068165665073, + "velocityX": 1.299300001038079, + "velocityY": -0.9749413773205733, + "timestamp": 0.7477745143774466 + }, + { + "x": 1.8153256423773063, + "y": 3.8157064891148127, + "heading": 2.4409110517776624, + "angularVelocity": 0.00008164374501723712, + "velocityX": 1.299300024087019, + "velocityY": -0.9749413455609843, + "timestamp": 0.8412463286746275 + }, + { + "x": 1.9367735753408017, + "y": 3.7245769560008215, + "heading": 2.440918683449934, + "angularVelocity": 0.00008164677586345695, + "velocityX": 1.2993000497173246, + "velocityY": -0.9749413103746718, + "timestamp": 0.9347181429718083 + }, + { + "x": 2.0582215109806032, + "y": 3.6334474265483334, + "heading": 2.4409263154021836, + "angularVelocity": 0.0000816497711865655, + "velocityX": 1.2993000783495519, + "velocityY": -0.9749412712023965, + "timestamp": 1.0281899572689892 + }, + { + "x": 2.1796694496259765, + "y": 3.542317901194656, + "heading": 2.440933947630772, + "angularVelocity": 0.00008165272757387169, + "velocityX": 1.2993001105043924, + "velocityY": -0.9749412273516357, + "timestamp": 1.12166177156617 + }, + { + "x": 2.3011173916676397, + "y": 3.451188380458844, + "heading": 2.440941580131694, + "angularVelocity": 0.00008165564110490276, + "velocityX": 1.2993001468393033, + "velocityY": -0.9749411779478068, + "timestamp": 1.215133585863351 + }, + { + "x": 2.422565337575994, + "y": 3.3600588649659713, + "heading": 2.440949212900517, + "angularVelocity": 0.00008165850722838425, + "velocityX": 1.2993001882067592, + "velocityY": -0.97494112185668, + "timestamp": 1.3086054001605318 + }, + { + "x": 2.5440132879286748, + "y": 3.268929355483818, + "heading": 2.44095684593231, + "angularVelocity": 0.00008166132058271907, + "velocityX": 1.2993002357539973, + "velocityY": -0.9749410575515263, + "timestamp": 1.4020772144577127 + }, + { + "x": 2.6654612434555944, + "y": 3.1777998529828566, + "heading": 2.4409644792215364, + "angularVelocity": 0.00008166407471316297, + "velocityX": 1.2993002911101328, + "velocityY": -0.9749409828638569, + "timestamp": 1.4955490287548936 + }, + { + "x": 2.7869092051206428, + "y": 3.0866703587450828, + "heading": 2.4409721127619197, + "angularVelocity": 0.00008166676169345355, + "velocityX": 1.2993003567783705, + "velocityY": -0.9749408944608698, + "timestamp": 1.5890208430520745 + }, + { + "x": 2.908357174291307, + "y": 2.995540874590004, + "heading": 2.440979746546289, + "angularVelocity": 0.00008166937195400168, + "velocityX": 1.2993004370765409, + "velocityY": -0.9749407865920395, + "timestamp": 1.6824926573492553 + }, + { + "x": 3.0298051531585033, + "y": 2.9044114034341826, + "heading": 2.440987380566619, + "angularVelocity": 0.00008167189636088749, + "velocityX": 1.299300540814034, + "velocityY": -0.9749406475206273, + "timestamp": 1.7759644716464362 + }, + { + "x": 3.1512531460668667, + "y": 2.813281951064894, + "heading": 2.4409950148158184, + "angularVelocity": 0.00008167434490013359, + "velocityX": 1.2993006910322324, + "velocityY": -0.9749404465345578, + "timestamp": 1.869436285943617 + }, + { + "x": 3.272701165942649, + "y": 2.722152534713991, + "heading": 2.4410026493051027, + "angularVelocity": 0.00008167691342827456, + "velocityX": 1.2993009795407924, + "velocityY": -0.9749400611950203, + "timestamp": 1.962908100240798 + }, + { + "x": 3.3941493332630563, + "y": 2.6310233151653177, + "heading": 2.441010284564204, + "angularVelocity": 0.00008168514924485609, + "velocityX": 1.2993025569641734, + "velocityY": -0.9749379557236404, + "timestamp": 2.056379914537979 + }, + { + "x": 3.506643194922811, + "y": 2.543133701847389, + "heading": 2.478368219245545, + "angularVelocity": 0.39967058478790624, + "velocityX": 1.2035057038916106, + "velocityY": -0.9402793128471436, + "timestamp": 2.1498517288351597 + }, + { + "x": 3.621033917156409, + "y": 2.462292474945906, + "heading": 2.526376588848021, + "angularVelocity": 0.513613327862023, + "velocityX": 1.2237991002283153, + "velocityY": -0.8648727695009685, + "timestamp": 2.2433235431323406 + }, + { + "x": 3.7386117914711243, + "y": 2.3880497601699076, + "heading": 2.5783804352338895, + "angularVelocity": 0.5563585854932618, + "velocityX": 1.2578965669896234, + "velocityY": -0.7942791667652267, + "timestamp": 2.3367953574295215 + }, + { + "x": 3.8565091125978217, + "y": 2.322003920666466, + "heading": 2.644821825276836, + "angularVelocity": 0.7108173789342068, + "velocityX": 1.2613141406655402, + "velocityY": -0.7065856162100131, + "timestamp": 2.4302671717267024 + }, + { + "x": 3.9764532017194134, + "y": 2.262287896924173, + "heading": 2.7144845491695992, + "angularVelocity": 0.7452805363473517, + "velocityX": 1.2832113083869967, + "velocityY": -0.6388666379410853, + "timestamp": 2.5237389860238832 + }, + { + "x": 4.100272385460963, + "y": 2.211007382543893, + "heading": 2.7840189996546836, + "angularVelocity": 0.7439082145555573, + "velocityX": 1.3246686680102626, + "velocityY": -0.5486200815278979, + "timestamp": 2.617210800321064 + }, + { + "x": 4.2303737393158425, + "y": 2.1552421719955963, + "heading": 2.8233254460416966, + "angularVelocity": 0.42051656622437983, + "velocityX": 1.3918779134985075, + "velocityY": -0.5965992098002822, + "timestamp": 2.710682614618245 + }, + { + "x": 4.368649457253469, + "y": 2.0958799166815227, + "heading": 2.828594846354344, + "angularVelocity": 0.056374216679845754, + "velocityX": 1.479330630065645, + "velocityY": -0.6350818774667083, + "timestamp": 2.804154428915426 + }, + { + "x": 4.510357504555393, + "y": 2.041351647410644, + "heading": 2.8285983828965033, + "angularVelocity": 0.00003783538584099394, + "velocityX": 1.5160511044685778, + "velocityY": -0.5833659021265345, + "timestamp": 2.8976262432126068 + }, + { + "x": 4.652065594721857, + "y": 1.986823489488553, + "heading": 2.8286019193610827, + "angularVelocity": 0.00003783455586217891, + "velocityX": 1.516051563051105, + "velocityY": -0.5833647108713029, + "timestamp": 2.9910980575097876 + }, + { + "x": 4.793773697805799, + "y": 1.9322953651089438, + "heading": 2.8286054557754516, + "angularVelocity": 0.000037834018690956495, + "velocityX": 1.5160517012476136, + "velocityY": -0.5833643520200054, + "timestamp": 3.0845698718069685 + }, + { + "x": 4.935481808422067, + "y": 1.8777672602773423, + "heading": 2.8286089921399973, + "angularVelocity": 0.0000378334856573034, + "velocityX": 1.5160517818315487, + "velocityY": -0.5833641428873607, + "timestamp": 3.1780416861041494 + }, + { + "x": 5.0771899244715755, + "y": 1.8232391695375483, + "heading": 2.82861252845304, + "angularVelocity": 0.00003783293466201255, + "velocityX": 1.516051839958576, + "velocityY": -0.5833639921273963, + "timestamp": 3.2715135004013303 + }, + { + "x": 5.218898044817724, + "y": 1.76871108993463, + "heading": 2.8286160647128784, + "angularVelocity": 0.0000378323654513879, + "velocityX": 1.5160518859258105, + "velocityY": -0.5833638729805077, + "timestamp": 3.364985314698511 + }, + { + "x": 5.360606168722747, + "y": 1.7141830195502608, + "heading": 2.828619600918001, + "angularVelocity": 0.00003783178008651746, + "velocityX": 1.5160519240001233, + "velocityY": -0.5833637743566714, + "timestamp": 3.458457128995692 + }, + { + "x": 5.5023142956547595, + "y": 1.6596549570012744, + "heading": 2.828623137067082, + "angularVelocity": 0.0000378311805264838, + "velocityX": 1.516051956384105, + "velocityY": -0.5833636905305171, + "timestamp": 3.551928943292873 + }, + { + "x": 5.644022425205511, + "y": 1.605126901225908, + "heading": 2.828626673158945, + "angularVelocity": 0.00003783056838692872, + "velocityX": 1.5160519844004554, + "velocityY": -0.5833636180635361, + "timestamp": 3.6454007575900538 + }, + { + "x": 5.785730557049162, + "y": 1.5505988513766498, + "heading": 2.828630209192539, + "angularVelocity": 0.000037829944994143904, + "velocityX": 1.516052008930833, + "velocityY": -0.5833635546635878, + "timestamp": 3.7388725718872347 + }, + { + "x": 5.927438690918852, + "y": 1.4960708067593358, + "heading": 2.8286337451669143, + "angularVelocity": 0.00003782931145939862, + "velocityX": 1.5160520306062335, + "velocityY": -0.5833634986900924, + "timestamp": 3.8323443861844155 + }, + { + "x": 6.069146826592036, + "y": 1.4415427667950262, + "heading": 2.828637281081213, + "angularVelocity": 0.00003782866873129857, + "velocityX": 1.5160520499007515, + "velocityY": -0.583363448910333, + "timestamp": 3.9258162004815964 + }, + { + "x": 6.210854963880611, + "y": 1.387014730994346, + "heading": 2.828640816934652, + "angularVelocity": 0.000037828017628644405, + "velocityX": 1.516052067182881, + "velocityY": -0.5833634043661104, + "timestamp": 4.019288014778777 + }, + { + "x": 6.35256310262391, + "y": 1.332486698939259, + "heading": 2.8286443527265157, + "angularVelocity": 0.00003782735887192692, + "velocityX": 1.5160520827461144, + "velocityY": -0.5833633642942074, + "timestamp": 4.112759829075958 + }, + { + "x": 6.494271242683516, + "y": 1.2779586702695989, + "heading": 2.828647888456148, + "angularVelocity": 0.00003782669309301873, + "velocityX": 1.516052096828514, + "velocityY": -0.583363328075519, + "timestamp": 4.206231643373139 + }, + { + "x": 6.635979383939319, + "y": 1.223430644672807, + "heading": 2.828651424122945, + "angularVelocity": 0.0000378260208545108, + "velocityX": 1.5160521096259227, + "velocityY": -0.5833632952007055, + "timestamp": 4.2997034576703195 + }, + { + "x": 6.7776875262864404, + "y": 1.1689026218759404, + "heading": 2.8286549597263497, + "angularVelocity": 0.0000378253426576497, + "velocityX": 1.516052121301291, + "velocityY": -0.5833632652459484, + "timestamp": 4.3931752719675 + }, + { + "x": 6.9193956696327925, + "y": 1.114374601639329, + "heading": 2.8286584952658473, + "angularVelocity": 0.00003782465895599354, + "velocityX": 1.516052131991471, + "velocityY": -0.5833632378552868, + "timestamp": 4.486647086264681 + }, + { + "x": 7.061103813897117, + "y": 1.0598465837514708, + "heading": 2.828662030740962, + "angularVelocity": 0.000037823970152435987, + "velocityX": 1.5160521418123194, + "velocityY": -0.5833632127273561, + "timestamp": 4.580118900561862 + }, + { + "x": 7.202811959007387, + "y": 1.0053185680248766, + "heading": 2.8286655661512494, + "angularVelocity": 0.00003782327661332265, + "velocityX": 1.5160521508625917, + "velocityY": -0.5833631896052619, + "timestamp": 4.673590714859043 + }, + { + "x": 7.344520104899491, + "y": 0.9507905542926514, + "heading": 2.828669101496299, + "angularVelocity": 0.000037822578668805064, + "velocityX": 1.5160521592269784, + "velocityY": -0.5833631682686824, + "timestamp": 4.767062529156224 + }, + { + "x": 7.486228251516145, + "y": 0.8962625424056586, + "heading": 2.828672636775727, + "angularVelocity": 0.000037821876620878625, + "velocityX": 1.5160521669785085, + "velocityY": -0.5833631485276237, + "timestamp": 4.860534343453405 + }, + { + "x": 7.627936398805981, + "y": 0.8417345322301597, + "heading": 2.828676171989175, + "angularVelocity": 0.00003782117074336596, + "velocityX": 1.516052174180496, + "velocityY": -0.5833631302173573, + "timestamp": 4.954006157750586 + }, + { + "x": 7.769644550650757, + "y": 0.7872065338976291, + "heading": 2.8286797071997065, + "angularVelocity": 0.000037821139539143325, + "velocityX": 1.5160522229111162, + "velocityY": -0.5833630035164005, + "timestamp": 5.0474779720477665 + }, + { + "x": 7.89860842703643, + "y": 0.754016059353999, + "heading": 2.902617724341904, + "angularVelocity": 0.7910193858773472, + "velocityX": 1.3797087106456418, + "velocityY": -0.3550853783377481, + "timestamp": 5.140949786344947 + }, + { + "x": 7.956855773925781, + "y": 0.7431064248085022, + "heading": 3.141592653589793, + "angularVelocity": 2.5566523025657903, + "velocityX": 0.6231541275550934, + "velocityY": -0.1167157675019664, + "timestamp": 5.234421600642128 + }, + { + "x": 7.956855773925781, + "y": 0.7431064248085022, + "heading": 3.141592653589793, + "angularVelocity": -3.8315823290727504e-21, + "velocityX": 3.876602036733102e-21, + "velocityY": -3.3558838471561212e-21, + "timestamp": 5.327893414939309 + }, + { + "x": 7.9018904258286256, + "y": 0.7580071883715975, + "heading": 2.929746437641678, + "angularVelocity": -2.3660710929042184, + "velocityX": -0.6138977779803758, + "velocityY": 0.166423864457769, + "timestamp": 5.417428432117549 + }, + { + "x": 7.77927283258943, + "y": 0.7936488654478715, + "heading": 2.8587142949291526, + "angularVelocity": -0.7933448269867456, + "velocityX": -1.3694931559023147, + "velocityY": 0.39807528048295576, + "timestamp": 5.506963449295789 + }, + { + "x": 7.643021297144732, + "y": 0.8351292518640833, + "heading": 2.8460714923728077, + "angularVelocity": -0.14120511677768, + "velocityX": -1.5217681275858608, + "velocityY": 0.4632867421428616, + "timestamp": 5.596498466474029 + }, + { + "x": 7.50584253248777, + "y": 0.8779834119507215, + "heading": 2.8388066609223235, + "angularVelocity": -0.08113955499692087, + "velocityX": -1.532124178676125, + "velocityY": 0.4786301654617149, + "timestamp": 5.686033483652269 + }, + { + "x": 7.368255234864718, + "y": 0.9216599007543135, + "heading": 2.8342113751430746, + "angularVelocity": -0.051323894539503874, + "velocityX": -1.536687007600088, + "velocityY": 0.487814602376678, + "timestamp": 5.775568500830508 + }, + { + "x": 7.230614869935536, + "y": 0.9659066250673984, + "heading": 2.8305583662971365, + "angularVelocity": -0.040799778243928046, + "velocityX": -1.5372797064993953, + "velocityY": 0.4941834570155044, + "timestamp": 5.865103518008748 + }, + { + "x": 7.093252862402945, + "y": 1.010681152263652, + "heading": 2.826475143484932, + "angularVelocity": -0.045604758237506364, + "velocityX": -1.5341707843663217, + "velocityY": 0.5000783895212758, + "timestamp": 5.954638535186988 + }, + { + "x": 6.956119170941034, + "y": 1.0557271520364861, + "heading": 2.821845381251112, + "angularVelocity": -0.051708955665952475, + "velocityX": -1.5316207645207218, + "velocityY": 0.5031104163766442, + "timestamp": 6.044173552365228 + }, + { + "x": 6.818992484234107, + "y": 1.100769539311748, + "heading": 2.817204104711705, + "angularVelocity": -0.0518375568093923, + "velocityX": -1.5315425297115504, + "velocityY": 0.5030700690613045, + "timestamp": 6.133708569543468 + }, + { + "x": 6.681872127416794, + "y": 1.1458071900530102, + "heading": 2.8125526096190336, + "angularVelocity": -0.05195168593547435, + "velocityX": -1.5314718323484902, + "velocityY": 0.5030171675915902, + "timestamp": 6.223243586721708 + }, + { + "x": 6.544758118625867, + "y": 1.1908399522969457, + "heading": 2.8078906701771453, + "angularVelocity": -0.052068336934676634, + "velocityX": -1.5314009324192217, + "velocityY": 0.5029625688716561, + "timestamp": 6.312778603899948 + }, + { + "x": 6.407650518790602, + "y": 1.2358677646327403, + "heading": 2.8032180092193197, + "angularVelocity": -0.05218808355755985, + "velocityX": -1.5313293519821691, + "velocityY": 0.5029072842657369, + "timestamp": 6.4023136210781875 + }, + { + "x": 6.270549391355044, + "y": 1.280890584680893, + "heading": 2.7985343668363263, + "angularVelocity": -0.05231073305843581, + "velocityX": -1.531257062950314, + "velocityY": 0.5028515263310278, + "timestamp": 6.491848638256427 + }, + { + "x": 6.1334547976207645, + "y": 1.3259083749515967, + "heading": 2.793839500454244, + "angularVelocity": -0.05243609182244443, + "velocityX": -1.5311840892526187, + "velocityY": 0.5027953496796241, + "timestamp": 6.581383655434667 + }, + { + "x": 5.996366796772198, + "y": 1.3709210989414258, + "heading": 2.7891331795005336, + "angularVelocity": -0.05256402580837642, + "velocityX": -1.5311104545349274, + "velocityY": 0.5027387653282189, + "timestamp": 6.670918672612907 + }, + { + "x": 5.8592854465592445, + "y": 1.4159287198651518, + "heading": 2.784415180924841, + "angularVelocity": -0.052694451002343774, + "velocityX": -1.5310361748193169, + "velocityY": 0.5026817701294257, + "timestamp": 6.760453689791147 + }, + { + "x": 5.722210803960778, + "y": 1.4609312002186288, + "heading": 2.7796852859248706, + "angularVelocity": -0.05282731995856272, + "velocityX": -1.530961258717216, + "velocityY": 0.5026243560537855, + "timestamp": 6.849988706969387 + }, + { + "x": 5.585142925732609, + "y": 1.5059285016384842, + "heading": 2.7749432775568774, + "angularVelocity": -0.052962611919236485, + "velocityX": -1.5308857087200383, + "velocityY": 0.5025665135047443, + "timestamp": 6.939523724147627 + }, + { + "x": 5.448081868853608, + "y": 1.5509205848763696, + "heading": 2.7701889389309033, + "angularVelocity": -0.05310032628362197, + "velocityX": -1.5308095223363887, + "velocityY": 0.502508232598188, + "timestamp": 7.0290587413258665 + }, + { + "x": 5.311027690899557, + "y": 1.5959074098195543, + "heading": 2.7654220517793227, + "angularVelocity": -0.05324047843863795, + "velocityX": -1.5307326928995175, + "velocityY": 0.502449503679975, + "timestamp": 7.118593758504106 + }, + { + "x": 5.173980450370318, + "y": 1.6408889355303347, + "heading": 2.7606423952580355, + "angularVelocity": -0.05338309716043176, + "velocityX": -1.5306552100885358, + "velocityY": 0.5023903175361529, + "timestamp": 7.208128775682346 + }, + { + "x": 5.0369402069873335, + "y": 1.6858651202909567, + "heading": 2.7558497448937236, + "angularVelocity": -0.05352822298309391, + "velocityX": -1.5305770602598503, + "velocityY": 0.5023306654544649, + "timestamp": 7.297663792860586 + }, + { + "x": 4.899907021964721, + "y": 1.7308359216420997, + "heading": 2.751043871646217, + "angularVelocity": -0.05367590691292534, + "velocityX": -1.5304982267420264, + "velocityY": 0.5022705391525029, + "timestamp": 7.387198810038826 + }, + { + "x": 4.762880958231051, + "y": 1.7758012963934744, + "heading": 2.7462245411420234, + "angularVelocity": -0.05382620851683086, + "velocityX": -1.5304186903866774, + "velocityY": 0.5022099304662124, + "timestamp": 7.476733827217066 + }, + { + "x": 4.625862080529766, + "y": 1.8207612005630536, + "heading": 2.7413915132887583, + "angularVelocity": -0.053979191668033985, + "velocityX": -1.5303384309238324, + "velocityY": 0.5021488305528153, + "timestamp": 7.566268844395306 + }, + { + "x": 4.488850455262, + "y": 1.8657155891757824, + "heading": 2.736544542679806, + "angularVelocity": -0.054134915720223636, + "velocityX": -1.5302574298390257, + "velocityY": 0.5020872283213718, + "timestamp": 7.6558038615735455 + }, + { + "x": 4.351846149940031, + "y": 1.910664415870897, + "heading": 2.7316833801907157, + "angularVelocity": -0.05429342219718387, + "velocityX": -1.5301756747221047, + "velocityY": 0.5020251082951581, + "timestamp": 7.745338878751785 + }, + { + "x": 4.214849232355917, + "y": 1.955607632403858, + "heading": 2.7268077754532425, + "angularVelocity": -0.05445472499064026, + "velocityX": -1.530093162448295, + "velocityY": 0.5019624494346333, + "timestamp": 7.834873895930025 + }, + { + "x": 4.077859769965423, + "y": 2.0005451883147503, + "heading": 2.721917478680163, + "angularVelocity": -0.05461881761126215, + "velocityX": -1.5300098967735323, + "velocityY": 0.5018992269966714, + "timestamp": 7.924408913108265 + }, + { + "x": 3.940877826047202, + "y": 2.045477029542233, + "heading": 2.717012252912238, + "angularVelocity": -0.05478555678567875, + "velocityX": -1.5299259243512187, + "velocityY": 0.5018354007576298, + "timestamp": 8.013943930286505 + }, + { + "x": 3.8039034641003533, + "y": 2.0904031002081847, + "heading": 2.712091859898031, + "angularVelocity": -0.05495495694618896, + "velocityX": -1.5298412427192656, + "velocityY": 0.5017709504262021, + "timestamp": 8.103478947464744 + }, + { + "x": 3.66693674900291, + "y": 2.135323343068443, + "heading": 2.707156056414113, + "angularVelocity": -0.05512707362408464, + "velocityX": -1.5297558364765886, + "velocityY": 0.501705860745354, + "timestamp": 8.193013964642983 + }, + { + "x": 3.5299779535424887, + "y": 2.1802377886608806, + "heading": 2.7022039564715556, + "angularVelocity": -0.05530908574796876, + "velocityX": -1.5296673835196157, + "velocityY": 0.5016411121363245, + "timestamp": 8.282548981821222 + }, + { + "x": 3.426502516345769, + "y": 2.233179068985383, + "heading": 2.594355857349509, + "angularVelocity": -1.2045354155386023, + "velocityX": -1.1556979655315056, + "velocityY": 0.5912913404496402, + "timestamp": 8.37208399899946 + }, + { + "x": 3.387338399887085, + "y": 2.2590601444244385, + "heading": 2.3477629859714533, + "angularVelocity": -2.7541500426269785, + "velocityX": -0.43741675260662705, + "velocityY": 0.28906093118330495, + "timestamp": 8.4616190161777 + }, + { + "x": 3.387338399887085, + "y": 2.2590601444244385, + "heading": 2.3477629859714533, + "angularVelocity": -2.400686558954734e-26, + "velocityX": 2.2132770883951947e-26, + "velocityY": 2.6443990744659133e-26, + "timestamp": 8.551154033355939 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceWing1.1.traj b/src/main/deploy/choreo/SourceWing1.1.traj new file mode 100644 index 00000000..0cfce415 --- /dev/null +++ b/src/main/deploy/choreo/SourceWing1.1.traj @@ -0,0 +1,185 @@ +{ + "samples": [ + { + "x": 0.7885606288909912, + "y": 4.587131977081299, + "heading": 2.068139227089701, + "angularVelocity": -4.000873155660283e-28, + "velocityX": -1.1169568989245428e-27, + "velocityY": -1.0444692137856757e-26, + "timestamp": 0 + }, + { + "x": 0.8077628028564999, + "y": 4.577523858678819, + "heading": 2.29346899836082, + "angularVelocity": 2.9086898452542567, + "velocityX": 0.24787300899124248, + "velocityY": -0.12402727021663285, + "timestamp": 0.07746778902493198 + }, + { + "x": 0.8565084293998517, + "y": 4.542709239371979, + "heading": 2.521391906348417, + "angularVelocity": 2.942163586393862, + "velocityX": 0.6292373534458789, + "velocityY": -0.44940767956648386, + "timestamp": 0.15493557804986396 + }, + { + "x": 0.9320047259554965, + "y": 4.504567603535149, + "heading": 2.6737831770723717, + "angularVelocity": 1.96715657748938, + "velocityX": 0.9745508101612039, + "velocityY": -0.492354775022096, + "timestamp": 0.23240336707479595 + }, + { + "x": 1.0259716426537533, + "y": 4.468035910816095, + "heading": 2.7708019811352167, + "angularVelocity": 1.252376055700012, + "velocityX": 1.2129804900978245, + "velocityY": -0.4715726778686935, + "timestamp": 0.30987115609972793 + }, + { + "x": 1.1315666128977582, + "y": 4.434097705174351, + "heading": 2.8306190294366744, + "angularVelocity": 0.7721538081099244, + "velocityX": 1.3630822768159876, + "velocityY": -0.43809441406442845, + "timestamp": 0.38733894512465994 + }, + { + "x": 1.2441769248956835, + "y": 4.4023522435792275, + "heading": 2.866815836119521, + "angularVelocity": 0.46724977101382253, + "velocityX": 1.4536404538624839, + "velocityY": -0.4097891781177288, + "timestamp": 0.46480673414959195 + }, + { + "x": 1.360949666856534, + "y": 4.372150081143396, + "heading": 2.8884951991985233, + "angularVelocity": 0.2798500299527736, + "velocityX": 1.5073715595945123, + "velocityY": -0.3898673605633771, + "timestamp": 0.542274523174524 + }, + { + "x": 1.483714050289093, + "y": 4.344503608621978, + "heading": 2.888502422435885, + "angularVelocity": 0.00009324181640315531, + "velocityX": 1.584715208446818, + "velocityY": -0.356877004873872, + "timestamp": 0.619742312199456 + }, + { + "x": 1.6064784345242633, + "y": 4.316857140064094, + "heading": 2.88850964608229, + "angularVelocity": 0.00009324709657913404, + "velocityX": 1.5847152188073954, + "velocityY": -0.3568769537102325, + "timestamp": 0.697210101224388 + }, + { + "x": 1.729242818618949, + "y": 4.289210671426577, + "heading": 2.8885168702684654, + "angularVelocity": 0.00009325406426226353, + "velocityX": 1.5847152169939396, + "velocityY": -0.3568769547381692, + "timestamp": 0.77467789024932 + }, + { + "x": 1.8520072025730983, + "y": 4.261564202709402, + "heading": 2.888524094994609, + "angularVelocity": 0.00009326103448260538, + "velocityX": 1.5847152151798087, + "velocityY": -0.3568769557664531, + "timestamp": 0.852145679274252 + }, + { + "x": 1.9747715863866588, + "y": 4.233917733912545, + "heading": 2.8885313202609173, + "angularVelocity": 0.00009326800724451237, + "velocityX": 1.5847152133650135, + "velocityY": -0.35687695679502923, + "timestamp": 0.929613468299184 + }, + { + "x": 2.09753597005958, + "y": 4.206271265035981, + "heading": 2.8885385460675863, + "angularVelocity": 0.00009327498254687209, + "velocityX": 1.584715211549548, + "velocityY": -0.35687695782393064, + "timestamp": 1.007081257324116 + }, + { + "x": 2.2203003530348537, + "y": 4.178624793690813, + "heading": 2.888545772490986, + "angularVelocity": 0.00009328294366610178, + "velocityX": 1.5847152025439089, + "velocityY": -0.3568769896901373, + "timestamp": 1.084549046349048 + }, + { + "x": 2.339535767958966, + "y": 4.149427051618625, + "heading": 2.9014383733628053, + "angularVelocity": 0.16642531088204846, + "velocityX": 1.5391611975105928, + "velocityY": -0.37690170895145225, + "timestamp": 1.16201683537398 + }, + { + "x": 2.4602156964640742, + "y": 4.1208550801313235, + "heading": 2.9091068110983676, + "angularVelocity": 0.0989887259218709, + "velocityX": 1.5578078324433486, + "velocityY": -0.36882389244523134, + "timestamp": 1.239484624398912 + }, + { + "x": 2.563006485689252, + "y": 4.105677497528831, + "heading": 2.9949971538769358, + "angularVelocity": 1.1087233011248803, + "velocityX": 1.3268842511059193, + "velocityY": -0.1959212053619926, + "timestamp": 1.316952413423844 + }, + { + "x": 2.6077051162719727, + "y": 4.099861145019531, + "heading": 3.141592653589793, + "angularVelocity": 1.8923413402914784, + "velocityX": 0.5769963380306965, + "velocityY": -0.07508091533924856, + "timestamp": 1.394420202448776 + }, + { + "x": 2.6077051162719727, + "y": 4.099861145019531, + "heading": 3.141592653589793, + "angularVelocity": 9.529530648984287e-27, + "velocityX": -6.64992161175884e-27, + "velocityY": -1.7807748804280264e-26, + "timestamp": 1.471887991473708 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceWing1.2.traj b/src/main/deploy/choreo/SourceWing1.2.traj new file mode 100644 index 00000000..6a290671 --- /dev/null +++ b/src/main/deploy/choreo/SourceWing1.2.traj @@ -0,0 +1,86 @@ +{ + "samples": [ + { + "x": 2.6077051162719727, + "y": 4.099861145019531, + "heading": 3.141592653589793, + "angularVelocity": 9.529530648984287e-27, + "velocityX": -6.64992161175884e-27, + "velocityY": -1.7807748804280264e-26, + "timestamp": 0 + }, + { + "x": 2.582723056141482, + "y": 4.106470038143139, + "heading": 3.0254564505751103, + "angularVelocity": -1.8328957625774476, + "velocityX": -0.39427423116151483, + "velocityY": 0.10430349785119646, + "timestamp": 0.06336214278294272 + }, + { + "x": 2.5176865762372205, + "y": 4.122909182686089, + "heading": 2.894011234282021, + "angularVelocity": -2.0745071192332674, + "velocityX": -1.0264248816056334, + "velocityY": 0.25944742114017705, + "timestamp": 0.12672428556588544 + }, + { + "x": 2.4297948421318796, + "y": 4.1567674114258075, + "heading": 2.8608182151959642, + "angularVelocity": -0.5238620038429632, + "velocityX": -1.3871332351626442, + "velocityY": 0.5343605385269978, + "timestamp": 0.19008642834882816 + }, + { + "x": 2.336899878456132, + "y": 4.201089714349744, + "heading": 2.8608175146596406, + "angularVelocity": -0.00001105607059469762, + "velocityX": -1.4660956778872583, + "velocityY": 0.6995076393765645, + "timestamp": 0.2534485711317709 + }, + { + "x": 2.2457294091191753, + "y": 4.241992503114134, + "heading": 2.8495777745026225, + "angularVelocity": -0.17738888969588018, + "velocityX": -1.4388792003022337, + "velocityY": 0.6455398597315226, + "timestamp": 0.3168107139147136 + }, + { + "x": 2.180218037779224, + "y": 4.270414402297769, + "heading": 2.7322527229675986, + "angularVelocity": -1.8516585200872955, + "velocityX": -1.033919758117567, + "velocityY": 0.44856278426375645, + "timestamp": 0.3801728566976563 + }, + { + "x": 2.152919054031372, + "y": 4.283941268920898, + "heading": 2.634960813022188, + "angularVelocity": -1.5354895789856624, + "velocityX": -0.4308406021142514, + "velocityY": 0.21348499323117995, + "timestamp": 0.44353499948059905 + }, + { + "x": 2.152919054031372, + "y": 4.283941268920898, + "heading": 2.634960813022188, + "angularVelocity": 1.4290307836372668e-27, + "velocityX": -1.821728341071586e-27, + "velocityY": 7.158150670810205e-27, + "timestamp": 0.5068971422635418 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceWing1.traj b/src/main/deploy/choreo/SourceWing1.traj new file mode 100644 index 00000000..776fd957 --- /dev/null +++ b/src/main/deploy/choreo/SourceWing1.traj @@ -0,0 +1,257 @@ +{ + "samples": [ + { + "x": 0.7885606288909912, + "y": 4.587131977081299, + "heading": 2.068139227089701, + "angularVelocity": -4.000873155660283e-28, + "velocityX": -1.1169568989245428e-27, + "velocityY": -1.0444692137856757e-26, + "timestamp": 0 + }, + { + "x": 0.8077628028564999, + "y": 4.577523858678819, + "heading": 2.29346899836082, + "angularVelocity": 2.9086898452542567, + "velocityX": 0.24787300899124248, + "velocityY": -0.12402727021663285, + "timestamp": 0.07746778902493198 + }, + { + "x": 0.8565084293998517, + "y": 4.542709239371979, + "heading": 2.521391906348417, + "angularVelocity": 2.942163586393862, + "velocityX": 0.6292373534458789, + "velocityY": -0.44940767956648386, + "timestamp": 0.15493557804986396 + }, + { + "x": 0.9320047259554965, + "y": 4.504567603535149, + "heading": 2.6737831770723717, + "angularVelocity": 1.96715657748938, + "velocityX": 0.9745508101612039, + "velocityY": -0.492354775022096, + "timestamp": 0.23240336707479595 + }, + { + "x": 1.0259716426537533, + "y": 4.468035910816095, + "heading": 2.7708019811352167, + "angularVelocity": 1.252376055700012, + "velocityX": 1.2129804900978245, + "velocityY": -0.4715726778686935, + "timestamp": 0.30987115609972793 + }, + { + "x": 1.1315666128977582, + "y": 4.434097705174351, + "heading": 2.8306190294366744, + "angularVelocity": 0.7721538081099244, + "velocityX": 1.3630822768159876, + "velocityY": -0.43809441406442845, + "timestamp": 0.38733894512465994 + }, + { + "x": 1.2441769248956835, + "y": 4.4023522435792275, + "heading": 2.866815836119521, + "angularVelocity": 0.46724977101382253, + "velocityX": 1.4536404538624839, + "velocityY": -0.4097891781177288, + "timestamp": 0.46480673414959195 + }, + { + "x": 1.360949666856534, + "y": 4.372150081143396, + "heading": 2.8884951991985233, + "angularVelocity": 0.2798500299527736, + "velocityX": 1.5073715595945123, + "velocityY": -0.3898673605633771, + "timestamp": 0.542274523174524 + }, + { + "x": 1.483714050289093, + "y": 4.344503608621978, + "heading": 2.888502422435885, + "angularVelocity": 0.00009324181640315531, + "velocityX": 1.584715208446818, + "velocityY": -0.356877004873872, + "timestamp": 0.619742312199456 + }, + { + "x": 1.6064784345242633, + "y": 4.316857140064094, + "heading": 2.88850964608229, + "angularVelocity": 0.00009324709657913404, + "velocityX": 1.5847152188073954, + "velocityY": -0.3568769537102325, + "timestamp": 0.697210101224388 + }, + { + "x": 1.729242818618949, + "y": 4.289210671426577, + "heading": 2.8885168702684654, + "angularVelocity": 0.00009325406426226353, + "velocityX": 1.5847152169939396, + "velocityY": -0.3568769547381692, + "timestamp": 0.77467789024932 + }, + { + "x": 1.8520072025730983, + "y": 4.261564202709402, + "heading": 2.888524094994609, + "angularVelocity": 0.00009326103448260538, + "velocityX": 1.5847152151798087, + "velocityY": -0.3568769557664531, + "timestamp": 0.852145679274252 + }, + { + "x": 1.9747715863866588, + "y": 4.233917733912545, + "heading": 2.8885313202609173, + "angularVelocity": 0.00009326800724451237, + "velocityX": 1.5847152133650135, + "velocityY": -0.35687695679502923, + "timestamp": 0.929613468299184 + }, + { + "x": 2.09753597005958, + "y": 4.206271265035981, + "heading": 2.8885385460675863, + "angularVelocity": 0.00009327498254687209, + "velocityX": 1.584715211549548, + "velocityY": -0.35687695782393064, + "timestamp": 1.007081257324116 + }, + { + "x": 2.2203003530348537, + "y": 4.178624793690813, + "heading": 2.888545772490986, + "angularVelocity": 0.00009328294366610178, + "velocityX": 1.5847152025439089, + "velocityY": -0.3568769896901373, + "timestamp": 1.084549046349048 + }, + { + "x": 2.339535767958966, + "y": 4.149427051618625, + "heading": 2.9014383733628053, + "angularVelocity": 0.16642531088204846, + "velocityX": 1.5391611975105928, + "velocityY": -0.37690170895145225, + "timestamp": 1.16201683537398 + }, + { + "x": 2.4602156964640742, + "y": 4.1208550801313235, + "heading": 2.9091068110983676, + "angularVelocity": 0.0989887259218709, + "velocityX": 1.5578078324433486, + "velocityY": -0.36882389244523134, + "timestamp": 1.239484624398912 + }, + { + "x": 2.563006485689252, + "y": 4.105677497528831, + "heading": 2.9949971538769358, + "angularVelocity": 1.1087233011248803, + "velocityX": 1.3268842511059193, + "velocityY": -0.1959212053619926, + "timestamp": 1.316952413423844 + }, + { + "x": 2.6077051162719727, + "y": 4.099861145019531, + "heading": 3.141592653589793, + "angularVelocity": 1.8923413402914784, + "velocityX": 0.5769963380306965, + "velocityY": -0.07508091533924856, + "timestamp": 1.394420202448776 + }, + { + "x": 2.6077051162719727, + "y": 4.099861145019531, + "heading": 3.141592653589793, + "angularVelocity": 9.529530648984287e-27, + "velocityX": -6.64992161175884e-27, + "velocityY": -1.7807748804280264e-26, + "timestamp": 1.471887991473708 + }, + { + "x": 2.582723056141482, + "y": 4.106470038143139, + "heading": 3.0254564505751103, + "angularVelocity": -1.8328957625774476, + "velocityX": -0.39427423116151483, + "velocityY": 0.10430349785119646, + "timestamp": 1.5352501342566507 + }, + { + "x": 2.5176865762372205, + "y": 4.122909182686089, + "heading": 2.894011234282021, + "angularVelocity": -2.0745071192332674, + "velocityX": -1.0264248816056334, + "velocityY": 0.25944742114017705, + "timestamp": 1.5986122770395934 + }, + { + "x": 2.4297948421318796, + "y": 4.1567674114258075, + "heading": 2.8608182151959642, + "angularVelocity": -0.5238620038429632, + "velocityX": -1.3871332351626442, + "velocityY": 0.5343605385269978, + "timestamp": 1.6619744198225361 + }, + { + "x": 2.336899878456132, + "y": 4.201089714349744, + "heading": 2.8608175146596406, + "angularVelocity": -0.00001105607059469762, + "velocityX": -1.4660956778872583, + "velocityY": 0.6995076393765645, + "timestamp": 1.7253365626054789 + }, + { + "x": 2.2457294091191753, + "y": 4.241992503114134, + "heading": 2.8495777745026225, + "angularVelocity": -0.17738888969588018, + "velocityX": -1.4388792003022337, + "velocityY": 0.6455398597315226, + "timestamp": 1.7886987053884216 + }, + { + "x": 2.180218037779224, + "y": 4.270414402297769, + "heading": 2.7322527229675986, + "angularVelocity": -1.8516585200872955, + "velocityX": -1.033919758117567, + "velocityY": 0.44856278426375645, + "timestamp": 1.8520608481713643 + }, + { + "x": 2.152919054031372, + "y": 4.283941268920898, + "heading": 2.634960813022188, + "angularVelocity": -1.5354895789856624, + "velocityX": -0.4308406021142514, + "velocityY": 0.21348499323117995, + "timestamp": 1.915422990954307 + }, + { + "x": 2.152919054031372, + "y": 4.283941268920898, + "heading": 2.634960813022188, + "angularVelocity": 1.4290307836372668e-27, + "velocityX": -1.821728341071586e-27, + "velocityY": 7.158150670810205e-27, + "timestamp": 1.9787851337372497 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.1.traj b/src/main/deploy/choreo/Test1.1.traj index f107b798..4e9bc5bc 100644 --- a/src/main/deploy/choreo/Test1.1.traj +++ b/src/main/deploy/choreo/Test1.1.traj @@ -1,598 +1,995 @@ { - "samples": [ - { - "x": 1.4869295358657837, - "y": 5.564249038696289, - "heading": 3.141, - "angularVelocity": 3.7405663636389993e-19, - "velocityX": -1.1249906625590385e-18, - "velocityY": 2.3803295269896554e-19, - "timestamp": 0 - }, - { - "x": 1.5092825889004946, - "y": 5.579306288055266, - "heading": 3.4062283948642453, - "angularVelocity": 3.352121601432042, - "velocityX": 0.282511802606821, - "velocityY": 0.19030289294706518, - "timestamp": 0.07912254577845217 - }, - { - "x": 1.5673643497428493, - "y": 5.60987650843196, - "heading": 3.626081158643754, - "angularVelocity": 2.77863612218835, - "velocityX": 0.7340734587204384, - "velocityY": 0.38636547997701703, - "timestamp": 0.15824509155690433 - }, - { - "x": 1.639918967505268, - "y": 5.662681063987355, - "heading": 3.7707265189248944, - "angularVelocity": 1.828118128126927, - "velocityX": 0.9169904361466855, - "velocityY": 0.6673768523986875, - "timestamp": 0.23736763733535649 - }, - { - "x": 1.719103738696596, - "y": 5.732106202864848, - "heading": 3.861411095653706, - "angularVelocity": 1.1461281463659418, - "velocityX": 1.000786443513203, - "velocityY": 0.8774381333973873, - "timestamp": 0.31649018311380867 - }, - { - "x": 1.8011465793982657, - "y": 5.8124573886259565, - "heading": 3.9165727578512475, - "angularVelocity": 0.6971674338183805, - "velocityX": 1.0369085055907419, - "velocityY": 1.0155283171258929, - "timestamp": 0.39561272889226085 - }, - { - "x": 1.8844011608490543, - "y": 5.899562457377884, - "heading": 3.9495869202931786, - "angularVelocity": 0.41725354154267424, - "velocityX": 1.0522232396807132, - "velocityY": 1.1008880957373124, - "timestamp": 0.474735274670713 - }, - { - "x": 1.968168332271066, - "y": 5.990724119399961, - "heading": 3.96923386729424, - "angularVelocity": 0.24831034956931133, - "velocityX": 1.0587016709063517, - "velocityY": 1.1521578473642025, - "timestamp": 0.5538578204491652 - }, - { - "x": 2.05213613980173, - "y": 6.084290626509761, - "heading": 3.9810005346780404, - "angularVelocity": 0.1487144690307097, - "velocityX": 1.0612374349756033, - "velocityY": 1.1825517769839073, - "timestamp": 0.6329803662276173 - }, - { - "x": 2.1361397889017213, - "y": 6.179275610733387, - "heading": 3.988261371091814, - "angularVelocity": 0.0917669716303672, - "velocityX": 1.0616904230458737, - "velocityY": 1.2004793739775566, - "timestamp": 0.7121029120060695 - }, - { - "x": 2.2200564987148903, - "y": 6.2750981229467735, - "heading": 3.9931304198236632, - "angularVelocity": 0.061538069635463585, - "velocityX": 1.0605916302053886, - "velocityY": 1.2110645742073904, - "timestamp": 0.7912254577845216 - }, - { - "x": 2.3037490732526433, - "y": 6.3714204686821265, - "heading": 3.997036412999259, - "angularVelocity": 0.049366373859271756, - "velocityX": 1.0577588690345867, - "velocityY": 1.217381781484405, - "timestamp": 0.8703480035629737 - }, - { - "x": 2.3870186605553965, - "y": 6.4680502574631, - "heading": 4.001128510533661, - "angularVelocity": 0.05171847662561717, - "velocityX": 1.0524128929813872, - "velocityY": 1.221267438127472, - "timestamp": 0.9494705493414258 - }, - { - "x": 2.4696215654523987, - "y": 6.564870888609008, - "heading": 4.006432047635611, - "angularVelocity": 0.06702940419537926, - "velocityX": 1.0439869456209716, - "velocityY": 1.2236794227654333, - "timestamp": 1.028593095119878 - }, - { - "x": 2.5522006651677756, - "y": 6.661687676467445, - "heading": 4.0117852212202445, - "angularVelocity": 0.06765674097018534, - "velocityX": 1.043686080913052, - "velocityY": 1.223630848905317, - "timestamp": 1.1077156408983302 - }, - { - "x": 2.626171983754375, - "y": 6.756004010665758, - "heading": 4.047305777483365, - "angularVelocity": 0.44893090728627943, - "velocityX": 0.9348955832857486, - "velocityY": 1.1920285586159498, - "timestamp": 1.1868381866767823 - }, - { - "x": 2.664895057678222, - "y": 6.813176155090332, - "heading": 3.896073037200959, - "angularVelocity": -1.9113735382816162, - "velocityX": 0.4894063195625013, - "velocityY": 0.7225771600506798, - "timestamp": 1.2659607324552344 - }, - { - "x": 2.67401621750823, - "y": 6.8287147997218245, - "heading": 3.8216991351700837, - "angularVelocity": -2.7569472556880648, - "velocityX": 0.338109953564034, - "velocityY": 0.5759980652369127, - "timestamp": 1.2929376366660208 - }, - { - "x": 2.6786727556459358, - "y": 6.8400421309827735, - "heading": 3.72670670460024, - "angularVelocity": -3.521250245306635, - "velocityX": 0.17261202773019518, - "velocityY": 0.41988996114756666, - "timestamp": 1.3199145408768072 - }, - { - "x": 2.678518823316929, - "y": 6.846619952079936, - "heading": 3.614568928899329, - "angularVelocity": -4.15680668266135, - "velocityX": -0.00570607834774554, - "velocityY": 0.24383157703223057, - "timestamp": 1.3468914450875935 - }, - { - "x": 2.6741686792707893, - "y": 6.846169187557711, - "heading": 3.4948207380964975, - "angularVelocity": -4.438915224191635, - "velocityX": -0.16125438308832984, - "velocityY": -0.0167092754120811, - "timestamp": 1.3738683492983799 - }, - { - "x": 2.663571596898345, - "y": 6.842490941859769, - "heading": 3.391106479438399, - "angularVelocity": -3.844557472114035, - "velocityX": -0.3928205508549501, - "velocityY": -0.1363479541318252, - "timestamp": 1.4008452535091662 - }, - { - "x": 2.648321534336774, - "y": 6.83512618854535, - "heading": 3.3066791856345255, - "angularVelocity": -3.129613878011262, - "velocityX": -0.5653006898944218, - "velocityY": -0.2730021672197161, - "timestamp": 1.4278221577199526 - }, - { - "x": 2.630755187717048, - "y": 6.821913643765559, - "heading": 3.2403542901418882, - "angularVelocity": -2.4585806797690686, - "velocityX": -0.6511624344467275, - "velocityY": -0.4897724615306165, - "timestamp": 1.454799061930739 - }, - { - "x": 2.610177790375068, - "y": 6.803317677931451, - "heading": 3.192043949934185, - "angularVelocity": -1.7908037123240415, - "velocityX": -0.7627783077407181, - "velocityY": -0.6893291271973806, - "timestamp": 1.4817759661415253 - }, - { - "x": 2.585880089062531, - "y": 6.779956801667491, - "heading": 3.1617447456020207, - "angularVelocity": -1.1231534980960616, - "velocityX": -0.9006853092812849, - "velocityY": -0.8659583798581432, - "timestamp": 1.5087528703523116 - }, - { - "x": 2.557481302008236, - "y": 6.752192795634918, - "heading": 3.1494506971418432, - "angularVelocity": -0.4557249550980476, - "velocityX": -1.0527074134380248, - "velocityY": -1.029176877214801, - "timestamp": 1.535729774563098 - }, - { - "x": 2.5262145192081267, - "y": 6.721488912807054, - "heading": 3.1494498734965517, - "angularVelocity": -0.00003053149779678828, - "velocityX": -1.1590204181995478, - "velocityY": -1.1381544223143767, - "timestamp": 1.5627066787738844 - }, - { - "x": 2.494951526671565, - "y": 6.690781099940809, - "heading": 3.1494491983381225, - "angularVelocity": -0.000025027276073123234, - "velocityX": -1.1588799178840683, - "velocityY": -1.1383001039076166, - "timestamp": 1.5896835829846707 - }, - { - "x": 2.46368853251831, - "y": 6.660073288720487, - "heading": 3.149448523179691, - "angularVelocity": -0.000025027276154336604, - "velocityX": -1.1588799778128431, - "velocityY": -1.138300042895271, - "timestamp": 1.616660487195457 - }, - { - "x": 2.4324255384011386, - "y": 6.629365477463429, - "heading": 3.1494478480212726, - "angularVelocity": -0.000025027275679943262, - "velocityX": -1.1588799764752862, - "velocityY": -1.138300044257102, - "timestamp": 1.6436373914062434 - }, - { - "x": 2.401162544284488, - "y": 6.5986576662058365, - "heading": 3.1494471728628666, - "angularVelocity": -0.000025027275214919653, - "velocityX": -1.1588799764559639, - "velocityY": -1.1383000442768603, - "timestamp": 1.6706142956170298 - }, - { - "x": 2.3698995501679407, - "y": 6.567949854948137, - "heading": 3.1494464977044734, - "angularVelocity": -0.000025027274749328804, - "velocityX": -1.1588799764521394, - "velocityY": -1.1383000442808404, - "timestamp": 1.6975911998278161 - }, - { - "x": 2.3386365560514903, - "y": 6.537242043690337, - "heading": 3.1494458225460926, - "angularVelocity": -0.000025027274284044715, - "velocityX": -1.1588799764485511, - "velocityY": -1.1383000442845796, - "timestamp": 1.7245681040386025 - }, - { - "x": 2.307373561935169, - "y": 6.5065342324324025, - "heading": 3.1494451473877243, - "angularVelocity": -0.00002502727381983999, - "velocityX": -1.1588799764437645, - "velocityY": -1.1383000442895392, - "timestamp": 1.7515450082493889 - }, - { - "x": 2.2761105678205396, - "y": 6.475826421172744, - "heading": 3.1494444722293684, - "angularVelocity": -0.00002502727335405626, - "velocityX": -1.158879976381056, - "velocityY": -1.1383000443534677, - "timestamp": 1.7785219124601752 - }, - { - "x": 2.244847573837531, - "y": 6.445118609779082, - "heading": 3.1494437970710263, - "angularVelocity": -0.000025027272855365495, - "velocityX": -1.1588799715020328, - "velocityY": -1.138300049320807, - "timestamp": 1.8054988166709616 - }, - { - "x": 2.2135845753549397, - "y": 6.414410802966355, - "heading": 3.149443121912666, - "angularVelocity": -0.000025027273518628322, - "velocityX": -1.1588801382959264, - "velocityY": -1.1382998795112838, - "timestamp": 1.832475720881748 - }, - { - "x": 2.182332647777799, - "y": 6.3836917291800725, - "heading": 3.14944244669543, - "angularVelocity": -0.000025029455959603075, - "velocityX": -1.1584697537175366, - "velocityY": -1.1387175320883667, - "timestamp": 1.8594526250925343 - }, - { - "x": 2.153094421241092, - "y": 6.351050761951557, - "heading": 3.149440476891046, - "angularVelocity": -0.00007301817765136917, - "velocityX": -1.0838243820804343, - "velocityY": -1.2099597112211005, - "timestamp": 1.8864295293033206 - }, - { - "x": 2.1314747036692956, - "y": 6.314801767366594, - "heading": 3.1444623769659996, - "angularVelocity": -0.18453191983123554, - "velocityX": -0.801415811201077, - "velocityY": -1.3437047595123932, - "timestamp": 1.913406433514107 - }, - { - "x": 2.1175303161219627, - "y": 6.274679449481932, - "heading": 3.139999999999996, - "angularVelocity": -0.16541471664709217, - "velocityX": -0.5169009549193061, - "velocityY": -1.487283995641412, - "timestamp": 1.9403833377248934 - }, - { - "x": 2.111393213272095, - "y": 6.231289386749268, - "heading": 3.139999999999996, - "angularVelocity": 1.2396255317011e-17, - "velocityX": -0.22749470442979314, - "velocityY": -1.6084151981870152, - "timestamp": 1.9673602419356797 - }, - { - "x": 2.1135515859889713, - "y": 6.18871804241084, - "heading": 3.139999999999996, - "angularVelocity": 4.342266970142451e-18, - "velocityX": 0.08225287430098827, - "velocityY": -1.6223404824025023, - "timestamp": 1.9936009385096722 - }, - { - "x": 2.1237593069102005, - "y": 6.1473322849843255, - "heading": 3.1399999999999966, - "angularVelocity": 4.3422681619637e-18, - "velocityX": 0.3890034280317459, - "velocityY": -1.5771592537498764, - "timestamp": 2.0198416350836648 - }, - { - "x": 2.141644452417822, - "y": 6.1086399046863455, - "heading": 3.1399999999999966, - "angularVelocity": 4.3422691347276405e-18, - "velocityX": 0.6815804396497286, - "velocityY": -1.4745180330437861, - "timestamp": 2.0460823316576575 - }, - { - "x": 2.1665552441015765, - "y": 6.0740504370872355, - "heading": 3.1399999999999966, - "angularVelocity": 4.3422696264537505e-18, - "velocityX": 0.9493189943912999, - "velocityY": -1.3181611814907124, - "timestamp": 2.0723230282316503 - }, - { - "x": 2.1935012145011026, - "y": 6.041021760141006, - "heading": 3.139999999999997, - "angularVelocity": 4.34227316559682e-18, - "velocityX": 1.0268770999862573, - "velocityY": -1.2586814093559167, - "timestamp": 2.098563724805643 - }, - { - "x": 2.220442534619127, - "y": 6.00798928987496, - "heading": 3.139999999999997, - "angularVelocity": 4.342268965346379e-18, - "velocityX": 1.0266998835971468, - "velocityY": -1.2588259680112164, - "timestamp": 2.1248044213796358 - }, - { - "x": 2.247383856273639, - "y": 5.974956820862076, - "heading": 3.1399999999999975, - "angularVelocity": 4.342269673161453e-18, - "velocityX": 1.0266999421507437, - "velocityY": -1.2588259202547785, - "timestamp": 2.1510451179536285 - }, - { - "x": 2.274325177898134, - "y": 5.941924351824711, - "heading": 3.1399999999999975, - "angularVelocity": 4.342269691700593e-18, - "velocityX": 1.0266999410068467, - "velocityY": -1.2588259211877415, - "timestamp": 2.1772858145276213 - }, - { - "x": 2.3012664995222347, - "y": 5.908891882787024, - "heading": 3.1399999999999975, - "angularVelocity": 4.3422697108078704e-18, - "velocityX": 1.0266999409917994, - "velocityY": -1.258825921200014, - "timestamp": 2.203526511101614 - }, - { - "x": 2.328207821146328, - "y": 5.87585941374933, - "heading": 3.139999999999998, - "angularVelocity": 4.3422696965810506e-18, - "velocityX": 1.0266999409915183, - "velocityY": -1.2588259212002433, - "timestamp": 2.2297672076756068 - }, - { - "x": 2.355149142770421, - "y": 5.8428269447116365, - "heading": 3.139999999999998, - "angularVelocity": 4.342269684490725e-18, - "velocityX": 1.0266999409915205, - "velocityY": -1.2588259212002417, - "timestamp": 2.2560079042495995 - }, - { - "x": 2.3820904643945138, - "y": 5.809794475673942, - "heading": 3.139999999999998, - "angularVelocity": 4.342269693460261e-18, - "velocityX": 1.026699940991488, - "velocityY": -1.258825921200268, - "timestamp": 2.2822486008235923 - }, - { - "x": 2.4090317860186072, - "y": 5.7767620066362495, - "heading": 3.1399999999999983, - "angularVelocity": 4.342269692405428e-18, - "velocityX": 1.026699940991538, - "velocityY": -1.258825921200227, - "timestamp": 2.308489297397585 - }, - { - "x": 2.4359731076427, - "y": 5.743729537598556, - "heading": 3.1399999999999983, - "angularVelocity": 4.342269716594298e-18, - "velocityX": 1.0266999409914905, - "velocityY": -1.258825921200266, - "timestamp": 2.3347299939715778 - }, - { - "x": 2.4629144292667973, - "y": 5.710697068560865, - "heading": 3.139999999999999, - "angularVelocity": 4.342269681682682e-18, - "velocityX": 1.026699940991683, - "velocityY": -1.258825921200109, - "timestamp": 2.3609706905455705 - }, - { - "x": 2.4898557508911936, - "y": 5.677664599523419, - "heading": 3.139999999999999, - "angularVelocity": 4.3422696993715505e-18, - "velocityX": 1.0266999410030797, - "velocityY": -1.2588259211908142, - "timestamp": 2.3872113871195633 - }, - { - "x": 2.5167970725019893, - "y": 5.644632130474532, - "heading": 3.139999999999999, - "angularVelocity": 4.342269692409483e-18, - "velocityX": 1.026699940484769, - "velocityY": -1.2588259216268345, - "timestamp": 2.413452083693556 - }, - { - "x": 2.5425090771755396, - "y": 5.613106913889164, - "heading": 3.1399999999999992, - "angularVelocity": 4.342265374488911e-18, - "velocityX": 0.9798522154718704, - "velocityY": -1.2013864226688296, - "timestamp": 2.4396927802675488 - }, - { - "x": 2.563078686478754, - "y": 5.587886733807481, - "heading": 3.1399999999999992, - "angularVelocity": 4.342265374369648e-18, - "velocityX": 0.7838819844288143, - "velocityY": -0.9611093977847911, - "timestamp": 2.4659334768415415 - }, - { - "x": 2.578505895291019, - "y": 5.568971596499493, - "heading": 3.1399999999999992, - "angularVelocity": 4.342265374398685e-18, - "velocityX": 0.5879115582455986, - "velocityY": -0.7208321339584491, - "timestamp": 2.4921741734155343 - }, - { - "x": 2.5887907019054897, - "y": 5.5563615040551815, - "heading": 3.1399999999999997, - "angularVelocity": 4.342265374398075e-18, - "velocityX": 0.39194106701667647, - "velocityY": -0.48055479048560223, - "timestamp": 2.518414869989527 - }, - { - "x": 2.5939331054687496, - "y": 5.55005645751953, - "heading": 3.1399999999999997, - "angularVelocity": 4.342265374359544e-18, - "velocityX": 0.19597054326510124, - "velocityY": -0.2402774071896897, - "timestamp": 2.5446555665635198 - }, - { - "x": 2.5939331054687496, - "y": 5.55005645751953, - "heading": 3.14, - "angularVelocity": 3.0510058962998796e-17, - "velocityX": 1.3525026943280928e-16, - "velocityY": 8.359694800915289e-17, - "timestamp": 2.5708962631375125 - } - ] + "samples": [ + { + "x": 1.4869295358657837, + "y": 5.564249038696289, + "heading": 3.141, + "angularVelocity": 3.7405663636389993e-19, + "velocityX": -1.1249906625590385e-18, + "velocityY": 2.3803295269896554e-19, + "timestamp": 0 + }, + { + "x": 1.5092825889004946, + "y": 5.579306288055266, + "heading": 3.4062283948642453, + "angularVelocity": 3.352121601432042, + "velocityX": 0.282511802606821, + "velocityY": 0.19030289294706518, + "timestamp": 0.07912254577845217 + }, + { + "x": 1.5673643497428493, + "y": 5.60987650843196, + "heading": 3.626081158643754, + "angularVelocity": 2.77863612218835, + "velocityX": 0.7340734587204384, + "velocityY": 0.38636547997701703, + "timestamp": 0.15824509155690433 + }, + { + "x": 1.639918967505268, + "y": 5.662681063987355, + "heading": 3.7707265189248944, + "angularVelocity": 1.828118128126927, + "velocityX": 0.9169904361466855, + "velocityY": 0.6673768523986875, + "timestamp": 0.23736763733535649 + }, + { + "x": 1.719103738696596, + "y": 5.732106202864848, + "heading": 3.861411095653706, + "angularVelocity": 1.1461281463659418, + "velocityX": 1.000786443513203, + "velocityY": 0.8774381333973873, + "timestamp": 0.31649018311380867 + }, + { + "x": 1.8011465793982657, + "y": 5.8124573886259565, + "heading": 3.9165727578512475, + "angularVelocity": 0.6971674338183805, + "velocityX": 1.0369085055907419, + "velocityY": 1.0155283171258929, + "timestamp": 0.39561272889226085 + }, + { + "x": 1.8844011608490543, + "y": 5.899562457377884, + "heading": 3.9495869202931786, + "angularVelocity": 0.41725354154267424, + "velocityX": 1.0522232396807132, + "velocityY": 1.1008880957373124, + "timestamp": 0.474735274670713 + }, + { + "x": 1.968168332271066, + "y": 5.990724119399961, + "heading": 3.96923386729424, + "angularVelocity": 0.24831034956931133, + "velocityX": 1.0587016709063517, + "velocityY": 1.1521578473642025, + "timestamp": 0.5538578204491652 + }, + { + "x": 2.05213613980173, + "y": 6.084290626509761, + "heading": 3.9810005346780404, + "angularVelocity": 0.1487144690307097, + "velocityX": 1.0612374349756033, + "velocityY": 1.1825517769839073, + "timestamp": 0.6329803662276173 + }, + { + "x": 2.1361397889017213, + "y": 6.179275610733387, + "heading": 3.988261371091814, + "angularVelocity": 0.0917669716303672, + "velocityX": 1.0616904230458737, + "velocityY": 1.2004793739775566, + "timestamp": 0.7121029120060695 + }, + { + "x": 2.2200564987148903, + "y": 6.2750981229467735, + "heading": 3.9931304198236632, + "angularVelocity": 0.061538069635463585, + "velocityX": 1.0605916302053886, + "velocityY": 1.2110645742073904, + "timestamp": 0.7912254577845216 + }, + { + "x": 2.3037490732526433, + "y": 6.3714204686821265, + "heading": 3.997036412999259, + "angularVelocity": 0.049366373859271756, + "velocityX": 1.0577588690345867, + "velocityY": 1.217381781484405, + "timestamp": 0.8703480035629737 + }, + { + "x": 2.3870186605553965, + "y": 6.4680502574631, + "heading": 4.001128510533661, + "angularVelocity": 0.05171847662561717, + "velocityX": 1.0524128929813872, + "velocityY": 1.221267438127472, + "timestamp": 0.9494705493414258 + }, + { + "x": 2.4696215654523987, + "y": 6.564870888609008, + "heading": 4.006432047635611, + "angularVelocity": 0.06702940419537926, + "velocityX": 1.0439869456209716, + "velocityY": 1.2236794227654333, + "timestamp": 1.028593095119878 + }, + { + "x": 2.5522006651677756, + "y": 6.661687676467445, + "heading": 4.0117852212202445, + "angularVelocity": 0.06765674097018534, + "velocityX": 1.043686080913052, + "velocityY": 1.223630848905317, + "timestamp": 1.1077156408983302 + }, + { + "x": 2.626171983754375, + "y": 6.756004010665758, + "heading": 4.047305777483365, + "angularVelocity": 0.44893090728627943, + "velocityX": 0.9348955832857486, + "velocityY": 1.1920285586159498, + "timestamp": 1.1868381866767823 + }, + { + "x": 2.664895057678222, + "y": 6.813176155090332, + "heading": 3.896073037200959, + "angularVelocity": -1.9113735382816162, + "velocityX": 0.4894063195625013, + "velocityY": 0.7225771600506798, + "timestamp": 1.2659607324552344 + }, + { + "x": 2.67401621750823, + "y": 6.8287147997218245, + "heading": 3.8216991351700837, + "angularVelocity": -2.7569472556880648, + "velocityX": 0.338109953564034, + "velocityY": 0.5759980652369127, + "timestamp": 1.2929376366660208 + }, + { + "x": 2.6786727556459358, + "y": 6.8400421309827735, + "heading": 3.72670670460024, + "angularVelocity": -3.521250245306635, + "velocityX": 0.17261202773019518, + "velocityY": 0.41988996114756666, + "timestamp": 1.3199145408768072 + }, + { + "x": 2.678518823316929, + "y": 6.846619952079936, + "heading": 3.614568928899329, + "angularVelocity": -4.15680668266135, + "velocityX": -0.00570607834774554, + "velocityY": 0.24383157703223057, + "timestamp": 1.3468914450875935 + }, + { + "x": 2.6741686792707893, + "y": 6.846169187557711, + "heading": 3.4948207380964975, + "angularVelocity": -4.438915224191635, + "velocityX": -0.16125438308832984, + "velocityY": -0.0167092754120811, + "timestamp": 1.3738683492983799 + }, + { + "x": 2.663571596898345, + "y": 6.842490941859769, + "heading": 3.391106479438399, + "angularVelocity": -3.844557472114035, + "velocityX": -0.3928205508549501, + "velocityY": -0.1363479541318252, + "timestamp": 1.4008452535091662 + }, + { + "x": 2.648321534336774, + "y": 6.83512618854535, + "heading": 3.3066791856345255, + "angularVelocity": -3.129613878011262, + "velocityX": -0.5653006898944218, + "velocityY": -0.2730021672197161, + "timestamp": 1.4278221577199526 + }, + { + "x": 2.630755187717048, + "y": 6.821913643765559, + "heading": 3.2403542901418882, + "angularVelocity": -2.4585806797690686, + "velocityX": -0.6511624344467275, + "velocityY": -0.4897724615306165, + "timestamp": 1.454799061930739 + }, + { + "x": 2.610177790375068, + "y": 6.803317677931451, + "heading": 3.192043949934185, + "angularVelocity": -1.7908037123240415, + "velocityX": -0.7627783077407181, + "velocityY": -0.6893291271973806, + "timestamp": 1.4817759661415253 + }, + { + "x": 2.585880089062531, + "y": 6.779956801667491, + "heading": 3.1617447456020207, + "angularVelocity": -1.1231534980960616, + "velocityX": -0.9006853092812849, + "velocityY": -0.8659583798581432, + "timestamp": 1.5087528703523116 + }, + { + "x": 2.557481302008236, + "y": 6.752192795634918, + "heading": 3.1494506971418432, + "angularVelocity": -0.4557249550980476, + "velocityX": -1.0527074134380248, + "velocityY": -1.029176877214801, + "timestamp": 1.535729774563098 + }, + { + "x": 2.5262145192081267, + "y": 6.721488912807054, + "heading": 3.1494498734965517, + "angularVelocity": -0.00003053149779678828, + "velocityX": -1.1590204181995478, + "velocityY": -1.1381544223143767, + "timestamp": 1.5627066787738844 + }, + { + "x": 2.494951526671565, + "y": 6.690781099940809, + "heading": 3.1494491983381225, + "angularVelocity": -0.000025027276073123234, + "velocityX": -1.1588799178840683, + "velocityY": -1.1383001039076166, + "timestamp": 1.5896835829846707 + }, + { + "x": 2.46368853251831, + "y": 6.660073288720487, + "heading": 3.149448523179691, + "angularVelocity": -0.000025027276154336604, + "velocityX": -1.1588799778128431, + "velocityY": -1.138300042895271, + "timestamp": 1.616660487195457 + }, + { + "x": 2.4324255384011386, + "y": 6.629365477463429, + "heading": 3.1494478480212726, + "angularVelocity": -0.000025027275679943262, + "velocityX": -1.1588799764752862, + "velocityY": -1.138300044257102, + "timestamp": 1.6436373914062434 + }, + { + "x": 2.401162544284488, + "y": 6.5986576662058365, + "heading": 3.1494471728628666, + "angularVelocity": -0.000025027275214919653, + "velocityX": -1.1588799764559639, + "velocityY": -1.1383000442768603, + "timestamp": 1.6706142956170298 + }, + { + "x": 2.3698995501679407, + "y": 6.567949854948137, + "heading": 3.1494464977044734, + "angularVelocity": -0.000025027274749328804, + "velocityX": -1.1588799764521394, + "velocityY": -1.1383000442808404, + "timestamp": 1.6975911998278161 + }, + { + "x": 2.3386365560514903, + "y": 6.537242043690337, + "heading": 3.1494458225460926, + "angularVelocity": -0.000025027274284044715, + "velocityX": -1.1588799764485511, + "velocityY": -1.1383000442845796, + "timestamp": 1.7245681040386025 + }, + { + "x": 2.307373561935169, + "y": 6.5065342324324025, + "heading": 3.1494451473877243, + "angularVelocity": -0.00002502727381983999, + "velocityX": -1.1588799764437645, + "velocityY": -1.1383000442895392, + "timestamp": 1.7515450082493889 + }, + { + "x": 2.2761105678205396, + "y": 6.475826421172744, + "heading": 3.1494444722293684, + "angularVelocity": -0.00002502727335405626, + "velocityX": -1.158879976381056, + "velocityY": -1.1383000443534677, + "timestamp": 1.7785219124601752 + }, + { + "x": 2.244847573837531, + "y": 6.445118609779082, + "heading": 3.1494437970710263, + "angularVelocity": -0.000025027272855365495, + "velocityX": -1.1588799715020328, + "velocityY": -1.138300049320807, + "timestamp": 1.8054988166709616 + }, + { + "x": 2.2135845753549397, + "y": 6.414410802966355, + "heading": 3.149443121912666, + "angularVelocity": -0.000025027273518628322, + "velocityX": -1.1588801382959264, + "velocityY": -1.1382998795112838, + "timestamp": 1.832475720881748 + }, + { + "x": 2.182332647777799, + "y": 6.3836917291800725, + "heading": 3.14944244669543, + "angularVelocity": -0.000025029455959603075, + "velocityX": -1.1584697537175366, + "velocityY": -1.1387175320883667, + "timestamp": 1.8594526250925343 + }, + { + "x": 2.153094421241092, + "y": 6.351050761951557, + "heading": 3.149440476891046, + "angularVelocity": -0.00007301817765136917, + "velocityX": -1.0838243820804343, + "velocityY": -1.2099597112211005, + "timestamp": 1.8864295293033206 + }, + { + "x": 2.1314747036692956, + "y": 6.314801767366594, + "heading": 3.1444623769659996, + "angularVelocity": -0.18453191983123554, + "velocityX": -0.801415811201077, + "velocityY": -1.3437047595123932, + "timestamp": 1.913406433514107 + }, + { + "x": 2.1175303161219627, + "y": 6.274679449481932, + "heading": 3.139999999999996, + "angularVelocity": -0.16541471664709217, + "velocityX": -0.5169009549193061, + "velocityY": -1.487283995641412, + "timestamp": 1.9403833377248934 + }, + { + "x": 2.111393213272095, + "y": 6.231289386749268, + "heading": 3.139999999999996, + "angularVelocity": 1.2396255317011e-17, + "velocityX": -0.22749470442979314, + "velocityY": -1.6084151981870152, + "timestamp": 1.9673602419356797 + }, + { + "x": 2.1135515859889713, + "y": 6.18871804241084, + "heading": 3.139999999999996, + "angularVelocity": 4.342266970142451e-18, + "velocityX": 0.08225287430098827, + "velocityY": -1.6223404824025023, + "timestamp": 1.9936009385096722 + }, + { + "x": 2.1237593069102005, + "y": 6.1473322849843255, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422681619637e-18, + "velocityX": 0.3890034280317459, + "velocityY": -1.5771592537498764, + "timestamp": 2.0198416350836648 + }, + { + "x": 2.141644452417822, + "y": 6.1086399046863455, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422691347276405e-18, + "velocityX": 0.6815804396497286, + "velocityY": -1.4745180330437861, + "timestamp": 2.0460823316576575 + }, + { + "x": 2.1665552441015765, + "y": 6.0740504370872355, + "heading": 3.1399999999999966, + "angularVelocity": 4.3422696264537505e-18, + "velocityX": 0.9493189943912999, + "velocityY": -1.3181611814907124, + "timestamp": 2.0723230282316503 + }, + { + "x": 2.1935012145011026, + "y": 6.041021760141006, + "heading": 3.139999999999997, + "angularVelocity": 4.34227316559682e-18, + "velocityX": 1.0268770999862573, + "velocityY": -1.2586814093559167, + "timestamp": 2.098563724805643 + }, + { + "x": 2.220442534619127, + "y": 6.00798928987496, + "heading": 3.139999999999997, + "angularVelocity": 4.342268965346379e-18, + "velocityX": 1.0266998835971468, + "velocityY": -1.2588259680112164, + "timestamp": 2.1248044213796358 + }, + { + "x": 2.247383856273639, + "y": 5.974956820862076, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269673161453e-18, + "velocityX": 1.0266999421507437, + "velocityY": -1.2588259202547785, + "timestamp": 2.1510451179536285 + }, + { + "x": 2.274325177898134, + "y": 5.941924351824711, + "heading": 3.1399999999999975, + "angularVelocity": 4.342269691700593e-18, + "velocityX": 1.0266999410068467, + "velocityY": -1.2588259211877415, + "timestamp": 2.1772858145276213 + }, + { + "x": 2.3012664995222347, + "y": 5.908891882787024, + "heading": 3.1399999999999975, + "angularVelocity": 4.3422697108078704e-18, + "velocityX": 1.0266999409917994, + "velocityY": -1.258825921200014, + "timestamp": 2.203526511101614 + }, + { + "x": 2.328207821146328, + "y": 5.87585941374933, + "heading": 3.139999999999998, + "angularVelocity": 4.3422696965810506e-18, + "velocityX": 1.0266999409915183, + "velocityY": -1.2588259212002433, + "timestamp": 2.2297672076756068 + }, + { + "x": 2.355149142770421, + "y": 5.8428269447116365, + "heading": 3.139999999999998, + "angularVelocity": 4.342269684490725e-18, + "velocityX": 1.0266999409915205, + "velocityY": -1.2588259212002417, + "timestamp": 2.2560079042495995 + }, + { + "x": 2.3820904643945138, + "y": 5.809794475673942, + "heading": 3.139999999999998, + "angularVelocity": 4.342269693460261e-18, + "velocityX": 1.026699940991488, + "velocityY": -1.258825921200268, + "timestamp": 2.2822486008235923 + }, + { + "x": 2.4090317860186072, + "y": 5.7767620066362495, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269692405428e-18, + "velocityX": 1.026699940991538, + "velocityY": -1.258825921200227, + "timestamp": 2.308489297397585 + }, + { + "x": 2.4359731076427, + "y": 5.743729537598556, + "heading": 3.1399999999999983, + "angularVelocity": 4.342269716594298e-18, + "velocityX": 1.0266999409914905, + "velocityY": -1.258825921200266, + "timestamp": 2.3347299939715778 + }, + { + "x": 2.4629144292667973, + "y": 5.710697068560865, + "heading": 3.139999999999999, + "angularVelocity": 4.342269681682682e-18, + "velocityX": 1.026699940991683, + "velocityY": -1.258825921200109, + "timestamp": 2.3609706905455705 + }, + { + "x": 2.4898557508911936, + "y": 5.677664599523419, + "heading": 3.139999999999999, + "angularVelocity": 4.3422696993715505e-18, + "velocityX": 1.0266999410030797, + "velocityY": -1.2588259211908142, + "timestamp": 2.3872113871195633 + }, + { + "x": 2.5167970725019893, + "y": 5.644632130474532, + "heading": 3.139999999999999, + "angularVelocity": 4.342269692409483e-18, + "velocityX": 1.026699940484769, + "velocityY": -1.2588259216268345, + "timestamp": 2.413452083693556 + }, + { + "x": 2.5425090771755396, + "y": 5.613106913889164, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374488911e-18, + "velocityX": 0.9798522154718704, + "velocityY": -1.2013864226688296, + "timestamp": 2.4396927802675488 + }, + { + "x": 2.563078686478754, + "y": 5.587886733807481, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374369648e-18, + "velocityX": 0.7838819844288143, + "velocityY": -0.9611093977847911, + "timestamp": 2.4659334768415415 + }, + { + "x": 2.578505895291019, + "y": 5.568971596499493, + "heading": 3.1399999999999992, + "angularVelocity": 4.342265374398685e-18, + "velocityX": 0.5879115582455986, + "velocityY": -0.7208321339584491, + "timestamp": 2.4921741734155343 + }, + { + "x": 2.5887907019054897, + "y": 5.5563615040551815, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374398075e-18, + "velocityX": 0.39194106701667647, + "velocityY": -0.48055479048560223, + "timestamp": 2.518414869989527 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, + "heading": 3.1399999999999997, + "angularVelocity": 4.342265374359544e-18, + "velocityX": 0.19597054326510124, + "velocityY": -0.2402774071896897, + "timestamp": 2.5446555665635198 + }, + { + "x": 2.5939331054687496, + "y": 5.55005645751953, + "heading": 3.14, + "angularVelocity": 3.0510058962998796e-17, + "velocityX": 1.3525026943280928e-16, + "velocityY": 8.359694800915289e-17, + "timestamp": 2.5708962631375125 + }, + { + "x": 2.5906368423225947, + "y": 5.5485199070890205, + "heading": 3.101591851410996, + "angularVelocity": -1.2805791583146104, + "velocityX": -0.10990183178303778, + "velocityY": -0.05123065102809662, + "timestamp": 2.60088905845952 + }, + { + "x": 2.584621501308477, + "y": 5.544975862182894, + "heading": 3.0243371709692783, + "angularVelocity": -2.5757746022741834, + "velocityX": -0.20055953270102006, + "velocityY": -0.11816320779855842, + "timestamp": 2.6308818537815273 + }, + { + "x": 2.5759613994637824, + "y": 5.536844199254011, + "heading": 2.9135350884263085, + "angularVelocity": -3.6942899570776517, + "velocityX": -0.28873940397378167, + "velocityY": -0.2711205421688905, + "timestamp": 2.6608746491035347 + }, + { + "x": 2.56275074361348, + "y": 5.521892690270415, + "heading": 2.8248688434153846, + "angularVelocity": -2.956251461684603, + "velocityX": -0.44046097433337356, + "velocityY": -0.49850335131840534, + "timestamp": 2.690867444425542 + }, + { + "x": 2.543962903792877, + "y": 5.501410245111836, + "heading": 2.7589030524712514, + "angularVelocity": -2.199387894182165, + "velocityX": -0.6264117638620101, + "velocityY": -0.6829121773583486, + "timestamp": 2.7208602397475494 + }, + { + "x": 2.518778715441927, + "y": 5.477002144640871, + "heading": 2.7154691851313926, + "angularVelocity": -1.448143358205586, + "velocityX": -0.839674597873837, + "velocityY": -0.8137987876364632, + "timestamp": 2.750853035069557 + }, + { + "x": 2.486733800013198, + "y": 5.449761748396881, + "heading": 2.6943182531197865, + "angularVelocity": -0.705200425130341, + "velocityX": -1.0684204351295292, + "velocityY": -0.9082313252703665, + "timestamp": 2.780845830391564 + }, + { + "x": 2.4478196867061426, + "y": 5.4204460978412365, + "heading": 2.694317441639593, + "angularVelocity": -0.000027055837428819432, + "velocityX": -1.2974487002285997, + "velocityY": -0.9774230858209427, + "timestamp": 2.8108386257135716 + }, + { + "x": 2.408901839144878, + "y": 5.391135398464328, + "heading": 2.6943166423252785, + "angularVelocity": -0.000026650210696695328, + "velocityX": -1.2975732052797133, + "velocityY": -0.9772580068715555, + "timestamp": 2.840831421035579 + }, + { + "x": 2.3699840051756724, + "y": 5.361824681040097, + "heading": 2.6943158430110494, + "angularVelocity": -0.00002665020784043696, + "velocityX": -1.2975727521022344, + "velocityY": -0.9772586085935405, + "timestamp": 2.8708242163575863 + }, + { + "x": 2.3310661711945366, + "y": 5.332513963631788, + "heading": 2.6943150436964616, + "angularVelocity": -0.000026650219800690006, + "velocityX": -1.297572752499997, + "velocityY": -0.9772586080626477, + "timestamp": 2.9008170116795937 + }, + { + "x": 2.292148337213513, + "y": 5.303203246223414, + "heading": 2.694314244381513, + "angularVelocity": -0.000026650231834296552, + "velocityX": -1.2975727524962546, + "velocityY": -0.9772586080648237, + "timestamp": 2.930809807001601 + }, + { + "x": 2.25323050323266, + "y": 5.273892528814898, + "heading": 2.6943134450662036, + "angularVelocity": -0.00002665024386857193, + "velocityX": -1.297572752490578, + "velocityY": -0.9772586080695672, + "timestamp": 2.9608026023236085 + }, + { + "x": 2.214312669251978, + "y": 5.2445818114062375, + "heading": 2.694312645750533, + "angularVelocity": -0.00002665025590264624, + "velocityX": -1.2975727524848706, + "velocityY": -0.9772586080743522, + "timestamp": 2.990795397645616 + }, + { + "x": 2.1753948352714723, + "y": 5.215271093997427, + "heading": 2.694311846434502, + "angularVelocity": -0.000026650267936849635, + "velocityX": -1.2975727524790048, + "velocityY": -0.9772586080793467, + "timestamp": 3.0207881929676232 + }, + { + "x": 2.136477001291495, + "y": 5.185960376588, + "heading": 2.6943110471181098, + "angularVelocity": -0.000026650279973527825, + "velocityX": -1.2975727524613627, + "velocityY": -0.9772586080999772, + "timestamp": 3.0507809882896306 + }, + { + "x": 2.097559167357152, + "y": 5.1566496591180675, + "heading": 2.694310247801348, + "angularVelocity": -0.000026650292283470146, + "velocityX": -1.2975727509398636, + "velocityY": -0.9772586101172457, + "timestamp": 3.080773783611638 + }, + { + "x": 2.058641302557044, + "y": 5.127338982627939, + "heading": 2.694309448489961, + "angularVelocity": -0.000026650113093683626, + "velocityX": -1.2975737800459084, + "velocityY": -0.9772572437956306, + "timestamp": 3.1107665789336454 + }, + { + "x": 2.0197569370912083, + "y": 5.0979839038638834, + "heading": 2.6943086027536207, + "angularVelocity": -0.000028197983267341566, + "velocityX": -1.2964568673313885, + "velocityY": -0.9787376751348899, + "timestamp": 3.1407593742556528 + }, + { + "x": 1.9877778714785685, + "y": 5.067888184962114, + "heading": 2.678621118987198, + "angularVelocity": -0.5230417371285907, + "velocityX": -1.0662249139900737, + "velocityY": -1.0034316101155079, + "timestamp": 3.17075216957766 + }, + { + "x": 1.9640654183560038, + "y": 5.031205594348394, + "heading": 2.6596531558314034, + "angularVelocity": -0.6324173173088112, + "velocityX": -0.7906049725553251, + "velocityY": -1.223046742388833, + "timestamp": 3.2007449648996675 + }, + { + "x": 1.9481708008996033, + "y": 4.987987936601904, + "heading": 2.649783348569472, + "angularVelocity": -0.32907260414160233, + "velocityX": -0.5299478519964009, + "velocityY": -1.4409346405525878, + "timestamp": 3.230737760221675 + }, + { + "x": 1.9410848617553718, + "y": 4.939785003662109, + "heading": 2.649783348569472, + "angularVelocity": 6.681257540282107e-18, + "velocityX": -0.23625470944103766, + "velocityY": -1.6071503980304298, + "timestamp": 3.2607305555436823 + }, + { + "x": 1.9456173734407256, + "y": 4.888006870907468, + "heading": 2.649783348569472, + "angularVelocity": -1.484655388526889e-19, + "velocityX": 0.14165581617647935, + "velocityY": -1.6182360167593688, + "timestamp": 3.292727206352592 + }, + { + "x": 1.9619962278910381, + "y": 4.838678856610239, + "heading": 2.649783348569472, + "angularVelocity": -1.4846556369937314e-19, + "velocityX": 0.5118927774045035, + "velocityY": -1.5416618005383362, + "timestamp": 3.324723857161502 + }, + { + "x": 1.9893341653194017, + "y": 4.794473003544163, + "heading": 2.649783348569472, + "angularVelocity": -1.4846558519027202e-19, + "velocityX": 0.8543999680324345, + "velocityY": -1.381577507287736, + "timestamp": 3.3567205079704117 + }, + { + "x": 2.026135553103086, + "y": 4.757769076744815, + "heading": 2.649783348569472, + "angularVelocity": -1.4846559448429508e-19, + "velocityX": 1.1501637469359431, + "velocityY": -1.1471177723711978, + "timestamp": 3.3887171587793214 + }, + { + "x": 2.0637808893572873, + "y": 4.721931260837698, + "heading": 2.649783348569472, + "angularVelocity": -1.4846636373679086e-19, + "velocityX": 1.1765398972283652, + "velocityY": -1.120048973905845, + "timestamp": 3.4207138095882312 + }, + { + "x": 2.1014301321789435, + "y": 4.6860975489840255, + "heading": 2.649783348569472, + "angularVelocity": -1.484648310808098e-19, + "velocityX": 1.1766619902345172, + "velocityY": -1.1199207088167735, + "timestamp": 3.452710460397141 + }, + { + "x": 2.139079370280558, + "y": 4.650263832171171, + "heading": 2.649783348569472, + "angularVelocity": -1.48465589810743e-19, + "velocityX": 1.1766618427177973, + "velocityY": -1.1199208638074638, + "timestamp": 3.484707111206051 + }, + { + "x": 2.17672860838618, + "y": 4.614430115362526, + "heading": 2.649783348569472, + "angularVelocity": -1.4846559463647802e-19, + "velocityX": 1.1766618428430395, + "velocityY": -1.1199208636758753, + "timestamp": 3.5167037620149606 + }, + { + "x": 2.2143778464918284, + "y": 4.57859639855391, + "heading": 2.649783348569472, + "angularVelocity": -1.4846559409367017e-19, + "velocityX": 1.176661842843871, + "velocityY": -1.1199208636750018, + "timestamp": 3.5487004128238704 + }, + { + "x": 2.2520270845974713, + "y": 4.5427626817452875, + "heading": 2.649783348569472, + "angularVelocity": -1.4846559515966518e-19, + "velocityX": 1.1766618428436975, + "velocityY": -1.1199208636751843, + "timestamp": 3.58069706363278 + }, + { + "x": 2.289676322703114, + "y": 4.506928964936664, + "heading": 2.649783348569472, + "angularVelocity": -1.4846559610313848e-19, + "velocityX": 1.1766618428436995, + "velocityY": -1.1199208636751825, + "timestamp": 3.61269371444169 + }, + { + "x": 2.3273255608087573, + "y": 4.471095248128042, + "heading": 2.649783348569472, + "angularVelocity": -1.484655947429445e-19, + "velocityX": 1.1766618428436957, + "velocityY": -1.1199208636751863, + "timestamp": 3.6446903652505998 + }, + { + "x": 2.3649747989143997, + "y": 4.435261531319418, + "heading": 2.649783348569472, + "angularVelocity": -1.4846559574023035e-19, + "velocityX": 1.1766618428436864, + "velocityY": -1.1199208636751963, + "timestamp": 3.6766870160595095 + }, + { + "x": 2.4026240370200447, + "y": 4.399427814510799, + "heading": 2.649783348569472, + "angularVelocity": -1.484655924580371e-19, + "velocityX": 1.1766618428437539, + "velocityY": -1.1199208636751252, + "timestamp": 3.7086836668684193 + }, + { + "x": 2.4402732751256884, + "y": 4.363594097702177, + "heading": 2.649783348569472, + "angularVelocity": -1.4846559654067578e-19, + "velocityX": 1.1766618428437225, + "velocityY": -1.1199208636751585, + "timestamp": 3.740680317677329 + }, + { + "x": 2.4779225132411047, + "y": 4.327760380902407, + "heading": 2.649783348569472, + "angularVelocity": -1.4846559492892923e-19, + "velocityX": 1.1766618431491565, + "velocityY": -1.1199208633985085, + "timestamp": 3.772676968486239 + }, + { + "x": 2.5129728702731393, + "y": 4.294400221963306, + "heading": 2.649783348569472, + "angularVelocity": -1.484655203554499e-19, + "velocityX": 1.0954383082580312, + "velocityY": -1.0426140891540008, + "timestamp": 3.8046736192951487 + }, + { + "x": 2.53926064365096, + "y": 4.2693800974206075, + "heading": 2.649783348569472, + "angularVelocity": -1.484655203554444e-19, + "velocityX": 0.8215789063304317, + "velocityY": -0.7819607337071065, + "timestamp": 3.8366702701040585 + }, + { + "x": 2.5567858275270727, + "y": 4.252700012844841, + "heading": 2.649783348569472, + "angularVelocity": -1.4846552035544855e-19, + "velocityX": 0.5477193216495216, + "velocityY": -0.5213072041629537, + "timestamp": 3.8686669209129683 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": -1.4846552035527083e-19, + "velocityX": 0.2738596760533411, + "velocityY": -0.260653616588761, + "timestamp": 3.900663571721878 + }, + { + "x": 2.5655484199523926, + "y": 4.244359970092773, + "heading": 2.649783348569472, + "angularVelocity": -4.948850678520552e-20, + "velocityX": 6.0427487543509175e-18, + "velocityY": -9.335003878092332e-18, + "timestamp": 3.932660222530788 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test1.traj b/src/main/deploy/choreo/Test1.traj deleted file mode 100644 index a894f9a7..00000000 --- a/src/main/deploy/choreo/Test1.traj +++ /dev/null @@ -1,994 +0,0 @@ -{ - "samples": [ - { - "x": 1.4869295358657837, - "y": 5.564249038696289, - "heading": 3.141, - "angularVelocity": 3.7405663636389993e-19, - "velocityX": -1.1249906625590385e-18, - "velocityY": 2.3803295269896554e-19, - "timestamp": 0 - }, - { - "x": 1.5092825889004946, - "y": 5.579306288055266, - "heading": 3.4062283948642453, - "angularVelocity": 3.352121601432042, - "velocityX": 0.282511802606821, - "velocityY": 0.19030289294706518, - "timestamp": 0.07912254577845217 - }, - { - "x": 1.5673643497428493, - "y": 5.60987650843196, - "heading": 3.626081158643754, - "angularVelocity": 2.77863612218835, - "velocityX": 0.7340734587204384, - "velocityY": 0.38636547997701703, - "timestamp": 0.15824509155690433 - }, - { - "x": 1.639918967505268, - "y": 5.662681063987355, - "heading": 3.7707265189248944, - "angularVelocity": 1.828118128126927, - "velocityX": 0.9169904361466855, - "velocityY": 0.6673768523986875, - "timestamp": 0.23736763733535649 - }, - { - "x": 1.719103738696596, - "y": 5.732106202864848, - "heading": 3.861411095653706, - "angularVelocity": 1.1461281463659418, - "velocityX": 1.000786443513203, - "velocityY": 0.8774381333973873, - "timestamp": 0.31649018311380867 - }, - { - "x": 1.8011465793982657, - "y": 5.8124573886259565, - "heading": 3.9165727578512475, - "angularVelocity": 0.6971674338183805, - "velocityX": 1.0369085055907419, - "velocityY": 1.0155283171258929, - "timestamp": 0.39561272889226085 - }, - { - "x": 1.8844011608490543, - "y": 5.899562457377884, - "heading": 3.9495869202931786, - "angularVelocity": 0.41725354154267424, - "velocityX": 1.0522232396807132, - "velocityY": 1.1008880957373124, - "timestamp": 0.474735274670713 - }, - { - "x": 1.968168332271066, - "y": 5.990724119399961, - "heading": 3.96923386729424, - "angularVelocity": 0.24831034956931133, - "velocityX": 1.0587016709063517, - "velocityY": 1.1521578473642025, - "timestamp": 0.5538578204491652 - }, - { - "x": 2.05213613980173, - "y": 6.084290626509761, - "heading": 3.9810005346780404, - "angularVelocity": 0.1487144690307097, - "velocityX": 1.0612374349756033, - "velocityY": 1.1825517769839073, - "timestamp": 0.6329803662276173 - }, - { - "x": 2.1361397889017213, - "y": 6.179275610733387, - "heading": 3.988261371091814, - "angularVelocity": 0.0917669716303672, - "velocityX": 1.0616904230458737, - "velocityY": 1.2004793739775566, - "timestamp": 0.7121029120060695 - }, - { - "x": 2.2200564987148903, - "y": 6.2750981229467735, - "heading": 3.9931304198236632, - "angularVelocity": 0.061538069635463585, - "velocityX": 1.0605916302053886, - "velocityY": 1.2110645742073904, - "timestamp": 0.7912254577845216 - }, - { - "x": 2.3037490732526433, - "y": 6.3714204686821265, - "heading": 3.997036412999259, - "angularVelocity": 0.049366373859271756, - "velocityX": 1.0577588690345867, - "velocityY": 1.217381781484405, - "timestamp": 0.8703480035629737 - }, - { - "x": 2.3870186605553965, - "y": 6.4680502574631, - "heading": 4.001128510533661, - "angularVelocity": 0.05171847662561717, - "velocityX": 1.0524128929813872, - "velocityY": 1.221267438127472, - "timestamp": 0.9494705493414258 - }, - { - "x": 2.4696215654523987, - "y": 6.564870888609008, - "heading": 4.006432047635611, - "angularVelocity": 0.06702940419537926, - "velocityX": 1.0439869456209716, - "velocityY": 1.2236794227654333, - "timestamp": 1.028593095119878 - }, - { - "x": 2.5522006651677756, - "y": 6.661687676467445, - "heading": 4.0117852212202445, - "angularVelocity": 0.06765674097018534, - "velocityX": 1.043686080913052, - "velocityY": 1.223630848905317, - "timestamp": 1.1077156408983302 - }, - { - "x": 2.626171983754375, - "y": 6.756004010665758, - "heading": 4.047305777483365, - "angularVelocity": 0.44893090728627943, - "velocityX": 0.9348955832857486, - "velocityY": 1.1920285586159498, - "timestamp": 1.1868381866767823 - }, - { - "x": 2.664895057678222, - "y": 6.813176155090332, - "heading": 3.896073037200959, - "angularVelocity": -1.9113735382816162, - "velocityX": 0.4894063195625013, - "velocityY": 0.7225771600506798, - "timestamp": 1.2659607324552344 - }, - { - "x": 2.67401621750823, - "y": 6.8287147997218245, - "heading": 3.8216991351700837, - "angularVelocity": -2.7569472556880648, - "velocityX": 0.338109953564034, - "velocityY": 0.5759980652369127, - "timestamp": 1.2929376366660208 - }, - { - "x": 2.6786727556459358, - "y": 6.8400421309827735, - "heading": 3.72670670460024, - "angularVelocity": -3.521250245306635, - "velocityX": 0.17261202773019518, - "velocityY": 0.41988996114756666, - "timestamp": 1.3199145408768072 - }, - { - "x": 2.678518823316929, - "y": 6.846619952079936, - "heading": 3.614568928899329, - "angularVelocity": -4.15680668266135, - "velocityX": -0.00570607834774554, - "velocityY": 0.24383157703223057, - "timestamp": 1.3468914450875935 - }, - { - "x": 2.6741686792707893, - "y": 6.846169187557711, - "heading": 3.4948207380964975, - "angularVelocity": -4.438915224191635, - "velocityX": -0.16125438308832984, - "velocityY": -0.0167092754120811, - "timestamp": 1.3738683492983799 - }, - { - "x": 2.663571596898345, - "y": 6.842490941859769, - "heading": 3.391106479438399, - "angularVelocity": -3.844557472114035, - "velocityX": -0.3928205508549501, - "velocityY": -0.1363479541318252, - "timestamp": 1.4008452535091662 - }, - { - "x": 2.648321534336774, - "y": 6.83512618854535, - "heading": 3.3066791856345255, - "angularVelocity": -3.129613878011262, - "velocityX": -0.5653006898944218, - "velocityY": -0.2730021672197161, - "timestamp": 1.4278221577199526 - }, - { - "x": 2.630755187717048, - "y": 6.821913643765559, - "heading": 3.2403542901418882, - "angularVelocity": -2.4585806797690686, - "velocityX": -0.6511624344467275, - "velocityY": -0.4897724615306165, - "timestamp": 1.454799061930739 - }, - { - "x": 2.610177790375068, - "y": 6.803317677931451, - "heading": 3.192043949934185, - "angularVelocity": -1.7908037123240415, - "velocityX": -0.7627783077407181, - "velocityY": -0.6893291271973806, - "timestamp": 1.4817759661415253 - }, - { - "x": 2.585880089062531, - "y": 6.779956801667491, - "heading": 3.1617447456020207, - "angularVelocity": -1.1231534980960616, - "velocityX": -0.9006853092812849, - "velocityY": -0.8659583798581432, - "timestamp": 1.5087528703523116 - }, - { - "x": 2.557481302008236, - "y": 6.752192795634918, - "heading": 3.1494506971418432, - "angularVelocity": -0.4557249550980476, - "velocityX": -1.0527074134380248, - "velocityY": -1.029176877214801, - "timestamp": 1.535729774563098 - }, - { - "x": 2.5262145192081267, - "y": 6.721488912807054, - "heading": 3.1494498734965517, - "angularVelocity": -0.00003053149779678828, - "velocityX": -1.1590204181995478, - "velocityY": -1.1381544223143767, - "timestamp": 1.5627066787738844 - }, - { - "x": 2.494951526671565, - "y": 6.690781099940809, - "heading": 3.1494491983381225, - "angularVelocity": -0.000025027276073123234, - "velocityX": -1.1588799178840683, - "velocityY": -1.1383001039076166, - "timestamp": 1.5896835829846707 - }, - { - "x": 2.46368853251831, - "y": 6.660073288720487, - "heading": 3.149448523179691, - "angularVelocity": -0.000025027276154336604, - "velocityX": -1.1588799778128431, - "velocityY": -1.138300042895271, - "timestamp": 1.616660487195457 - }, - { - "x": 2.4324255384011386, - "y": 6.629365477463429, - "heading": 3.1494478480212726, - "angularVelocity": -0.000025027275679943262, - "velocityX": -1.1588799764752862, - "velocityY": -1.138300044257102, - "timestamp": 1.6436373914062434 - }, - { - "x": 2.401162544284488, - "y": 6.5986576662058365, - "heading": 3.1494471728628666, - "angularVelocity": -0.000025027275214919653, - "velocityX": -1.1588799764559639, - "velocityY": -1.1383000442768603, - "timestamp": 1.6706142956170298 - }, - { - "x": 2.3698995501679407, - "y": 6.567949854948137, - "heading": 3.1494464977044734, - "angularVelocity": -0.000025027274749328804, - "velocityX": -1.1588799764521394, - "velocityY": -1.1383000442808404, - "timestamp": 1.6975911998278161 - }, - { - "x": 2.3386365560514903, - "y": 6.537242043690337, - "heading": 3.1494458225460926, - "angularVelocity": -0.000025027274284044715, - "velocityX": -1.1588799764485511, - "velocityY": -1.1383000442845796, - "timestamp": 1.7245681040386025 - }, - { - "x": 2.307373561935169, - "y": 6.5065342324324025, - "heading": 3.1494451473877243, - "angularVelocity": -0.00002502727381983999, - "velocityX": -1.1588799764437645, - "velocityY": -1.1383000442895392, - "timestamp": 1.7515450082493889 - }, - { - "x": 2.2761105678205396, - "y": 6.475826421172744, - "heading": 3.1494444722293684, - "angularVelocity": -0.00002502727335405626, - "velocityX": -1.158879976381056, - "velocityY": -1.1383000443534677, - "timestamp": 1.7785219124601752 - }, - { - "x": 2.244847573837531, - "y": 6.445118609779082, - "heading": 3.1494437970710263, - "angularVelocity": -0.000025027272855365495, - "velocityX": -1.1588799715020328, - "velocityY": -1.138300049320807, - "timestamp": 1.8054988166709616 - }, - { - "x": 2.2135845753549397, - "y": 6.414410802966355, - "heading": 3.149443121912666, - "angularVelocity": -0.000025027273518628322, - "velocityX": -1.1588801382959264, - "velocityY": -1.1382998795112838, - "timestamp": 1.832475720881748 - }, - { - "x": 2.182332647777799, - "y": 6.3836917291800725, - "heading": 3.14944244669543, - "angularVelocity": -0.000025029455959603075, - "velocityX": -1.1584697537175366, - "velocityY": -1.1387175320883667, - "timestamp": 1.8594526250925343 - }, - { - "x": 2.153094421241092, - "y": 6.351050761951557, - "heading": 3.149440476891046, - "angularVelocity": -0.00007301817765136917, - "velocityX": -1.0838243820804343, - "velocityY": -1.2099597112211005, - "timestamp": 1.8864295293033206 - }, - { - "x": 2.1314747036692956, - "y": 6.314801767366594, - "heading": 3.1444623769659996, - "angularVelocity": -0.18453191983123554, - "velocityX": -0.801415811201077, - "velocityY": -1.3437047595123932, - "timestamp": 1.913406433514107 - }, - { - "x": 2.1175303161219627, - "y": 6.274679449481932, - "heading": 3.139999999999996, - "angularVelocity": -0.16541471664709217, - "velocityX": -0.5169009549193061, - "velocityY": -1.487283995641412, - "timestamp": 1.9403833377248934 - }, - { - "x": 2.111393213272095, - "y": 6.231289386749268, - "heading": 3.139999999999996, - "angularVelocity": 1.2396255317011e-17, - "velocityX": -0.22749470442979314, - "velocityY": -1.6084151981870152, - "timestamp": 1.9673602419356797 - }, - { - "x": 2.1135515859889713, - "y": 6.18871804241084, - "heading": 3.139999999999996, - "angularVelocity": 4.342266970142451e-18, - "velocityX": 0.08225287430098827, - "velocityY": -1.6223404824025023, - "timestamp": 1.9936009385096722 - }, - { - "x": 2.1237593069102005, - "y": 6.1473322849843255, - "heading": 3.1399999999999966, - "angularVelocity": 4.3422681619637e-18, - "velocityX": 0.3890034280317459, - "velocityY": -1.5771592537498764, - "timestamp": 2.0198416350836648 - }, - { - "x": 2.141644452417822, - "y": 6.1086399046863455, - "heading": 3.1399999999999966, - "angularVelocity": 4.3422691347276405e-18, - "velocityX": 0.6815804396497286, - "velocityY": -1.4745180330437861, - "timestamp": 2.0460823316576575 - }, - { - "x": 2.1665552441015765, - "y": 6.0740504370872355, - "heading": 3.1399999999999966, - "angularVelocity": 4.3422696264537505e-18, - "velocityX": 0.9493189943912999, - "velocityY": -1.3181611814907124, - "timestamp": 2.0723230282316503 - }, - { - "x": 2.1935012145011026, - "y": 6.041021760141006, - "heading": 3.139999999999997, - "angularVelocity": 4.34227316559682e-18, - "velocityX": 1.0268770999862573, - "velocityY": -1.2586814093559167, - "timestamp": 2.098563724805643 - }, - { - "x": 2.220442534619127, - "y": 6.00798928987496, - "heading": 3.139999999999997, - "angularVelocity": 4.342268965346379e-18, - "velocityX": 1.0266998835971468, - "velocityY": -1.2588259680112164, - "timestamp": 2.1248044213796358 - }, - { - "x": 2.247383856273639, - "y": 5.974956820862076, - "heading": 3.1399999999999975, - "angularVelocity": 4.342269673161453e-18, - "velocityX": 1.0266999421507437, - "velocityY": -1.2588259202547785, - "timestamp": 2.1510451179536285 - }, - { - "x": 2.274325177898134, - "y": 5.941924351824711, - "heading": 3.1399999999999975, - "angularVelocity": 4.342269691700593e-18, - "velocityX": 1.0266999410068467, - "velocityY": -1.2588259211877415, - "timestamp": 2.1772858145276213 - }, - { - "x": 2.3012664995222347, - "y": 5.908891882787024, - "heading": 3.1399999999999975, - "angularVelocity": 4.3422697108078704e-18, - "velocityX": 1.0266999409917994, - "velocityY": -1.258825921200014, - "timestamp": 2.203526511101614 - }, - { - "x": 2.328207821146328, - "y": 5.87585941374933, - "heading": 3.139999999999998, - "angularVelocity": 4.3422696965810506e-18, - "velocityX": 1.0266999409915183, - "velocityY": -1.2588259212002433, - "timestamp": 2.2297672076756068 - }, - { - "x": 2.355149142770421, - "y": 5.8428269447116365, - "heading": 3.139999999999998, - "angularVelocity": 4.342269684490725e-18, - "velocityX": 1.0266999409915205, - "velocityY": -1.2588259212002417, - "timestamp": 2.2560079042495995 - }, - { - "x": 2.3820904643945138, - "y": 5.809794475673942, - "heading": 3.139999999999998, - "angularVelocity": 4.342269693460261e-18, - "velocityX": 1.026699940991488, - "velocityY": -1.258825921200268, - "timestamp": 2.2822486008235923 - }, - { - "x": 2.4090317860186072, - "y": 5.7767620066362495, - "heading": 3.1399999999999983, - "angularVelocity": 4.342269692405428e-18, - "velocityX": 1.026699940991538, - "velocityY": -1.258825921200227, - "timestamp": 2.308489297397585 - }, - { - "x": 2.4359731076427, - "y": 5.743729537598556, - "heading": 3.1399999999999983, - "angularVelocity": 4.342269716594298e-18, - "velocityX": 1.0266999409914905, - "velocityY": -1.258825921200266, - "timestamp": 2.3347299939715778 - }, - { - "x": 2.4629144292667973, - "y": 5.710697068560865, - "heading": 3.139999999999999, - "angularVelocity": 4.342269681682682e-18, - "velocityX": 1.026699940991683, - "velocityY": -1.258825921200109, - "timestamp": 2.3609706905455705 - }, - { - "x": 2.4898557508911936, - "y": 5.677664599523419, - "heading": 3.139999999999999, - "angularVelocity": 4.3422696993715505e-18, - "velocityX": 1.0266999410030797, - "velocityY": -1.2588259211908142, - "timestamp": 2.3872113871195633 - }, - { - "x": 2.5167970725019893, - "y": 5.644632130474532, - "heading": 3.139999999999999, - "angularVelocity": 4.342269692409483e-18, - "velocityX": 1.026699940484769, - "velocityY": -1.2588259216268345, - "timestamp": 2.413452083693556 - }, - { - "x": 2.5425090771755396, - "y": 5.613106913889164, - "heading": 3.1399999999999992, - "angularVelocity": 4.342265374488911e-18, - "velocityX": 0.9798522154718704, - "velocityY": -1.2013864226688296, - "timestamp": 2.4396927802675488 - }, - { - "x": 2.563078686478754, - "y": 5.587886733807481, - "heading": 3.1399999999999992, - "angularVelocity": 4.342265374369648e-18, - "velocityX": 0.7838819844288143, - "velocityY": -0.9611093977847911, - "timestamp": 2.4659334768415415 - }, - { - "x": 2.578505895291019, - "y": 5.568971596499493, - "heading": 3.1399999999999992, - "angularVelocity": 4.342265374398685e-18, - "velocityX": 0.5879115582455986, - "velocityY": -0.7208321339584491, - "timestamp": 2.4921741734155343 - }, - { - "x": 2.5887907019054897, - "y": 5.5563615040551815, - "heading": 3.1399999999999997, - "angularVelocity": 4.342265374398075e-18, - "velocityX": 0.39194106701667647, - "velocityY": -0.48055479048560223, - "timestamp": 2.518414869989527 - }, - { - "x": 2.5939331054687496, - "y": 5.55005645751953, - "heading": 3.1399999999999997, - "angularVelocity": 4.342265374359544e-18, - "velocityX": 0.19597054326510124, - "velocityY": -0.2402774071896897, - "timestamp": 2.5446555665635198 - }, - { - "x": 2.5939331054687496, - "y": 5.55005645751953, - "heading": 3.14, - "angularVelocity": 3.0510058962998796e-17, - "velocityX": 1.3525026943280928e-16, - "velocityY": 8.359694800915289e-17, - "timestamp": 2.5708962631375125 - }, - { - "x": 2.5906368423225947, - "y": 5.5485199070890205, - "heading": 3.101591851410996, - "angularVelocity": -1.2805791583146104, - "velocityX": -0.10990183178303778, - "velocityY": -0.05123065102809662, - "timestamp": 2.60088905845952 - }, - { - "x": 2.584621501308477, - "y": 5.544975862182894, - "heading": 3.0243371709692783, - "angularVelocity": -2.5757746022741834, - "velocityX": -0.20055953270102006, - "velocityY": -0.11816320779855842, - "timestamp": 2.6308818537815273 - }, - { - "x": 2.5759613994637824, - "y": 5.536844199254011, - "heading": 2.9135350884263085, - "angularVelocity": -3.6942899570776517, - "velocityX": -0.28873940397378167, - "velocityY": -0.2711205421688905, - "timestamp": 2.6608746491035347 - }, - { - "x": 2.56275074361348, - "y": 5.521892690270415, - "heading": 2.8248688434153846, - "angularVelocity": -2.956251461684603, - "velocityX": -0.44046097433337356, - "velocityY": -0.49850335131840534, - "timestamp": 2.690867444425542 - }, - { - "x": 2.543962903792877, - "y": 5.501410245111836, - "heading": 2.7589030524712514, - "angularVelocity": -2.199387894182165, - "velocityX": -0.6264117638620101, - "velocityY": -0.6829121773583486, - "timestamp": 2.7208602397475494 - }, - { - "x": 2.518778715441927, - "y": 5.477002144640871, - "heading": 2.7154691851313926, - "angularVelocity": -1.448143358205586, - "velocityX": -0.839674597873837, - "velocityY": -0.8137987876364632, - "timestamp": 2.750853035069557 - }, - { - "x": 2.486733800013198, - "y": 5.449761748396881, - "heading": 2.6943182531197865, - "angularVelocity": -0.705200425130341, - "velocityX": -1.0684204351295292, - "velocityY": -0.9082313252703665, - "timestamp": 2.780845830391564 - }, - { - "x": 2.4478196867061426, - "y": 5.4204460978412365, - "heading": 2.694317441639593, - "angularVelocity": -0.000027055837428819432, - "velocityX": -1.2974487002285997, - "velocityY": -0.9774230858209427, - "timestamp": 2.8108386257135716 - }, - { - "x": 2.408901839144878, - "y": 5.391135398464328, - "heading": 2.6943166423252785, - "angularVelocity": -0.000026650210696695328, - "velocityX": -1.2975732052797133, - "velocityY": -0.9772580068715555, - "timestamp": 2.840831421035579 - }, - { - "x": 2.3699840051756724, - "y": 5.361824681040097, - "heading": 2.6943158430110494, - "angularVelocity": -0.00002665020784043696, - "velocityX": -1.2975727521022344, - "velocityY": -0.9772586085935405, - "timestamp": 2.8708242163575863 - }, - { - "x": 2.3310661711945366, - "y": 5.332513963631788, - "heading": 2.6943150436964616, - "angularVelocity": -0.000026650219800690006, - "velocityX": -1.297572752499997, - "velocityY": -0.9772586080626477, - "timestamp": 2.9008170116795937 - }, - { - "x": 2.292148337213513, - "y": 5.303203246223414, - "heading": 2.694314244381513, - "angularVelocity": -0.000026650231834296552, - "velocityX": -1.2975727524962546, - "velocityY": -0.9772586080648237, - "timestamp": 2.930809807001601 - }, - { - "x": 2.25323050323266, - "y": 5.273892528814898, - "heading": 2.6943134450662036, - "angularVelocity": -0.00002665024386857193, - "velocityX": -1.297572752490578, - "velocityY": -0.9772586080695672, - "timestamp": 2.9608026023236085 - }, - { - "x": 2.214312669251978, - "y": 5.2445818114062375, - "heading": 2.694312645750533, - "angularVelocity": -0.00002665025590264624, - "velocityX": -1.2975727524848706, - "velocityY": -0.9772586080743522, - "timestamp": 2.990795397645616 - }, - { - "x": 2.1753948352714723, - "y": 5.215271093997427, - "heading": 2.694311846434502, - "angularVelocity": -0.000026650267936849635, - "velocityX": -1.2975727524790048, - "velocityY": -0.9772586080793467, - "timestamp": 3.0207881929676232 - }, - { - "x": 2.136477001291495, - "y": 5.185960376588, - "heading": 2.6943110471181098, - "angularVelocity": -0.000026650279973527825, - "velocityX": -1.2975727524613627, - "velocityY": -0.9772586080999772, - "timestamp": 3.0507809882896306 - }, - { - "x": 2.097559167357152, - "y": 5.1566496591180675, - "heading": 2.694310247801348, - "angularVelocity": -0.000026650292283470146, - "velocityX": -1.2975727509398636, - "velocityY": -0.9772586101172457, - "timestamp": 3.080773783611638 - }, - { - "x": 2.058641302557044, - "y": 5.127338982627939, - "heading": 2.694309448489961, - "angularVelocity": -0.000026650113093683626, - "velocityX": -1.2975737800459084, - "velocityY": -0.9772572437956306, - "timestamp": 3.1107665789336454 - }, - { - "x": 2.0197569370912083, - "y": 5.0979839038638834, - "heading": 2.6943086027536207, - "angularVelocity": -0.000028197983267341566, - "velocityX": -1.2964568673313885, - "velocityY": -0.9787376751348899, - "timestamp": 3.1407593742556528 - }, - { - "x": 1.9877778714785685, - "y": 5.067888184962114, - "heading": 2.678621118987198, - "angularVelocity": -0.5230417371285907, - "velocityX": -1.0662249139900737, - "velocityY": -1.0034316101155079, - "timestamp": 3.17075216957766 - }, - { - "x": 1.9640654183560038, - "y": 5.031205594348394, - "heading": 2.6596531558314034, - "angularVelocity": -0.6324173173088112, - "velocityX": -0.7906049725553251, - "velocityY": -1.223046742388833, - "timestamp": 3.2007449648996675 - }, - { - "x": 1.9481708008996033, - "y": 4.987987936601904, - "heading": 2.649783348569472, - "angularVelocity": -0.32907260414160233, - "velocityX": -0.5299478519964009, - "velocityY": -1.4409346405525878, - "timestamp": 3.230737760221675 - }, - { - "x": 1.9410848617553718, - "y": 4.939785003662109, - "heading": 2.649783348569472, - "angularVelocity": 6.681257540282107e-18, - "velocityX": -0.23625470944103766, - "velocityY": -1.6071503980304298, - "timestamp": 3.2607305555436823 - }, - { - "x": 1.9456173734407256, - "y": 4.888006870907468, - "heading": 2.649783348569472, - "angularVelocity": -1.484655388526889e-19, - "velocityX": 0.14165581617647935, - "velocityY": -1.6182360167593688, - "timestamp": 3.292727206352592 - }, - { - "x": 1.9619962278910381, - "y": 4.838678856610239, - "heading": 2.649783348569472, - "angularVelocity": -1.4846556369937314e-19, - "velocityX": 0.5118927774045035, - "velocityY": -1.5416618005383362, - "timestamp": 3.324723857161502 - }, - { - "x": 1.9893341653194017, - "y": 4.794473003544163, - "heading": 2.649783348569472, - "angularVelocity": -1.4846558519027202e-19, - "velocityX": 0.8543999680324345, - "velocityY": -1.381577507287736, - "timestamp": 3.3567205079704117 - }, - { - "x": 2.026135553103086, - "y": 4.757769076744815, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559448429508e-19, - "velocityX": 1.1501637469359431, - "velocityY": -1.1471177723711978, - "timestamp": 3.3887171587793214 - }, - { - "x": 2.0637808893572873, - "y": 4.721931260837698, - "heading": 2.649783348569472, - "angularVelocity": -1.4846636373679086e-19, - "velocityX": 1.1765398972283652, - "velocityY": -1.120048973905845, - "timestamp": 3.4207138095882312 - }, - { - "x": 2.1014301321789435, - "y": 4.6860975489840255, - "heading": 2.649783348569472, - "angularVelocity": -1.484648310808098e-19, - "velocityX": 1.1766619902345172, - "velocityY": -1.1199207088167735, - "timestamp": 3.452710460397141 - }, - { - "x": 2.139079370280558, - "y": 4.650263832171171, - "heading": 2.649783348569472, - "angularVelocity": -1.48465589810743e-19, - "velocityX": 1.1766618427177973, - "velocityY": -1.1199208638074638, - "timestamp": 3.484707111206051 - }, - { - "x": 2.17672860838618, - "y": 4.614430115362526, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559463647802e-19, - "velocityX": 1.1766618428430395, - "velocityY": -1.1199208636758753, - "timestamp": 3.5167037620149606 - }, - { - "x": 2.2143778464918284, - "y": 4.57859639855391, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559409367017e-19, - "velocityX": 1.176661842843871, - "velocityY": -1.1199208636750018, - "timestamp": 3.5487004128238704 - }, - { - "x": 2.2520270845974713, - "y": 4.5427626817452875, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559515966518e-19, - "velocityX": 1.1766618428436975, - "velocityY": -1.1199208636751843, - "timestamp": 3.58069706363278 - }, - { - "x": 2.289676322703114, - "y": 4.506928964936664, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559610313848e-19, - "velocityX": 1.1766618428436995, - "velocityY": -1.1199208636751825, - "timestamp": 3.61269371444169 - }, - { - "x": 2.3273255608087573, - "y": 4.471095248128042, - "heading": 2.649783348569472, - "angularVelocity": -1.484655947429445e-19, - "velocityX": 1.1766618428436957, - "velocityY": -1.1199208636751863, - "timestamp": 3.6446903652505998 - }, - { - "x": 2.3649747989143997, - "y": 4.435261531319418, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559574023035e-19, - "velocityX": 1.1766618428436864, - "velocityY": -1.1199208636751963, - "timestamp": 3.6766870160595095 - }, - { - "x": 2.4026240370200447, - "y": 4.399427814510799, - "heading": 2.649783348569472, - "angularVelocity": -1.484655924580371e-19, - "velocityX": 1.1766618428437539, - "velocityY": -1.1199208636751252, - "timestamp": 3.7086836668684193 - }, - { - "x": 2.4402732751256884, - "y": 4.363594097702177, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559654067578e-19, - "velocityX": 1.1766618428437225, - "velocityY": -1.1199208636751585, - "timestamp": 3.740680317677329 - }, - { - "x": 2.4779225132411047, - "y": 4.327760380902407, - "heading": 2.649783348569472, - "angularVelocity": -1.4846559492892923e-19, - "velocityX": 1.1766618431491565, - "velocityY": -1.1199208633985085, - "timestamp": 3.772676968486239 - }, - { - "x": 2.5129728702731393, - "y": 4.294400221963306, - "heading": 2.649783348569472, - "angularVelocity": -1.484655203554499e-19, - "velocityX": 1.0954383082580312, - "velocityY": -1.0426140891540008, - "timestamp": 3.8046736192951487 - }, - { - "x": 2.53926064365096, - "y": 4.2693800974206075, - "heading": 2.649783348569472, - "angularVelocity": -1.484655203554444e-19, - "velocityX": 0.8215789063304317, - "velocityY": -0.7819607337071065, - "timestamp": 3.8366702701040585 - }, - { - "x": 2.5567858275270727, - "y": 4.252700012844841, - "heading": 2.649783348569472, - "angularVelocity": -1.4846552035544855e-19, - "velocityX": 0.5477193216495216, - "velocityY": -0.5213072041629537, - "timestamp": 3.8686669209129683 - }, - { - "x": 2.5655484199523926, - "y": 4.244359970092773, - "heading": 2.649783348569472, - "angularVelocity": -1.4846552035527083e-19, - "velocityX": 0.2738596760533411, - "velocityY": -0.260653616588761, - "timestamp": 3.900663571721878 - }, - { - "x": 2.5655484199523926, - "y": 4.244359970092773, - "heading": 2.649783348569472, - "angularVelocity": -4.948850678520552e-20, - "velocityX": 6.0427487543509175e-18, - "velocityY": -9.335003878092332e-18, - "timestamp": 3.932660222530788 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.1.traj b/src/main/deploy/choreo/Test2.1.traj index 60dd1202..bef35b81 100644 --- a/src/main/deploy/choreo/Test2.1.traj +++ b/src/main/deploy/choreo/Test2.1.traj @@ -1,796 +1,1220 @@ { - "samples": [ - { - "x": 0.3137660622596741, - "y": 6.865104675292969, - "heading": 3.135568639860519, - "angularVelocity": 1.3323616061473399e-26, - "velocityX": -9.030177506128314e-25, - "velocityY": 7.578270673418084e-24, - "timestamp": 0 - }, - { - "x": 0.3285749308509426, - "y": 6.865947459797136, - "heading": 3.135568639860519, - "angularVelocity": 3.3337145202478786e-22, - "velocityX": 0.41797046933441173, - "velocityY": 0.023787032249159984, - "timestamp": 0.03543041836149472 - }, - { - "x": 0.35819266531481686, - "y": 6.867633028650745, - "heading": 3.135568639860519, - "angularVelocity": 3.3337147019213317e-22, - "velocityX": 0.8359408619363751, - "velocityY": 0.04757406013141577, - "timestamp": 0.07086083672298944 - }, - { - "x": 0.4026192574953566, - "y": 6.87016138138964, - "heading": 3.135568639860519, - "angularVelocity": 3.3337143389947597e-22, - "velocityX": 1.2539110243423486, - "velocityY": 0.07136107491303612, - "timestamp": 0.10629125508448417 - }, - { - "x": 0.4600803298886451, - "y": 6.873431536777097, - "heading": 3.135568639860519, - "angularVelocity": 3.331660178480041e-22, - "velocityX": 1.621800561512317, - "velocityY": 0.09229796143220789, - "timestamp": 0.14172167344597889 - }, - { - "x": 0.5175414022820589, - "y": 6.8767016921645565, - "heading": 3.135568639860519, - "angularVelocity": 3.3316629368551667e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233462, - "timestamp": 0.1771520918074736 - }, - { - "x": 0.5750024746754727, - "y": 6.879971847552017, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614681891394e-22, - "velocityX": 1.6218005615158566, - "velocityY": 0.09229796143233254, - "timestamp": 0.2125825101689683 - }, - { - "x": 0.6324635470688867, - "y": 6.883242002939478, - "heading": 3.135568639860519, - "angularVelocity": 3.331683104806914e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233347, - "timestamp": 0.24801292853046303 - }, - { - "x": 0.6899246194623005, - "y": 6.886512158326939, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614878052174e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233253, - "timestamp": 0.28344334689195777 - }, - { - "x": 0.7473856918557144, - "y": 6.8897823137144, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614758461554e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233254, - "timestamp": 0.3188737652534525 - }, - { - "x": 0.8048467642491284, - "y": 6.89305246910186, - "heading": 3.135568639860519, - "angularVelocity": 3.331638534198991e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233299, - "timestamp": 0.35430418361494725 - }, - { - "x": 0.8623078366425422, - "y": 6.89632262448932, - "heading": 3.135568639860519, - "angularVelocity": 3.3317076614902997e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233253, - "timestamp": 0.389734601976442 - }, - { - "x": 0.9197689090359561, - "y": 6.899592779876781, - "heading": 3.135568639860519, - "angularVelocity": 3.331661479116866e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233396, - "timestamp": 0.42516502033793674 - }, - { - "x": 0.97722998142937, - "y": 6.902862935264242, - "heading": 3.135568639860519, - "angularVelocity": 3.33166145502926e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233347, - "timestamp": 0.4605954386994315 - }, - { - "x": 1.0346910538227838, - "y": 6.906133090651703, - "heading": 3.135568639860519, - "angularVelocity": 3.3316600659908063e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233301, - "timestamp": 0.4960258570609262 - }, - { - "x": 1.0921521262161977, - "y": 6.9094032460391634, - "heading": 3.135568639860519, - "angularVelocity": 3.331661460674642e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.092297961432333, - "timestamp": 0.5314562754224209 - }, - { - "x": 1.1496131986096116, - "y": 6.912673401426624, - "heading": 3.135568639860519, - "angularVelocity": 3.331661475383932e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.092297961432333, - "timestamp": 0.5668866937839157 - }, - { - "x": 1.2070742710030253, - "y": 6.915943556814084, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614753921174e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233254, - "timestamp": 0.6023171121454104 - }, - { - "x": 1.2645353433964392, - "y": 6.919213712201545, - "heading": 3.135568639860519, - "angularVelocity": 3.331662731932369e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.092297961432333, - "timestamp": 0.6377475305069051 - }, - { - "x": 1.3219964157898532, - "y": 6.922483867589006, - "heading": 3.135568639860519, - "angularVelocity": 3.3316627143346654e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233393, - "timestamp": 0.6731779488683999 - }, - { - "x": 1.379457488183267, - "y": 6.925754022976466, - "heading": 3.135568639860519, - "angularVelocity": 3.331661483573468e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233157, - "timestamp": 0.7086083672298946 - }, - { - "x": 1.436918560576681, - "y": 6.929024178363926, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614890219236e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233392, - "timestamp": 0.7440387855913894 - }, - { - "x": 1.494379632970095, - "y": 6.932294333751387, - "heading": 3.135568639860519, - "angularVelocity": 3.331662743669949e-22, - "velocityX": 1.6218005615158566, - "velocityY": 0.09229796143233207, - "timestamp": 0.7794692039528841 - }, - { - "x": 1.5518407053635086, - "y": 6.935564489138847, - "heading": 3.135568639860519, - "angularVelocity": 3.331638909213181e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233347, - "timestamp": 0.8148996223143788 - }, - { - "x": 1.6093017777569225, - "y": 6.938834644526308, - "heading": 3.135568639860519, - "angularVelocity": 3.3316600948450887e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233622, - "timestamp": 0.8503300406758736 - }, - { - "x": 1.666762850150336, - "y": 6.942104799913777, - "heading": 3.135568639860519, - "angularVelocity": 3.331671581625241e-22, - "velocityX": 1.621800561515844, - "velocityY": 0.09229796143255317, - "timestamp": 0.8857604590373683 - }, - { - "x": 1.7242239225437261, - "y": 6.945374955301653, - "heading": 3.135568639860519, - "angularVelocity": 3.3334679678699274e-22, - "velocityX": 1.6218005615151887, - "velocityY": 0.09229796144406777, - "timestamp": 0.9211908773988631 - }, - { - "x": 1.7816849946975708, - "y": 6.948645114898682, - "heading": 3.135568639860519, - "angularVelocity": 1.7645788213167404e-22, - "velocityX": 1.6218005547541783, - "velocityY": 0.09229808024460076, - "timestamp": 0.9566212957603578 - }, - { - "x": 1.8178857407766151, - "y": 6.94853231479887, - "heading": 3.1384797599068888, - "angularVelocity": 0.12817174361754557, - "velocityX": 1.5938582646199322, - "velocityY": -0.004966399613447973, - "timestamp": 0.9793339464913666 - }, - { - "x": 1.8540603970824046, - "y": 6.948528771707492, - "heading": 3.1414999940123267, - "angularVelocity": 0.13297585302602363, - "velocityX": 1.592709575567121, - "velocityY": -0.00015599638369279018, - "timestamp": 1.0020465972223755 - }, - { - "x": 1.8901908310793394, - "y": 6.948640868874459, - "heading": 3.1447030512216227, - "angularVelocity": 0.14102524831780386, - "velocityX": 1.5907625413183106, - "velocityY": 0.0049354506567203535, - "timestamp": 1.0247592479533845 - }, - { - "x": 1.926257669084107, - "y": 6.9488776015696425, - "heading": 3.148167194864803, - "angularVelocity": 0.15252044704983372, - "velocityX": 1.5879625162168578, - "velocityY": 0.010422944375284186, - "timestamp": 1.0474718986843934 - }, - { - "x": 1.9622398024006038, - "y": 6.94925072993569, - "heading": 3.151976954699645, - "angularVelocity": 0.16773734954856945, - "velocityX": 1.584233110553301, - "velocityY": 0.01642821749276163, - "timestamp": 1.0701845494154023 - }, - { - "x": 1.9981171224268564, - "y": 6.949774504499785, - "heading": 3.156211459419019, - "angularVelocity": 0.1864381559653407, - "velocityX": 1.5796183567984214, - "velocityY": 0.023060917472754097, - "timestamp": 1.0928972001464112 - }, - { - "x": 2.033847488660219, - "y": 6.950468453992633, - "heading": 3.161038496208879, - "angularVelocity": 0.2125263513725333, - "velocityX": 1.5731482272380108, - "velocityY": 0.03055343478252191, - "timestamp": 1.1156098508774202 - }, - { - "x": 2.0694053227389695, - "y": 6.951354952814641, - "heading": 3.166554769842392, - "angularVelocity": 0.24287229609803831, - "velocityX": 1.565551925218666, - "velocityY": 0.039031059496573456, - "timestamp": 1.138322501608429 - }, - { - "x": 2.104784911670157, - "y": 6.952454847185295, - "heading": 3.1727732170956275, - "angularVelocity": 0.27378782542301205, - "velocityX": 1.5577040896809682, - "velocityY": 0.04842650836667849, - "timestamp": 1.161035152339438 - }, - { - "x": 2.1400047240253706, - "y": 6.9537191803371154, - "heading": 3.1796054213345153, - "angularVelocity": 0.3008105183231903, - "velocityX": 1.550669394441415, - "velocityY": 0.05566647269821489, - "timestamp": 1.183747803070447 - }, - { - "x": 2.175095855891208, - "y": 6.955084615506324, - "heading": 3.18691368664007, - "angularVelocity": 0.3217706903570311, - "velocityX": 1.5450038078527182, - "velocityY": 0.060117825320333544, - "timestamp": 1.2064604538014558 - }, - { - "x": 2.2101380445057046, - "y": 6.956471002562802, - "heading": 3.194369840668752, - "angularVelocity": 0.3282819833311086, - "velocityX": 1.5428489184071512, - "velocityY": 0.06104030185188484, - "timestamp": 1.2291731045324648 - }, - { - "x": 2.2451610724837057, - "y": 6.957849512193169, - "heading": 3.2018529792356185, - "angularVelocity": 0.32947006738627316, - "velocityX": 1.5420053076493287, - "velocityY": 0.060693471963871995, - "timestamp": 1.2518857552634737 - }, - { - "x": 2.2801629678458126, - "y": 6.959221595789265, - "heading": 3.2093707720743723, - "angularVelocity": 0.3309958369804056, - "velocityX": 1.5410748739388525, - "velocityY": 0.060410544429463274, - "timestamp": 1.2745984059944826 - }, - { - "x": 2.315144049729391, - "y": 6.960586594052822, - "heading": 3.2169217131634897, - "angularVelocity": 0.3324552989672944, - "velocityX": 1.540158491312511, - "velocityY": 0.06009858909567994, - "timestamp": 1.2973110567254915 - }, - { - "x": 2.3501040543802802, - "y": 6.961944386116353, - "heading": 3.2245066187504112, - "angularVelocity": 0.3339506989629348, - "velocityX": 1.539230496031878, - "velocityY": 0.05978131216881497, - "timestamp": 1.3200237074565004 - }, - { - "x": 2.3850426980471595, - "y": 6.963294864074251, - "heading": 3.232126373384074, - "angularVelocity": 0.3354850441678861, - "velocityX": 1.5382900076554706, - "velocityY": 0.05945928433854099, - "timestamp": 1.3427363581875094 - }, - { - "x": 2.419959710539657, - "y": 6.964637896582436, - "heading": 3.239781798872735, - "angularVelocity": 0.3370555722150803, - "velocityX": 1.5373376232491214, - "velocityY": 0.05913147364836824, - "timestamp": 1.3654490089185183 - }, - { - "x": 2.454854803951486, - "y": 6.965973356645481, - "heading": 3.247473776388813, - "angularVelocity": 0.3386648968090985, - "velocityX": 1.5363725628109135, - "velocityY": 0.05879807156202029, - "timestamp": 1.3881616596495272 - }, - { - "x": 2.48972762858419, - "y": 6.967301155248667, - "heading": 3.2552034126870217, - "angularVelocity": 0.3403229499609042, - "velocityX": 1.5353921057348447, - "velocityY": 0.05846075030657316, - "timestamp": 1.4108743103805361 - }, - { - "x": 2.5245780070203394, - "y": 6.968621054178632, - "heading": 3.262971149366171, - "angularVelocity": 0.34200044596925644, - "velocityX": 1.5344038372663136, - "velocityY": 0.05811294091547401, - "timestamp": 1.433586961111545 - }, - { - "x": 2.559405532428149, - "y": 6.969932985582836, - "heading": 3.2707782920481354, - "angularVelocity": 0.3437354263236719, - "velocityX": 1.5333976566751537, - "velocityY": 0.05776214409057404, - "timestamp": 1.456299611842554 - }, - { - "x": 2.594207801112396, - "y": 6.97123839692077, - "heading": 3.2786335848605472, - "angularVelocity": 0.34585539598366793, - "velocityX": 1.5322856454060756, - "velocityY": 0.05747507648462563, - "timestamp": 1.4790122625735629 - }, - { - "x": 2.6290202140808105, - "y": 6.972513675689697, - "heading": 3.286405336491994, - "angularVelocity": 0.3421772175995684, - "velocityX": 1.5327322812606092, - "velocityY": 0.05614838990089804, - "timestamp": 1.5017249133045718 - }, - { - "x": 2.7840510289336455, - "y": 6.98699854094784, - "heading": 3.286405287600397, - "angularVelocity": -5.100684883763116e-7, - "velocityX": 1.6173808639626774, - "velocityY": 0.1511154018498607, - "timestamp": 1.5975779175703702 - }, - { - "x": 2.93908160001489, - "y": 7.001486015045406, - "heading": 3.2864052387079807, - "angularVelocity": -5.100770354356606e-7, - "velocityX": 1.6173783207811374, - "velocityY": 0.15114261893547631, - "timestamp": 1.6934309218361685 - }, - { - "x": 3.094112171101734, - "y": 7.015973489083046, - "heading": 3.286405189815564, - "angularVelocity": -5.100770390190521e-7, - "velocityX": 1.617378320839561, - "velocityY": 0.15114261831028078, - "timestamp": 1.7892839261019668 - }, - { - "x": 3.2491427421885786, - "y": 7.030460963120681, - "heading": 3.286405140923147, - "angularVelocity": -5.100770430673946e-7, - "velocityX": 1.617378320839565, - "velocityY": 0.15114261831023715, - "timestamp": 1.8851369303677652 - }, - { - "x": 3.404173313275423, - "y": 7.0449484371583155, - "heading": 3.28640509203073, - "angularVelocity": -5.100770460466358e-7, - "velocityX": 1.6173783208395647, - "velocityY": 0.15114261831023496, - "timestamp": 1.9809899346335635 - }, - { - "x": 3.5592038843622675, - "y": 7.05943591119595, - "heading": 3.286405043138312, - "angularVelocity": -5.100770488976204e-7, - "velocityX": 1.6173783208395647, - "velocityY": 0.15114261831023276, - "timestamp": 2.076842938899362 - }, - { - "x": 3.714234455449112, - "y": 7.073923385233584, - "heading": 3.2864049942458946, - "angularVelocity": -5.100770524050277e-7, - "velocityX": 1.6173783208395642, - "velocityY": 0.15114261831023054, - "timestamp": 2.1726959431651602 - }, - { - "x": 3.8692650265359565, - "y": 7.088410859271219, - "heading": 3.2864049453534765, - "angularVelocity": -5.100770565197187e-7, - "velocityX": 1.6173783208395647, - "velocityY": 0.15114261831022827, - "timestamp": 2.2685489474309586 - }, - { - "x": 4.0242955976228005, - "y": 7.102898333308853, - "heading": 3.2864048964610575, - "angularVelocity": -5.100770606946053e-7, - "velocityX": 1.6173783208395642, - "velocityY": 0.15114261831022607, - "timestamp": 2.364401951696757 - }, - { - "x": 4.179326168709644, - "y": 7.117385807346487, - "heading": 3.286404847568639, - "angularVelocity": -5.100770634020749e-7, - "velocityX": 1.617378320839564, - "velocityY": 0.151142618310224, - "timestamp": 2.4602549559625553 - }, - { - "x": 4.334356739796489, - "y": 7.13187328138412, - "heading": 3.2864047986762195, - "angularVelocity": -5.100770666030285e-7, - "velocityX": 1.617378320839564, - "velocityY": 0.15114261831022163, - "timestamp": 2.5561079602283536 - }, - { - "x": 4.489387310883332, - "y": 7.146360755421753, - "heading": 3.2864047497838, - "angularVelocity": -5.100770706092546e-7, - "velocityX": 1.617378320839564, - "velocityY": 0.15114261831021944, - "timestamp": 2.651960964494152 - }, - { - "x": 4.644417881970177, - "y": 7.160848229459387, - "heading": 3.28640470089138, - "angularVelocity": -5.100770740673254e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.15114261831021727, - "timestamp": 2.7478139687599503 - }, - { - "x": 4.79944845305702, - "y": 7.175335703497019, - "heading": 3.28640465199896, - "angularVelocity": -5.100770777885376e-7, - "velocityX": 1.6173783208395638, - "velocityY": 0.15114261831021514, - "timestamp": 2.8436669730257487 - }, - { - "x": 4.954479024143865, - "y": 7.1898231775346515, - "heading": 3.286404603106539, - "angularVelocity": -5.100770810299081e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.15114261831021283, - "timestamp": 2.939519977291547 - }, - { - "x": 5.109509595230708, - "y": 7.2043106515722855, - "heading": 3.2864045542141187, - "angularVelocity": -5.100770846564483e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.15114261831021056, - "timestamp": 3.0353729815573454 - }, - { - "x": 5.264540166317553, - "y": 7.218798125609918, - "heading": 3.286404505321697, - "angularVelocity": -5.100770889061609e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.1511426183102084, - "timestamp": 3.1312259858231437 - }, - { - "x": 5.419570737404396, - "y": 7.233285599647549, - "heading": 3.2864044564292754, - "angularVelocity": -5.100770922378371e-7, - "velocityX": 1.6173783208395631, - "velocityY": 0.15114261831020614, - "timestamp": 3.227078990088942 - }, - { - "x": 5.574601308491241, - "y": 7.247773073685181, - "heading": 3.2864044075368533, - "angularVelocity": -5.100770952675197e-7, - "velocityX": 1.6173783208395631, - "velocityY": 0.15114261831020392, - "timestamp": 3.3229319943547404 - }, - { - "x": 5.7296318795780845, - "y": 7.262260547722812, - "heading": 3.286404358644431, - "angularVelocity": -5.100770991840634e-7, - "velocityX": 1.617378320839563, - "velocityY": 0.1511426183102018, - "timestamp": 3.4187849986205388 - }, - { - "x": 5.884662450664929, - "y": 7.276748021760444, - "heading": 3.286404309752008, - "angularVelocity": -5.100771023487481e-7, - "velocityX": 1.617378320839563, - "velocityY": 0.15114261831019962, - "timestamp": 3.514638002886337 - }, - { - "x": 6.039693021751773, - "y": 7.291235495798076, - "heading": 3.286404260859585, - "angularVelocity": -5.100771061559551e-7, - "velocityX": 1.6173783208395627, - "velocityY": 0.15114261831019746, - "timestamp": 3.6104910071521354 - }, - { - "x": 6.194723592838616, - "y": 7.305722969835706, - "heading": 3.2864042119671617, - "angularVelocity": -5.100771091660107e-7, - "velocityX": 1.6173783208395627, - "velocityY": 0.1511426183101951, - "timestamp": 3.706344011417934 - }, - { - "x": 6.349754163925461, - "y": 7.320210443873337, - "heading": 3.2864041630747383, - "angularVelocity": -5.100771129976334e-7, - "velocityX": 1.6173783208395625, - "velocityY": 0.15114261831019293, - "timestamp": 3.802197015683732 - }, - { - "x": 6.504784735012304, - "y": 7.334697917910969, - "heading": 3.286404114182314, - "angularVelocity": -5.100771163404491e-7, - "velocityX": 1.6173783208395625, - "velocityY": 0.1511426183101907, - "timestamp": 3.8980500199495305 - }, - { - "x": 6.659815306099149, - "y": 7.349185391948599, - "heading": 3.28640406528989, - "angularVelocity": -5.100771202032421e-7, - "velocityX": 1.6173783208395622, - "velocityY": 0.1511426183101885, - "timestamp": 3.993903024215329 - }, - { - "x": 6.814845877185992, - "y": 7.363672865986229, - "heading": 3.2864040163974653, - "angularVelocity": -5.100771233944824e-7, - "velocityX": 1.6173783208395625, - "velocityY": 0.15114261831018627, - "timestamp": 4.089756028481127 - }, - { - "x": 6.969876448272837, - "y": 7.378160340023859, - "heading": 3.2864039675050396, - "angularVelocity": -5.100771270486632e-7, - "velocityX": 1.6173783208395622, - "velocityY": 0.15114261831018408, - "timestamp": 4.1856090327469255 - }, - { - "x": 7.12490701935968, - "y": 7.392647814061489, - "heading": 3.286403918612615, - "angularVelocity": -5.100771304657537e-7, - "velocityX": 1.617378320839562, - "velocityY": 0.15114261831018191, - "timestamp": 4.281462037012724 - }, - { - "x": 7.279937590446525, - "y": 7.407135288099118, - "heading": 3.286403869720189, - "angularVelocity": -5.10077134427039e-7, - "velocityX": 1.6173783208395618, - "velocityY": 0.1511426183101796, - "timestamp": 4.377315041278522 - }, - { - "x": 7.4349681615333685, - "y": 7.421622762136748, - "heading": 3.286403820827763, - "angularVelocity": -5.100771387503622e-7, - "velocityX": 1.6173783208395618, - "velocityY": 0.15114261831017736, - "timestamp": 4.473168045544321 - }, - { - "x": 7.589998732620213, - "y": 7.436110236174376, - "heading": 3.2864037719353365, - "angularVelocity": -5.100771410554518e-7, - "velocityX": 1.6173783208395616, - "velocityY": 0.15114261831017525, - "timestamp": 4.569021049810119 - }, - { - "x": 7.745029303707056, - "y": 7.450597710212007, - "heading": 3.28640372304291, - "angularVelocity": -5.100771448287922e-7, - "velocityX": 1.6173783208395598, - "velocityY": 0.1511426183101905, - "timestamp": 4.664874054075917 - }, - { - "x": 7.90005987484092, - "y": 7.465085183746354, - "heading": 3.2864036741504377, - "angularVelocity": -5.1007762076643e-7, - "velocityX": 1.6173783213301043, - "velocityY": 0.15114261305961757, - "timestamp": 4.760727058341716 - }, - { - "x": 7.999455451965332, - "y": 7.473754405975342, - "heading": 3.153293227446569, - "angularVelocity": -1.3886935284235506, - "velocityX": 1.0369583915052876, - "velocityY": 0.09044288486721058, - "timestamp": 4.856580062607514 - }, - { - "x": 7.999455451965332, - "y": 7.473754405975342, - "heading": 3.153293227446569, - "angularVelocity": 5.239369635456734e-24, - "velocityX": 2.4359888438060422e-22, - "velocityY": -8.774946070326848e-22, - "timestamp": 4.952433066873312 - } - ] + "samples": [ + { + "x": 0.3137660622596741, + "y": 6.865104675292969, + "heading": 3.135568639860519, + "angularVelocity": 1.3323616061473399e-26, + "velocityX": -9.030177506128314e-25, + "velocityY": 7.578270673418084e-24, + "timestamp": 0 + }, + { + "x": 0.3285749308509426, + "y": 6.865947459797136, + "heading": 3.135568639860519, + "angularVelocity": 3.3337145202478786e-22, + "velocityX": 0.41797046933441173, + "velocityY": 0.023787032249159984, + "timestamp": 0.03543041836149472 + }, + { + "x": 0.35819266531481686, + "y": 6.867633028650745, + "heading": 3.135568639860519, + "angularVelocity": 3.3337147019213317e-22, + "velocityX": 0.8359408619363751, + "velocityY": 0.04757406013141577, + "timestamp": 0.07086083672298944 + }, + { + "x": 0.4026192574953566, + "y": 6.87016138138964, + "heading": 3.135568639860519, + "angularVelocity": 3.3337143389947597e-22, + "velocityX": 1.2539110243423486, + "velocityY": 0.07136107491303612, + "timestamp": 0.10629125508448417 + }, + { + "x": 0.4600803298886451, + "y": 6.873431536777097, + "heading": 3.135568639860519, + "angularVelocity": 3.331660178480041e-22, + "velocityX": 1.621800561512317, + "velocityY": 0.09229796143220789, + "timestamp": 0.14172167344597889 + }, + { + "x": 0.5175414022820589, + "y": 6.8767016921645565, + "heading": 3.135568639860519, + "angularVelocity": 3.3316629368551667e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233462, + "timestamp": 0.1771520918074736 + }, + { + "x": 0.5750024746754727, + "y": 6.879971847552017, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614681891394e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233254, + "timestamp": 0.2125825101689683 + }, + { + "x": 0.6324635470688867, + "y": 6.883242002939478, + "heading": 3.135568639860519, + "angularVelocity": 3.331683104806914e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.24801292853046303 + }, + { + "x": 0.6899246194623005, + "y": 6.886512158326939, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614878052174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.28344334689195777 + }, + { + "x": 0.7473856918557144, + "y": 6.8897823137144, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614758461554e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.3188737652534525 + }, + { + "x": 0.8048467642491284, + "y": 6.89305246910186, + "heading": 3.135568639860519, + "angularVelocity": 3.331638534198991e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233299, + "timestamp": 0.35430418361494725 + }, + { + "x": 0.8623078366425422, + "y": 6.89632262448932, + "heading": 3.135568639860519, + "angularVelocity": 3.3317076614902997e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233253, + "timestamp": 0.389734601976442 + }, + { + "x": 0.9197689090359561, + "y": 6.899592779876781, + "heading": 3.135568639860519, + "angularVelocity": 3.331661479116866e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233396, + "timestamp": 0.42516502033793674 + }, + { + "x": 0.97722998142937, + "y": 6.902862935264242, + "heading": 3.135568639860519, + "angularVelocity": 3.33166145502926e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.4605954386994315 + }, + { + "x": 1.0346910538227838, + "y": 6.906133090651703, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600659908063e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233301, + "timestamp": 0.4960258570609262 + }, + { + "x": 1.0921521262161977, + "y": 6.9094032460391634, + "heading": 3.135568639860519, + "angularVelocity": 3.331661460674642e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5314562754224209 + }, + { + "x": 1.1496131986096116, + "y": 6.912673401426624, + "heading": 3.135568639860519, + "angularVelocity": 3.331661475383932e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.5668866937839157 + }, + { + "x": 1.2070742710030253, + "y": 6.915943556814084, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614753921174e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233254, + "timestamp": 0.6023171121454104 + }, + { + "x": 1.2645353433964392, + "y": 6.919213712201545, + "heading": 3.135568639860519, + "angularVelocity": 3.331662731932369e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.092297961432333, + "timestamp": 0.6377475305069051 + }, + { + "x": 1.3219964157898532, + "y": 6.922483867589006, + "heading": 3.135568639860519, + "angularVelocity": 3.3316627143346654e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233393, + "timestamp": 0.6731779488683999 + }, + { + "x": 1.379457488183267, + "y": 6.925754022976466, + "heading": 3.135568639860519, + "angularVelocity": 3.331661483573468e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233157, + "timestamp": 0.7086083672298946 + }, + { + "x": 1.436918560576681, + "y": 6.929024178363926, + "heading": 3.135568639860519, + "angularVelocity": 3.3316614890219236e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233392, + "timestamp": 0.7440387855913894 + }, + { + "x": 1.494379632970095, + "y": 6.932294333751387, + "heading": 3.135568639860519, + "angularVelocity": 3.331662743669949e-22, + "velocityX": 1.6218005615158566, + "velocityY": 0.09229796143233207, + "timestamp": 0.7794692039528841 + }, + { + "x": 1.5518407053635086, + "y": 6.935564489138847, + "heading": 3.135568639860519, + "angularVelocity": 3.331638909213181e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233347, + "timestamp": 0.8148996223143788 + }, + { + "x": 1.6093017777569225, + "y": 6.938834644526308, + "heading": 3.135568639860519, + "angularVelocity": 3.3316600948450887e-22, + "velocityX": 1.6218005615158564, + "velocityY": 0.09229796143233622, + "timestamp": 0.8503300406758736 + }, + { + "x": 1.666762850150336, + "y": 6.942104799913777, + "heading": 3.135568639860519, + "angularVelocity": 3.331671581625241e-22, + "velocityX": 1.621800561515844, + "velocityY": 0.09229796143255317, + "timestamp": 0.8857604590373683 + }, + { + "x": 1.7242239225437261, + "y": 6.945374955301653, + "heading": 3.135568639860519, + "angularVelocity": 3.3334679678699274e-22, + "velocityX": 1.6218005615151887, + "velocityY": 0.09229796144406777, + "timestamp": 0.9211908773988631 + }, + { + "x": 1.7816849946975708, + "y": 6.948645114898682, + "heading": 3.135568639860519, + "angularVelocity": 1.7645788213167404e-22, + "velocityX": 1.6218005547541783, + "velocityY": 0.09229808024460076, + "timestamp": 0.9566212957603578 + }, + { + "x": 1.8178857407766151, + "y": 6.94853231479887, + "heading": 3.1384797599068888, + "angularVelocity": 0.12817174361754557, + "velocityX": 1.5938582646199322, + "velocityY": -0.004966399613447973, + "timestamp": 0.9793339464913666 + }, + { + "x": 1.8540603970824046, + "y": 6.948528771707492, + "heading": 3.1414999940123267, + "angularVelocity": 0.13297585302602363, + "velocityX": 1.592709575567121, + "velocityY": -0.00015599638369279018, + "timestamp": 1.0020465972223755 + }, + { + "x": 1.8901908310793394, + "y": 6.948640868874459, + "heading": 3.1447030512216227, + "angularVelocity": 0.14102524831780386, + "velocityX": 1.5907625413183106, + "velocityY": 0.0049354506567203535, + "timestamp": 1.0247592479533845 + }, + { + "x": 1.926257669084107, + "y": 6.9488776015696425, + "heading": 3.148167194864803, + "angularVelocity": 0.15252044704983372, + "velocityX": 1.5879625162168578, + "velocityY": 0.010422944375284186, + "timestamp": 1.0474718986843934 + }, + { + "x": 1.9622398024006038, + "y": 6.94925072993569, + "heading": 3.151976954699645, + "angularVelocity": 0.16773734954856945, + "velocityX": 1.584233110553301, + "velocityY": 0.01642821749276163, + "timestamp": 1.0701845494154023 + }, + { + "x": 1.9981171224268564, + "y": 6.949774504499785, + "heading": 3.156211459419019, + "angularVelocity": 0.1864381559653407, + "velocityX": 1.5796183567984214, + "velocityY": 0.023060917472754097, + "timestamp": 1.0928972001464112 + }, + { + "x": 2.033847488660219, + "y": 6.950468453992633, + "heading": 3.161038496208879, + "angularVelocity": 0.2125263513725333, + "velocityX": 1.5731482272380108, + "velocityY": 0.03055343478252191, + "timestamp": 1.1156098508774202 + }, + { + "x": 2.0694053227389695, + "y": 6.951354952814641, + "heading": 3.166554769842392, + "angularVelocity": 0.24287229609803831, + "velocityX": 1.565551925218666, + "velocityY": 0.039031059496573456, + "timestamp": 1.138322501608429 + }, + { + "x": 2.104784911670157, + "y": 6.952454847185295, + "heading": 3.1727732170956275, + "angularVelocity": 0.27378782542301205, + "velocityX": 1.5577040896809682, + "velocityY": 0.04842650836667849, + "timestamp": 1.161035152339438 + }, + { + "x": 2.1400047240253706, + "y": 6.9537191803371154, + "heading": 3.1796054213345153, + "angularVelocity": 0.3008105183231903, + "velocityX": 1.550669394441415, + "velocityY": 0.05566647269821489, + "timestamp": 1.183747803070447 + }, + { + "x": 2.175095855891208, + "y": 6.955084615506324, + "heading": 3.18691368664007, + "angularVelocity": 0.3217706903570311, + "velocityX": 1.5450038078527182, + "velocityY": 0.060117825320333544, + "timestamp": 1.2064604538014558 + }, + { + "x": 2.2101380445057046, + "y": 6.956471002562802, + "heading": 3.194369840668752, + "angularVelocity": 0.3282819833311086, + "velocityX": 1.5428489184071512, + "velocityY": 0.06104030185188484, + "timestamp": 1.2291731045324648 + }, + { + "x": 2.2451610724837057, + "y": 6.957849512193169, + "heading": 3.2018529792356185, + "angularVelocity": 0.32947006738627316, + "velocityX": 1.5420053076493287, + "velocityY": 0.060693471963871995, + "timestamp": 1.2518857552634737 + }, + { + "x": 2.2801629678458126, + "y": 6.959221595789265, + "heading": 3.2093707720743723, + "angularVelocity": 0.3309958369804056, + "velocityX": 1.5410748739388525, + "velocityY": 0.060410544429463274, + "timestamp": 1.2745984059944826 + }, + { + "x": 2.315144049729391, + "y": 6.960586594052822, + "heading": 3.2169217131634897, + "angularVelocity": 0.3324552989672944, + "velocityX": 1.540158491312511, + "velocityY": 0.06009858909567994, + "timestamp": 1.2973110567254915 + }, + { + "x": 2.3501040543802802, + "y": 6.961944386116353, + "heading": 3.2245066187504112, + "angularVelocity": 0.3339506989629348, + "velocityX": 1.539230496031878, + "velocityY": 0.05978131216881497, + "timestamp": 1.3200237074565004 + }, + { + "x": 2.3850426980471595, + "y": 6.963294864074251, + "heading": 3.232126373384074, + "angularVelocity": 0.3354850441678861, + "velocityX": 1.5382900076554706, + "velocityY": 0.05945928433854099, + "timestamp": 1.3427363581875094 + }, + { + "x": 2.419959710539657, + "y": 6.964637896582436, + "heading": 3.239781798872735, + "angularVelocity": 0.3370555722150803, + "velocityX": 1.5373376232491214, + "velocityY": 0.05913147364836824, + "timestamp": 1.3654490089185183 + }, + { + "x": 2.454854803951486, + "y": 6.965973356645481, + "heading": 3.247473776388813, + "angularVelocity": 0.3386648968090985, + "velocityX": 1.5363725628109135, + "velocityY": 0.05879807156202029, + "timestamp": 1.3881616596495272 + }, + { + "x": 2.48972762858419, + "y": 6.967301155248667, + "heading": 3.2552034126870217, + "angularVelocity": 0.3403229499609042, + "velocityX": 1.5353921057348447, + "velocityY": 0.05846075030657316, + "timestamp": 1.4108743103805361 + }, + { + "x": 2.5245780070203394, + "y": 6.968621054178632, + "heading": 3.262971149366171, + "angularVelocity": 0.34200044596925644, + "velocityX": 1.5344038372663136, + "velocityY": 0.05811294091547401, + "timestamp": 1.433586961111545 + }, + { + "x": 2.559405532428149, + "y": 6.969932985582836, + "heading": 3.2707782920481354, + "angularVelocity": 0.3437354263236719, + "velocityX": 1.5333976566751537, + "velocityY": 0.05776214409057404, + "timestamp": 1.456299611842554 + }, + { + "x": 2.594207801112396, + "y": 6.97123839692077, + "heading": 3.2786335848605472, + "angularVelocity": 0.34585539598366793, + "velocityX": 1.5322856454060756, + "velocityY": 0.05747507648462563, + "timestamp": 1.4790122625735629 + }, + { + "x": 2.6290202140808105, + "y": 6.972513675689697, + "heading": 3.286405336491994, + "angularVelocity": 0.3421772175995684, + "velocityX": 1.5327322812606092, + "velocityY": 0.05614838990089804, + "timestamp": 1.5017249133045718 + }, + { + "x": 2.7840510289336455, + "y": 6.98699854094784, + "heading": 3.286405287600397, + "angularVelocity": -5.100684883763116e-7, + "velocityX": 1.6173808639626774, + "velocityY": 0.1511154018498607, + "timestamp": 1.5975779175703702 + }, + { + "x": 2.93908160001489, + "y": 7.001486015045406, + "heading": 3.2864052387079807, + "angularVelocity": -5.100770354356606e-7, + "velocityX": 1.6173783207811374, + "velocityY": 0.15114261893547631, + "timestamp": 1.6934309218361685 + }, + { + "x": 3.094112171101734, + "y": 7.015973489083046, + "heading": 3.286405189815564, + "angularVelocity": -5.100770390190521e-7, + "velocityX": 1.617378320839561, + "velocityY": 0.15114261831028078, + "timestamp": 1.7892839261019668 + }, + { + "x": 3.2491427421885786, + "y": 7.030460963120681, + "heading": 3.286405140923147, + "angularVelocity": -5.100770430673946e-7, + "velocityX": 1.617378320839565, + "velocityY": 0.15114261831023715, + "timestamp": 1.8851369303677652 + }, + { + "x": 3.404173313275423, + "y": 7.0449484371583155, + "heading": 3.28640509203073, + "angularVelocity": -5.100770460466358e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023496, + "timestamp": 1.9809899346335635 + }, + { + "x": 3.5592038843622675, + "y": 7.05943591119595, + "heading": 3.286405043138312, + "angularVelocity": -5.100770488976204e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831023276, + "timestamp": 2.076842938899362 + }, + { + "x": 3.714234455449112, + "y": 7.073923385233584, + "heading": 3.2864049942458946, + "angularVelocity": -5.100770524050277e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831023054, + "timestamp": 2.1726959431651602 + }, + { + "x": 3.8692650265359565, + "y": 7.088410859271219, + "heading": 3.2864049453534765, + "angularVelocity": -5.100770565197187e-7, + "velocityX": 1.6173783208395647, + "velocityY": 0.15114261831022827, + "timestamp": 2.2685489474309586 + }, + { + "x": 4.0242955976228005, + "y": 7.102898333308853, + "heading": 3.2864048964610575, + "angularVelocity": -5.100770606946053e-7, + "velocityX": 1.6173783208395642, + "velocityY": 0.15114261831022607, + "timestamp": 2.364401951696757 + }, + { + "x": 4.179326168709644, + "y": 7.117385807346487, + "heading": 3.286404847568639, + "angularVelocity": -5.100770634020749e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.151142618310224, + "timestamp": 2.4602549559625553 + }, + { + "x": 4.334356739796489, + "y": 7.13187328138412, + "heading": 3.2864047986762195, + "angularVelocity": -5.100770666030285e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831022163, + "timestamp": 2.5561079602283536 + }, + { + "x": 4.489387310883332, + "y": 7.146360755421753, + "heading": 3.2864047497838, + "angularVelocity": -5.100770706092546e-7, + "velocityX": 1.617378320839564, + "velocityY": 0.15114261831021944, + "timestamp": 2.651960964494152 + }, + { + "x": 4.644417881970177, + "y": 7.160848229459387, + "heading": 3.28640470089138, + "angularVelocity": -5.100770740673254e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021727, + "timestamp": 2.7478139687599503 + }, + { + "x": 4.79944845305702, + "y": 7.175335703497019, + "heading": 3.28640465199896, + "angularVelocity": -5.100770777885376e-7, + "velocityX": 1.6173783208395638, + "velocityY": 0.15114261831021514, + "timestamp": 2.8436669730257487 + }, + { + "x": 4.954479024143865, + "y": 7.1898231775346515, + "heading": 3.286404603106539, + "angularVelocity": -5.100770810299081e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021283, + "timestamp": 2.939519977291547 + }, + { + "x": 5.109509595230708, + "y": 7.2043106515722855, + "heading": 3.2864045542141187, + "angularVelocity": -5.100770846564483e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.15114261831021056, + "timestamp": 3.0353729815573454 + }, + { + "x": 5.264540166317553, + "y": 7.218798125609918, + "heading": 3.286404505321697, + "angularVelocity": -5.100770889061609e-7, + "velocityX": 1.6173783208395636, + "velocityY": 0.1511426183102084, + "timestamp": 3.1312259858231437 + }, + { + "x": 5.419570737404396, + "y": 7.233285599647549, + "heading": 3.2864044564292754, + "angularVelocity": -5.100770922378371e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020614, + "timestamp": 3.227078990088942 + }, + { + "x": 5.574601308491241, + "y": 7.247773073685181, + "heading": 3.2864044075368533, + "angularVelocity": -5.100770952675197e-7, + "velocityX": 1.6173783208395631, + "velocityY": 0.15114261831020392, + "timestamp": 3.3229319943547404 + }, + { + "x": 5.7296318795780845, + "y": 7.262260547722812, + "heading": 3.286404358644431, + "angularVelocity": -5.100770991840634e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.1511426183102018, + "timestamp": 3.4187849986205388 + }, + { + "x": 5.884662450664929, + "y": 7.276748021760444, + "heading": 3.286404309752008, + "angularVelocity": -5.100771023487481e-7, + "velocityX": 1.617378320839563, + "velocityY": 0.15114261831019962, + "timestamp": 3.514638002886337 + }, + { + "x": 6.039693021751773, + "y": 7.291235495798076, + "heading": 3.286404260859585, + "angularVelocity": -5.100771061559551e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.15114261831019746, + "timestamp": 3.6104910071521354 + }, + { + "x": 6.194723592838616, + "y": 7.305722969835706, + "heading": 3.2864042119671617, + "angularVelocity": -5.100771091660107e-7, + "velocityX": 1.6173783208395627, + "velocityY": 0.1511426183101951, + "timestamp": 3.706344011417934 + }, + { + "x": 6.349754163925461, + "y": 7.320210443873337, + "heading": 3.2864041630747383, + "angularVelocity": -5.100771129976334e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831019293, + "timestamp": 3.802197015683732 + }, + { + "x": 6.504784735012304, + "y": 7.334697917910969, + "heading": 3.286404114182314, + "angularVelocity": -5.100771163404491e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.1511426183101907, + "timestamp": 3.8980500199495305 + }, + { + "x": 6.659815306099149, + "y": 7.349185391948599, + "heading": 3.28640406528989, + "angularVelocity": -5.100771202032421e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.1511426183101885, + "timestamp": 3.993903024215329 + }, + { + "x": 6.814845877185992, + "y": 7.363672865986229, + "heading": 3.2864040163974653, + "angularVelocity": -5.100771233944824e-7, + "velocityX": 1.6173783208395625, + "velocityY": 0.15114261831018627, + "timestamp": 4.089756028481127 + }, + { + "x": 6.969876448272837, + "y": 7.378160340023859, + "heading": 3.2864039675050396, + "angularVelocity": -5.100771270486632e-7, + "velocityX": 1.6173783208395622, + "velocityY": 0.15114261831018408, + "timestamp": 4.1856090327469255 + }, + { + "x": 7.12490701935968, + "y": 7.392647814061489, + "heading": 3.286403918612615, + "angularVelocity": -5.100771304657537e-7, + "velocityX": 1.617378320839562, + "velocityY": 0.15114261831018191, + "timestamp": 4.281462037012724 + }, + { + "x": 7.279937590446525, + "y": 7.407135288099118, + "heading": 3.286403869720189, + "angularVelocity": -5.10077134427039e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.1511426183101796, + "timestamp": 4.377315041278522 + }, + { + "x": 7.4349681615333685, + "y": 7.421622762136748, + "heading": 3.286403820827763, + "angularVelocity": -5.100771387503622e-7, + "velocityX": 1.6173783208395618, + "velocityY": 0.15114261831017736, + "timestamp": 4.473168045544321 + }, + { + "x": 7.589998732620213, + "y": 7.436110236174376, + "heading": 3.2864037719353365, + "angularVelocity": -5.100771410554518e-7, + "velocityX": 1.6173783208395616, + "velocityY": 0.15114261831017525, + "timestamp": 4.569021049810119 + }, + { + "x": 7.745029303707056, + "y": 7.450597710212007, + "heading": 3.28640372304291, + "angularVelocity": -5.100771448287922e-7, + "velocityX": 1.6173783208395598, + "velocityY": 0.1511426183101905, + "timestamp": 4.664874054075917 + }, + { + "x": 7.90005987484092, + "y": 7.465085183746354, + "heading": 3.2864036741504377, + "angularVelocity": -5.1007762076643e-7, + "velocityX": 1.6173783213301043, + "velocityY": 0.15114261305961757, + "timestamp": 4.760727058341716 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": -1.3886935284235506, + "velocityX": 1.0369583915052876, + "velocityY": 0.09044288486721058, + "timestamp": 4.856580062607514 + }, + { + "x": 7.999455451965332, + "y": 7.473754405975342, + "heading": 3.153293227446569, + "angularVelocity": 5.239369635456734e-24, + "velocityX": 2.4359888438060422e-22, + "velocityY": -8.774946070326848e-22, + "timestamp": 4.952433066873312 + }, + { + "x": 7.928671024084623, + "y": 7.451618987983874, + "heading": 3.153293227446569, + "angularVelocity": 2.2670564289793535e-36, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 5.031658209240836 + }, + { + "x": 7.805841482889084, + "y": 7.413208234129291, + "heading": 3.153293227446569, + "angularVelocity": 3.3814188338755363e-34, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 5.1108833516083605 + }, + { + "x": 7.683011941693537, + "y": 7.374797480274705, + "heading": 3.153293227446569, + "angularVelocity": 6.0567834914455396e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.190108493975885 + }, + { + "x": 7.56018240049799, + "y": 7.336386726420119, + "heading": 3.153293227446569, + "angularVelocity": -6.892446902508999e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.269333636343409 + }, + { + "x": 7.437352859302442, + "y": 7.297975972565533, + "heading": 3.153293227446569, + "angularVelocity": -4.625140327997389e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.348558778710933 + }, + { + "x": 7.314523318106895, + "y": 7.2595652187109465, + "heading": 3.153293227446569, + "angularVelocity": 2.407409636834091e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.427783921078457 + }, + { + "x": 7.191693776911348, + "y": 7.22115446485636, + "heading": 3.153293227446569, + "angularVelocity": -8.071167864078362e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.507009063445981 + }, + { + "x": 7.0688642357158, + "y": 7.182743711001775, + "heading": 3.153293227446569, + "angularVelocity": 6.706721989227974e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.586234205813505 + }, + { + "x": 6.946034694520253, + "y": 7.1443329571471885, + "heading": 3.153293227446569, + "angularVelocity": -3.932169261853096e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.665459348181029 + }, + { + "x": 6.823205153324706, + "y": 7.105922203292602, + "heading": 3.153293227446569, + "angularVelocity": -5.711604766985455e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.744684490548553 + }, + { + "x": 6.700375612129158, + "y": 7.067511449438016, + "heading": 3.153293227446569, + "angularVelocity": 9.147104299173202e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.823909632916077 + }, + { + "x": 6.577546070933611, + "y": 7.0291006955834305, + "heading": 3.153293227446569, + "angularVelocity": 4.987399043745874e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.903134775283601 + }, + { + "x": 6.4547165297380635, + "y": 6.990689941728844, + "heading": 3.153293227446569, + "angularVelocity": 1.0726945561042014e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 5.9823599176511255 + }, + { + "x": 6.331886988542516, + "y": 6.952279187874258, + "heading": 3.153293227446569, + "angularVelocity": 1.8749531736864053e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.06158506001865 + }, + { + "x": 6.209057447346969, + "y": 6.913868434019672, + "heading": 3.153293227446569, + "angularVelocity": -8.352063684746622e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.140810202386174 + }, + { + "x": 6.0862279061514215, + "y": 6.875457680165086, + "heading": 3.153293227446569, + "angularVelocity": 6.429027209138877e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.220035344753698 + }, + { + "x": 5.963398364955874, + "y": 6.8370469263105, + "heading": 3.153293227446569, + "angularVelocity": -7.349253697576567e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.299260487121222 + }, + { + "x": 5.840568823760327, + "y": 6.798636172455914, + "heading": 3.153293227446569, + "angularVelocity": -5.316547406258321e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.378485629488746 + }, + { + "x": 5.7177392825647795, + "y": 6.760225418601328, + "heading": 3.153293227446569, + "angularVelocity": 3.261106516242251e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.45771077185627 + }, + { + "x": 5.594909741369232, + "y": 6.721814664746741, + "heading": 3.153293227446569, + "angularVelocity": -2.0479976334874182e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.536935914223794 + }, + { + "x": 5.472080200173685, + "y": 6.683403910892156, + "heading": 3.153293227446569, + "angularVelocity": -7.212902306224552e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.616161056591318 + }, + { + "x": 5.3492506589781375, + "y": 6.64499315703757, + "heading": 3.153293227446569, + "angularVelocity": 2.340395125909778e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.695386198958842 + }, + { + "x": 5.22642111778259, + "y": 6.606582403182983, + "heading": 3.153293227446569, + "angularVelocity": -2.132648663400479e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.774611341326366 + }, + { + "x": 5.103591576587043, + "y": 6.568171649328397, + "heading": 3.153293227446569, + "angularVelocity": -2.6423732915608732e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 6.8538364836938905 + }, + { + "x": 4.980762035391495, + "y": 6.529760895473811, + "heading": 3.153293227446569, + "angularVelocity": -1.5836433593674017e-28, + "velocityX": -1.5503858689927383, + "velocityY": -0.4848303544397451, + "timestamp": 6.933061626061415 + }, + { + "x": 4.857932494195948, + "y": 6.491350141619225, + "heading": 3.153293227446569, + "angularVelocity": 3.462450644614712e-28, + "velocityX": -1.5503858689927386, + "velocityY": -0.4848303544397447, + "timestamp": 7.012286768428939 + }, + { + "x": 4.735102953000401, + "y": 6.452939387764639, + "heading": 3.153293227446569, + "angularVelocity": 9.425461275139336e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.091511910796463 + }, + { + "x": 4.612273411804853, + "y": 6.414528633910053, + "heading": 3.153293227446569, + "angularVelocity": 2.8373243283513945e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.170737053163987 + }, + { + "x": 4.489443870609306, + "y": 6.3761178800554665, + "heading": 3.153293227446569, + "angularVelocity": 4.424546575494848e-31, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.249962195531511 + }, + { + "x": 4.366614329413759, + "y": 6.337707126200881, + "heading": 3.153293227446569, + "angularVelocity": 2.6048393166300173e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.329187337899035 + }, + { + "x": 4.243784788218211, + "y": 6.299296372346295, + "heading": 3.153293227446569, + "angularVelocity": -8.141761105066643e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.408412480266559 + }, + { + "x": 4.120955247022664, + "y": 6.2608856184917085, + "heading": 3.153293227446569, + "angularVelocity": -1.5769317536066289e-37, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.487637622634083 + }, + { + "x": 3.998125705827117, + "y": 6.222474864637123, + "heading": 3.153293227446569, + "angularVelocity": -1.9751877406999063e-34, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.566862765001607 + }, + { + "x": 3.8752961646315702, + "y": 6.184064110782536, + "heading": 3.153293227446569, + "angularVelocity": 6.296656188647329e-32, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.6460879073691315 + }, + { + "x": 3.7524666234360233, + "y": 6.145653356927951, + "heading": 3.153293227446569, + "angularVelocity": 3.349857109653225e-36, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.725313049736656 + }, + { + "x": 3.6296370822404764, + "y": 6.107242603073365, + "heading": 3.153293227446569, + "angularVelocity": 1.1073851910178502e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.80453819210418 + }, + { + "x": 3.5068075410449295, + "y": 6.068831849218778, + "heading": 3.153293227446569, + "angularVelocity": -1.0776722091283702e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.883763334471704 + }, + { + "x": 3.3839779998493826, + "y": 6.030421095364192, + "heading": 3.153293227446569, + "angularVelocity": -2.2657744543933122e-30, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 7.962988476839228 + }, + { + "x": 3.2611484586538357, + "y": 5.992010341509607, + "heading": 3.153293227446569, + "angularVelocity": 6.322058172960319e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.042213619206752 + }, + { + "x": 3.138318917458289, + "y": 5.95359958765502, + "heading": 3.153293227446569, + "angularVelocity": -1.267303885279781e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.121438761574275 + }, + { + "x": 3.015489376262742, + "y": 5.915188833800434, + "heading": 3.153293227446569, + "angularVelocity": 4.539047579816208e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.200663903941798 + }, + { + "x": 2.892659835067195, + "y": 5.876778079945848, + "heading": 3.153293227446569, + "angularVelocity": -2.4352191603126546e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.279889046309322 + }, + { + "x": 2.769830293871648, + "y": 5.838367326091261, + "heading": 3.153293227446569, + "angularVelocity": 1.9777124768596482e-35, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.359114188676845 + }, + { + "x": 2.6470007526761012, + "y": 5.799956572236676, + "heading": 3.153293227446569, + "angularVelocity": 1.6826010379363495e-33, + "velocityX": -1.5503858689927386, + "velocityY": -0.48483035443974487, + "timestamp": 8.438339331044368 + }, + { + "x": 2.5241712114805623, + "y": 5.761545818382093, + "heading": 3.153293227446569, + "angularVelocity": -1.9705474475500694e-33, + "velocityX": -1.5503858689926344, + "velocityY": -0.4848303544397123, + "timestamp": 8.517564473411891 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": 2.748235734113482e-34, + "velocityX": -0.8934591439715088, + "velocityY": -0.2793989045646846, + "timestamp": 8.596789615779414 + }, + { + "x": 2.4533867835998535, + "y": 5.739410400390625, + "heading": 3.153293227446569, + "angularVelocity": -7.888608661057978e-31, + "velocityX": -1.038819230645035e-25, + "velocityY": -4.640517458430809e-27, + "timestamp": 8.676014758146938 + } + ], + "eventMarkers": [] } \ No newline at end of file diff --git a/src/main/deploy/choreo/Test2.traj b/src/main/deploy/choreo/Test2.traj deleted file mode 100644 index 027e49e3..00000000 --- a/src/main/deploy/choreo/Test2.traj +++ /dev/null @@ -1,1219 +0,0 @@ -{ - "samples": [ - { - "x": 0.3137660622596741, - "y": 6.865104675292969, - "heading": 3.135568639860519, - "angularVelocity": 1.3323616061473399e-26, - "velocityX": -9.030177506128314e-25, - "velocityY": 7.578270673418084e-24, - "timestamp": 0 - }, - { - "x": 0.3285749308509426, - "y": 6.865947459797136, - "heading": 3.135568639860519, - "angularVelocity": 3.3337145202478786e-22, - "velocityX": 0.41797046933441173, - "velocityY": 0.023787032249159984, - "timestamp": 0.03543041836149472 - }, - { - "x": 0.35819266531481686, - "y": 6.867633028650745, - "heading": 3.135568639860519, - "angularVelocity": 3.3337147019213317e-22, - "velocityX": 0.8359408619363751, - "velocityY": 0.04757406013141577, - "timestamp": 0.07086083672298944 - }, - { - "x": 0.4026192574953566, - "y": 6.87016138138964, - "heading": 3.135568639860519, - "angularVelocity": 3.3337143389947597e-22, - "velocityX": 1.2539110243423486, - "velocityY": 0.07136107491303612, - "timestamp": 0.10629125508448417 - }, - { - "x": 0.4600803298886451, - "y": 6.873431536777097, - "heading": 3.135568639860519, - "angularVelocity": 3.331660178480041e-22, - "velocityX": 1.621800561512317, - "velocityY": 0.09229796143220789, - "timestamp": 0.14172167344597889 - }, - { - "x": 0.5175414022820589, - "y": 6.8767016921645565, - "heading": 3.135568639860519, - "angularVelocity": 3.3316629368551667e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233462, - "timestamp": 0.1771520918074736 - }, - { - "x": 0.5750024746754727, - "y": 6.879971847552017, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614681891394e-22, - "velocityX": 1.6218005615158566, - "velocityY": 0.09229796143233254, - "timestamp": 0.2125825101689683 - }, - { - "x": 0.6324635470688867, - "y": 6.883242002939478, - "heading": 3.135568639860519, - "angularVelocity": 3.331683104806914e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233347, - "timestamp": 0.24801292853046303 - }, - { - "x": 0.6899246194623005, - "y": 6.886512158326939, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614878052174e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233253, - "timestamp": 0.28344334689195777 - }, - { - "x": 0.7473856918557144, - "y": 6.8897823137144, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614758461554e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233254, - "timestamp": 0.3188737652534525 - }, - { - "x": 0.8048467642491284, - "y": 6.89305246910186, - "heading": 3.135568639860519, - "angularVelocity": 3.331638534198991e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233299, - "timestamp": 0.35430418361494725 - }, - { - "x": 0.8623078366425422, - "y": 6.89632262448932, - "heading": 3.135568639860519, - "angularVelocity": 3.3317076614902997e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233253, - "timestamp": 0.389734601976442 - }, - { - "x": 0.9197689090359561, - "y": 6.899592779876781, - "heading": 3.135568639860519, - "angularVelocity": 3.331661479116866e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233396, - "timestamp": 0.42516502033793674 - }, - { - "x": 0.97722998142937, - "y": 6.902862935264242, - "heading": 3.135568639860519, - "angularVelocity": 3.33166145502926e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233347, - "timestamp": 0.4605954386994315 - }, - { - "x": 1.0346910538227838, - "y": 6.906133090651703, - "heading": 3.135568639860519, - "angularVelocity": 3.3316600659908063e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233301, - "timestamp": 0.4960258570609262 - }, - { - "x": 1.0921521262161977, - "y": 6.9094032460391634, - "heading": 3.135568639860519, - "angularVelocity": 3.331661460674642e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.092297961432333, - "timestamp": 0.5314562754224209 - }, - { - "x": 1.1496131986096116, - "y": 6.912673401426624, - "heading": 3.135568639860519, - "angularVelocity": 3.331661475383932e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.092297961432333, - "timestamp": 0.5668866937839157 - }, - { - "x": 1.2070742710030253, - "y": 6.915943556814084, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614753921174e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233254, - "timestamp": 0.6023171121454104 - }, - { - "x": 1.2645353433964392, - "y": 6.919213712201545, - "heading": 3.135568639860519, - "angularVelocity": 3.331662731932369e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.092297961432333, - "timestamp": 0.6377475305069051 - }, - { - "x": 1.3219964157898532, - "y": 6.922483867589006, - "heading": 3.135568639860519, - "angularVelocity": 3.3316627143346654e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233393, - "timestamp": 0.6731779488683999 - }, - { - "x": 1.379457488183267, - "y": 6.925754022976466, - "heading": 3.135568639860519, - "angularVelocity": 3.331661483573468e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233157, - "timestamp": 0.7086083672298946 - }, - { - "x": 1.436918560576681, - "y": 6.929024178363926, - "heading": 3.135568639860519, - "angularVelocity": 3.3316614890219236e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233392, - "timestamp": 0.7440387855913894 - }, - { - "x": 1.494379632970095, - "y": 6.932294333751387, - "heading": 3.135568639860519, - "angularVelocity": 3.331662743669949e-22, - "velocityX": 1.6218005615158566, - "velocityY": 0.09229796143233207, - "timestamp": 0.7794692039528841 - }, - { - "x": 1.5518407053635086, - "y": 6.935564489138847, - "heading": 3.135568639860519, - "angularVelocity": 3.331638909213181e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233347, - "timestamp": 0.8148996223143788 - }, - { - "x": 1.6093017777569225, - "y": 6.938834644526308, - "heading": 3.135568639860519, - "angularVelocity": 3.3316600948450887e-22, - "velocityX": 1.6218005615158564, - "velocityY": 0.09229796143233622, - "timestamp": 0.8503300406758736 - }, - { - "x": 1.666762850150336, - "y": 6.942104799913777, - "heading": 3.135568639860519, - "angularVelocity": 3.331671581625241e-22, - "velocityX": 1.621800561515844, - "velocityY": 0.09229796143255317, - "timestamp": 0.8857604590373683 - }, - { - "x": 1.7242239225437261, - "y": 6.945374955301653, - "heading": 3.135568639860519, - "angularVelocity": 3.3334679678699274e-22, - "velocityX": 1.6218005615151887, - "velocityY": 0.09229796144406777, - "timestamp": 0.9211908773988631 - }, - { - "x": 1.7816849946975708, - "y": 6.948645114898682, - "heading": 3.135568639860519, - "angularVelocity": 1.7645788213167404e-22, - "velocityX": 1.6218005547541783, - "velocityY": 0.09229808024460076, - "timestamp": 0.9566212957603578 - }, - { - "x": 1.8178857407766151, - "y": 6.94853231479887, - "heading": 3.1384797599068888, - "angularVelocity": 0.12817174361754557, - "velocityX": 1.5938582646199322, - "velocityY": -0.004966399613447973, - "timestamp": 0.9793339464913666 - }, - { - "x": 1.8540603970824046, - "y": 6.948528771707492, - "heading": 3.1414999940123267, - "angularVelocity": 0.13297585302602363, - "velocityX": 1.592709575567121, - "velocityY": -0.00015599638369279018, - "timestamp": 1.0020465972223755 - }, - { - "x": 1.8901908310793394, - "y": 6.948640868874459, - "heading": 3.1447030512216227, - "angularVelocity": 0.14102524831780386, - "velocityX": 1.5907625413183106, - "velocityY": 0.0049354506567203535, - "timestamp": 1.0247592479533845 - }, - { - "x": 1.926257669084107, - "y": 6.9488776015696425, - "heading": 3.148167194864803, - "angularVelocity": 0.15252044704983372, - "velocityX": 1.5879625162168578, - "velocityY": 0.010422944375284186, - "timestamp": 1.0474718986843934 - }, - { - "x": 1.9622398024006038, - "y": 6.94925072993569, - "heading": 3.151976954699645, - "angularVelocity": 0.16773734954856945, - "velocityX": 1.584233110553301, - "velocityY": 0.01642821749276163, - "timestamp": 1.0701845494154023 - }, - { - "x": 1.9981171224268564, - "y": 6.949774504499785, - "heading": 3.156211459419019, - "angularVelocity": 0.1864381559653407, - "velocityX": 1.5796183567984214, - "velocityY": 0.023060917472754097, - "timestamp": 1.0928972001464112 - }, - { - "x": 2.033847488660219, - "y": 6.950468453992633, - "heading": 3.161038496208879, - "angularVelocity": 0.2125263513725333, - "velocityX": 1.5731482272380108, - "velocityY": 0.03055343478252191, - "timestamp": 1.1156098508774202 - }, - { - "x": 2.0694053227389695, - "y": 6.951354952814641, - "heading": 3.166554769842392, - "angularVelocity": 0.24287229609803831, - "velocityX": 1.565551925218666, - "velocityY": 0.039031059496573456, - "timestamp": 1.138322501608429 - }, - { - "x": 2.104784911670157, - "y": 6.952454847185295, - "heading": 3.1727732170956275, - "angularVelocity": 0.27378782542301205, - "velocityX": 1.5577040896809682, - "velocityY": 0.04842650836667849, - "timestamp": 1.161035152339438 - }, - { - "x": 2.1400047240253706, - "y": 6.9537191803371154, - "heading": 3.1796054213345153, - "angularVelocity": 0.3008105183231903, - "velocityX": 1.550669394441415, - "velocityY": 0.05566647269821489, - "timestamp": 1.183747803070447 - }, - { - "x": 2.175095855891208, - "y": 6.955084615506324, - "heading": 3.18691368664007, - "angularVelocity": 0.3217706903570311, - "velocityX": 1.5450038078527182, - "velocityY": 0.060117825320333544, - "timestamp": 1.2064604538014558 - }, - { - "x": 2.2101380445057046, - "y": 6.956471002562802, - "heading": 3.194369840668752, - "angularVelocity": 0.3282819833311086, - "velocityX": 1.5428489184071512, - "velocityY": 0.06104030185188484, - "timestamp": 1.2291731045324648 - }, - { - "x": 2.2451610724837057, - "y": 6.957849512193169, - "heading": 3.2018529792356185, - "angularVelocity": 0.32947006738627316, - "velocityX": 1.5420053076493287, - "velocityY": 0.060693471963871995, - "timestamp": 1.2518857552634737 - }, - { - "x": 2.2801629678458126, - "y": 6.959221595789265, - "heading": 3.2093707720743723, - "angularVelocity": 0.3309958369804056, - "velocityX": 1.5410748739388525, - "velocityY": 0.060410544429463274, - "timestamp": 1.2745984059944826 - }, - { - "x": 2.315144049729391, - "y": 6.960586594052822, - "heading": 3.2169217131634897, - "angularVelocity": 0.3324552989672944, - "velocityX": 1.540158491312511, - "velocityY": 0.06009858909567994, - "timestamp": 1.2973110567254915 - }, - { - "x": 2.3501040543802802, - "y": 6.961944386116353, - "heading": 3.2245066187504112, - "angularVelocity": 0.3339506989629348, - "velocityX": 1.539230496031878, - "velocityY": 0.05978131216881497, - "timestamp": 1.3200237074565004 - }, - { - "x": 2.3850426980471595, - "y": 6.963294864074251, - "heading": 3.232126373384074, - "angularVelocity": 0.3354850441678861, - "velocityX": 1.5382900076554706, - "velocityY": 0.05945928433854099, - "timestamp": 1.3427363581875094 - }, - { - "x": 2.419959710539657, - "y": 6.964637896582436, - "heading": 3.239781798872735, - "angularVelocity": 0.3370555722150803, - "velocityX": 1.5373376232491214, - "velocityY": 0.05913147364836824, - "timestamp": 1.3654490089185183 - }, - { - "x": 2.454854803951486, - "y": 6.965973356645481, - "heading": 3.247473776388813, - "angularVelocity": 0.3386648968090985, - "velocityX": 1.5363725628109135, - "velocityY": 0.05879807156202029, - "timestamp": 1.3881616596495272 - }, - { - "x": 2.48972762858419, - "y": 6.967301155248667, - "heading": 3.2552034126870217, - "angularVelocity": 0.3403229499609042, - "velocityX": 1.5353921057348447, - "velocityY": 0.05846075030657316, - "timestamp": 1.4108743103805361 - }, - { - "x": 2.5245780070203394, - "y": 6.968621054178632, - "heading": 3.262971149366171, - "angularVelocity": 0.34200044596925644, - "velocityX": 1.5344038372663136, - "velocityY": 0.05811294091547401, - "timestamp": 1.433586961111545 - }, - { - "x": 2.559405532428149, - "y": 6.969932985582836, - "heading": 3.2707782920481354, - "angularVelocity": 0.3437354263236719, - "velocityX": 1.5333976566751537, - "velocityY": 0.05776214409057404, - "timestamp": 1.456299611842554 - }, - { - "x": 2.594207801112396, - "y": 6.97123839692077, - "heading": 3.2786335848605472, - "angularVelocity": 0.34585539598366793, - "velocityX": 1.5322856454060756, - "velocityY": 0.05747507648462563, - "timestamp": 1.4790122625735629 - }, - { - "x": 2.6290202140808105, - "y": 6.972513675689697, - "heading": 3.286405336491994, - "angularVelocity": 0.3421772175995684, - "velocityX": 1.5327322812606092, - "velocityY": 0.05614838990089804, - "timestamp": 1.5017249133045718 - }, - { - "x": 2.7840510289336455, - "y": 6.98699854094784, - "heading": 3.286405287600397, - "angularVelocity": -5.100684883763116e-7, - "velocityX": 1.6173808639626774, - "velocityY": 0.1511154018498607, - "timestamp": 1.5975779175703702 - }, - { - "x": 2.93908160001489, - "y": 7.001486015045406, - "heading": 3.2864052387079807, - "angularVelocity": -5.100770354356606e-7, - "velocityX": 1.6173783207811374, - "velocityY": 0.15114261893547631, - "timestamp": 1.6934309218361685 - }, - { - "x": 3.094112171101734, - "y": 7.015973489083046, - "heading": 3.286405189815564, - "angularVelocity": -5.100770390190521e-7, - "velocityX": 1.617378320839561, - "velocityY": 0.15114261831028078, - "timestamp": 1.7892839261019668 - }, - { - "x": 3.2491427421885786, - "y": 7.030460963120681, - "heading": 3.286405140923147, - "angularVelocity": -5.100770430673946e-7, - "velocityX": 1.617378320839565, - "velocityY": 0.15114261831023715, - "timestamp": 1.8851369303677652 - }, - { - "x": 3.404173313275423, - "y": 7.0449484371583155, - "heading": 3.28640509203073, - "angularVelocity": -5.100770460466358e-7, - "velocityX": 1.6173783208395647, - "velocityY": 0.15114261831023496, - "timestamp": 1.9809899346335635 - }, - { - "x": 3.5592038843622675, - "y": 7.05943591119595, - "heading": 3.286405043138312, - "angularVelocity": -5.100770488976204e-7, - "velocityX": 1.6173783208395647, - "velocityY": 0.15114261831023276, - "timestamp": 2.076842938899362 - }, - { - "x": 3.714234455449112, - "y": 7.073923385233584, - "heading": 3.2864049942458946, - "angularVelocity": -5.100770524050277e-7, - "velocityX": 1.6173783208395642, - "velocityY": 0.15114261831023054, - "timestamp": 2.1726959431651602 - }, - { - "x": 3.8692650265359565, - "y": 7.088410859271219, - "heading": 3.2864049453534765, - "angularVelocity": -5.100770565197187e-7, - "velocityX": 1.6173783208395647, - "velocityY": 0.15114261831022827, - "timestamp": 2.2685489474309586 - }, - { - "x": 4.0242955976228005, - "y": 7.102898333308853, - "heading": 3.2864048964610575, - "angularVelocity": -5.100770606946053e-7, - "velocityX": 1.6173783208395642, - "velocityY": 0.15114261831022607, - "timestamp": 2.364401951696757 - }, - { - "x": 4.179326168709644, - "y": 7.117385807346487, - "heading": 3.286404847568639, - "angularVelocity": -5.100770634020749e-7, - "velocityX": 1.617378320839564, - "velocityY": 0.151142618310224, - "timestamp": 2.4602549559625553 - }, - { - "x": 4.334356739796489, - "y": 7.13187328138412, - "heading": 3.2864047986762195, - "angularVelocity": -5.100770666030285e-7, - "velocityX": 1.617378320839564, - "velocityY": 0.15114261831022163, - "timestamp": 2.5561079602283536 - }, - { - "x": 4.489387310883332, - "y": 7.146360755421753, - "heading": 3.2864047497838, - "angularVelocity": -5.100770706092546e-7, - "velocityX": 1.617378320839564, - "velocityY": 0.15114261831021944, - "timestamp": 2.651960964494152 - }, - { - "x": 4.644417881970177, - "y": 7.160848229459387, - "heading": 3.28640470089138, - "angularVelocity": -5.100770740673254e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.15114261831021727, - "timestamp": 2.7478139687599503 - }, - { - "x": 4.79944845305702, - "y": 7.175335703497019, - "heading": 3.28640465199896, - "angularVelocity": -5.100770777885376e-7, - "velocityX": 1.6173783208395638, - "velocityY": 0.15114261831021514, - "timestamp": 2.8436669730257487 - }, - { - "x": 4.954479024143865, - "y": 7.1898231775346515, - "heading": 3.286404603106539, - "angularVelocity": -5.100770810299081e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.15114261831021283, - "timestamp": 2.939519977291547 - }, - { - "x": 5.109509595230708, - "y": 7.2043106515722855, - "heading": 3.2864045542141187, - "angularVelocity": -5.100770846564483e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.15114261831021056, - "timestamp": 3.0353729815573454 - }, - { - "x": 5.264540166317553, - "y": 7.218798125609918, - "heading": 3.286404505321697, - "angularVelocity": -5.100770889061609e-7, - "velocityX": 1.6173783208395636, - "velocityY": 0.1511426183102084, - "timestamp": 3.1312259858231437 - }, - { - "x": 5.419570737404396, - "y": 7.233285599647549, - "heading": 3.2864044564292754, - "angularVelocity": -5.100770922378371e-7, - "velocityX": 1.6173783208395631, - "velocityY": 0.15114261831020614, - "timestamp": 3.227078990088942 - }, - { - "x": 5.574601308491241, - "y": 7.247773073685181, - "heading": 3.2864044075368533, - "angularVelocity": -5.100770952675197e-7, - "velocityX": 1.6173783208395631, - "velocityY": 0.15114261831020392, - "timestamp": 3.3229319943547404 - }, - { - "x": 5.7296318795780845, - "y": 7.262260547722812, - "heading": 3.286404358644431, - "angularVelocity": -5.100770991840634e-7, - "velocityX": 1.617378320839563, - "velocityY": 0.1511426183102018, - "timestamp": 3.4187849986205388 - }, - { - "x": 5.884662450664929, - "y": 7.276748021760444, - "heading": 3.286404309752008, - "angularVelocity": -5.100771023487481e-7, - "velocityX": 1.617378320839563, - "velocityY": 0.15114261831019962, - "timestamp": 3.514638002886337 - }, - { - "x": 6.039693021751773, - "y": 7.291235495798076, - "heading": 3.286404260859585, - "angularVelocity": -5.100771061559551e-7, - "velocityX": 1.6173783208395627, - "velocityY": 0.15114261831019746, - "timestamp": 3.6104910071521354 - }, - { - "x": 6.194723592838616, - "y": 7.305722969835706, - "heading": 3.2864042119671617, - "angularVelocity": -5.100771091660107e-7, - "velocityX": 1.6173783208395627, - "velocityY": 0.1511426183101951, - "timestamp": 3.706344011417934 - }, - { - "x": 6.349754163925461, - "y": 7.320210443873337, - "heading": 3.2864041630747383, - "angularVelocity": -5.100771129976334e-7, - "velocityX": 1.6173783208395625, - "velocityY": 0.15114261831019293, - "timestamp": 3.802197015683732 - }, - { - "x": 6.504784735012304, - "y": 7.334697917910969, - "heading": 3.286404114182314, - "angularVelocity": -5.100771163404491e-7, - "velocityX": 1.6173783208395625, - "velocityY": 0.1511426183101907, - "timestamp": 3.8980500199495305 - }, - { - "x": 6.659815306099149, - "y": 7.349185391948599, - "heading": 3.28640406528989, - "angularVelocity": -5.100771202032421e-7, - "velocityX": 1.6173783208395622, - "velocityY": 0.1511426183101885, - "timestamp": 3.993903024215329 - }, - { - "x": 6.814845877185992, - "y": 7.363672865986229, - "heading": 3.2864040163974653, - "angularVelocity": -5.100771233944824e-7, - "velocityX": 1.6173783208395625, - "velocityY": 0.15114261831018627, - "timestamp": 4.089756028481127 - }, - { - "x": 6.969876448272837, - "y": 7.378160340023859, - "heading": 3.2864039675050396, - "angularVelocity": -5.100771270486632e-7, - "velocityX": 1.6173783208395622, - "velocityY": 0.15114261831018408, - "timestamp": 4.1856090327469255 - }, - { - "x": 7.12490701935968, - "y": 7.392647814061489, - "heading": 3.286403918612615, - "angularVelocity": -5.100771304657537e-7, - "velocityX": 1.617378320839562, - "velocityY": 0.15114261831018191, - "timestamp": 4.281462037012724 - }, - { - "x": 7.279937590446525, - "y": 7.407135288099118, - "heading": 3.286403869720189, - "angularVelocity": -5.10077134427039e-7, - "velocityX": 1.6173783208395618, - "velocityY": 0.1511426183101796, - "timestamp": 4.377315041278522 - }, - { - "x": 7.4349681615333685, - "y": 7.421622762136748, - "heading": 3.286403820827763, - "angularVelocity": -5.100771387503622e-7, - "velocityX": 1.6173783208395618, - "velocityY": 0.15114261831017736, - "timestamp": 4.473168045544321 - }, - { - "x": 7.589998732620213, - "y": 7.436110236174376, - "heading": 3.2864037719353365, - "angularVelocity": -5.100771410554518e-7, - "velocityX": 1.6173783208395616, - "velocityY": 0.15114261831017525, - "timestamp": 4.569021049810119 - }, - { - "x": 7.745029303707056, - "y": 7.450597710212007, - "heading": 3.28640372304291, - "angularVelocity": -5.100771448287922e-7, - "velocityX": 1.6173783208395598, - "velocityY": 0.1511426183101905, - "timestamp": 4.664874054075917 - }, - { - "x": 7.90005987484092, - "y": 7.465085183746354, - "heading": 3.2864036741504377, - "angularVelocity": -5.1007762076643e-7, - "velocityX": 1.6173783213301043, - "velocityY": 0.15114261305961757, - "timestamp": 4.760727058341716 - }, - { - "x": 7.999455451965332, - "y": 7.473754405975342, - "heading": 3.153293227446569, - "angularVelocity": -1.3886935284235506, - "velocityX": 1.0369583915052876, - "velocityY": 0.09044288486721058, - "timestamp": 4.856580062607514 - }, - { - "x": 7.999455451965332, - "y": 7.473754405975342, - "heading": 3.153293227446569, - "angularVelocity": 5.239369635456734e-24, - "velocityX": 2.4359888438060422e-22, - "velocityY": -8.774946070326848e-22, - "timestamp": 4.952433066873312 - }, - { - "x": 7.928671024084623, - "y": 7.451618987983874, - "heading": 3.153293227446569, - "angularVelocity": 2.2670564289793535e-36, - "velocityX": -0.8934591439715088, - "velocityY": -0.2793989045646846, - "timestamp": 5.031658209240836 - }, - { - "x": 7.805841482889084, - "y": 7.413208234129291, - "heading": 3.153293227446569, - "angularVelocity": 3.3814188338755363e-34, - "velocityX": -1.5503858689926344, - "velocityY": -0.4848303544397123, - "timestamp": 5.1108833516083605 - }, - { - "x": 7.683011941693537, - "y": 7.374797480274705, - "heading": 3.153293227446569, - "angularVelocity": 6.0567834914455396e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.190108493975885 - }, - { - "x": 7.56018240049799, - "y": 7.336386726420119, - "heading": 3.153293227446569, - "angularVelocity": -6.892446902508999e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.269333636343409 - }, - { - "x": 7.437352859302442, - "y": 7.297975972565533, - "heading": 3.153293227446569, - "angularVelocity": -4.625140327997389e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.348558778710933 - }, - { - "x": 7.314523318106895, - "y": 7.2595652187109465, - "heading": 3.153293227446569, - "angularVelocity": 2.407409636834091e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.427783921078457 - }, - { - "x": 7.191693776911348, - "y": 7.22115446485636, - "heading": 3.153293227446569, - "angularVelocity": -8.071167864078362e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.507009063445981 - }, - { - "x": 7.0688642357158, - "y": 7.182743711001775, - "heading": 3.153293227446569, - "angularVelocity": 6.706721989227974e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.586234205813505 - }, - { - "x": 6.946034694520253, - "y": 7.1443329571471885, - "heading": 3.153293227446569, - "angularVelocity": -3.932169261853096e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.665459348181029 - }, - { - "x": 6.823205153324706, - "y": 7.105922203292602, - "heading": 3.153293227446569, - "angularVelocity": -5.711604766985455e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.744684490548553 - }, - { - "x": 6.700375612129158, - "y": 7.067511449438016, - "heading": 3.153293227446569, - "angularVelocity": 9.147104299173202e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.823909632916077 - }, - { - "x": 6.577546070933611, - "y": 7.0291006955834305, - "heading": 3.153293227446569, - "angularVelocity": 4.987399043745874e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.903134775283601 - }, - { - "x": 6.4547165297380635, - "y": 6.990689941728844, - "heading": 3.153293227446569, - "angularVelocity": 1.0726945561042014e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 5.9823599176511255 - }, - { - "x": 6.331886988542516, - "y": 6.952279187874258, - "heading": 3.153293227446569, - "angularVelocity": 1.8749531736864053e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.06158506001865 - }, - { - "x": 6.209057447346969, - "y": 6.913868434019672, - "heading": 3.153293227446569, - "angularVelocity": -8.352063684746622e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.140810202386174 - }, - { - "x": 6.0862279061514215, - "y": 6.875457680165086, - "heading": 3.153293227446569, - "angularVelocity": 6.429027209138877e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.220035344753698 - }, - { - "x": 5.963398364955874, - "y": 6.8370469263105, - "heading": 3.153293227446569, - "angularVelocity": -7.349253697576567e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.299260487121222 - }, - { - "x": 5.840568823760327, - "y": 6.798636172455914, - "heading": 3.153293227446569, - "angularVelocity": -5.316547406258321e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.378485629488746 - }, - { - "x": 5.7177392825647795, - "y": 6.760225418601328, - "heading": 3.153293227446569, - "angularVelocity": 3.261106516242251e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.45771077185627 - }, - { - "x": 5.594909741369232, - "y": 6.721814664746741, - "heading": 3.153293227446569, - "angularVelocity": -2.0479976334874182e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.536935914223794 - }, - { - "x": 5.472080200173685, - "y": 6.683403910892156, - "heading": 3.153293227446569, - "angularVelocity": -7.212902306224552e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.616161056591318 - }, - { - "x": 5.3492506589781375, - "y": 6.64499315703757, - "heading": 3.153293227446569, - "angularVelocity": 2.340395125909778e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.695386198958842 - }, - { - "x": 5.22642111778259, - "y": 6.606582403182983, - "heading": 3.153293227446569, - "angularVelocity": -2.132648663400479e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.774611341326366 - }, - { - "x": 5.103591576587043, - "y": 6.568171649328397, - "heading": 3.153293227446569, - "angularVelocity": -2.6423732915608732e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 6.8538364836938905 - }, - { - "x": 4.980762035391495, - "y": 6.529760895473811, - "heading": 3.153293227446569, - "angularVelocity": -1.5836433593674017e-28, - "velocityX": -1.5503858689927383, - "velocityY": -0.4848303544397451, - "timestamp": 6.933061626061415 - }, - { - "x": 4.857932494195948, - "y": 6.491350141619225, - "heading": 3.153293227446569, - "angularVelocity": 3.462450644614712e-28, - "velocityX": -1.5503858689927386, - "velocityY": -0.4848303544397447, - "timestamp": 7.012286768428939 - }, - { - "x": 4.735102953000401, - "y": 6.452939387764639, - "heading": 3.153293227446569, - "angularVelocity": 9.425461275139336e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.091511910796463 - }, - { - "x": 4.612273411804853, - "y": 6.414528633910053, - "heading": 3.153293227446569, - "angularVelocity": 2.8373243283513945e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.170737053163987 - }, - { - "x": 4.489443870609306, - "y": 6.3761178800554665, - "heading": 3.153293227446569, - "angularVelocity": 4.424546575494848e-31, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.249962195531511 - }, - { - "x": 4.366614329413759, - "y": 6.337707126200881, - "heading": 3.153293227446569, - "angularVelocity": 2.6048393166300173e-30, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.329187337899035 - }, - { - "x": 4.243784788218211, - "y": 6.299296372346295, - "heading": 3.153293227446569, - "angularVelocity": -8.141761105066643e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.408412480266559 - }, - { - "x": 4.120955247022664, - "y": 6.2608856184917085, - "heading": 3.153293227446569, - "angularVelocity": -1.5769317536066289e-37, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.487637622634083 - }, - { - "x": 3.998125705827117, - "y": 6.222474864637123, - "heading": 3.153293227446569, - "angularVelocity": -1.9751877406999063e-34, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.566862765001607 - }, - { - "x": 3.8752961646315702, - "y": 6.184064110782536, - "heading": 3.153293227446569, - "angularVelocity": 6.296656188647329e-32, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.6460879073691315 - }, - { - "x": 3.7524666234360233, - "y": 6.145653356927951, - "heading": 3.153293227446569, - "angularVelocity": 3.349857109653225e-36, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.725313049736656 - }, - { - "x": 3.6296370822404764, - "y": 6.107242603073365, - "heading": 3.153293227446569, - "angularVelocity": 1.1073851910178502e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.80453819210418 - }, - { - "x": 3.5068075410449295, - "y": 6.068831849218778, - "heading": 3.153293227446569, - "angularVelocity": -1.0776722091283702e-30, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.883763334471704 - }, - { - "x": 3.3839779998493826, - "y": 6.030421095364192, - "heading": 3.153293227446569, - "angularVelocity": -2.2657744543933122e-30, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 7.962988476839228 - }, - { - "x": 3.2611484586538357, - "y": 5.992010341509607, - "heading": 3.153293227446569, - "angularVelocity": 6.322058172960319e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.042213619206752 - }, - { - "x": 3.138318917458289, - "y": 5.95359958765502, - "heading": 3.153293227446569, - "angularVelocity": -1.267303885279781e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.121438761574275 - }, - { - "x": 3.015489376262742, - "y": 5.915188833800434, - "heading": 3.153293227446569, - "angularVelocity": 4.539047579816208e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.200663903941798 - }, - { - "x": 2.892659835067195, - "y": 5.876778079945848, - "heading": 3.153293227446569, - "angularVelocity": -2.4352191603126546e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.279889046309322 - }, - { - "x": 2.769830293871648, - "y": 5.838367326091261, - "heading": 3.153293227446569, - "angularVelocity": 1.9777124768596482e-35, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.359114188676845 - }, - { - "x": 2.6470007526761012, - "y": 5.799956572236676, - "heading": 3.153293227446569, - "angularVelocity": 1.6826010379363495e-33, - "velocityX": -1.5503858689927386, - "velocityY": -0.48483035443974487, - "timestamp": 8.438339331044368 - }, - { - "x": 2.5241712114805623, - "y": 5.761545818382093, - "heading": 3.153293227446569, - "angularVelocity": -1.9705474475500694e-33, - "velocityX": -1.5503858689926344, - "velocityY": -0.4848303544397123, - "timestamp": 8.517564473411891 - }, - { - "x": 2.4533867835998535, - "y": 5.739410400390625, - "heading": 3.153293227446569, - "angularVelocity": 2.748235734113482e-34, - "velocityX": -0.8934591439715088, - "velocityY": -0.2793989045646846, - "timestamp": 8.596789615779414 - }, - { - "x": 2.4533867835998535, - "y": 5.739410400390625, - "heading": 3.153293227446569, - "angularVelocity": -7.888608661057978e-31, - "velocityX": -1.038819230645035e-25, - "velocityY": -4.640517458430809e-27, - "timestamp": 8.676014758146938 - } - ] -} \ No newline at end of file diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 3e721756..c2a8d5be 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -46,7 +46,8 @@ public class AimbotUtils { /** Linear interpolation tables for aiming */ public static double getWristAngle(double distance) { // return m_angleLerpTable.get(distance); - return 52.409 - ((0.1224 + m_offsetNudge.get()) * distance); +// return 52.409 - ((0.1224 + m_offsetNudge.get()) * distance); + return 48.903-(0.09001 * distance); } public static double getLeftSpeed(double distance) { @@ -63,12 +64,17 @@ public static double getDistanceFromSpeaker(Pose2d drivePose) { .getDistance(drivePose.getTranslation()); } - /** Gets the angle the drivebase should be at to aim at the speaker */ + /** Gets the angle the drivebase should be at with a default of the speaker */ public static Rotation2d getDrivebaseAimingAngle(Pose2d drivePose) { + return getDrivebaseAimingAngle(drivePose, FieldConstants.CENTER_SPEAKER); + } + + /** Gets the angle the drivebase should be at to aim at the speaker */ + public static Rotation2d getDrivebaseAimingAngle(Pose2d drivePose, Translation3d target) { Transform3d robotToPoint = new Transform3d( new Pose3d(new Pose2d(drivePose.getTranslation(), new Rotation2d())), - new Pose3d(AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER), new Rotation3d())); + new Pose3d(AllianceFlipUtil.apply(target), new Rotation3d())); return Rotation2d.fromRadians( MathUtil.angleModulus( From e4da026ebef84f200cb35e6e91f1af1c216420a8 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Thu, 14 Mar 2024 19:45:27 -0400 Subject: [PATCH 23/30] mockup of shooting on the move --- src/main/java/frc/robot/Constants.java | 4 ++ src/main/java/frc/robot/RobotContainer.java | 35 +++++++++------- .../frc/robot/commands/AimbotCommand.java | 36 +++++++++++++++- .../subsystems/drive/DriveSubsystem.java | 42 +++++++++---------- .../java/lib/utils/FieldRelativeAccel.java | 35 ++++++++++++++++ .../java/lib/utils/FieldRelativeSpeed.java | 28 +++++++++++++ 6 files changed, 139 insertions(+), 41 deletions(-) create mode 100644 src/main/java/lib/utils/FieldRelativeAccel.java create mode 100644 src/main/java/lib/utils/FieldRelativeSpeed.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 6d3aea16..543f4f66 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -12,6 +12,7 @@ // GNU General Public License for more details. package frc.robot; +import com.fasterxml.jackson.core.SerializableString; import edu.wpi.first.math.geometry.*; import com.gos.lib.properties.GosDoubleProperty; import com.pathplanner.lib.path.PathConstraints; @@ -275,6 +276,9 @@ private ArmSetpoints() { } public static class ShooterConstants { + public static final GosDoubleProperty ACCEL_COMP_FACTOR = + new GosDoubleProperty(false, "Shooter/Acceleration Compensation", 0.100); + private ShooterConstants() { throw new IllegalStateException("Static classes should not be constructed"); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc1dda4e..22023469 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -185,23 +185,26 @@ private void configureButtonBindings() { intakeTrigger.whileTrue(m_shooter.intakeCommand(0.75, 0.5, 0.1) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.INTAKE))); - spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false) - .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) - .alongWith(DriveCommands.alignmentDrive( - m_driveSubsystem, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) - ))); +// spinUpTrigger.whileTrue(m_shooter.runShooterVelocity(false) +// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) +// .alongWith(DriveCommands.alignmentDrive( +// m_driveSubsystem, +// () -> -controller.getLeftY(), +// () -> -controller.getLeftX(), +// () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) +// ))); + spinUpTrigger.whileTrue(new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, controller.getHID(), false)); - shootTrigger.whileTrue(m_shooter.runShooterVelocity(true) - .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) - .alongWith(DriveCommands.alignmentDrive( - m_driveSubsystem, - () -> -controller.getLeftY(), - () -> -controller.getLeftX(), - () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) - ))); +// shootTrigger.whileTrue(m_shooter.runShooterVelocity(true) +// .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AUTO_AIM)) +// .alongWith(DriveCommands.alignmentDrive( +// m_driveSubsystem, +// () -> -controller.getLeftY(), +// () -> -controller.getLeftX(), +// () -> AllianceFlipUtil.apply(FieldConstants.CENTER_SPEAKER) +// ))); + + shootTrigger.whileTrue(new AimbotCommand(m_armSubsystem, m_driveSubsystem, m_shooter, controller.getHID(), true)); ampLineupTrigger.whileTrue(m_driveSubsystem.pathfollowFactory(FieldConstants.AMP_LINEUP) .finallyDo(() -> m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP).schedule())) diff --git a/src/main/java/frc/robot/commands/AimbotCommand.java b/src/main/java/frc/robot/commands/AimbotCommand.java index e5685f40..3b586bc5 100644 --- a/src/main/java/frc/robot/commands/AimbotCommand.java +++ b/src/main/java/frc/robot/commands/AimbotCommand.java @@ -4,17 +4,22 @@ import com.gos.lib.properties.pid.WpiPidPropertyBuilder; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.GenericHID; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import frc.robot.Constants; +import frc.robot.Constants.ShooterConstants; import frc.robot.subsystems.arm.ArmSubsystem; import frc.robot.subsystems.drive.DriveSubsystem; import frc.robot.subsystems.shooter.ShooterSubsystem; import lib.utils.AimbotUtils; +import lib.utils.FieldConstants; +import lib.utils.FieldRelativeAccel; +import lib.utils.FieldRelativeSpeed; public class AimbotCommand extends Command { @@ -72,8 +77,35 @@ public void execute() { m_smallProperty.updateIfChanged(); m_fastProperty.updateIfChanged(); + // get the robots velocity and acceleration + FieldRelativeSpeed fieldRelativeSpeed = m_driveSubsystem.getFieldRelativeVelocity(); + FieldRelativeAccel fieldRelativeAccel = m_driveSubsystem.getFieldRelativeAcceleration(); + + // TODO make an actual equation for shot time based on distance + double shotTime = 0.5; + + Translation2d target = FieldConstants.CENTER_SPEAKER.toTranslation2d(); + Translation2d movingTarget = new Translation2d(); + + // loop over movement calcs to better adjust for acceleration + if (false) { + for (int i = 0; i < 5; i++) { + double virtualGoalX = target.getX() + - shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + double virtualGoalY = target.getY() + - shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + + movingTarget = new Translation2d(virtualGoalX, virtualGoalY); + } + } else { + movingTarget = target; + } + // get our desired rotation and error from it - double desiredRotationDegs = AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose()).getDegrees(); + double desiredRotationDegs = + AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose(), + new Translation3d(movingTarget.getX(), movingTarget.getY(), 0.0)) + .getDegrees(); double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees()); // if we're far from our setpoint, move faster diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 19bf6a66..0153f428 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -52,10 +52,7 @@ import lib.logger.DataLogUtil; import lib.logger.DataLogUtil.DataLogTable; -import lib.utils.AimbotUtils; -import lib.utils.AllianceFlipUtil; -import lib.utils.LocalADStarAK; -import lib.utils.PoseEstimator; +import lib.utils.*; import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; @@ -89,7 +86,6 @@ public class DriveSubsystem extends SubsystemBase { new Rotation2d(), new Rotation2d() }; - private Rotation2d m_avgRotationRads = new Rotation2d(); private final VisionSubsystem[] m_cameras; @@ -105,6 +101,9 @@ public class DriveSubsystem extends SubsystemBase { new SwerveModuleState() }; + private FieldRelativeSpeed m_fieldRelVel = new FieldRelativeSpeed(); + private FieldRelativeSpeed m_lastFieldRelVel = new FieldRelativeSpeed(); + private FieldRelativeAccel m_fieldRelAccel = new FieldRelativeAccel(); public DriveSubsystem( GyroIO gyroIO, @@ -246,24 +245,6 @@ public void periodic() { m_rawGyroRotation = m_rawGyroRotation.plus(new Rotation2d(twist.dtheta)); } - // Update average heading, used for aiming - m_prevRotations = new Rotation2d[] { - m_rawGyroRotation, - m_prevRotations[1], - m_prevRotations[2], - m_prevRotations[3], - m_prevRotations[4] - }; - - double totalRotationRads = 0.0; - - for (Rotation2d mPrevRotation : m_prevRotations) { - totalRotationRads += mPrevRotation.getRadians(); - } - - totalRotationRads /= m_prevRotations.length; - m_avgRotationRads = new Rotation2d(totalRotationRads); - // Apply update m_wpiPoseEstimator.updateWithTime(sampleTimestamps[i], m_rawGyroRotation, modulePositions); } @@ -291,6 +272,11 @@ public void periodic() { -AimbotUtils.getDrivebaseAimingAngle(getVisionPose()).getDegrees()); m_field.setRobotPose(getVisionPose()); + + // calculate field relative velocities and acceleration + m_fieldRelVel = new FieldRelativeSpeed(kinematics.toChassisSpeeds(getModuleStates()), getRotation()); + m_fieldRelAccel = new FieldRelativeAccel(m_fieldRelVel, m_lastFieldRelVel, 0.02); + m_lastFieldRelVel = m_fieldRelVel; } /** @@ -386,6 +372,16 @@ public Rotation2d getRotation() { return m_wpiPoseEstimator.getEstimatedPosition().getRotation(); } + /** Returns the robot's field relative velocity, used for shoot on move */ + public FieldRelativeSpeed getFieldRelativeVelocity() { + return m_fieldRelVel; + } + + /** Returns the robot's field relative acceleration, used for shoot on move */ + public FieldRelativeAccel getFieldRelativeAcceleration() { + return m_fieldRelAccel; + } + /** Resets the current odometry pose. */ public void setPose(Pose2d pose) { gyroIO.resetGyro(pose.getRotation().getDegrees()); diff --git a/src/main/java/lib/utils/FieldRelativeAccel.java b/src/main/java/lib/utils/FieldRelativeAccel.java new file mode 100644 index 00000000..ff23b26e --- /dev/null +++ b/src/main/java/lib/utils/FieldRelativeAccel.java @@ -0,0 +1,35 @@ +package lib.utils; + +public class FieldRelativeAccel { + public double ax; + public double ay; + public double alpha; + + public FieldRelativeAccel(double ax, double ay, double alpha) { + this.ax = ax; + this.ay = ay; + this.alpha = alpha; + } + + public FieldRelativeAccel(FieldRelativeSpeed newSpeed, FieldRelativeSpeed oldSpeed, double time) { + this.ax = (newSpeed.vx - oldSpeed.vx) / time; + this.ay = (newSpeed.vy - oldSpeed.vy) / time; + this.alpha = (newSpeed.omega - oldSpeed.omega) / time; + + if (Math.abs(this.ax) > 6.0) { + this.ax = 6.0 * Math.signum(this.ax); + } + if (Math.abs(this.ay) > 6.0) { + this.ay = 6.0 * Math.signum(this.ay); + } + if (Math.abs(this.alpha) > 4 * Math.PI) { + this.alpha = 4 * Math.PI * Math.signum(this.alpha); + } + } + + public FieldRelativeAccel() { + this.ax = 0.0; + this.ay = 0.0; + this.alpha = 0.0; + } +} diff --git a/src/main/java/lib/utils/FieldRelativeSpeed.java b/src/main/java/lib/utils/FieldRelativeSpeed.java new file mode 100644 index 00000000..4a2a46da --- /dev/null +++ b/src/main/java/lib/utils/FieldRelativeSpeed.java @@ -0,0 +1,28 @@ +package lib.utils; + +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; + +public class FieldRelativeSpeed { + public double vx; + public double vy; + public double omega; + + public FieldRelativeSpeed(double vx, double vy, double omega) { + this.vx = vx; + this.vy = vy; + this.omega = omega; + } + + public FieldRelativeSpeed(ChassisSpeeds chassisSpeed, Rotation2d gyro) { + this(chassisSpeed.vxMetersPerSecond * gyro.getCos() - chassisSpeed.vyMetersPerSecond * gyro.getSin(), + chassisSpeed.vyMetersPerSecond * gyro.getCos() + chassisSpeed.vxMetersPerSecond * gyro.getSin(), + chassisSpeed.omegaRadiansPerSecond); + } + + public FieldRelativeSpeed() { + this.vx = 0.0; + this.vy = 0.0; + this.omega = 0.0; + } +} From 968e73122b2fd892f24c3816159b2faeaa655678 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Fri, 15 Mar 2024 20:15:36 -0400 Subject: [PATCH 24/30] better wrist alg and simple ff on aimbot alignment --- src/main/java/frc/robot/RobotContainer.java | 7 +++ .../frc/robot/commands/AimbotCommand.java | 44 +++++++++++++------ .../robot/subsystems/arm/ArmSubsystem.java | 4 +- .../subsystems/shooter/ShooterSubsystem.java | 4 +- .../subsystems/vision/VisionSubsystem.java | 8 ++-- src/main/java/lib/utils/AimbotUtils.java | 3 +- 6 files changed, 47 insertions(+), 23 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 22023469..87189773 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -225,6 +225,13 @@ private void configureButtonBindings() { .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST))); controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); + controller.rightTrigger().whileTrue(m_shooter.runShooterVelocity(false, m_leftRPM.get(), m_rightRPM.get())); + controller.leftTrigger().whileTrue(m_shooter.runShooterVelocity(true, m_leftRPM.get(), m_rightRPM.get())); + controller.pov(0).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE)); + + // 96.240234375 + // 60.029296875 + // 2250 m_driveSubsystem.setDefaultCommand( DriveCommands.joystickDrive( diff --git a/src/main/java/frc/robot/commands/AimbotCommand.java b/src/main/java/frc/robot/commands/AimbotCommand.java index 3b586bc5..41a75888 100644 --- a/src/main/java/frc/robot/commands/AimbotCommand.java +++ b/src/main/java/frc/robot/commands/AimbotCommand.java @@ -2,6 +2,7 @@ import com.gos.lib.properties.pid.PidProperty; import com.gos.lib.properties.pid.WpiPidPropertyBuilder; +import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.controller.PIDController; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; @@ -36,6 +37,8 @@ public class AimbotCommand extends Command { private boolean m_runKicker; + private static final double TOLERENCE_DEGREES = 10.0; + public AimbotCommand(ArmSubsystem armSubsystem, DriveSubsystem driveSubsystem, ShooterSubsystem shooterSubsystem, @@ -49,6 +52,9 @@ public AimbotCommand(ArmSubsystem armSubsystem, m_smallController = new PIDController(0.0, 0.0, 0.0); m_fastController = new PIDController(0.0, 0.0, 0.0); + m_smallController.enableContinuousInput(-180, 180); + m_fastController.enableContinuousInput(-180, 180); + m_smallProperty = new WpiPidPropertyBuilder("Drive/Aimbot Small", false, m_smallController) .addP(1.0) .addI(0.0) @@ -82,18 +88,19 @@ public void execute() { FieldRelativeAccel fieldRelativeAccel = m_driveSubsystem.getFieldRelativeAcceleration(); // TODO make an actual equation for shot time based on distance - double shotTime = 0.5; + double distance = AimbotUtils.getDistanceFromSpeaker(m_driveSubsystem.getVisionPose()); + double shotTime = 1.05; Translation2d target = FieldConstants.CENTER_SPEAKER.toTranslation2d(); Translation2d movingTarget = new Translation2d(); // loop over movement calcs to better adjust for acceleration - if (false) { + if (true) { for (int i = 0; i < 5; i++) { double virtualGoalX = target.getX() - - shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + + shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); double virtualGoalY = target.getY() - - shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + + shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); movingTarget = new Translation2d(virtualGoalX, virtualGoalY); } @@ -103,46 +110,53 @@ public void execute() { // get our desired rotation and error from it double desiredRotationDegs = - AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose(), - new Translation3d(movingTarget.getX(), movingTarget.getY(), 0.0)) + AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose()) .getDegrees(); double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees()); // if we're far from our setpoint, move faster - double omega; - if (error > 15.0) { + double omega;// = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs);; + if (error > 5.0) { omega = m_fastController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); } else { omega = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); } + // add a feedforward component to compensate for horizontal movement + Translation2d linearFieldVelocity = new Translation2d(fieldRelativeSpeed.vx, fieldRelativeSpeed.vy); + Translation2d tangentalVelocity = linearFieldVelocity + .rotateBy(Rotation2d.fromDegrees(-desiredRotationDegs).unaryMinus()); + double tangentalComponent = tangentalVelocity.getX(); + double x = -DriveCommands.setSensitivity(-m_driverController.getLeftY(), 0.25); double y = -DriveCommands.setSensitivity(-m_driverController.getLeftX(), 0.25); + x = MathUtil.applyDeadband(x, 0.1); + y = MathUtil.applyDeadband(y, 0.1); Rotation2d heading; // if red change heading goal if (DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { - heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); - } else { heading = m_driveSubsystem.getRotation(); + } else { + heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); } // Convert to field relative speeds & send command m_driveSubsystem.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( x * Constants.DriveConstants.MAX_LINEAR_SPEED, y * Constants.DriveConstants.MAX_LINEAR_SPEED, - omega * Constants.DriveConstants.MAX_ANGULAR_SPEED, + omega + tangentalComponent * 0.5, heading )); m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM); - m_shooterSubsystem.runShooterVelocity(m_runKicker); + m_shooterSubsystem.runShooterVelocity(m_runKicker).execute(); // set shooter speeds and rumble controller - if (m_shooterSubsystem.atSpeed() && error < 10.0) { + if (m_shooterSubsystem.atSpeed() && error < 15.0) { m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 1.0); } else { m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0); @@ -152,7 +166,9 @@ public void execute() { @Override public void end(boolean interrupted) { m_driveSubsystem.stopWithX(); - m_shooterSubsystem.runShooterVelocity(false, 0.0, 0.0); + m_shooterSubsystem.setShooterPowerLeft(0.0); + m_shooterSubsystem.setShooterPowerRight(0.0); + m_shooterSubsystem.setKickerPower(0.0); m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0); } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 2268c93c..9757d7ef 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -152,8 +152,8 @@ public void handleState() { m_desiredArmPoseDegs = m_desiredArmPoseDegs >= 0 ? m_desiredArmPoseDegs : 0; } case ANTI_DEFENSE -> { - m_desiredArmPoseDegs = 45.0; - m_desiredWristPoseDegs = 35.0; + m_desiredArmPoseDegs = 68.0; + m_desiredWristPoseDegs = 65.0; } case INTAKE -> { m_armVelocityMult = 1.0; diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index ad713901..2cde3bc1 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -170,7 +170,7 @@ public void setupLogging() { } public boolean atSpeed() { - return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 75 - && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 75; + return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 100 + && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 100; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index d2acceb2..faad2dd4 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -33,11 +33,11 @@ public class VisionSubsystem { private AprilTagFieldLayout m_aprilTagFieldLayout; private final String m_name; - private final double xyStdDevCoefficient = Units.inchesToMeters(4.0); - private final double thetaStdDevCoefficient = Units.degreesToRadians(14.0); + private final double xyStdDevCoefficient = Units.inchesToMeters(3.5); + private final double thetaStdDevCoefficient = Units.degreesToRadians(12.0); - private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(2.0); - private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(7.0); + private final double xyStdDevMultiTagCoefficient = Units.inchesToMeters(1.0); + private final double thetaStdDevMultiTagCoefficient = Units.degreesToRadians(5.0); private final PhotonVisionIOInputsAutoLogged inputs = new PhotonVisionIOInputsAutoLogged(); diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index c2a8d5be..a13af2d5 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -47,7 +47,8 @@ public class AimbotUtils { public static double getWristAngle(double distance) { // return m_angleLerpTable.get(distance); // return 52.409 - ((0.1224 + m_offsetNudge.get()) * distance); - return 48.903-(0.09001 * distance); +// return 48.903-(0.09001 * distance); + return 50.218 - (0.1108 * distance); } public static double getLeftSpeed(double distance) { From e9cec3be99607575c9bf43bce124986e16fd8100 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Fri, 15 Mar 2024 20:39:08 -0400 Subject: [PATCH 25/30] track width is different? --- src/main/java/frc/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 543f4f66..61dff654 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -73,7 +73,7 @@ private DriveConstants() { public static final double MAX_LINEAR_SPEED = Units.feetToMeters(17.8); public static final double TRACK_WIDTH_X = Units.inchesToMeters(18.6); - public static final double TRACK_WIDTH_Y = Units.inchesToMeters(18.6); + public static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.6); public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; From cbe52ade94624599a47e0de6e7b5db9e607962db Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Fri, 15 Mar 2024 20:43:21 -0400 Subject: [PATCH 26/30] new wrist angle model --- src/main/java/lib/utils/AimbotUtils.java | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index a13af2d5..28f01d38 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -17,6 +17,8 @@ public class AimbotUtils { private static final LoggedDashboardNumber m_offsetNudge = new LoggedDashboardNumber("Wrist Angle Nudge", 0.01); + private static final double Y_TARGET = 0.35; + static { // angle measurements, meters -> degrees m_angleLerpTable.put(Units.inchesToMeters(18.0), 57.0 + 3.0); @@ -48,7 +50,14 @@ public static double getWristAngle(double distance) { // return m_angleLerpTable.get(distance); // return 52.409 - ((0.1224 + m_offsetNudge.get()) * distance); // return 48.903-(0.09001 * distance); - return 50.218 - (0.1108 * distance); +// return 50.218 - (0.1108 * distance); + if (distance <= 100.0) { + return 49.319 + (1.427 * Y_TARGET) + (-0.1059 * distance); + } else if (100.0 < distance && distance <= 150) { + return 49.319 + (1.427 * Y_TARGET) + (-0.10599 * distance); + } else { + return 50.951 - (0.117 * distance); + } } public static double getLeftSpeed(double distance) { From 094cb922381210f15327d33298fd10303a5eb54f Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 16 Mar 2024 00:08:34 -0400 Subject: [PATCH 27/30] logger for aimbot and fixed swapped drive motor --- src/main/java/frc/robot/Constants.java | 10 +++++----- .../java/frc/robot/commands/AimbotCommand.java | 5 ++++- .../robot/subsystems/drive/DriveSubsystem.java | 2 +- .../robot/subsystems/shooter/ShooterIOKraken.java | 2 +- .../subsystems/shooter/ShooterSubsystem.java | 4 ++-- src/main/java/lib/utils/AimbotUtils.java | 15 +++++++-------- 6 files changed, 20 insertions(+), 18 deletions(-) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 61dff654..921f88a9 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -73,7 +73,7 @@ private DriveConstants() { public static final double MAX_LINEAR_SPEED = Units.feetToMeters(17.8); public static final double TRACK_WIDTH_X = Units.inchesToMeters(18.6); - public static final double TRACK_WIDTH_Y = Units.inchesToMeters(20.6); + public static final double TRACK_WIDTH_Y = Units.inchesToMeters(18.6); public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0); public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS; @@ -124,7 +124,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(-0.017822) + 180, // offset 0.457764 + Units.rotationsToDegrees(-0.019775) + 180, // offset 0.457764 true, // inversion ModuleConstants.GearRatios.L3_KRAKEN ); @@ -135,7 +135,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(-0.453857) + 180, + Units.rotationsToDegrees(-0.451416) + 180, true, ModuleConstants.GearRatios.L3_KRAKEN ); @@ -146,7 +146,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(0.428467) + 180, + Units.rotationsToDegrees(0.429688) + 180, true, ModuleConstants.GearRatios.L3_KRAKEN ); @@ -157,7 +157,7 @@ private DriveConstants() { DRIVE_FF_GAINS, DRIVE_FB_GAINS, TURN_FB_GAINS, - Units.rotationsToDegrees(-0.093750) + 180, + Units.rotationsToDegrees(-0.092285) + 180, true, ModuleConstants.GearRatios.L3_KRAKEN ); diff --git a/src/main/java/frc/robot/commands/AimbotCommand.java b/src/main/java/frc/robot/commands/AimbotCommand.java index 41a75888..63bd826f 100644 --- a/src/main/java/frc/robot/commands/AimbotCommand.java +++ b/src/main/java/frc/robot/commands/AimbotCommand.java @@ -21,6 +21,7 @@ import lib.utils.FieldConstants; import lib.utils.FieldRelativeAccel; import lib.utils.FieldRelativeSpeed; +import org.littletonrobotics.junction.Logger; public class AimbotCommand extends Command { @@ -144,11 +145,13 @@ public void execute() { heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); } + Logger.recordOutput("Aimbot/Tangental Velocity", tangentalComponent); + // Convert to field relative speeds & send command m_driveSubsystem.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( x * Constants.DriveConstants.MAX_LINEAR_SPEED, y * Constants.DriveConstants.MAX_LINEAR_SPEED, - omega + tangentalComponent * 0.5, + omega + tangentalComponent, heading )); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 0153f428..704d66e5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -344,7 +344,7 @@ public double getCharacterizationVelocity() { } /** Returns the module states (turn angles and drive velocities) for all the modules. */ -// @AutoLogOutput(key = "SwerveStates/Measured") + @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { SwerveModuleState[] states = new SwerveModuleState[4]; for (int i = 0; i < 4; i++) { diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java index becc0a86..f77374a9 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java @@ -77,7 +77,7 @@ public ShooterIOKraken() { shooterConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast; m_indexer = TalonFXFactory.createTalon(ShooterConstants.INDEXER_ID, shooterConfig); - m_intake = TalonFXFactory.createTalon(ShooterConstants.INTAKE_ID, shooterConfig); + m_intake = TalonFXFactory.createTalon(ShooterConstants.INTAKE_ID, "rio", shooterConfig); m_kicker = TalonFXFactory.createTalon(ShooterConstants.KICKER_ID, shooterConfig); m_leftProperty = new Phoenix6PidPropertyBuilder("Shooter/Left PID", false, m_leftTalon, 0) diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index 2cde3bc1..a747de73 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -88,8 +88,8 @@ public Command runShooterVelocity(boolean runKicker, double leftRPM, double righ m_leftSpeedSetpoint = leftRPM; m_rightSpeedSetpoint = rightRPM; - m_io.setLeftVelocityRpm(m_leftPower.getValue()); - m_io.setRightVelocityRpm(m_rightPower.getValue()); + m_io.setLeftVelocityRpm(m_leftSpeedSetpoint); + m_io.setRightVelocityRpm(m_rightSpeedSetpoint); if (runKicker) { m_io.setKickerVoltage(12.0); diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 28f01d38..04afe75c 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -50,14 +50,13 @@ public static double getWristAngle(double distance) { // return m_angleLerpTable.get(distance); // return 52.409 - ((0.1224 + m_offsetNudge.get()) * distance); // return 48.903-(0.09001 * distance); -// return 50.218 - (0.1108 * distance); - if (distance <= 100.0) { - return 49.319 + (1.427 * Y_TARGET) + (-0.1059 * distance); - } else if (100.0 < distance && distance <= 150) { - return 49.319 + (1.427 * Y_TARGET) + (-0.10599 * distance); - } else { - return 50.951 - (0.117 * distance); - } +// if (distance <= 100.0) { +// return 50.218 - (0.1108 * distance); +// } else if (100.0 < distance && distance <= 150) { + return 49.319 + (1.427 * Y_TARGET) + (-0.10599 * distance); +// } else { +// return 50.951 - (0.117 * distance); +// } } public static double getLeftSpeed(double distance) { From 91bd7d229f05cf6145a97a7bce1d71db556cf946 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 16 Mar 2024 18:29:27 -0400 Subject: [PATCH 28/30] autos, driver practice, i forgot to commit --- Crescendo2024.chor | 8445 ++++++++++++----- src/main/deploy/choreo/AmpWing3.1.traj | 306 +- src/main/deploy/choreo/AmpWing3.2.traj | 138 +- src/main/deploy/choreo/AmpWing3.traj | 378 +- src/main/deploy/choreo/AmpWing321.1.traj | 419 + src/main/deploy/choreo/AmpWing321.2.traj | 437 + src/main/deploy/choreo/AmpWing321.3.traj | 482 + src/main/deploy/choreo/AmpWing321.traj | 1310 +++ .../deploy/choreo/AmpWing3Contested5.1.traj | 401 + .../deploy/choreo/AmpWing3Contested5.2.traj | 311 + .../deploy/choreo/AmpWing3Contested5.3.traj | 257 + .../deploy/choreo/AmpWing3Contested5.traj | 941 ++ src/main/deploy/choreo/FrontWing1.1.traj | 282 +- src/main/deploy/choreo/FrontWing1.2.traj | 128 +- src/main/deploy/choreo/FrontWing1.traj | 424 +- src/main/deploy/choreo/FrontWing2.1.traj | 148 +- src/main/deploy/choreo/FrontWing2.traj | 148 +- src/main/deploy/choreo/FrontWing3.1.traj | 270 +- src/main/deploy/choreo/FrontWing3.traj | 270 +- .../deploy/choreo/FrontWing3Contested5.1.traj | 279 +- .../deploy/choreo/FrontWing3Contested5.2.traj | 1079 +-- .../deploy/choreo/FrontWing3Contested5.traj | 1356 ++- .../deploy/choreo/SourceContested1.1.traj | 885 +- .../deploy/choreo/SourceContested1.2.traj | 600 +- src/main/deploy/choreo/SourceContested1.traj | 1479 ++- src/main/deploy/choreo/SourceWing1.1.traj | 324 +- src/main/deploy/choreo/SourceWing1.2.traj | 115 +- src/main/deploy/choreo/SourceWing1.traj | 369 +- .../choreo/SourceWing1Contested1.1.traj | 176 + .../choreo/SourceWing1Contested1.2.traj | 131 + .../choreo/SourceWing1Contested1.3.traj | 374 + .../choreo/SourceWing1Contested1.4.traj | 293 + .../deploy/choreo/SourceWing1Contested1.traj | 932 ++ .../deploy/pathplanner/autos/AmpWing3.auto | 50 + .../pathplanner/autos/AmpWing3Contested5.auto | 139 + .../deploy/pathplanner/autos/ChoreoTest2.auto | 37 - .../autos/FrontWing3Contested5.auto | 17 +- .../deploy/pathplanner/autos/New Auto.auto | 37 - .../pathplanner/autos/PathPlannerTest.auto | 19 - .../pathplanner/autos/SourceContested1.auto | 82 + .../deploy/pathplanner/autos/SourceWing1.auto | 56 + .../autos/SourceWing1Contested1.auto | 126 + src/main/java/frc/robot/Constants.java | 4 +- src/main/java/frc/robot/RobotContainer.java | 32 +- .../frc/robot/commands/AimbotCommand.java | 64 +- .../frc/robot/commands/auto/AutoFactory.java | 7 +- .../commands/auto/IntakeControlCommand.java | 20 +- .../commands/auto/ShooterAutoCommand.java | 13 +- .../robot/subsystems/arm/ArmSubsystem.java | 5 + .../subsystems/drive/DriveSubsystem.java | 6 +- .../subsystems/shooter/ShooterIOKraken.java | 4 +- .../subsystems/shooter/ShooterSubsystem.java | 6 +- .../subsystems/vision/VisionSubsystem.java | 4 + src/main/java/lib/utils/AimbotUtils.java | 24 +- 54 files changed, 17085 insertions(+), 7554 deletions(-) create mode 100644 src/main/deploy/choreo/AmpWing321.1.traj create mode 100644 src/main/deploy/choreo/AmpWing321.2.traj create mode 100644 src/main/deploy/choreo/AmpWing321.3.traj create mode 100644 src/main/deploy/choreo/AmpWing321.traj create mode 100644 src/main/deploy/choreo/AmpWing3Contested5.1.traj create mode 100644 src/main/deploy/choreo/AmpWing3Contested5.2.traj create mode 100644 src/main/deploy/choreo/AmpWing3Contested5.3.traj create mode 100644 src/main/deploy/choreo/AmpWing3Contested5.traj create mode 100644 src/main/deploy/choreo/SourceWing1Contested1.1.traj create mode 100644 src/main/deploy/choreo/SourceWing1Contested1.2.traj create mode 100644 src/main/deploy/choreo/SourceWing1Contested1.3.traj create mode 100644 src/main/deploy/choreo/SourceWing1Contested1.4.traj create mode 100644 src/main/deploy/choreo/SourceWing1Contested1.traj create mode 100644 src/main/deploy/pathplanner/autos/AmpWing3.auto create mode 100644 src/main/deploy/pathplanner/autos/AmpWing3Contested5.auto delete mode 100644 src/main/deploy/pathplanner/autos/ChoreoTest2.auto delete mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/PathPlannerTest.auto create mode 100644 src/main/deploy/pathplanner/autos/SourceContested1.auto create mode 100644 src/main/deploy/pathplanner/autos/SourceWing1.auto create mode 100644 src/main/deploy/pathplanner/autos/SourceWing1Contested1.auto diff --git a/Crescendo2024.chor b/Crescendo2024.chor index 761b12bf..044eb081 100644 --- a/Crescendo2024.chor +++ b/Crescendo2024.chor @@ -3,8 +3,8 @@ "robotConfiguration": { "mass": 74.08797700309194, "rotationalInertia": 6, - "motorMaxTorque": 1.7, - "motorMaxVelocity": 1750, + "motorMaxTorque": 0.774863387978142, + "motorMaxVelocity": 4800, "gearing": 5.731, "wheelbase": 0.4724397448825378, "trackWidth": 0.4724397448825378, @@ -39,109 +39,109 @@ "x": 1.2263859510421753, "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": -3.740256292064187e-22, - "velocityX": -5.996486579954623e-19, - "velocityY": 3.7218569679078715e-21, + "angularVelocity": 2.58414356962129e-20, + "velocityX": 2.577520988220736e-18, + "velocityY": -1.6028256561920876e-20, "timestamp": 0 }, { - "x": 1.3163279724628432, - "y": 5.593551858929642, - "heading": 3.140264869702983, - "angularVelocity": -0.007887547277553473, - "velocityX": 0.9650288514473437, - "velocityY": -0.0059937949143052385, - "timestamp": 0.0932013807522681 + "x": 1.272754287158708, + "y": 5.593822495121423, + "heading": 3.1409509910936695, + "angularVelocity": -0.0004947956205997497, + "velocityX": 0.4681363319992983, + "velocityY": -0.0029075951069526523, + "timestamp": 0.09904878760104914 }, { - "x": 1.467723714747352, - "y": 5.592611539861448, - "heading": 3.1402648694385524, - "angularVelocity": -2.83720041018159e-9, - "velocityX": 1.6243937703766675, - "velocityY": -0.010089110918776865, - "timestamp": 0.1864027615045362 + "x": 1.3654909581138261, + "y": 5.593246507588987, + "heading": 3.140852973337843, + "angularVelocity": -0.0009895906673899658, + "velocityX": 0.9362726510782554, + "velocityY": -0.005815190133657064, + "timestamp": 0.19809757520209828 }, { - "x": 1.6191194570316485, - "y": 5.591671220793256, - "heading": 3.1402648691741217, - "angularVelocity": -2.837195579018617e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777356, - "timestamp": 0.2796041422568043 + "x": 1.5045959617651867, + "y": 5.592382526307659, + "heading": 3.140705946827063, + "angularVelocity": -0.001484384759681857, + "velocityX": 1.4044089486233002, + "velocityY": -0.008722785026614378, + "timestamp": 0.29714636280314743 }, { - "x": 1.7705151993162385, - "y": 5.590730901725062, - "heading": 3.1402648689096915, - "angularVelocity": -2.83719540339334e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777356, - "timestamp": 0.3728055230090724 + "x": 1.6900692938625956, + "y": 5.591230551303714, + "heading": 3.140509911748851, + "angularVelocity": -0.001979176958749809, + "velocityX": 1.8725452031004766, + "velocityY": -0.011630379652077128, + "timestamp": 0.39619515040419656 }, { - "x": 1.9219109416013613, - "y": 5.589790582656871, - "heading": 3.1402648686452608, - "angularVelocity": -2.837195245417577e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777356, - "timestamp": 0.46600690376134046 + "x": 1.9219109416008546, + "y": 5.589790582656859, + "heading": 3.1402648686451275, + "angularVelocity": -0.0024739636865663884, + "velocityX": 2.3406813283735364, + "velocityY": -0.014537973475053038, + "timestamp": 0.4952439380052457 }, { - "x": 2.0733066838851757, - "y": 5.588850263588676, - "heading": 3.14026486838083, - "angularVelocity": -2.837195862058808e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777355, - "timestamp": 0.5592082845136086 + "x": 2.1537525893397524, + "y": 5.588350614009982, + "heading": 3.1400198255413954, + "angularVelocity": -0.0024739636866560476, + "velocityX": 2.3406813283735377, + "velocityY": -0.014537973475053236, + "timestamp": 0.5942927256062949 }, { - "x": 2.224702426169452, - "y": 5.58790994452049, - "heading": 3.1402648681163994, - "angularVelocity": -2.8371952366306506e-9, - "velocityX": 1.6243937703767442, - "velocityY": -0.010089110918777355, - "timestamp": 0.6524096652658767 + "x": 2.339225921431913, + "y": 5.587198639006089, + "heading": 3.1398237904631805, + "angularVelocity": -0.0019791769587811803, + "velocityX": 1.8725452031004775, + "velocityY": -0.011630379652077307, + "timestamp": 0.693341513207344 }, { - "x": 2.3760981684546945, - "y": 5.586969625452291, - "heading": 3.140264867851969, - "angularVelocity": -2.8371952652077662e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777358, - "timestamp": 0.7456110460181449 + "x": 2.4783309250888186, + "y": 5.586334657724714, + "heading": 3.1396767639523975, + "angularVelocity": -0.001484384759709258, + "velocityX": 1.4044089486233011, + "velocityY": -0.008722785026614523, + "timestamp": 0.7923903008083932 }, { - "x": 2.5274939107386767, - "y": 5.5860293063840984, - "heading": 3.1402648675875384, - "angularVelocity": -2.837200309142864e-9, - "velocityX": 1.624393770376668, - "velocityY": -0.010089110918776867, - "timestamp": 0.838812426770413 + "x": 2.571067596041391, + "y": 5.585758670192281, + "heading": 3.1395787461965696, + "angularVelocity": -0.000989590667408484, + "velocityX": 0.9362726510782562, + "velocityY": -0.005815190133657172, + "timestamp": 0.8914390884094424 }, { "x": 2.617435932159424, "y": 5.585470676422119, "heading": 3.139529737290238, - "angularVelocity": -0.007887547280595224, - "velocityX": 0.9650288514473356, - "velocityY": -0.0059937949145238275, - "timestamp": 0.9320138075226811 + "angularVelocity": -0.0004947956206057336, + "velocityX": 0.4681363319992992, + "velocityY": -0.002907595106952706, + "timestamp": 0.9904878760104916 }, { "x": 2.617435932159424, "y": 5.585470676422119, "heading": 3.139529737290238, - "angularVelocity": 3.1865843751901534e-21, - "velocityX": -4.755464983211591e-19, - "velocityY": 2.948079481483161e-21, - "timestamp": 1.0252151882749492 + "angularVelocity": 3.961776114386473e-20, + "velocityX": 6.7461684746138455e-19, + "velocityY": -4.213391536884766e-21, + "timestamp": 1.0895366636115407 } ], "trajectoryWaypoints": [ @@ -157,7 +157,7 @@ "controlIntervalCount": 11 }, { - "timestamp": 1.0252151882749492, + "timestamp": 1.0895366636115407, "isStopPoint": true, "x": 2.617435932159424, "y": 5.585470676422119, @@ -201,13 +201,13 @@ "controlIntervalCount": 16 }, { - "x": 2.593973159790039, - "y": 4.3895583152771, + "x": 2.5814027786254883, + "y": 4.346181392669678, "heading": 2.599013995570559, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 10 }, { "x": 1.9637236595153809, @@ -224,226 +224,244 @@ "x": 1.2263859510421753, "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": 4.442897870116303e-27, - "velocityX": 5.36651543276403e-26, - "velocityY": -1.1808751253669755e-25, + "angularVelocity": 8.13803032196424e-26, + "velocityX": -1.9489451876882144e-24, + "velocityY": 8.026375946046321e-25, "timestamp": 0 }, { - "x": 1.2664719468759371, - "y": 5.574445984484986, - "heading": 2.936921539881242, - "angularVelocity": -2.425035961622917, - "velocityX": 0.4763363139734096, - "velocityY": -0.2336705712390479, - "timestamp": 0.08415481805151542 - }, - { - "x": 1.353066220057362, - "y": 5.511048868833387, - "heading": 2.8414585534340224, - "angularVelocity": -1.1343733924869426, - "velocityX": 1.028987706068311, - "velocityY": -0.753339109031048, - "timestamp": 0.16830963610303085 - }, - { - "x": 1.4537891107085041, - "y": 5.418622426082042, - "heading": 2.841458332012736, - "angularVelocity": -0.0000026311183569063204, - "velocityX": 1.196876102678809, - "velocityY": -1.0982905660228062, - "timestamp": 0.25246445415454627 - }, - { - "x": 1.554511990655445, - "y": 5.326195971665319, - "heading": 2.841458110592174, - "angularVelocity": -0.000002631109743829199, - "velocityX": 1.1968759754822746, - "velocityY": -1.0982907046408665, - "timestamp": 0.3366192722060617 - }, - { - "x": 1.6552348706023827, - "y": 5.233769517248598, - "heading": 2.8414578891715823, - "angularVelocity": -0.0000026311100983002847, - "velocityX": 1.196875975482236, - "velocityY": -1.098290704640839, - "timestamp": 0.4207740902575771 - }, - { - "x": 1.7559577505493258, - "y": 5.141343062831889, - "heading": 2.841457667750961, - "angularVelocity": -0.0000026311104525941368, - "velocityX": 1.1968759754823024, - "velocityY": -1.0982907046406973, - "timestamp": 0.5049289083090925 - }, - { - "x": 1.8566806304962746, - "y": 5.048916608415192, - "heading": 2.8414574463303097, - "angularVelocity": -0.0000026311108067721062, - "velocityX": 1.1968759754823688, - "velocityY": -1.0982907046405554, - "timestamp": 0.5890837263606079 - }, - { - "x": 1.957403510443229, - "y": 4.956490153998508, - "heading": 2.841457224909629, - "angularVelocity": -0.000002631111160911123, - "velocityX": 1.1968759754824354, - "velocityY": -1.0982907046404133, - "timestamp": 0.6732385444121234 - }, - { - "x": 2.058126390390189, - "y": 4.864063699581835, - "heading": 2.841457003488918, - "angularVelocity": -0.000002631111515369215, - "velocityX": 1.1968759754825018, - "velocityY": -1.0982907046402715, - "timestamp": 0.7573933624636389 - }, - { - "x": 2.158849270337155, - "y": 4.7716372451651745, - "heading": 2.8414567820681773, - "angularVelocity": -0.0000026311118694940932, - "velocityX": 1.1968759754825686, - "velocityY": -1.0982907046401296, - "timestamp": 0.8415481805151543 - }, - { - "x": 2.2595721502841255, - "y": 4.679210790748526, - "heading": 2.8414565606474067, - "angularVelocity": -0.000002631112223792794, - "velocityX": 1.196875975482635, - "velocityY": -1.0982907046399877, - "timestamp": 0.9257029985666698 - }, - { - "x": 2.3602950302311068, - "y": 4.586784336331894, - "heading": 2.8414563392266063, - "angularVelocity": -0.00000263111257823472, - "velocityX": 1.1968759754827565, - "velocityY": -1.098290704639786, - "timestamp": 1.0098578166181853 - }, - { - "x": 2.4610179157680534, - "y": 4.494357888007175, - "heading": 2.841456117805399, - "angularVelocity": -0.0000026311174148596253, - "velocityX": 1.1968760419075375, - "velocityY": -1.0982906322504282, - "timestamp": 1.0940126346697008 - }, - { - "x": 2.552458083373214, - "y": 4.421585489236337, - "heading": 2.775799514245796, - "angularVelocity": -0.7801882896284313, - "velocityX": 1.0865707956160633, - "velocityY": -0.8647442945725389, - "timestamp": 1.1781674527212163 - }, - { - "x": 2.593973159790039, - "y": 4.3895583152771, + "x": 1.2482482072967283, + "y": 5.573952919421757, + "heading": 3.124773295402233, + "angularVelocity": -0.20280711940248144, + "velocityX": 0.27324224631752764, + "velocityY": -0.2519364652079374, + "timestamp": 0.0800105274685353 + }, + { + "x": 1.2920158355867717, + "y": 5.53361430645858, + "heading": 3.093062693754785, + "angularVelocity": -0.396330366149863, + "velocityX": 0.547023368984232, + "velocityY": -0.504166317101698, + "timestamp": 0.1600210549370706 + }, + { + "x": 1.3577502186593637, + "y": 5.473058444102122, + "heading": 3.04703030236452, + "angularVelocity": -0.5753291828799363, + "velocityX": 0.821571675032918, + "velocityY": -0.7568486831969985, + "timestamp": 0.2400315824056059 + }, + { + "x": 1.4455473443213254, + "y": 5.392224990475853, + "heading": 2.988789648390865, + "angularVelocity": -0.7279123862364011, + "velocityX": 1.0973196707956758, + "velocityY": -1.0102852235045834, + "timestamp": 0.3200421098741412 + }, + { + "x": 1.5555774477752369, + "y": 5.2910218114000225, + "heading": 2.923252756085671, + "angularVelocity": -0.8191033652536196, + "velocityX": 1.3751953266047607, + "velocityY": -1.2648732895259254, + "timestamp": 0.40005263734267654 + }, + { + "x": 1.6878271047063615, + "y": 5.1697931195328435, + "heading": 2.871035349733247, + "angularVelocity": -0.6526316974095567, + "velocityX": 1.6529032005586097, + "velocityY": -1.515159263446336, + "timestamp": 0.48006316481121186 + }, + { + "x": 1.831736570260106, + "y": 5.036624867836762, + "heading": 2.8710353148498213, + "angularVelocity": -4.3598544731556554e-7, + "velocityX": 1.798631631447972, + "velocityY": -1.6643841243072752, + "timestamp": 0.5600736922797471 + }, + { + "x": 1.975646009225953, + "y": 4.903456587408076, + "heading": 2.871035279966796, + "angularVelocity": -4.359804455802179e-7, + "velocityX": 1.7986312991429831, + "velocityY": -1.6643844834175752, + "timestamp": 0.6400842197482824 + }, + { + "x": 2.119555478641212, + "y": 4.770288339884976, + "heading": 2.8710352450833696, + "angularVelocity": -4.359854554729099e-7, + "velocityX": 1.7986316797105584, + "velocityY": -1.6643840721518786, + "timestamp": 0.7200947472168178 + }, + { + "x": 2.2519584690825245, + "y": 4.649115765781956, + "heading": 2.818147172105817, + "angularVelocity": -0.6610139271779065, + "velocityX": 1.6548196172482494, + "velocityY": -1.5144578836912652, + "timestamp": 0.8001052746853531 + }, + { + "x": 2.3620785943033495, + "y": 4.547955940973104, + "heading": 2.7526679608031075, + "angularVelocity": -0.8183824475905371, + "velocityX": 1.3763204506323357, + "velocityY": -1.2643314324934865, + "timestamp": 0.8801158021538884 + }, + { + "x": 2.4499444602008893, + "y": 4.4671542394648975, + "heading": 2.6942149976329213, + "angularVelocity": -0.7305659020079995, + "velocityX": 1.098178810683297, + "velocityY": -1.0098883742515268, + "timestamp": 0.9601263296224237 + }, + { + "x": 2.5157266326233696, + "y": 4.40663000080001, + "heading": 2.6477299996969115, + "angularVelocity": -0.580986020299514, + "velocityX": 0.822168963307353, + "velocityY": -0.756453439063869, + "timestamp": 1.040136857090959 + }, + { + "x": 2.559524861701357, + "y": 4.366320347788332, + "heading": 2.615543117832924, + "angularVelocity": -0.4022830855182806, + "velocityX": 0.5474058285043993, + "velocityY": -0.5038043653384214, + "timestamp": 1.1201473845594943 + }, + { + "x": 2.5814027786254883, + "y": 4.346181392669678, "heading": 2.599013995570559, - "angularVelocity": -2.1007177339153333, - "velocityX": 0.4933178798082817, - "velocityY": -0.38057445433049264, - "timestamp": 1.2623222707727317 + "angularVelocity": -0.20658684282346457, + "velocityX": 0.27343797893014415, + "velocityY": -0.25170381643308454, + "timestamp": 1.2001579120280295 }, { - "x": 2.593973159790039, - "y": 4.3895583152771, + "x": 2.5814027786254883, + "y": 4.346181392669678, "heading": 2.599013995570559, - "angularVelocity": -1.7281844842695858e-24, - "velocityX": -1.1064710362675692e-24, - "velocityY": 7.502253930064392e-24, - "timestamp": 1.3464770888242472 - }, - { - "x": 2.5399319472382507, - "y": 4.43667284651752, - "heading": 2.699289039214539, - "angularVelocity": 1.1496592855145864, - "velocityX": -0.6195856870550529, - "velocityY": 0.5401708775667837, - "timestamp": 1.4336986203748725 - }, - { - "x": 2.4355875318293467, - "y": 4.532520831999065, - "heading": 2.6992890864875387, - "angularVelocity": 5.419877275969795e-7, - "velocityX": -1.19631487264518, - "velocityY": 1.0989028027547665, - "timestamp": 1.5209201519254978 - }, - { - "x": 2.331243168301696, - "y": 4.628368873961615, - "heading": 2.6992891337586675, - "angularVelocity": 5.419662778006892e-7, - "velocityX": -1.1963142778235591, - "velocityY": 1.0989034503128068, - "timestamp": 1.608141683476123 - }, - { - "x": 2.2268988047740805, - "y": 4.724216915924203, - "heading": 2.6992891810297954, - "angularVelocity": 5.419662665035014e-7, - "velocityX": -1.1963142778231501, - "velocityY": 1.098903450313253, - "timestamp": 1.6953632150267484 - }, - { - "x": 2.122554441246435, - "y": 4.82006495788676, - "heading": 2.699289228300922, - "angularVelocity": 5.419662547194978e-7, - "velocityX": -1.1963142778234912, - "velocityY": 1.0989034503128838, - "timestamp": 1.7825847465773736 - }, - { - "x": 2.0182100346281864, - "y": 4.915912952938407, - "heading": 2.699289275573439, - "angularVelocity": 5.419821924710352e-7, - "velocityX": -1.1963147718597973, - "velocityY": 1.0989029124765504, - "timestamp": 1.869806278127999 + "angularVelocity": -6.228298358184451e-25, + "velocityX": 4.332162235643869e-24, + "velocityY": -9.3329937041195e-25, + "timestamp": 1.2801684394965647 + }, + { + "x": 2.5566960314145897, + "y": 4.370853223600912, + "heading": 2.6069134460708416, + "angularVelocity": 0.0917703358537092, + "velocityX": -0.287025849369625, + "velocityY": 0.28662021625483314, + "timestamp": 1.3662469138401139 + }, + { + "x": 2.5072824665913997, + "y": 4.4201968838581225, + "heading": 2.6227096530109684, + "angularVelocity": 0.18350937398218814, + "velocityX": -0.5740525166137891, + "velocityY": 0.5732404138608942, + "timestamp": 1.452325388183663 + }, + { + "x": 2.433161972783442, + "y": 4.494212390724074, + "heading": 2.6463978103323864, + "angularVelocity": 0.2751925786564924, + "velocityX": -0.8610804777061288, + "velocityY": 0.8598608122461178, + "timestamp": 1.5384038625272123 + }, + { + "x": 2.3343343934772123, + "y": 4.5928997989282285, + "heading": 2.6779701614467033, + "angularVelocity": 0.3667856726678033, + "velocityX": -1.148110257063775, + "velocityY": 1.146481846440282, + "timestamp": 1.6244823368707615 + }, + { + "x": 2.210797624410119, + "y": 4.716260426889983, + "heading": 2.7172978240148544, + "angularVelocity": 0.45688150107296166, + "velocityX": -1.4351644822844225, + "velocityY": 1.4331181971163696, + "timestamp": 1.7105608112143107 + }, + { + "x": 2.1119680830208534, + "y": 4.81494906749223, + "heading": 2.7487514884444098, + "angularVelocity": 0.3654068531003482, + "velocityX": -1.1481330511833399, + "velocityY": 1.1464961635863742, + "timestamp": 1.79663928555786 + }, + { + "x": 2.037845879093483, + "y": 4.888965610568293, + "heading": 2.772338013759402, + "angularVelocity": 0.27401188851065944, + "velocityX": -0.8611003446871089, + "velocityY": 0.859872850216351, + "timestamp": 1.882717759901409 + }, + { + "x": 1.9884310645624592, + "y": 4.938309990590957, + "heading": 2.7880613204490317, + "angularVelocity": 0.18266246944475417, + "velocityX": -0.574067034852447, + "velocityY": 0.5732487755966209, + "timestamp": 1.9687962342449583 }, { "x": 1.9637236595153809, "y": 4.962982177734375, "heading": 2.79592312968437, - "angularVelocity": 1.1079128329091787, - "velocityX": -0.6246895020546673, - "velocityY": 0.5396514365108126, - "timestamp": 1.9570278096786242 + "angularVelocity": 0.09133304575033727, + "velocityX": -0.28703349165400144, + "velocityY": 0.28662435448086987, + "timestamp": 2.0548747085885073 }, { "x": 1.9637236595153809, "y": 4.962982177734375, "heading": 2.79592312968437, - "angularVelocity": 3.9745579613293646e-24, - "velocityX": 5.1209291302608005e-24, - "velocityY": -1.0346712550534795e-24, - "timestamp": 2.0442493412292495 + "angularVelocity": 1.375760922880761e-26, + "velocityX": -3.7767209080452686e-25, + "velocityY": 4.039448182062531e-25, + "timestamp": 2.1409531829320563 } ], "trajectoryWaypoints": [ @@ -459,18 +477,18 @@ "controlIntervalCount": 16 }, { - "timestamp": 1.3464770888242472, + "timestamp": 1.2801684394965647, "isStopPoint": true, - "x": 2.593973159790039, - "y": 4.3895583152771, + "x": 2.5814027786254883, + "y": 4.346181392669678, "heading": 2.599013995570559, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 10 }, { - "timestamp": 2.0442493412292495, + "timestamp": 2.1409531829320563, "isStopPoint": true, "x": 1.9637236595153809, "y": 4.962982177734375, @@ -517,7 +535,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { "x": 2.6714627742767334, @@ -526,7 +544,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 38 + "controlIntervalCount": 25 }, { "x": 7.936469078063965, @@ -535,7 +553,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 36 + "controlIntervalCount": 24 }, { "x": 2.812295436859131, @@ -552,829 +570,595 @@ "x": 1.2263859510421753, "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": -9.716627282364513e-28, - "velocityX": -2.1006296109192272e-27, - "velocityY": 1.6816897032610772e-27, + "angularVelocity": 1.2684261528823466e-28, + "velocityX": -8.90285455606977e-27, + "velocityY": -9.123583685606214e-27, "timestamp": 0 }, { - "x": 1.2629372172985613, - "y": 5.611868166522511, - "heading": 3.3611684637424437, - "angularVelocity": 2.6151040269567423, - "velocityX": 0.4341464801664794, - "velocityY": 0.21092109874697887, - "timestamp": 0.08419109200740245 - }, - { - "x": 1.3467908643065862, - "y": 5.672396849103953, - "heading": 3.4705725495976436, - "angularVelocity": 1.2994734151397, - "velocityX": 0.9959919156370148, - "velocityY": 0.7189440252909496, - "timestamp": 0.1683821840148049 - }, - { - "x": 1.4464012359618026, - "y": 5.766107214252463, - "heading": 3.4705728088664487, - "angularVelocity": 0.0000030795277620559296, - "velocityX": 1.1831462127425316, - "velocityY": 1.1130674625323809, - "timestamp": 0.25257327602220736 - }, - { - "x": 1.5460115925515412, - "y": 5.859817595415609, - "heading": 3.4705730681338594, - "angularVelocity": 0.0000030795111997424536, - "velocityX": 1.1831460337986872, - "velocityY": 1.1130676527500878, - "timestamp": 0.3367643680296098 - }, - { - "x": 1.6456219491412756, - "y": 5.9535279765787505, - "heading": 3.4705733274013144, - "angularVelocity": 0.000003079511724050569, - "velocityX": 1.183146033798638, - "velocityY": 1.1130676527500365, - "timestamp": 0.42095546003701223 - }, - { - "x": 1.7452323057310184, - "y": 6.0472383577418745, - "heading": 3.4705735866688134, - "angularVelocity": 0.000003079512247611481, - "velocityX": 1.1831460337987365, - "velocityY": 1.113067652749828, - "timestamp": 0.5051465520444147 - }, - { - "x": 1.8448426623207694, - "y": 6.1409487389049815, - "heading": 3.4705738459363564, - "angularVelocity": 0.0000030795127711126187, - "velocityX": 1.1831460337988353, - "velocityY": 1.1130676527496197, - "timestamp": 0.5893376440518172 - }, - { - "x": 1.9444530189105287, - "y": 6.234659120068071, - "heading": 3.470574105203944, - "angularVelocity": 0.000003079513294842049, - "velocityX": 1.183146033798934, - "velocityY": 1.1130676527494117, - "timestamp": 0.6735287360592197 - }, - { - "x": 2.0440633755002966, - "y": 6.328369501231142, - "heading": 3.470574364471575, - "angularVelocity": 0.0000030795138187518754, - "velocityX": 1.1831460337990323, - "velocityY": 1.1130676527492032, - "timestamp": 0.7577198280666222 - }, - { - "x": 2.1436737320900723, - "y": 6.422079882394196, - "heading": 3.4705746237392505, - "angularVelocity": 0.0000030795143424110884, - "velocityX": 1.1831460337991306, - "velocityY": 1.113067652748995, - "timestamp": 0.8419109200740247 - }, - { - "x": 2.2432840886798564, - "y": 6.515790263557233, - "heading": 3.47057488300697, - "angularVelocity": 0.0000030795148666763967, - "velocityX": 1.1831460337992294, - "velocityY": 1.1130676527487866, - "timestamp": 0.9261020120814272 - }, - { - "x": 2.342894445269649, - "y": 6.609500644720252, - "heading": 3.4705751422747335, - "angularVelocity": 0.0000030795153898968674, - "velocityX": 1.1831460337993278, - "velocityY": 1.1130676527485786, - "timestamp": 1.0102931040888297 - }, - { - "x": 2.4425048018594557, - "y": 6.703211025883247, - "heading": 3.470575401542541, - "angularVelocity": 0.000003079515913944498, - "velocityX": 1.1831460337994937, - "velocityY": 1.1130676527482986, - "timestamp": 1.0944841960962322 - }, - { - "x": 2.5421151652954292, - "y": 6.796921399768766, - "heading": 3.4705756608109883, - "angularVelocity": 0.0000030795235106850793, - "velocityX": 1.183146115116498, - "velocityY": 1.1130675663083123, - "timestamp": 1.1786752881036346 - }, - { - "x": 2.632005364209705, - "y": 6.868564037074326, - "heading": 3.5437695190039196, - "angularVelocity": 0.8693777031244078, - "velocityX": 1.0676925167614182, - "velocityY": 0.8509527029208858, - "timestamp": 1.2628663801110371 + "x": 1.2489662072831478, + "y": 5.614521121433257, + "heading": 3.1501023791424476, + "angularVelocity": 0.11304779998591061, + "velocityX": 0.2804374824661162, + "velocityY": 0.2534916497154969, + "timestamp": 0.08051796800629471 + }, + { + "x": 1.2941269040941792, + "y": 5.65534214931359, + "heading": 3.1683083464792072, + "angularVelocity": 0.22611061589800957, + "velocityX": 0.5608772542235648, + "velocityY": 0.506980353467706, + "timestamp": 0.16103593601258942 + }, + { + "x": 1.3618682728307967, + "y": 5.716573194164052, + "heading": 3.1956213802625175, + "angularVelocity": 0.33921663027033977, + "velocityX": 0.8413198993213754, + "velocityY": 0.7604643580383921, + "timestamp": 0.24155390401888413 + }, + { + "x": 1.4521905498783176, + "y": 5.798213731151769, + "heading": 3.2320485280043223, + "angularVelocity": 0.4524101718383757, + "velocityX": 1.121765480227465, + "velocityY": 1.0139418444009154, + "timestamp": 0.32207187202517884 + }, + { + "x": 1.5650939157362251, + "y": 5.900263093874502, + "heading": 3.2776019617285486, + "angularVelocity": 0.5657548849303424, + "velocityX": 1.4022133028627977, + "velocityY": 1.2674110543221202, + "timestamp": 0.4025898400314736 + }, + { + "x": 1.700578414867027, + "y": 6.022720499588671, + "heading": 3.3323005252490514, + "angularVelocity": 0.6793336304292585, + "velocityX": 1.6826616777041643, + "velocityY": 1.5208705428904563, + "timestamp": 0.4831078080377683 + }, + { + "x": 1.8586438590589127, + "y": 6.165585103627761, + "heading": 3.3961709105895657, + "angularVelocity": 0.7932438798694066, + "velocityX": 1.9631077150323568, + "velocityY": 1.7743195410485293, + "timestamp": 0.563625776044063 + }, + { + "x": 2.039289707486661, + "y": 6.328856070494732, + "heading": 3.469248537641403, + "angularVelocity": 0.9075940297713951, + "velocityX": 2.2435470355340517, + "velocityY": 2.027758162677519, + "timestamp": 0.6441437440503578 + }, + { + "x": 2.1973352722504775, + "y": 6.471697816178928, + "heading": 3.5342798000329774, + "angularVelocity": 0.8076614947174351, + "velocityX": 1.9628608207234048, + "velocityY": 1.774035649695346, + "timestamp": 0.7246617120566525 + }, + { + "x": 2.3328015579338945, + "y": 6.594133674846713, + "heading": 3.590056917522341, + "angularVelocity": 0.6927288265024656, + "velocityX": 1.682435474189146, + "velocityY": 1.520602937448862, + "timestamp": 0.8051796800629473 + }, + { + "x": 2.4456892468776004, + "y": 6.6961637450440366, + "heading": 3.6365583693082697, + "angularVelocity": 0.5775288787999897, + "velocityX": 1.4020186020452068, + "velocityY": 1.2671714491024832, + "timestamp": 0.885697648069242 + }, + { + "x": 2.5359988834189062, + "y": 6.777787981324914, + "heading": 3.67376995651372, + "angularVelocity": 0.4621525868926695, + "velocityX": 1.121608490346475, + "velocityY": 1.013739396335684, + "timestamp": 0.9662156160755367 + }, + { + "x": 2.603730869861698, + "y": 6.839006281819378, + "heading": 3.701682776944854, + "angularVelocity": 0.3466657334044004, + "velocityX": 0.8412033750963059, + "velocityY": 0.7603060783858558, + "timestamp": 1.0467335840818315 + }, + { + "x": 2.648885462775646, + "y": 6.879818541544654, + "heading": 3.720292007909602, + "angularVelocity": 0.23111898406692608, + "velocityX": 0.5608014463357825, + "velocityY": 0.5068714565932112, + "timestamp": 1.127251552088126 }, { "x": 2.6714627742767334, "y": 6.900224685668945, "heading": 3.729596046054495, - "angularVelocity": 2.207199391525135, - "velocityX": 0.46866490416300804, - "velocityY": 0.3760569894002088, - "timestamp": 1.3470574721184396 + "angularVelocity": 0.11555232173973205, + "velocityX": 0.28040091000958045, + "velocityY": 0.25343590542047173, + "timestamp": 1.2077695200944207 }, { "x": 2.6714627742767334, "y": 6.900224685668945, "heading": 3.729596046054495, - "angularVelocity": -1.0211590332128658e-23, - "velocityX": -1.1480030002254912e-23, - "velocityY": -4.344519998520169e-23, - "timestamp": 1.4312485641258421 - }, - { - "x": 2.720827723501713, - "y": 6.91216585910853, - "heading": 3.486294732820543, - "angularVelocity": -2.69475141570759, - "velocityX": 0.5467552354822697, - "velocityY": 0.13225779017707837, - "timestamp": 1.5215356725454756 - }, - { - "x": 2.8392095989605575, - "y": 6.942185818091124, - "heading": 3.389947764111763, - "angularVelocity": -1.0671176693463396, - "velocityX": 1.311171412297561, - "velocityY": 0.33249441150564496, - "timestamp": 1.6118227809651091 - }, - { - "x": 2.985101373007063, - "y": 6.9572215101019514, - "heading": 3.3899472630700718, - "angularVelocity": -0.000005549426712336868, - "velocityX": 1.615864951266793, - "velocityY": 0.16653199192676998, - "timestamp": 1.7021098893847426 - }, - { - "x": 3.1309931487591207, - "y": 6.972257185567622, - "heading": 3.3899467620302377, - "angularVelocity": -0.000005549406141075235, - "velocityX": 1.6158649701561478, - "velocityY": 0.16653180870033996, - "timestamp": 1.7923969978043761 - }, - { - "x": 3.276884924511046, - "y": 6.987292861034595, - "heading": 3.389946260989866, - "angularVelocity": -0.000005549412101088006, - "velocityX": 1.615864970155008, - "velocityY": 0.16653180870111933, - "timestamp": 1.8826841062240096 - }, - { - "x": 3.4227767002625384, - "y": 7.002328536502587, - "heading": 3.3899457599489553, - "angularVelocity": -0.000005549418061933105, - "velocityX": 1.6158649701538554, - "velocityY": 0.16653180870202178, - "timestamp": 1.9729712146436431 - }, - { - "x": 3.5686684760143703, - "y": 7.017364211969376, - "heading": 3.389945258907507, - "angularVelocity": -0.000005549424021960657, - "velocityX": 1.6158649701527033, - "velocityY": 0.16653180870292417, - "timestamp": 2.0632583230632764 - }, - { - "x": 3.7145602517660743, - "y": 7.032399887436731, - "heading": 3.3899447578655204, - "angularVelocity": -0.000005549429982841055, - "velocityX": 1.6158649701515504, - "velocityY": 0.16653180870382667, - "timestamp": 2.1535454314829097 - }, - { - "x": 3.8604520275173986, - "y": 7.04743556290717, - "heading": 3.3899442568229956, - "angularVelocity": -0.000005549435944010941, - "velocityX": 1.615864970150398, - "velocityY": 0.1665318087047291, - "timestamp": 2.243832539902543 - }, - { - "x": 4.00634380326868, - "y": 7.062471238372739, - "heading": 3.389943755779933, - "angularVelocity": -0.000005549441903913974, - "velocityX": 1.6158649701492453, - "velocityY": 0.1665318087056315, - "timestamp": 2.334119648322176 - }, - { - "x": 4.1522355790200765, - "y": 7.077506913841867, - "heading": 3.389943254736332, - "angularVelocity": -0.0000055494478640805644, - "velocityX": 1.615864970148093, - "velocityY": 0.16653180870653383, - "timestamp": 2.4244067567418095 - }, - { - "x": 4.298127354771182, - "y": 7.0925425893096525, - "heading": 3.389942753692193, - "angularVelocity": -0.000005549453823846852, - "velocityX": 1.6158649701469403, - "velocityY": 0.16653180870743617, - "timestamp": 2.5146938651614428 - }, - { - "x": 4.444019130522188, - "y": 7.10757826477934, - "heading": 3.389942252647516, - "angularVelocity": -0.000005549459785648991, - "velocityX": 1.6158649701457874, - "velocityY": 0.16653180870833867, - "timestamp": 2.604980973581076 - }, - { - "x": 4.5899109062731025, - "y": 7.122613940245511, - "heading": 3.3899417516023003, - "angularVelocity": -0.000005549465746228024, - "velocityX": 1.6158649701446353, - "velocityY": 0.1665318087092411, - "timestamp": 2.6952680820007093 - }, - { - "x": 4.735802682023898, - "y": 7.137649615713241, - "heading": 3.389941250556547, - "angularVelocity": -0.00000554947170730299, - "velocityX": 1.6158649701434824, - "velocityY": 0.16653180871014361, - "timestamp": 2.7855551904203426 - }, - { - "x": 4.881694457774774, - "y": 7.152685291182225, - "heading": 3.389940749510255, - "angularVelocity": -0.000005549477667769927, - "velocityX": 1.61586497014233, - "velocityY": 0.1665318087110459, - "timestamp": 2.875842298839976 - }, - { - "x": 5.027586233525358, - "y": 7.167720966650761, - "heading": 3.3899402484634247, - "angularVelocity": -0.000005549483629157811, - "velocityX": 1.6158649701411771, - "velocityY": 0.16653180871194842, - "timestamp": 2.966129407259609 - }, - { - "x": 5.173478009275799, - "y": 7.182756642119004, - "heading": 3.3899397474160566, - "angularVelocity": -0.000005549489589198961, - "velocityX": 1.6158649701400245, - "velocityY": 0.1665318087128507, - "timestamp": 3.0564165156792424 - }, - { - "x": 5.319369785026301, - "y": 7.197792317586818, - "heading": 3.38993924636815, - "angularVelocity": -0.0000055494955497599425, - "velocityX": 1.615864970138872, - "velocityY": 0.16653180871375314, - "timestamp": 3.1467036240988757 - }, - { - "x": 5.4652615607764785, - "y": 7.212827993058087, - "heading": 3.3899387453197054, - "angularVelocity": -0.000005549501510730972, - "velocityX": 1.6158649701377192, - "velocityY": 0.16653180871465564, - "timestamp": 3.236990732518509 - }, - { - "x": 5.611153336526806, - "y": 7.227863668525338, - "heading": 3.3899382442707227, - "angularVelocity": -0.000005549507472025305, - "velocityX": 1.6158649701365668, - "velocityY": 0.16653180871555806, - "timestamp": 3.3272778409381423 - }, - { - "x": 5.757045112276785, - "y": 7.242899343993592, - "heading": 3.3899377432212017, - "angularVelocity": -0.000005549513433122579, - "velocityX": 1.6158649701354142, - "velocityY": 0.16653180871646053, - "timestamp": 3.4175649493577755 - }, - { - "x": 5.902936888026919, - "y": 7.257935019462863, - "heading": 3.3899372421711425, - "angularVelocity": -0.000005549519393768537, - "velocityX": 1.6158649701342616, - "velocityY": 0.1665318087173629, - "timestamp": 3.507852057777409 - }, - { - "x": 6.04882866377676, - "y": 7.272970694932001, - "heading": 3.389936741120545, - "angularVelocity": -0.000005549525355144424, - "velocityX": 1.6158649701331087, - "velocityY": 0.16653180871826534, - "timestamp": 3.598139166197042 - }, - { - "x": 6.194720439526491, - "y": 7.288006370401304, - "heading": 3.3899362400694093, - "angularVelocity": -0.000005549531315809206, - "velocityX": 1.615864970131956, - "velocityY": 0.16653180871916778, - "timestamp": 3.6884262746166754 - }, - { - "x": 6.34061221527629, - "y": 7.30304204587104, - "heading": 3.3899357390177354, - "angularVelocity": -0.000005549537277178968, - "velocityX": 1.6158649701308037, - "velocityY": 0.1665318087200703, - "timestamp": 3.7787133830363087 - }, - { - "x": 6.486503991025816, - "y": 7.318077721337987, - "heading": 3.389935237965523, - "angularVelocity": -0.0000055495432383876534, - "velocityX": 1.6158649701296508, - "velocityY": 0.16653180872097262, - "timestamp": 3.869000491455942 - }, - { - "x": 6.632395766775363, - "y": 7.333113396808981, - "heading": 3.389934736912773, - "angularVelocity": -0.000005549549199340779, - "velocityX": 1.615864970128498, - "velocityY": 0.16653180872187498, - "timestamp": 3.959287599875575 - }, - { - "x": 6.778287542524652, - "y": 7.3481490722785985, - "heading": 3.389934235859484, - "angularVelocity": -0.00000554955516023689, - "velocityX": 1.6158649701273455, - "velocityY": 0.16653180872277748, - "timestamp": 4.049574708295209 - }, - { - "x": 6.924179318273876, - "y": 7.363184747747038, - "heading": 3.3899337348056577, - "angularVelocity": -0.000005549561121255593, - "velocityX": 1.6158649701261931, - "velocityY": 0.16653180872368004, - "timestamp": 4.139861816714842 - }, - { - "x": 7.070071094023151, - "y": 7.378220423218671, - "heading": 3.389933233751293, - "angularVelocity": -0.0000055495670825865805, - "velocityX": 1.6158649701250403, - "velocityY": 0.1665318087245824, - "timestamp": 4.2301489251344755 - }, - { - "x": 7.215962869772269, - "y": 7.393256098685687, - "heading": 3.38993273269639, - "angularVelocity": -0.000005549573043968301, - "velocityX": 1.6158649701238874, - "velocityY": 0.16653180872548476, - "timestamp": 4.320436033554109 - }, - { - "x": 7.361854645521179, - "y": 7.4082917741551615, - "heading": 3.389932231640949, - "angularVelocity": -0.00000554957900552128, - "velocityX": 1.6158649701227348, - "velocityY": 0.16653180872638726, - "timestamp": 4.410723141973742 - }, - { - "x": 7.507746421269845, - "y": 7.423327449627381, - "heading": 3.3899317305849697, - "angularVelocity": -0.0000055495849662374065, - "velocityX": 1.615864970121582, - "velocityY": 0.16653180872728968, - "timestamp": 4.501010250393375 - }, - { - "x": 7.653638197018768, - "y": 7.4383631250955204, - "heading": 3.389931229528452, - "angularVelocity": -0.000005549590927964199, - "velocityX": 1.615864970120429, - "velocityY": 0.16653180872819684, - "timestamp": 4.591297358813009 - }, - { - "x": 7.79952997270157, - "y": 7.45339880119461, - "heading": 3.389930728467707, - "angularVelocity": -0.000005549637747689064, - "velocityX": 1.6158649693899627, - "velocityY": 0.16653181569922934, - "timestamp": 4.681584467232642 - }, - { - "x": 7.908273186523536, - "y": 7.465055949439882, - "heading": 3.2483837688222263, - "angularVelocity": -1.567742750023664, - "velocityX": 1.2044157324947478, - "velocityY": 0.1291119900382222, - "timestamp": 4.771871575652275 + "angularVelocity": 7.708394912303633e-28, + "velocityX": 2.52399716725713e-26, + "velocityY": 2.1878261081032532e-26, + "timestamp": 1.2882874881007154 + }, + { + "x": 2.7043659064830394, + "y": 6.904431852524175, + "heading": 3.7255178406599083, + "angularVelocity": -0.04862383285360791, + "velocityX": 0.39229912325731986, + "velocityY": 0.05016142105727286, + "timestamp": 1.3721600492553427 + }, + { + "x": 2.770172988802701, + "y": 6.912846664761324, + "heading": 3.7174222347433226, + "angularVelocity": -0.09652269830726562, + "velocityX": 0.7846079982980328, + "velocityY": 0.10032854751670894, + "timestamp": 1.4560326104099701 + }, + { + "x": 2.868885003911175, + "y": 6.925469706226451, + "heading": 3.7053835964173025, + "angularVelocity": -0.1435348838796717, + "velocityX": 1.1769285896312263, + "velocityY": 0.15050263508533468, + "timestamp": 1.5399051715645975 + }, + { + "x": 3.000503155089929, + "y": 6.942301704976706, + "heading": 3.689495079575308, + "angularVelocity": -0.1894364095154122, + "velocityX": 1.5692635275093452, + "velocityY": 0.2006854031704579, + "timestamp": 1.6237777327192249 + }, + { + "x": 3.1650289492172274, + "y": 6.963343594326816, + "heading": 3.669876864825881, + "angularVelocity": -0.23390503973354093, + "velocityX": 1.9616164316716036, + "velocityY": 0.250879299027448, + "timestamp": 1.7076502938738523 + }, + { + "x": 3.362464324784135, + "y": 6.988596614292936, + "heading": 3.64668996545627, + "angularVelocity": -0.2764539326140709, + "velocityX": 2.3539924481728387, + "velocityY": 0.301087979411566, + "timestamp": 1.7915228550284796 + }, + { + "x": 3.592811857209642, + "y": 7.0180624930033995, + "heading": 3.620161246344741, + "angularVelocity": -0.316297949488161, + "velocityX": 2.7463991710094446, + "velocityY": 0.35131726401129654, + "timestamp": 1.875395416183107 + }, + { + "x": 3.8560750973941316, + "y": 7.05174380499803, + "heading": 3.5906339439313264, + "angularVelocity": -0.3520496096330931, + "velocityX": 3.1388482307001198, + "velocityY": 0.40157724446419313, + "timestamp": 1.9592679773377344 + }, + { + "x": 4.152259097249652, + "y": 7.089644787764795, + "heading": 3.558687346907366, + "angularVelocity": -0.38089449736802783, + "velocityX": 3.5313575235824133, + "velocityY": 0.4518877478522514, + "timestamp": 2.0431405384923615 + }, + { + "x": 4.481370515300424, + "y": 7.131773671322726, + "heading": 3.52550603083455, + "angularVelocity": -0.39561586788368536, + "velocityX": 3.9239462050529617, + "velocityY": 0.5022963765260808, + "timestamp": 2.1270130996469887 + }, + { + "x": 4.843397664915269, + "y": 7.178152540283051, + "heading": 3.4948677816779656, + "angularVelocity": -0.3652952614634004, + "velocityX": 4.316395548568172, + "velocityY": 0.5529683167158834, + "timestamp": 2.210885660801616 + }, + { + "x": 5.214084962853593, + "y": 7.225506645206567, + "heading": 3.4948677687882737, + "angularVelocity": -1.5368186922063484e-7, + "velocityX": 4.419649201541919, + "velocityY": 0.5645959092177173, + "timestamp": 2.294758221956243 + }, + { + "x": 5.584772271414454, + "y": 7.2728606669772775, + "heading": 3.4948677558987455, + "angularVelocity": -1.5367991215657251e-7, + "velocityX": 4.419649328192821, + "velocityY": 0.5645949177992666, + "timestamp": 2.37863078311087 + }, + { + "x": 5.955780374663183, + "y": 7.317631980849406, + "heading": 3.4948677392182756, + "angularVelocity": -1.9887875297845992e-7, + "velocityX": 4.423474115268035, + "velocityY": 0.5338016778763695, + "timestamp": 2.4625033442654973 + }, + { + "x": 6.300300548190503, + "y": 7.351378145453142, + "heading": 3.441189014071279, + "angularVelocity": -0.6400034100310172, + "velocityX": 4.107662491576513, + "velocityY": 0.40235047242115085, + "timestamp": 2.5463759054201245 + }, + { + "x": 6.611937161645573, + "y": 7.380701076408195, + "heading": 3.3909317361493, + "angularVelocity": -0.5992100065875497, + "velocityX": 3.7155967239457195, + "velocityY": 0.3496129193073305, + "timestamp": 2.6302484665747516 + }, + { + "x": 6.890683200327295, + "y": 7.405709842435669, + "heading": 3.344663622374181, + "angularVelocity": -0.551647799210751, + "velocityX": 3.323447321083073, + "velocityY": 0.29817577623948227, + "timestamp": 2.714121027729379 + }, + { + "x": 7.136536630210955, + "y": 7.426441407681926, + "heading": 3.302577269433113, + "angularVelocity": -0.5017892903434469, + "velocityX": 2.931273666848028, + "velocityY": 0.2471793511591556, + "timestamp": 2.797993588884006 + }, + { + "x": 7.349496491178017, + "y": 7.442914379961346, + "heading": 3.264769514381459, + "angularVelocity": -0.45077620775108873, + "velocityX": 2.539088565263299, + "velocityY": 0.196404784266102, + "timestamp": 2.881866150038633 + }, + { + "x": 7.529562223309363, + "y": 7.455139968848337, + "heading": 3.2312984812041172, + "angularVelocity": -0.39907012158165334, + "velocityX": 2.1468967878467162, + "velocityY": 0.14576386745185102, + "timestamp": 2.9657387111932603 + }, + { + "x": 7.676733458548059, + "y": 7.463125667757038, + "heading": 3.2022028135705005, + "angularVelocity": -0.3469032927225913, + "velocityX": 1.7547006221423278, + "velocityY": 0.0952122934934494, + "timestamp": 3.0496112723478874 + }, + { + "x": 7.791009935801521, + "y": 7.466876839352261, + "heading": 3.177509999915845, + "angularVelocity": -0.29440872336224383, + "velocityX": 1.3625013434701525, + "velocityY": 0.04472465778536462, + "timestamp": 3.1334838335025146 + }, + { + "x": 7.872391460113929, + "y": 7.466397510828961, + "heading": 3.1572405778603585, + "angularVelocity": -0.24166928702841778, + "velocityX": 0.9702997403688861, + "velocityY": -0.005714962279684649, + "timestamp": 3.2173563946571417 + }, + { + "x": 7.920877880694152, + "y": 7.461690816856239, + "heading": 3.1414104927277844, + "angularVelocity": -0.1887397369849021, + "velocityX": 0.5780963394075183, + "velocityY": -0.05611720815399605, + "timestamp": 3.301228955811769 }, { "x": 7.936469078063965, "y": 7.452759265899658, "heading": 3.1300325241315123, - "angularVelocity": -1.3108321526994196, - "velocityX": 0.3122914448573039, - "velocityY": -0.13619534121251922, - "timestamp": 4.862158684071908 - }, - { - "x": 7.897313989149588, - "y": 7.432003540468781, - "heading": 3.2231598958654137, - "angularVelocity": 0.9773255115620281, - "velocityX": -0.41091321048106677, - "velocityY": -0.21782102937809103, - "timestamp": 4.957446661958681 - }, - { - "x": 7.764258804459074, - "y": 7.410354349811778, - "heading": 3.302887541742508, - "angularVelocity": 0.8367020441112922, - "velocityX": -1.3963480770719, - "velocityY": -0.2271975031683705, - "timestamp": 5.052734639845453 - }, - { - "x": 7.611104944022113, - "y": 7.387923020288099, - "heading": 3.3028890943761, - "angularVelocity": 0.000016294118377845236, - "velocityX": -1.607273696395563, - "velocityY": -0.23540566212575834, - "timestamp": 5.1480226177322255 - }, - { - "x": 7.457951083368431, - "y": 7.365491692124674, - "heading": 3.3028906469716444, - "angularVelocity": 0.000016293719092580313, - "velocityX": -1.6072736986022782, - "velocityY": -0.23540564768491373, - "timestamp": 5.243310595618998 - }, - { - "x": 7.3047972227130815, - "y": 7.3430603639785605, - "heading": 3.3028921995394867, - "angularVelocity": 0.0000162934283709874, - "velocityX": -1.6072736986590501, - "velocityY": -0.23540564774414025, - "timestamp": 5.33859857350577 - }, - { - "x": 7.15164336205228, - "y": 7.320629035813856, - "heading": 3.3028937520796284, - "angularVelocity": 0.000016293137669547086, - "velocityX": -1.6072736987158178, - "velocityY": -0.23540564780336143, - "timestamp": 5.4338865513925425 - }, - { - "x": 6.998489501387815, - "y": 7.298197707651154, - "heading": 3.3028953045920724, - "angularVelocity": 0.00001629284698917995, - "velocityX": -1.6072736987725829, - "velocityY": -0.23540564786256962, - "timestamp": 5.529174529279315 - }, - { - "x": 6.845335640713116, - "y": 7.275766379466654, - "heading": 3.3028968570768193, - "angularVelocity": 0.000016292556328916256, - "velocityX": -1.607273698829345, - "velocityY": -0.23540564792176438, - "timestamp": 5.624462507166087 - }, - { - "x": 6.692181780035948, - "y": 7.253335051289657, - "heading": 3.3028984095338725, - "angularVelocity": 0.0000162922656907247, - "velocityX": -1.6072736988861036, - "velocityY": -0.2354056479809454, - "timestamp": 5.7197504850528595 - }, - { - "x": 6.539027919353228, - "y": 7.230903723107964, - "heading": 3.302899961963233, - "angularVelocity": 0.00001629197507403925, - "velocityX": -1.6072736989428598, - "velocityY": -0.23540564804011319, - "timestamp": 5.815038462939632 - }, - { - "x": 6.385874058664987, - "y": 7.2084723949147005, - "heading": 3.3029015143649034, - "angularVelocity": 0.000016291684475842535, - "velocityX": -1.607273698999613, - "velocityY": -0.2354056480992679, - "timestamp": 5.910326440826404 - }, - { - "x": 6.232720197971391, - "y": 7.186041066717774, - "heading": 3.3029030667388852, - "angularVelocity": 0.000016291393901831136, - "velocityX": -1.6072736990563632, - "velocityY": -0.2354056481584089, - "timestamp": 6.005614418713177 - }, - { - "x": 6.079566337272002, - "y": 7.163609738524357, - "heading": 3.302904619085181, - "angularVelocity": 0.000016291103348699848, - "velocityX": -1.6072736991131096, - "velocityY": -0.2354056482175359, - "timestamp": 6.100902396599949 - }, - { - "x": 5.9264124765671236, - "y": 7.141178410308786, - "heading": 3.3029061714037917, - "angularVelocity": 0.000016290812813611415, - "velocityX": -1.6072736991698542, - "velocityY": -0.23540564827665061, - "timestamp": 6.196190374486721 - }, - { - "x": 5.773258615858539, - "y": 7.118747082103052, - "heading": 3.302907723694721, - "angularVelocity": 0.000016290522303354113, - "velocityX": -1.6072736992265952, - "velocityY": -0.23540564833575084, - "timestamp": 6.291478352373494 - }, - { - "x": 5.620104755142444, - "y": 7.096315753876903, - "heading": 3.30290927595797, - "angularVelocity": 0.000016290231812342035, - "velocityX": -1.6072736992833332, - "velocityY": -0.2354056483948381, - "timestamp": 6.386766330260266 - }, - { - "x": 5.466950894422645, - "y": 7.07388442565226, - "heading": 3.30291082819354, - "angularVelocity": 0.000016289941343623743, - "velocityX": -1.6072736993400683, - "velocityY": -0.23540564845391188, - "timestamp": 6.482054308147038 - }, - { - "x": 5.313797033694609, - "y": 7.0514530974310325, - "heading": 3.3029123804014344, - "angularVelocity": 0.000016289650893731917, - "velocityX": -1.607273699396801, - "velocityY": -0.23540564851297285, - "timestamp": 6.577342286033811 - }, - { - "x": 5.160643172963514, - "y": 7.02902176918985, - "heading": 3.3029139325816548, - "angularVelocity": 0.000016289360468842725, - "velocityX": -1.6072736994535295, - "velocityY": -0.23540564857201896, - "timestamp": 6.672630263920583 - }, - { - "x": 5.007489312227579, - "y": 7.006590440945802, - "heading": 3.3029154847342026, - "angularVelocity": 0.000016289070060669773, - "velocityX": -1.6072736995102561, - "velocityY": -0.23540564863105312, - "timestamp": 6.767918241807355 - }, - { - "x": 4.854335451483483, - "y": 6.984159112697374, - "heading": 3.30291703685908, - "angularVelocity": 0.00001628877967680068, - "velocityX": -1.6072736995669794, - "velocityY": -0.23540564869007324, - "timestamp": 6.863206219694128 - }, - { - "x": 4.701181590737123, - "y": 6.961727784450269, - "heading": 3.3029185889562895, - "angularVelocity": 0.00001628848931160111, - "velocityX": -1.6072736996236994, - "velocityY": -0.23540564874908013, - "timestamp": 6.9584941975809 - }, - { - "x": 4.548027729985049, - "y": 6.939296456188679, - "heading": 3.3029201410258326, - "angularVelocity": 0.000016288198966670516, - "velocityX": -1.6072736996804171, - "velocityY": -0.23540564880807416, - "timestamp": 7.053782175467672 - }, - { - "x": 4.394873869225821, - "y": 6.916865127932724, - "heading": 3.3029216930677117, - "angularVelocity": 0.000016287908648086418, - "velocityX": -1.607273699737131, - "velocityY": -0.2354056488670535, - "timestamp": 7.149070153354445 - }, - { - "x": 4.2417200084613995, - "y": 6.8944337996586516, - "heading": 3.302923245081929, - "angularVelocity": 0.000016287618349487135, - "velocityX": -1.607273699793842, - "velocityY": -0.23540564892601998, - "timestamp": 7.244358131241217 - }, - { - "x": 4.088566147690897, - "y": 6.872002471380619, - "heading": 3.3029247970684854, - "angularVelocity": 0.00001628732806480796, - "velocityX": -1.607273699850551, - "velocityY": -0.23540564898497424, - "timestamp": 7.339646109127989 - }, - { - "x": 3.935412286918391, - "y": 6.849571143103449, - "heading": 3.3029263490273832, - "angularVelocity": 0.00001628703780483265, - "velocityX": -1.6072736999072568, - "velocityY": -0.2354056490439145, - "timestamp": 7.434934087014762 - }, - { - "x": 3.7822584261353844, - "y": 6.827139814806336, - "heading": 3.302927900958626, - "angularVelocity": 0.000016286747572595648, - "velocityX": -1.6072736999639583, - "velocityY": -0.2354056491028402, - "timestamp": 7.530222064901534 - }, - { - "x": 3.629104565353085, - "y": 6.804708486525145, - "heading": 3.3029294528622137, - "angularVelocity": 0.000016286457354449532, - "velocityX": -1.607273700020658, - "velocityY": -0.2354056491617539, - "timestamp": 7.6255100427883065 - }, - { - "x": 3.475950704559541, - "y": 6.782277158221499, - "heading": 3.3029310047381495, - "angularVelocity": 0.000016286167156294597, - "velocityX": -1.607273700077355, - "velocityY": -0.2354056492206542, - "timestamp": 7.720798020675079 - }, - { - "x": 3.3227968437635895, - "y": 6.759845829932118, - "heading": 3.3029325565864354, - "angularVelocity": 0.000016285876982629207, - "velocityX": -1.6072737001340485, - "velocityY": -0.23540564927954047, - "timestamp": 7.816085998561851 - }, - { - "x": 3.16964298296173, - "y": 6.737414501611197, - "heading": 3.3029341084070736, - "angularVelocity": 0.00001628558683240146, - "velocityX": -1.6072737001907338, - "velocityY": -0.23540564933844627, - "timestamp": 7.9113739764486235 - }, - { - "x": 3.0164891229666777, - "y": 6.714983167772551, - "heading": 3.302935660207411, - "angularVelocity": 0.00001628537379077082, - "velocityX": -1.607273691741332, - "velocityY": -0.23540570734761523, - "timestamp": 8.006661954335396 - }, - { - "x": 2.8768170840789846, - "y": 6.685183755439867, - "heading": 3.351795711849992, - "angularVelocity": 0.512761974030336, - "velocityX": -1.4657886753698728, - "velocityY": -0.3127300316833431, - "timestamp": 8.101949932222169 + "angularVelocity": -0.13565781752265163, + "velocityX": 0.18589151392514228, + "velocityY": -0.1064895459686019, + "timestamp": 3.385101516966396 + }, + { + "x": 7.915740116584839, + "y": 7.438450021195838, + "heading": 3.1229758381188732, + "angularVelocity": -0.07896960965622021, + "velocityX": -0.23197262761210297, + "velocityY": -0.16013118150815783, + "timestamp": 3.474461031890667 + }, + { + "x": 7.857668877454443, + "y": 7.419352965299199, + "heading": 3.1208942356371594, + "angularVelocity": -0.023294693167012128, + "velocityX": -0.6498607247321063, + "velocityY": -0.21371038006220794, + "timestamp": 3.5638205468149375 + }, + { + "x": 7.762252743153032, + "y": 7.395475025916455, + "heading": 3.123675562058841, + "angularVelocity": 0.031125128913673642, + "velocityX": -1.0677781138614328, + "velocityY": -0.2672120523816525, + "timestamp": 3.6531800617392083 + }, + { + "x": 7.629488438537512, + "y": 7.366825038470432, + "heading": 3.1311770789195053, + "angularVelocity": 0.08394760051038563, + "velocityX": -1.4857321542986437, + "velocityY": -0.3206148496923158, + "timestamp": 3.742539576663479 + }, + { + "x": 7.459371747842553, + "y": 7.333414662601681, + "heading": 3.14321079663535, + "angularVelocity": 0.13466632765456588, + "velocityX": -1.9037333723121508, + "velocityY": -0.3738871668793641, + "timestamp": 3.8318990915877498 + }, + { + "x": 7.251897047158697, + "y": 7.295259995220328, + "heading": 3.159517771914932, + "angularVelocity": 0.1824872851357962, + "velocityX": -2.3217975260909065, + "velocityY": -0.4269793475679505, + "timestamp": 3.9212586065120205 + }, + { + "x": 7.007056479155703, + "y": 7.252384696832851, + "heading": 3.179718570361924, + "angularVelocity": 0.2260620871108287, + "velocityX": -2.739949609288821, + "velocityY": -0.47980674944141977, + "timestamp": 4.010618121436291 + }, + { + "x": 6.724838378878884, + "y": 7.204826948209543, + "heading": 3.2032038202698816, + "angularVelocity": 0.2628175626049468, + "velocityX": -3.1582322320794676, + "velocityY": -0.5322068798561834, + "timestamp": 4.0999776363605624 + }, + { + "x": 6.405224102249246, + "y": 7.152658687501974, + "heading": 3.2288345481257252, + "angularVelocity": 0.2868270701510058, + "velocityX": -3.5767234960988827, + "velocityY": -0.5838019683945127, + "timestamp": 4.189337151284834 + }, + { + "x": 6.048184900918235, + "y": 7.096065538236557, + "heading": 3.253716466109861, + "angularVelocity": 0.2784473260091271, + "velocityX": -3.995536475702559, + "velocityY": -0.6333197904373026, + "timestamp": 4.278696666209105 + }, + { + "x": 5.654508056038536, + "y": 7.03656993570583, + "heading": 3.253716532797699, + "angularVelocity": 7.462869273514143e-7, + "velocityX": -4.4055391886731465, + "velocityY": -0.6658004195876279, + "timestamp": 4.368056181133376 + }, + { + "x": 5.259674739202379, + "y": 6.9853073355549045, + "heading": 3.253716547435891, + "angularVelocity": 1.6381235089876507e-7, + "velocityX": -4.418480977328111, + "velocityY": -0.573666947435528, + "timestamp": 4.457415696057647 + }, + { + "x": 4.864841395212643, + "y": 6.934044944547169, + "heading": 3.253716562074652, + "angularVelocity": 1.63818712302411e-7, + "velocityX": -4.41848128119701, + "velocityY": -0.5736646069663508, + "timestamp": 4.546775210981918 + }, + { + "x": 4.491651487875698, + "y": 6.885589548517627, + "heading": 3.312737130762719, + "angularVelocity": 0.6604844345684403, + "velocityX": -4.17627499044966, + "velocityY": -0.5422522276514719, + "timestamp": 4.63613472590619 + }, + { + "x": 4.155780465580293, + "y": 6.841979518441731, + "heading": 3.3658484725068094, + "angularVelocity": 0.5943557525922112, + "velocityX": -3.7586486741791827, + "velocityY": -0.48802894815235986, + "timestamp": 4.725494240830461 + }, + { + "x": 3.857228363312986, + "y": 6.803214924393239, + "heading": 3.413052217911606, + "angularVelocity": 0.528245318305502, + "velocityX": -3.3410219663828666, + "velocityY": -0.43380488447528814, + "timestamp": 4.814853755754732 + }, + { + "x": 3.5959952091477, + "y": 6.769295821195016, + "heading": 3.454350393436854, + "angularVelocity": 0.46215756162336336, + "velocityX": -2.9233949444183165, + "velocityY": -0.37958020728927433, + "timestamp": 4.904213270679003 + }, + { + "x": 3.3720810297941446, + "y": 6.740222250549493, + "heading": 3.4897450705258297, + "angularVelocity": 0.3960929859452755, + "velocityX": -2.5057676235520625, + "velocityY": -0.32535506342174403, + "timestamp": 4.993572785603274 + }, + { + "x": 3.1854858494686082, + "y": 6.715994242916669, + "heading": 3.519238149861463, + "angularVelocity": 0.33004968033484894, + "velocityX": -2.0881400316873893, + "velocityY": -0.2711295786839986, + "timestamp": 5.0829323005275455 + }, + { + "x": 3.036209688317166, + "y": 6.696611819522511, + "heading": 3.542831216825285, + "angularVelocity": 0.2640241163329568, + "velocityX": -1.6705122143730164, + "velocityY": -0.21690385641174914, + "timestamp": 5.172291815451817 + }, + { + "x": 2.924252561165524, + "y": 6.682074994259449, + "heading": 3.560525448236687, + "angularVelocity": 0.19801172182276905, + "velocityX": -1.2528842311478772, + "velocityY": -0.1626779786728065, + "timestamp": 5.261651330376088 + }, + { + "x": 2.849614476676597, + "y": 6.6723837752661295, + "heading": 3.5723215560021737, + "angularVelocity": 0.13200729408035738, + "velocityX": -0.8352561509782144, + "velocityY": -0.10845200985627608, + "timestamp": 5.351010845300359 }, { "x": 2.812295436859131, "y": 6.667538166046143, "heading": 3.5782197562990956, - "angularVelocity": 2.3762078855126445, - "velocityX": -0.6771226407787638, - "velocityY": -0.18518169637911341, - "timestamp": 8.197237910108942 + "angularVelocity": 0.06600528552466699, + "velocityX": -0.41762804832918915, + "velocityY": -0.05422600183195843, + "timestamp": 5.44037036022463 }, { "x": 2.812295436859131, "y": 6.667538166046143, "heading": 3.5782197562990956, - "angularVelocity": -9.273365059762218e-23, - "velocityX": 5.842590739873529e-23, - "velocityY": -3.6959137012591573e-22, - "timestamp": 8.292525887995716 + "angularVelocity": -6.3889947065303214e-27, + "velocityX": 2.638834492143492e-26, + "velocityY": 4.3134967976763594e-26, + "timestamp": 5.5297298751489015 } ], "trajectoryWaypoints": [ @@ -1387,10 +1171,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 17 + "controlIntervalCount": 16 }, { - "timestamp": 1.4312485641258421, + "timestamp": 1.2882874881007154, "isStopPoint": true, "x": 2.6714627742767334, "y": 6.900224685668945, @@ -1398,10 +1182,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 38 + "controlIntervalCount": 25 }, { - "timestamp": 4.862158684071908, + "timestamp": 3.385101516966396, "isStopPoint": false, "x": 7.936469078063965, "y": 7.452759265899658, @@ -1409,10 +1193,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 36 + "controlIntervalCount": 24 }, { - "timestamp": 8.292525887995716, + "timestamp": 5.5297298751489015, "isStopPoint": true, "x": 2.812295436859131, "y": 6.667538166046143, @@ -1468,7 +1252,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 11 }, { "x": 2.0338082313537598, @@ -1485,253 +1269,271 @@ "x": 0.875186562538147, "y": 6.525386810302734, "heading": -2.143762813465651, - "angularVelocity": -5.355945535245033e-29, - "velocityX": 2.43610406010345e-28, - "velocityY": -8.419270817143307e-28, + "angularVelocity": 7.837232067421535e-25, + "velocityX": -1.3869968870640015e-24, + "velocityY": -1.5166283413174535e-25, "timestamp": 0 }, { - "x": 0.8966416033945684, - "y": 6.537208607893962, - "heading": -2.364630778366916, - "angularVelocity": -2.838662406313846, - "velocityX": 0.27574672466546707, - "velocityY": 0.15193734597170733, - "timestamp": 0.07780705603103885 - }, - { - "x": 0.9545363231559146, - "y": 6.574628260062144, - "heading": -2.5677938220576824, - "angularVelocity": -2.611113362388633, - "velocityX": 0.7440805849054467, - "velocityY": 0.48092877531896794, - "timestamp": 0.1556141120620777 - }, - { - "x": 1.0367704734566419, - "y": 6.613390261480878, - "heading": -2.7011163551758717, - "angularVelocity": -1.7135018328543843, - "velocityX": 1.0568983649493484, - "velocityY": 0.49818105704026555, - "timestamp": 0.23342116809311655 - }, - { - "x": 1.1598322354696735, - "y": 6.642210050914376, - "heading": -2.701117699890818, - "angularVelocity": -0.000017282686365776702, - "velocityX": 1.5816272750885192, - "velocityY": 0.37040071818167397, - "timestamp": 0.3112282241241554 - }, - { - "x": 1.2828941160720575, - "y": 6.671029334799358, - "heading": -2.7011190439216617, - "angularVelocity": -0.00001727389406811502, - "velocityX": 1.5816287992350209, - "velocityY": 0.37039422071804506, - "timestamp": 0.3890352801551943 - }, - { - "x": 1.4059559966738584, - "y": 6.699848618684591, - "heading": -2.7011203879554953, - "angularVelocity": -0.000017273932497369342, - "velocityX": 1.5816287992275255, - "velocityY": 0.3703942207212755, - "timestamp": 0.46684233618623316 - }, - { - "x": 1.5290178772749528, - "y": 6.728667902570602, - "heading": -2.7011217319923193, - "angularVelocity": -0.00001727397092835146, - "velocityX": 1.5816287992184466, - "velocityY": 0.3703942207312666, - "timestamp": 0.544649392217272 - }, - { - "x": 1.6520797578753408, - "y": 6.75748718645739, - "heading": -2.7011230760321334, - "angularVelocity": -0.000017274009359396956, - "velocityX": 1.5816287992093674, - "velocityY": 0.37039422074125794, - "timestamp": 0.6224564482483109 - }, - { - "x": 1.7751416384750223, - "y": 6.786306470344956, - "heading": -2.701124420074938, - "angularVelocity": -0.000017274047791505146, - "velocityX": 1.5816287992002884, - "velocityY": 0.37039422075124934, - "timestamp": 0.7002635042793498 - }, - { - "x": 1.898203519073992, - "y": 6.81512575423332, - "heading": -2.7011257641207327, - "angularVelocity": -0.000017274086223033065, - "velocityX": 1.581628799191142, - "velocityY": 0.3703942207615267, - "timestamp": 0.7780705603103887 - }, - { - "x": 2.021265394654385, - "y": 6.843945059515562, - "heading": -2.7011271081971797, - "angularVelocity": -0.000017274480179424693, - "velocityX": 1.581628734690864, - "velocityY": 0.37039449572214483, - "timestamp": 0.8558776163414276 - }, - { - "x": 2.1197062132467392, - "y": 6.880631402807114, - "heading": -2.784861574958977, - "angularVelocity": -1.0761808894092335, - "velocityX": 1.2651914056879914, - "velocityY": 0.4715040661211618, - "timestamp": 0.9336846723724664 - }, - { - "x": 2.228161206604935, - "y": 6.914841049698727, - "heading": -2.8360417692393636, - "angularVelocity": -0.6577834568110343, - "velocityX": 1.393896632137462, - "velocityY": 0.43967280908258105, - "timestamp": 1.0114917284035052 - }, - { - "x": 2.342602667471001, - "y": 6.947096322596154, - "heading": -2.866867411656533, - "angularVelocity": -0.39618055211949504, - "velocityX": 1.4708365372468657, - "velocityY": 0.4145545988086245, - "timestamp": 1.089298784434544 - }, - { - "x": 2.460575781873871, - "y": 6.9780078455456644, - "heading": -2.885328334561652, - "angularVelocity": -0.23726540813669628, - "velocityX": 1.5162264249634116, - "velocityY": 0.39728431489785143, - "timestamp": 1.167105840465583 - }, - { - "x": 2.563184602851941, - "y": 6.995518176666575, - "heading": -2.9725697719134425, - "angularVelocity": -1.121253544369907, - "velocityX": 1.3187598427723222, - "velocityY": 0.22504811278202433, - "timestamp": 1.2449128964966218 + "x": 0.8969727039190931, + "y": 6.531365834673865, + "heading": -2.166346916414201, + "angularVelocity": -0.3175047636593087, + "velocityX": 0.3062864035806097, + "velocityY": 0.08405774292624062, + "timestamp": 0.07112996569961132 + }, + { + "x": 0.9406148218819743, + "y": 6.543392626223336, + "heading": -2.210869956131151, + "angularVelocity": -0.6259392828189381, + "velocityX": 0.6135546043588166, + "velocityY": 0.16908192533455152, + "timestamp": 0.14225993139922263 + }, + { + "x": 1.0062172991621152, + "y": 6.561523867966903, + "heading": -2.2764288840652025, + "angularVelocity": -0.9216780479118013, + "velocityX": 0.9222902982575069, + "velocityY": 0.25490300136143174, + "timestamp": 0.21338989709883394 + }, + { + "x": 1.093969891095599, + "y": 6.585785930725148, + "heading": -2.36133395682105, + "angularVelocity": -1.1936610951621995, + "velocityX": 1.2336937192416417, + "velocityY": 0.3410948187534151, + "timestamp": 0.28451986279844527 + }, + { + "x": 1.2042232731861569, + "y": 6.6162032972801414, + "heading": -2.461705007120923, + "angularVelocity": -1.4110937537035995, + "velocityX": 1.5500272073259296, + "velocityY": 0.42763083400670887, + "timestamp": 0.3556498284980566 + }, + { + "x": 1.3374731837843992, + "y": 6.652906460035935, + "heading": -2.567532529869651, + "angularVelocity": -1.4878050580770381, + "velocityX": 1.8733301680612304, + "velocityY": 0.5160014122710946, + "timestamp": 0.4267797941976679 + }, + { + "x": 1.4887376373930754, + "y": 6.6960750532748765, + "heading": -2.6261176444741485, + "angularVelocity": -0.8236347934133412, + "velocityX": 2.1265925284918663, + "velocityY": 0.6068974280297831, + "timestamp": 0.49790975989727926 + }, + { + "x": 1.6559451101368896, + "y": 6.7447998856875095, + "heading": -2.6266322508442816, + "angularVelocity": -0.007234733843487786, + "velocityX": 2.3507318061975107, + "velocityY": 0.685011330082775, + "timestamp": 0.5690397255968905 + }, + { + "x": 1.8243976661229355, + "y": 6.7896009456539, + "heading": -2.626632286559664, + "angularVelocity": -5.021144321983056e-7, + "velocityX": 2.368236148143772, + "velocityY": 0.6298479062339171, + "timestamp": 0.6401696912965018 + }, + { + "x": 1.9921751026428776, + "y": 6.83686703905938, + "heading": -2.6266327970809837, + "angularVelocity": -0.000007177303057357513, + "velocityX": 2.358744797213626, + "velocityY": 0.6645032503613142, + "timestamp": 0.7112996569961131 + }, + { + "x": 2.144093876666576, + "y": 6.878851502304941, + "heading": -2.6858581481034687, + "angularVelocity": -0.8326357315087016, + "velocityX": 2.135791470296306, + "velocityY": 0.5902500139373705, + "timestamp": 0.7824296226957244 + }, + { + "x": 2.277702419689923, + "y": 6.914840816688254, + "heading": -2.79218877780944, + "angularVelocity": -1.4948781242917446, + "velocityX": 1.8783720997081357, + "velocityY": 0.505965580460125, + "timestamp": 0.8535595883953356 + }, + { + "x": 2.3883482703840326, + "y": 6.944322663737028, + "heading": -2.894178661554579, + "angularVelocity": -1.4338525646961797, + "velocityX": 1.5555448341051872, + "velocityY": 0.41447857817447836, + "timestamp": 0.9246895540949469 + }, + { + "x": 2.4764066005629384, + "y": 6.967574702744683, + "heading": -2.9819793987887278, + "angularVelocity": -1.2343705830667766, + "velocityX": 1.2379920236540676, + "velocityY": 0.3268951247052539, + "timestamp": 0.9958195197945582 + }, + { + "x": 2.5421906012565776, + "y": 6.984807873214321, + "heading": -3.0509427511393503, + "angularVelocity": -0.9695400760048395, + "velocityX": 0.9248422946167467, + "velocityY": 0.24227722170451613, + "timestamp": 1.0669494854941695 + }, + { + "x": 2.5859077991162414, + "y": 6.996185319772124, + "heading": -3.098476979135992, + "angularVelocity": -0.6682728935563261, + "velocityX": 0.6146101355409838, + "velocityY": 0.1599529318747363, + "timestamp": 1.1380794511937808 }, { "x": 2.6077051162719727, "y": 7.001829624176025, "heading": -3.122903212305612, - "angularVelocity": -1.9321311981293672, - "velocityX": 0.5721912084974853, - "velocityY": 0.08111664714486487, - "timestamp": 1.3227199525276607 + "angularVelocity": -0.34340285320499864, + "velocityX": 0.30644352125493224, + "velocityY": 0.0793519910825985, + "timestamp": 1.2092094168933922 }, { "x": 2.6077051162719727, "y": 7.001829624176025, "heading": -3.122903212305612, - "angularVelocity": -5.850460431476152e-29, - "velocityX": -4.4073165345897844e-27, - "velocityY": 1.309519926897454e-26, - "timestamp": 1.4005270085586996 - }, - { - "x": 2.581564908929706, - "y": 6.99268752896088, - "heading": -2.9753776937459495, - "angularVelocity": 2.140746378487904, - "velocityX": -0.37932118285182687, - "velocityY": -0.1326611654355847, - "timestamp": 1.4694401315900536 - }, - { - "x": 2.510121983919206, - "y": 6.965703523715206, - "heading": -2.8481100670568136, - "angularVelocity": 1.846783618139497, - "velocityX": -1.0367100178872335, - "velocityY": -0.3915655546969944, - "timestamp": 1.5383532546214076 - }, - { - "x": 2.4183443604796597, - "y": 6.911820320186112, - "heading": -2.82871535814287, - "angularVelocity": 0.2814370915263766, - "velocityX": -1.3317873200694863, - "velocityY": -0.7819004735074551, - "timestamp": 1.6072663776527616 - }, - { - "x": 2.324659626384285, - "y": 6.850545155910229, - "heading": -2.828714867091888, - "angularVelocity": 0.000007125652715720525, - "velocityX": -1.3594614490588457, - "velocityY": -0.8891653952180393, - "timestamp": 1.6761795006841156 - }, - { - "x": 2.2309748959640565, - "y": 6.7892699860148875, - "heading": -2.8287143760416544, - "angularVelocity": 0.000007125641845880462, - "velocityX": -1.359461395728709, - "velocityY": -0.8891654767621404, - "timestamp": 1.7450926237154696 - }, - { - "x": 2.136919509671741, - "y": 6.728565387611802, - "heading": -2.828713709965057, - "angularVelocity": 0.000009665453660450363, - "velocityX": -1.3648399920799144, - "velocityY": -0.8808858999970965, - "timestamp": 1.8140057467468236 - }, - { - "x": 2.062260898631176, - "y": 6.694685917301612, - "heading": -2.7155906116912156, - "angularVelocity": 1.6415320231876958, - "velocityX": -1.0833729158754979, - "velocityY": -0.491625815518109, - "timestamp": 1.8829188697781776 + "angularVelocity": -1.2711795884725035e-24, + "velocityX": 1.0813970849471872e-24, + "velocityY": 4.939088142936104e-25, + "timestamp": 1.2803393825930036 + }, + { + "x": 2.588497605107588, + "y": 6.99102525005929, + "heading": -3.105999231961874, + "angularVelocity": 0.2434967604471089, + "velocityX": -0.27667843014925436, + "velocityY": -0.15563376450908756, + "timestamp": 1.3497611700233554 + }, + { + "x": 2.550087465438114, + "y": 6.9694228225612935, + "heading": -3.0720638130010824, + "angularVelocity": 0.4888295190445427, + "velocityX": -0.5532865270576512, + "velocityY": -0.31117648072184184, + "timestamp": 1.4191829574537072 + }, + { + "x": 2.4924860648996834, + "y": 6.937030311414511, + "heading": -3.0208719743457277, + "angularVelocity": 0.7374030624998452, + "velocityX": -0.8297308765813584, + "velocityY": -0.46660439533166076, + "timestamp": 1.488604744884059 + }, + { + "x": 2.415713178984927, + "y": 6.893852911543239, + "heading": -2.9521086467399438, + "angularVelocity": 0.9905150839680044, + "velocityX": -1.1058903660724633, + "velocityY": -0.621957478617077, + "timestamp": 1.5580265323144107 + }, + { + "x": 2.3197977520281423, + "y": 6.8398868515705145, + "heading": -2.865405613995347, + "angularVelocity": 1.248931149051482, + "velocityX": -1.3816329211202274, + "velocityY": -0.7773648874550575, + "timestamp": 1.6274483197447625 + }, + { + "x": 2.2244467831783776, + "y": 6.785620081909038, + "heading": -2.773269561572217, + "angularVelocity": 1.327192165940218, + "velocityX": -1.3735020716000237, + "velocityY": -0.781696520215938, + "timestamp": 1.6968701071751142 + }, + { + "x": 2.14818394170963, + "y": 6.7421795937838125, + "heading": -2.6995256157206273, + "angularVelocity": 1.0622593940781788, + "velocityX": -1.0985433289982525, + "velocityY": -0.6257471859077053, + "timestamp": 1.766291894605466 + }, + { + "x": 2.090994438671986, + "y": 6.709584746202325, + "heading": -2.6442247517444515, + "angularVelocity": 0.7965923382721457, + "velocityX": -0.8237976167787333, + "velocityY": -0.46951899091028315, + "timestamp": 1.8357136820358178 + }, + { + "x": 2.052870310008216, + "y": 6.687849855649161, + "heading": -2.607365454456659, + "angularVelocity": 0.5309471082802596, + "velocityX": -0.5491666244119513, + "velocityY": -0.3130845712517861, + "timestamp": 1.9051354694661695 }, { "x": 2.03380823135376, "y": 6.676982402801514, "heading": -2.588935989400422, - "angularVelocity": 1.8378882964449172, - "velocityX": -0.4128773450663434, - "velocityY": -0.256896128362137, - "timestamp": 1.9518319928095316 + "angularVelocity": 0.2654709096150401, + "velocityX": -0.2745835185183043, + "velocityY": -0.1565423946848157, + "timestamp": 1.9745572568965213 }, { "x": 2.03380823135376, "y": 6.676982402801514, "heading": -2.588935989400422, - "angularVelocity": -3.9702743259995845e-28, - "velocityX": -1.31352865617337e-26, - "velocityY": 2.3602776479280726e-26, - "timestamp": 2.0207451158408856 + "angularVelocity": -5.507504748429065e-28, + "velocityX": -9.813305241361081e-27, + "velocityY": 1.6191757102806756e-26, + "timestamp": 2.043979044326873 } ], "trajectoryWaypoints": [ @@ -1747,7 +1549,7 @@ "controlIntervalCount": 18 }, { - "timestamp": 1.4005270085586996, + "timestamp": 1.2803393825930036, "isStopPoint": true, "x": 2.6077051162719727, "y": 7.001829624176025, @@ -1755,10 +1557,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 9 + "controlIntervalCount": 11 }, { - "timestamp": 2.0207451158408856, + "timestamp": 2.043979044326873, "isStopPoint": true, "x": 2.0338082313537598, "y": 6.676982402801514, @@ -1814,7 +1616,7 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 9 }, { "x": 2.152919054031372, @@ -1831,253 +1633,262 @@ "x": 0.7885606288909912, "y": 4.587131977081299, "heading": 2.068139227089701, - "angularVelocity": -4.000873155660283e-28, - "velocityX": -1.1169568989245428e-27, - "velocityY": -1.0444692137856757e-26, + "angularVelocity": 7.405153834607566e-23, + "velocityX": -9.165802676126164e-23, + "velocityY": 1.4488206597403162e-23, "timestamp": 0 }, { - "x": 0.8077628028564999, - "y": 4.577523858678819, - "heading": 2.29346899836082, - "angularVelocity": 2.9086898452542567, - "velocityX": 0.24787300899124248, - "velocityY": -0.12402727021663285, - "timestamp": 0.07746778902493198 - }, - { - "x": 0.8565084293998517, - "y": 4.542709239371979, - "heading": 2.521391906348417, - "angularVelocity": 2.942163586393862, - "velocityX": 0.6292373534458789, - "velocityY": -0.44940767956648386, - "timestamp": 0.15493557804986396 - }, - { - "x": 0.9320047259554965, - "y": 4.504567603535149, - "heading": 2.6737831770723717, - "angularVelocity": 1.96715657748938, - "velocityX": 0.9745508101612039, - "velocityY": -0.492354775022096, - "timestamp": 0.23240336707479595 - }, - { - "x": 1.0259716426537533, - "y": 4.468035910816095, - "heading": 2.7708019811352167, - "angularVelocity": 1.252376055700012, - "velocityX": 1.2129804900978245, - "velocityY": -0.4715726778686935, - "timestamp": 0.30987115609972793 - }, - { - "x": 1.1315666128977582, - "y": 4.434097705174351, - "heading": 2.8306190294366744, - "angularVelocity": 0.7721538081099244, - "velocityX": 1.3630822768159876, - "velocityY": -0.43809441406442845, - "timestamp": 0.38733894512465994 - }, - { - "x": 1.2441769248956835, - "y": 4.4023522435792275, - "heading": 2.866815836119521, - "angularVelocity": 0.46724977101382253, - "velocityX": 1.4536404538624839, - "velocityY": -0.4097891781177288, - "timestamp": 0.46480673414959195 - }, - { - "x": 1.360949666856534, - "y": 4.372150081143396, - "heading": 2.8884951991985233, - "angularVelocity": 0.2798500299527736, - "velocityX": 1.5073715595945123, - "velocityY": -0.3898673605633771, - "timestamp": 0.542274523174524 - }, - { - "x": 1.483714050289093, - "y": 4.344503608621978, - "heading": 2.888502422435885, - "angularVelocity": 0.00009324181640315531, - "velocityX": 1.584715208446818, - "velocityY": -0.356877004873872, - "timestamp": 0.619742312199456 - }, - { - "x": 1.6064784345242633, - "y": 4.316857140064094, - "heading": 2.88850964608229, - "angularVelocity": 0.00009324709657913404, - "velocityX": 1.5847152188073954, - "velocityY": -0.3568769537102325, - "timestamp": 0.697210101224388 - }, - { - "x": 1.729242818618949, - "y": 4.289210671426577, - "heading": 2.8885168702684654, - "angularVelocity": 0.00009325406426226353, - "velocityX": 1.5847152169939396, - "velocityY": -0.3568769547381692, - "timestamp": 0.77467789024932 - }, - { - "x": 1.8520072025730983, - "y": 4.261564202709402, - "heading": 2.888524094994609, - "angularVelocity": 0.00009326103448260538, - "velocityX": 1.5847152151798087, - "velocityY": -0.3568769557664531, - "timestamp": 0.852145679274252 - }, - { - "x": 1.9747715863866588, - "y": 4.233917733912545, - "heading": 2.8885313202609173, - "angularVelocity": 0.00009326800724451237, - "velocityX": 1.5847152133650135, - "velocityY": -0.35687695679502923, - "timestamp": 0.929613468299184 - }, - { - "x": 2.09753597005958, - "y": 4.206271265035981, - "heading": 2.8885385460675863, - "angularVelocity": 0.00009327498254687209, - "velocityX": 1.584715211549548, - "velocityY": -0.35687695782393064, - "timestamp": 1.007081257324116 - }, - { - "x": 2.2203003530348537, - "y": 4.178624793690813, - "heading": 2.888545772490986, - "angularVelocity": 0.00009328294366610178, - "velocityX": 1.5847152025439089, - "velocityY": -0.3568769896901373, - "timestamp": 1.084549046349048 - }, - { - "x": 2.339535767958966, - "y": 4.149427051618625, - "heading": 2.9014383733628053, - "angularVelocity": 0.16642531088204846, - "velocityX": 1.5391611975105928, - "velocityY": -0.37690170895145225, - "timestamp": 1.16201683537398 - }, - { - "x": 2.4602156964640742, - "y": 4.1208550801313235, - "heading": 2.9091068110983676, - "angularVelocity": 0.0989887259218709, - "velocityX": 1.5578078324433486, - "velocityY": -0.36882389244523134, - "timestamp": 1.239484624398912 - }, - { - "x": 2.563006485689252, - "y": 4.105677497528831, - "heading": 2.9949971538769358, - "angularVelocity": 1.1087233011248803, - "velocityX": 1.3268842511059193, - "velocityY": -0.1959212053619926, - "timestamp": 1.316952413423844 + "x": 0.8092723820430497, + "y": 4.581756141058556, + "heading": 2.0914655240689815, + "angularVelocity": 0.3351714697575674, + "velocityX": 0.2976035481069952, + "velocityY": -0.0772444448648893, + "timestamp": 0.06959511499040384 + }, + { + "x": 0.8507515012715718, + "y": 4.570851691637496, + "heading": 2.1374460572115312, + "angularVelocity": 0.6606862155323626, + "velocityX": 0.59600618857002, + "velocityY": -0.15668412104160437, + "timestamp": 0.13919022998080768 + }, + { + "x": 0.9130744655083555, + "y": 4.554273162378978, + "heading": 2.20529724746531, + "angularVelocity": 0.9749418513517004, + "velocityX": 0.8955077413892276, + "velocityY": -0.23821397896682367, + "timestamp": 0.20878534497121154 + }, + { + "x": 0.9963957154773515, + "y": 4.531931817223244, + "heading": 2.2937212565875593, + "angularVelocity": 1.2705490771075152, + "velocityX": 1.1972284258814936, + "velocityY": -0.32101886977002797, + "timestamp": 0.27838045996161537 + }, + { + "x": 1.1010580704953445, + "y": 4.503787887410387, + "heading": 2.3994943696463134, + "angularVelocity": 1.5198353084600695, + "velocityX": 1.503875021004259, + "velocityY": -0.40439519090868825, + "timestamp": 0.3479755749520192 + }, + { + "x": 1.227655866348761, + "y": 4.469686827398336, + "heading": 2.513230323560048, + "angularVelocity": 1.634251971987077, + "velocityX": 1.8190615227932214, + "velocityY": -0.48999214983341605, + "timestamp": 0.417570689942423 + }, + { + "x": 1.3716687344293004, + "y": 4.429078416516423, + "heading": 2.5833875612817048, + "angularVelocity": 1.0080770429265136, + "velocityX": 2.0692956409425562, + "velocityY": -0.5834951330637533, + "timestamp": 0.48716580493282685 + }, + { + "x": 1.5310835689434033, + "y": 4.382853965770288, + "heading": 2.5991130952622044, + "angularVelocity": 0.22595743943619906, + "velocityX": 2.290603795052096, + "velocityY": -0.6641910247940317, + "timestamp": 0.5567609199232307 + }, + { + "x": 1.69623233687307, + "y": 4.340284063453759, + "heading": 2.5991131342417417, + "angularVelocity": 5.600901392666899e-7, + "velocityX": 2.372993678542496, + "velocityY": -0.6116794594333097, + "timestamp": 0.6263560349136346 + }, + { + "x": 1.8613811081667575, + "y": 4.297714174187834, + "heading": 2.5991131732212707, + "angularVelocity": 5.600900148939747e-7, + "velocityX": 2.372993726879466, + "velocityY": -0.6116792719114585, + "timestamp": 0.6959511499040384 + }, + { + "x": 2.0217130268983476, + "y": 4.252932737260293, + "heading": 2.613201494408751, + "angularVelocity": 0.20243261598781087, + "velocityX": 2.3037812173123697, + "velocityY": -0.6434566123457787, + "timestamp": 0.7655462648944422 + }, + { + "x": 2.16688524944999, + "y": 4.21338989219855, + "heading": 2.683886289405167, + "angularVelocity": 1.015657420871611, + "velocityX": 2.0859542019819104, + "velocityY": -0.5681842046987992, + "timestamp": 0.835141379884846 + }, + { + "x": 2.294050574677846, + "y": 4.179894439044184, + "heading": 2.796544364902464, + "angularVelocity": 1.6187641260860197, + "velocityX": 1.8272162528273361, + "velocityY": -0.48129029112149246, + "timestamp": 0.9047364948752499 + }, + { + "x": 2.3992728564214008, + "y": 4.152626972084835, + "heading": 2.9035177926378313, + "angularVelocity": 1.5370824195077049, + "velocityX": 1.5119205099102537, + "velocityY": -0.3918014499021688, + "timestamp": 0.9743316098656537 + }, + { + "x": 2.4829722545309574, + "y": 4.131221801674751, + "heading": 2.995152785659621, + "angularVelocity": 1.316687141538951, + "velocityX": 1.2026619701841783, + "velocityY": -0.3075671390590503, + "timestamp": 1.0439267248560575 + }, + { + "x": 2.5454789720874493, + "y": 4.115415801961452, + "heading": 3.0669003824978076, + "angularVelocity": 1.0309286341157697, + "velocityX": 0.8981480606090146, + "velocityY": -0.2271136374367465, + "timestamp": 1.1135218398464615 + }, + { + "x": 2.5870063195907287, + "y": 4.1050110004584175, + "heading": 3.1162558708577963, + "angularVelocity": 0.7091803550693766, + "velocityX": 0.5966991721906145, + "velocityY": -0.14950476774798632, + "timestamp": 1.1831169548368654 }, { "x": 2.6077051162719727, "y": 4.099861145019531, "heading": 3.141592653589793, - "angularVelocity": 1.8923413402914784, - "velocityX": 0.5769963380306965, - "velocityY": -0.07508091533924856, - "timestamp": 1.394420202448776 + "angularVelocity": 0.36405978689007856, + "velocityX": 0.29741737885042957, + "velocityY": -0.07399736949348479, + "timestamp": 1.2527120698272693 }, { "x": 2.6077051162719727, "y": 4.099861145019531, "heading": 3.141592653589793, - "angularVelocity": 9.529530648984287e-27, - "velocityX": -6.64992161175884e-27, - "velocityY": -1.7807748804280264e-26, - "timestamp": 1.471887991473708 - }, - { - "x": 2.582723056141482, - "y": 4.106470038143139, - "heading": 3.0254564505751103, - "angularVelocity": -1.8328957625774476, - "velocityX": -0.39427423116151483, - "velocityY": 0.10430349785119646, - "timestamp": 1.5352501342566507 - }, - { - "x": 2.5176865762372205, - "y": 4.122909182686089, - "heading": 2.894011234282021, - "angularVelocity": -2.0745071192332674, - "velocityX": -1.0264248816056334, - "velocityY": 0.25944742114017705, - "timestamp": 1.5986122770395934 - }, - { - "x": 2.4297948421318796, - "y": 4.1567674114258075, - "heading": 2.8608182151959642, - "angularVelocity": -0.5238620038429632, - "velocityX": -1.3871332351626442, - "velocityY": 0.5343605385269978, - "timestamp": 1.6619744198225361 - }, - { - "x": 2.336899878456132, - "y": 4.201089714349744, - "heading": 2.8608175146596406, - "angularVelocity": -0.00001105607059469762, - "velocityX": -1.4660956778872583, - "velocityY": 0.6995076393765645, - "timestamp": 1.7253365626054789 - }, - { - "x": 2.2457294091191753, - "y": 4.241992503114134, - "heading": 2.8495777745026225, - "angularVelocity": -0.17738888969588018, - "velocityX": -1.4388792003022337, - "velocityY": 0.6455398597315226, - "timestamp": 1.7886987053884216 - }, - { - "x": 2.180218037779224, - "y": 4.270414402297769, - "heading": 2.7322527229675986, - "angularVelocity": -1.8516585200872955, - "velocityX": -1.033919758117567, - "velocityY": 0.44856278426375645, - "timestamp": 1.8520608481713643 + "angularVelocity": -7.013511745248326e-23, + "velocityX": -2.566963461380566e-23, + "velocityY": 1.8780462981611305e-24, + "timestamp": 1.3223071848176733 + }, + { + "x": 2.58482285126615, + "y": 4.108903526368604, + "heading": 3.1172534062149904, + "angularVelocity": -0.328671725346054, + "velocityX": -0.3089969629494254, + "velocityY": 0.12210628510695666, + "timestamp": 1.3963605502210295 + }, + { + "x": 2.5390877283653834, + "y": 4.12699559592087, + "heading": 3.068221389953176, + "angularVelocity": -0.6621173257251091, + "velocityX": -0.6175968188839946, + "velocityY": 0.2443112403294962, + "timestamp": 1.4704139156243856 + }, + { + "x": 2.470551207754016, + "y": 4.154160087682156, + "heading": 2.993960294612306, + "angularVelocity": -1.0028051383807046, + "velocityX": -0.9255017680569747, + "velocityY": 0.3668231904562775, + "timestamp": 1.5444672810277418 + }, + { + "x": 2.379281332174413, + "y": 4.190453494467717, + "heading": 2.893905888815326, + "angularVelocity": -1.3511122047188575, + "velocityX": -1.232487883331929, + "velocityY": 0.49009800686138194, + "timestamp": 1.618520646431098 + }, + { + "x": 2.2887269490195696, + "y": 4.227798636551948, + "heading": 2.7901909299452776, + "angularVelocity": -1.4005434905642753, + "velocityX": -1.22282603446329, + "velocityY": 0.5043004039157333, + "timestamp": 1.692574011834454 + }, + { + "x": 2.220820804164644, + "y": 4.255857671323432, + "heading": 2.7125381341765133, + "angularVelocity": -1.0486058985409061, + "velocityX": -0.9169893155435849, + "velocityY": 0.37890289818224415, + "timestamp": 1.7666273772378103 + }, + { + "x": 2.1755524378616315, + "y": 4.274580208282235, + "heading": 2.660825121415167, + "angularVelocity": -0.6983208997953653, + "velocityX": -0.6112938427099196, + "velocityY": 0.25282493046506116, + "timestamp": 1.8406807426411664 }, { "x": 2.152919054031372, "y": 4.283941268920898, "heading": 2.634960813022188, - "angularVelocity": -1.5354895789856624, - "velocityX": -0.4308406021142514, - "velocityY": 0.21348499323117995, - "timestamp": 1.915422990954307 + "angularVelocity": -0.34926580651804395, + "velocityX": -0.3056361275004237, + "velocityY": 0.12640965859786993, + "timestamp": 1.9147341080445226 }, { "x": 2.152919054031372, "y": 4.283941268920898, "heading": 2.634960813022188, - "angularVelocity": 1.4290307836372668e-27, - "velocityX": -1.821728341071586e-27, - "velocityY": 7.158150670810205e-27, - "timestamp": 1.9787851337372497 + "angularVelocity": -3.288439423134819e-23, + "velocityX": 3.096618824782888e-23, + "velocityY": -2.3851261489110276e-23, + "timestamp": 1.9887874734478788 } ], "trajectoryWaypoints": [ @@ -2093,7 +1904,7 @@ "controlIntervalCount": 19 }, { - "timestamp": 1.471887991473708, + "timestamp": 1.3223071848176733, "isStopPoint": true, "x": 2.6077051162719727, "y": 4.099861145019531, @@ -2101,10 +1912,10 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 8 + "controlIntervalCount": 9 }, { - "timestamp": 1.9787851337372497, + "timestamp": 1.9887874734478788, "isStopPoint": true, "x": 2.152919054031372, "y": 4.283941268920898, @@ -2168,154 +1979,154 @@ "x": 1.2650032043457031, "y": 5.615814685821533, "heading": 3.141, - "angularVelocity": -9.961906884794862e-27, - "velocityX": 6.399458285179746e-26, - "velocityY": 1.0314201039068904e-25, + "angularVelocity": -2.847593507618242e-26, + "velocityX": 3.1541489300939427e-25, + "velocityY": -2.217525612259016e-25, "timestamp": 0 }, { - "x": 1.310864623992567, - "y": 5.640347561603675, - "heading": 3.338413018068366, - "angularVelocity": 2.2732552294766206, - "velocityX": 0.5281045447942572, - "velocityY": 0.28250157315633995, - "timestamp": 0.08684155457274238 - }, - { - "x": 1.4026758531664765, - "y": 5.712962256076443, - "heading": 3.4150278612810783, - "angularVelocity": 0.8822371224197347, - "velocityX": 1.0572269189055614, - "velocityY": 0.8361745115000444, - "timestamp": 0.17368310914548477 - }, - { - "x": 1.505326244994725, - "y": 5.809724596960637, - "heading": 3.4150280522977696, - "angularVelocity": 0.0000021996000891052408, - "velocityX": 1.1820423106574365, - "velocityY": 1.1142400819545595, - "timestamp": 0.26052466371822713 - }, - { - "x": 1.6079766300540685, - "y": 5.906486945025769, - "heading": 3.4150282433141586, - "angularVelocity": 0.0000021995966111503638, - "velocityX": 1.182042232711982, - "velocityY": 1.1142401646446862, - "timestamp": 0.34736621829096953 - }, - { - "x": 1.7106270151134115, - "y": 6.003249293090899, - "heading": 3.4150284343305666, - "angularVelocity": 0.0000021995968268735563, - "velocityX": 1.182042232711972, - "velocityY": 1.1142401646446563, - "timestamp": 0.4342077728637119 - }, - { - "x": 1.8132774001727587, - "y": 6.100011641156021, - "heading": 3.4150286253469933, - "angularVelocity": 0.0000021995970422363537, - "velocityX": 1.1820422327120208, - "velocityY": 1.1142401646445645, - "timestamp": 0.5210493274364543 - }, - { - "x": 1.9159277852321102, - "y": 6.1967739892211355, - "heading": 3.4150288163634386, - "angularVelocity": 0.000002199597258109715, - "velocityX": 1.1820422327120694, - "velocityY": 1.1142401646444724, - "timestamp": 0.6078908820091966 - }, - { - "x": 2.018578170291466, - "y": 6.293536337286241, - "heading": 3.4150290073799026, - "angularVelocity": 0.0000021995974740409558, - "velocityX": 1.1820422327121183, - "velocityY": 1.1142401646443802, - "timestamp": 0.694732436581939 - }, - { - "x": 2.121228555350826, - "y": 6.390298685351339, - "heading": 3.415029198396385, - "angularVelocity": 0.000002199597689726547, - "velocityX": 1.182042232712167, - "velocityY": 1.1142401646442885, - "timestamp": 0.7815739911546813 - }, - { - "x": 2.22387894041019, - "y": 6.487061033416429, - "heading": 3.4150293894128865, - "angularVelocity": 0.0000021995979051884085, - "velocityX": 1.1820422327122158, - "velocityY": 1.1142401646441964, - "timestamp": 0.8684155457274236 - }, - { - "x": 2.3265293254695587, - "y": 6.583823381481512, - "heading": 3.415029580429407, - "angularVelocity": 0.000002199598120574345, - "velocityX": 1.1820422327122646, - "velocityY": 1.1142401646441045, - "timestamp": 0.955257100300166 - }, - { - "x": 2.4291797105289343, - "y": 6.680585729546584, - "heading": 3.415029771445946, - "angularVelocity": 0.0000021995983364688408, - "velocityX": 1.1820422327123468, - "velocityY": 1.114240164643977, - "timestamp": 1.0420986548729083 - }, - { - "x": 2.5318300994211365, - "y": 6.777348073545511, - "heading": 3.4150299624626883, - "angularVelocity": 0.0000021996006785706527, - "velocityX": 1.1820422768482035, - "velocityY": 1.1142401178214183, - "timestamp": 1.1289402094456507 - }, - { - "x": 2.6271100513248182, - "y": 6.858057139717029, - "heading": 3.467307103119733, - "angularVelocity": 0.6019830127897501, - "velocityX": 1.0971700399935989, - "velocityY": 0.9293830190926906, - "timestamp": 1.215781764018393 + "x": 1.2878690502136103, + "y": 5.636590724334488, + "heading": 3.156944146176203, + "angularVelocity": 0.1956833117689311, + "velocityX": 0.2806336818780737, + "velocityY": 0.2549853706008959, + "timestamp": 0.08147933532027608 + }, + { + "x": 1.3336452622258208, + "y": 5.6781705965802045, + "heading": 3.18797373905128, + "angularVelocity": 0.38082776145764136, + "velocityX": 0.5618137633582192, + "velocityY": 0.5103118733391074, + "timestamp": 0.16295867064055217 + }, + { + "x": 1.4023952169052167, + "y": 5.740596168815711, + "heading": 3.232737562480919, + "angularVelocity": 0.5493886671225605, + "velocityX": 0.8437716681064715, + "velocityY": 0.7661522027656068, + "timestamp": 0.24443800596082826 + }, + { + "x": 1.4942170903836536, + "y": 5.82393396269605, + "heading": 3.2887738175604304, + "angularVelocity": 0.6877357904215389, + "velocityX": 1.1269344934823864, + "velocityY": 1.0228089558261442, + "timestamp": 0.32591734128110433 + }, + { + "x": 1.6092733760907945, + "y": 5.928274751901453, + "heading": 3.3503605428052587, + "angularVelocity": 0.7558569912572904, + "velocityX": 1.4120916089322733, + "velocityY": 1.2805797788514484, + "timestamp": 0.4073966766013804 + }, + { + "x": 1.7472843038638464, + "y": 6.053006887878574, + "heading": 3.393001703956897, + "angularVelocity": 0.523337125714468, + "velocityX": 1.6938150910358227, + "velocityY": 1.5308438082714806, + "timestamp": 0.4888760119216565 + }, + { + "x": 1.8948651009567858, + "y": 6.187498734607365, + "heading": 3.3930017381142776, + "angularVelocity": 4.1921525430622926e-7, + "velocityX": 1.8112665808187325, + "velocityY": 1.650625231540434, + "timestamp": 0.5703553472419326 + }, + { + "x": 2.0424458885331562, + "y": 6.321990591778967, + "heading": 3.3930017722714747, + "angularVelocity": 4.1921300750866336e-7, + "velocityX": 1.811266464021397, + "velocityY": 1.650625359705577, + "timestamp": 0.6518346825622087 + }, + { + "x": 2.190026686851105, + "y": 6.45648243716353, + "heading": 3.3930018064288547, + "angularVelocity": 4.19215254408376e-7, + "velocityX": 1.811266595853337, + "velocityY": 1.650625215042643, + "timestamp": 0.7333140178824847 + }, + { + "x": 2.3281712431329007, + "y": 6.581195834448366, + "heading": 3.4359493310429845, + "angularVelocity": 0.5270971399719079, + "velocityX": 1.6954551204766442, + "velocityY": 1.5306138273541086, + "timestamp": 0.8147933532027608 + }, + { + "x": 2.443305156721716, + "y": 6.685507263891603, + "heading": 3.4972346317576646, + "angularVelocity": 0.7521575927660913, + "velocityX": 1.4130443398470416, + "velocityY": 1.280219445006684, + "timestamp": 0.8962726885230369 + }, + { + "x": 2.5351903686585104, + "y": 6.768824405464408, + "heading": 3.5531988851388214, + "angularVelocity": 0.6868521099389763, + "velocityX": 1.1277118495826628, + "velocityY": 1.0225554890121025, + "timestamp": 0.977752023843313 + }, + { + "x": 2.6039864921588998, + "y": 6.831227820433193, + "heading": 3.598162002435054, + "angularVelocity": 0.5518346108187143, + "velocityX": 0.8443383003796903, + "velocityY": 0.7658802655113843, + "timestamp": 1.0592313591635891 + }, + { + "x": 2.6497929927743678, + "y": 6.872785794141501, + "heading": 3.6294863595083644, + "angularVelocity": 0.384445417358672, + "velocityX": 0.5621854969166529, + "velocityY": 0.5100431114828528, + "timestamp": 1.1407106944838652 }, { "x": 2.6726744174957275, "y": 6.893547058105469, "heading": 3.6456400220863263, - "angularVelocity": 2.0535436041418818, - "velocityX": 0.5246839073193058, - "velocityY": 0.40867437902337306, - "timestamp": 1.3026233185911353 + "angularVelocity": 0.1982547171557739, + "velocityX": 0.28082488193378885, + "velocityY": 0.2548040418145309, + "timestamp": 1.2221900298041413 }, { "x": 2.6726744174957275, "y": 6.893547058105469, "heading": 3.6456400220863263, - "angularVelocity": 9.465973134511326e-26, - "velocityX": -9.092672379053801e-26, - "velocityY": -8.734024503832287e-26, - "timestamp": 1.3894648731638777 + "angularVelocity": 4.786100698151895e-26, + "velocityX": 2.8420295041943254e-25, + "velocityY": -2.9253363934820537e-25, + "timestamp": 1.3036693651244173 } ], "trajectoryWaypoints": [ @@ -2331,7 +2142,7 @@ "controlIntervalCount": 16 }, { - "timestamp": 1.3894648731638777, + "timestamp": 1.3036693651244173, "isStopPoint": true, "x": 2.6726744174957275, "y": 6.893547058105469, @@ -2372,21 +2183,30 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 57 + "controlIntervalCount": 31 + }, + { + "x": 6.819417953491211, + "y": 0.648584246635437, + "heading": 3.1544674706525395, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 }, { - "x": 7.956855773925781, - "y": 0.7431064248085022, + "x": 8.146759986877441, + "y": 0.667546272277832, "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 36 + "controlIntervalCount": 30 }, { - "x": 3.387338399887085, - "y": 2.2590601444244385, - "heading": 2.3477629859714533, + "x": 1.775516152381897, + "y": 3.2369022369384766, + "heading": 2.1727145819710088, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -2398,847 +2218,658 @@ "x": 0.8210453391075134, "y": 4.576303482055664, "heading": 2.111215568027718, - "angularVelocity": -1.8256752618245288e-22, - "velocityX": -7.65916010942737e-22, - "velocityY": 5.834863940365417e-22, + "angularVelocity": 1.0347096801356489e-22, + "velocityX": -4.48084535985214e-23, + "velocityY": -5.937888990335369e-23, "timestamp": 0 }, { - "x": 0.8644346902480902, - "y": 4.538605095268569, - "heading": 2.3610664104627634, - "angularVelocity": 2.6730073050757213, - "velocityX": 0.4641971643197831, - "velocityY": -0.4033128817553312, - "timestamp": 0.09347181429718082 - }, - { - "x": 0.9651901704673621, - "y": 4.45361330197089, - "heading": 2.440857637900237, - "angularVelocity": 0.8536394424076145, - "velocityX": 1.0779236604837226, - "velocityY": -0.9092772397405119, - "timestamp": 0.18694362859436164 - }, - { - "x": 1.0866380874820318, - "y": 4.362483747012655, - "heading": 2.440865267791502, - "angularVelocity": 0.00008162772192147527, - "velocityX": 1.299299879090211, - "velocityY": -0.9749415440733835, - "timestamp": 0.28041544289154247 - }, - { - "x": 1.2080860091215304, - "y": 4.271354198213206, - "heading": 2.4408728977229917, - "angularVelocity": 0.00008162815226084984, - "velocityX": 1.2992999285685376, - "velocityY": -0.9749414781841598, - "timestamp": 0.3738872571887233 - }, - { - "x": 1.3295339322232647, - "y": 4.1802246514640125, - "heading": 2.440880527950737, - "angularVelocity": 0.0000816313217297415, - "velocityX": 1.299299944212136, - "velocityY": -0.9749414562496848, - "timestamp": 0.4673590714859041 - }, - { - "x": 1.4509818569281794, - "y": 4.089095106952003, - "heading": 2.440888158472548, - "angularVelocity": 0.00008163446776368671, - "velocityX": 1.2992999613636238, - "velocityY": -0.9749414323153683, - "timestamp": 0.5608308857830849 - }, - { - "x": 1.5724297833961638, - "y": 3.997965564889279, - "heading": 2.440895789286055, - "angularVelocity": 0.00008163758844700525, - "velocityX": 1.2992999802256675, - "velocityY": -0.9749414061119055, - "timestamp": 0.6543027000802657 - }, - { - "x": 1.6938777118095218, - "y": 3.906836025517733, - "heading": 2.4409034203886897, - "angularVelocity": 0.00008164068165665073, - "velocityX": 1.299300001038079, - "velocityY": -0.9749413773205733, - "timestamp": 0.7477745143774466 - }, - { - "x": 1.8153256423773063, - "y": 3.8157064891148127, - "heading": 2.4409110517776624, - "angularVelocity": 0.00008164374501723712, - "velocityX": 1.299300024087019, - "velocityY": -0.9749413455609843, - "timestamp": 0.8412463286746275 - }, - { - "x": 1.9367735753408017, - "y": 3.7245769560008215, - "heading": 2.440918683449934, - "angularVelocity": 0.00008164677586345695, - "velocityX": 1.2993000497173246, - "velocityY": -0.9749413103746718, - "timestamp": 0.9347181429718083 - }, - { - "x": 2.0582215109806032, - "y": 3.6334474265483334, - "heading": 2.4409263154021836, - "angularVelocity": 0.0000816497711865655, - "velocityX": 1.2993000783495519, - "velocityY": -0.9749412712023965, - "timestamp": 1.0281899572689892 - }, - { - "x": 2.1796694496259765, - "y": 3.542317901194656, - "heading": 2.440933947630772, - "angularVelocity": 0.00008165272757387169, - "velocityX": 1.2993001105043924, - "velocityY": -0.9749412273516357, - "timestamp": 1.12166177156617 - }, - { - "x": 2.3011173916676397, - "y": 3.451188380458844, - "heading": 2.440941580131694, - "angularVelocity": 0.00008165564110490276, - "velocityX": 1.2993001468393033, - "velocityY": -0.9749411779478068, - "timestamp": 1.215133585863351 - }, - { - "x": 2.422565337575994, - "y": 3.3600588649659713, - "heading": 2.440949212900517, - "angularVelocity": 0.00008165850722838425, - "velocityX": 1.2993001882067592, - "velocityY": -0.97494112185668, - "timestamp": 1.3086054001605318 - }, - { - "x": 2.5440132879286748, - "y": 3.268929355483818, - "heading": 2.44095684593231, - "angularVelocity": 0.00008166132058271907, - "velocityX": 1.2993002357539973, - "velocityY": -0.9749410575515263, - "timestamp": 1.4020772144577127 - }, - { - "x": 2.6654612434555944, - "y": 3.1777998529828566, - "heading": 2.4409644792215364, - "angularVelocity": 0.00008166407471316297, - "velocityX": 1.2993002911101328, - "velocityY": -0.9749409828638569, - "timestamp": 1.4955490287548936 - }, - { - "x": 2.7869092051206428, - "y": 3.0866703587450828, - "heading": 2.4409721127619197, - "angularVelocity": 0.00008166676169345355, - "velocityX": 1.2993003567783705, - "velocityY": -0.9749408944608698, - "timestamp": 1.5890208430520745 - }, - { - "x": 2.908357174291307, - "y": 2.995540874590004, - "heading": 2.440979746546289, - "angularVelocity": 0.00008166937195400168, - "velocityX": 1.2993004370765409, - "velocityY": -0.9749407865920395, - "timestamp": 1.6824926573492553 - }, - { - "x": 3.0298051531585033, - "y": 2.9044114034341826, - "heading": 2.440987380566619, - "angularVelocity": 0.00008167189636088749, - "velocityX": 1.299300540814034, - "velocityY": -0.9749406475206273, - "timestamp": 1.7759644716464362 - }, - { - "x": 3.1512531460668667, - "y": 2.813281951064894, - "heading": 2.4409950148158184, - "angularVelocity": 0.00008167434490013359, - "velocityX": 1.2993006910322324, - "velocityY": -0.9749404465345578, - "timestamp": 1.869436285943617 - }, - { - "x": 3.272701165942649, - "y": 2.722152534713991, - "heading": 2.4410026493051027, - "angularVelocity": 0.00008167691342827456, - "velocityX": 1.2993009795407924, - "velocityY": -0.9749400611950203, - "timestamp": 1.962908100240798 - }, - { - "x": 3.3941493332630563, - "y": 2.6310233151653177, - "heading": 2.441010284564204, - "angularVelocity": 0.00008168514924485609, - "velocityX": 1.2993025569641734, - "velocityY": -0.9749379557236404, - "timestamp": 2.056379914537979 - }, - { - "x": 3.506643194922811, - "y": 2.543133701847389, - "heading": 2.478368219245545, - "angularVelocity": 0.39967058478790624, - "velocityX": 1.2035057038916106, - "velocityY": -0.9402793128471436, - "timestamp": 2.1498517288351597 - }, - { - "x": 3.621033917156409, - "y": 2.462292474945906, - "heading": 2.526376588848021, - "angularVelocity": 0.513613327862023, - "velocityX": 1.2237991002283153, - "velocityY": -0.8648727695009685, - "timestamp": 2.2433235431323406 - }, - { - "x": 3.7386117914711243, - "y": 2.3880497601699076, - "heading": 2.5783804352338895, - "angularVelocity": 0.5563585854932618, - "velocityX": 1.2578965669896234, - "velocityY": -0.7942791667652267, - "timestamp": 2.3367953574295215 - }, - { - "x": 3.8565091125978217, - "y": 2.322003920666466, - "heading": 2.644821825276836, - "angularVelocity": 0.7108173789342068, - "velocityX": 1.2613141406655402, - "velocityY": -0.7065856162100131, - "timestamp": 2.4302671717267024 - }, - { - "x": 3.9764532017194134, - "y": 2.262287896924173, - "heading": 2.7144845491695992, - "angularVelocity": 0.7452805363473517, - "velocityX": 1.2832113083869967, - "velocityY": -0.6388666379410853, - "timestamp": 2.5237389860238832 - }, - { - "x": 4.100272385460963, - "y": 2.211007382543893, - "heading": 2.7840189996546836, - "angularVelocity": 0.7439082145555573, - "velocityX": 1.3246686680102626, - "velocityY": -0.5486200815278979, - "timestamp": 2.617210800321064 - }, - { - "x": 4.2303737393158425, - "y": 2.1552421719955963, - "heading": 2.8233254460416966, - "angularVelocity": 0.42051656622437983, - "velocityX": 1.3918779134985075, - "velocityY": -0.5965992098002822, - "timestamp": 2.710682614618245 - }, - { - "x": 4.368649457253469, - "y": 2.0958799166815227, - "heading": 2.828594846354344, - "angularVelocity": 0.056374216679845754, - "velocityX": 1.479330630065645, - "velocityY": -0.6350818774667083, - "timestamp": 2.804154428915426 - }, - { - "x": 4.510357504555393, - "y": 2.041351647410644, - "heading": 2.8285983828965033, - "angularVelocity": 0.00003783538584099394, - "velocityX": 1.5160511044685778, - "velocityY": -0.5833659021265345, - "timestamp": 2.8976262432126068 - }, - { - "x": 4.652065594721857, - "y": 1.986823489488553, - "heading": 2.8286019193610827, - "angularVelocity": 0.00003783455586217891, - "velocityX": 1.516051563051105, - "velocityY": -0.5833647108713029, - "timestamp": 2.9910980575097876 - }, - { - "x": 4.793773697805799, - "y": 1.9322953651089438, - "heading": 2.8286054557754516, - "angularVelocity": 0.000037834018690956495, - "velocityX": 1.5160517012476136, - "velocityY": -0.5833643520200054, - "timestamp": 3.0845698718069685 - }, - { - "x": 4.935481808422067, - "y": 1.8777672602773423, - "heading": 2.8286089921399973, - "angularVelocity": 0.0000378334856573034, - "velocityX": 1.5160517818315487, - "velocityY": -0.5833641428873607, - "timestamp": 3.1780416861041494 - }, - { - "x": 5.0771899244715755, - "y": 1.8232391695375483, - "heading": 2.82861252845304, - "angularVelocity": 0.00003783293466201255, - "velocityX": 1.516051839958576, - "velocityY": -0.5833639921273963, - "timestamp": 3.2715135004013303 - }, - { - "x": 5.218898044817724, - "y": 1.76871108993463, - "heading": 2.8286160647128784, - "angularVelocity": 0.0000378323654513879, - "velocityX": 1.5160518859258105, - "velocityY": -0.5833638729805077, - "timestamp": 3.364985314698511 - }, - { - "x": 5.360606168722747, - "y": 1.7141830195502608, - "heading": 2.828619600918001, - "angularVelocity": 0.00003783178008651746, - "velocityX": 1.5160519240001233, - "velocityY": -0.5833637743566714, - "timestamp": 3.458457128995692 - }, - { - "x": 5.5023142956547595, - "y": 1.6596549570012744, - "heading": 2.828623137067082, - "angularVelocity": 0.0000378311805264838, - "velocityX": 1.516051956384105, - "velocityY": -0.5833636905305171, - "timestamp": 3.551928943292873 - }, - { - "x": 5.644022425205511, - "y": 1.605126901225908, - "heading": 2.828626673158945, - "angularVelocity": 0.00003783056838692872, - "velocityX": 1.5160519844004554, - "velocityY": -0.5833636180635361, - "timestamp": 3.6454007575900538 - }, - { - "x": 5.785730557049162, - "y": 1.5505988513766498, - "heading": 2.828630209192539, - "angularVelocity": 0.000037829944994143904, - "velocityX": 1.516052008930833, - "velocityY": -0.5833635546635878, - "timestamp": 3.7388725718872347 - }, - { - "x": 5.927438690918852, - "y": 1.4960708067593358, - "heading": 2.8286337451669143, - "angularVelocity": 0.00003782931145939862, - "velocityX": 1.5160520306062335, - "velocityY": -0.5833634986900924, - "timestamp": 3.8323443861844155 - }, - { - "x": 6.069146826592036, - "y": 1.4415427667950262, - "heading": 2.828637281081213, - "angularVelocity": 0.00003782866873129857, - "velocityX": 1.5160520499007515, - "velocityY": -0.583363448910333, - "timestamp": 3.9258162004815964 - }, - { - "x": 6.210854963880611, - "y": 1.387014730994346, - "heading": 2.828640816934652, - "angularVelocity": 0.000037828017628644405, - "velocityX": 1.516052067182881, - "velocityY": -0.5833634043661104, - "timestamp": 4.019288014778777 - }, - { - "x": 6.35256310262391, - "y": 1.332486698939259, - "heading": 2.8286443527265157, - "angularVelocity": 0.00003782735887192692, - "velocityX": 1.5160520827461144, - "velocityY": -0.5833633642942074, - "timestamp": 4.112759829075958 - }, - { - "x": 6.494271242683516, - "y": 1.2779586702695989, - "heading": 2.828647888456148, - "angularVelocity": 0.00003782669309301873, - "velocityX": 1.516052096828514, - "velocityY": -0.583363328075519, - "timestamp": 4.206231643373139 - }, - { - "x": 6.635979383939319, - "y": 1.223430644672807, - "heading": 2.828651424122945, - "angularVelocity": 0.0000378260208545108, - "velocityX": 1.5160521096259227, - "velocityY": -0.5833632952007055, - "timestamp": 4.2997034576703195 - }, - { - "x": 6.7776875262864404, - "y": 1.1689026218759404, - "heading": 2.8286549597263497, - "angularVelocity": 0.0000378253426576497, - "velocityX": 1.516052121301291, - "velocityY": -0.5833632652459484, - "timestamp": 4.3931752719675 - }, - { - "x": 6.9193956696327925, - "y": 1.114374601639329, - "heading": 2.8286584952658473, - "angularVelocity": 0.00003782465895599354, - "velocityX": 1.516052131991471, - "velocityY": -0.5833632378552868, - "timestamp": 4.486647086264681 - }, - { - "x": 7.061103813897117, - "y": 1.0598465837514708, - "heading": 2.828662030740962, - "angularVelocity": 0.000037823970152435987, - "velocityX": 1.5160521418123194, - "velocityY": -0.5833632127273561, - "timestamp": 4.580118900561862 - }, - { - "x": 7.202811959007387, - "y": 1.0053185680248766, - "heading": 2.8286655661512494, - "angularVelocity": 0.00003782327661332265, - "velocityX": 1.5160521508625917, - "velocityY": -0.5833631896052619, - "timestamp": 4.673590714859043 - }, - { - "x": 7.344520104899491, - "y": 0.9507905542926514, - "heading": 2.828669101496299, - "angularVelocity": 0.000037822578668805064, - "velocityX": 1.5160521592269784, - "velocityY": -0.5833631682686824, - "timestamp": 4.767062529156224 - }, - { - "x": 7.486228251516145, - "y": 0.8962625424056586, - "heading": 2.828672636775727, - "angularVelocity": 0.000037821876620878625, - "velocityX": 1.5160521669785085, - "velocityY": -0.5833631485276237, - "timestamp": 4.860534343453405 - }, - { - "x": 7.627936398805981, - "y": 0.8417345322301597, - "heading": 2.828676171989175, - "angularVelocity": 0.00003782117074336596, - "velocityX": 1.516052174180496, - "velocityY": -0.5833631302173573, - "timestamp": 4.954006157750586 - }, - { - "x": 7.769644550650757, - "y": 0.7872065338976291, - "heading": 2.8286797071997065, - "angularVelocity": 0.000037821139539143325, - "velocityX": 1.5160522229111162, - "velocityY": -0.5833630035164005, - "timestamp": 5.0474779720477665 - }, - { - "x": 7.89860842703643, - "y": 0.754016059353999, - "heading": 2.902617724341904, - "angularVelocity": 0.7910193858773472, - "velocityX": 1.3797087106456418, - "velocityY": -0.3550853783377481, - "timestamp": 5.140949786344947 - }, - { - "x": 7.956855773925781, - "y": 0.7431064248085022, + "x": 0.8376034098013856, + "y": 4.562699117747838, + "heading": 2.123652893108774, + "angularVelocity": 0.1828236177507875, + "velocityX": 0.24339690146379658, + "velocityY": -0.19997861949792262, + "timestamp": 0.06802909402006102 + }, + { + "x": 0.8707358038741012, + "y": 4.535482743319388, + "heading": 2.1482712139566598, + "angularVelocity": 0.3618792988868299, + "velocityX": 0.4870327107832017, + "velocityY": -0.4000696293327653, + "timestamp": 0.13605818804012204 + }, + { + "x": 0.9204609539749747, + "y": 4.4946461788288365, + "heading": 2.1847789288455908, + "angularVelocity": 0.5366485533111098, + "velocityX": 0.7309394725469969, + "velocityY": -0.600280880978807, + "timestamp": 0.20408728206018306 + }, + { + "x": 0.9868005236891458, + "y": 4.440179990916992, + "heading": 2.232822407442878, + "angularVelocity": 0.70621958574253, + "velocityX": 0.9751646801970973, + "velocityY": -0.8006307991663675, + "timestamp": 0.2721163760802441 + }, + { + "x": 1.0697804545770624, + "y": 4.372072363754176, + "heading": 2.291950205070784, + "angularVelocity": 0.8691545651110574, + "velocityX": 1.2197712182297578, + "velocityY": -1.0011544052421333, + "timestamp": 0.3401454701003051 + }, + { + "x": 1.1694318368712635, + "y": 4.290307500716387, + "heading": 2.3615667397256703, + "angularVelocity": 1.0233347313776844, + "velocityX": 1.4648347700296431, + "velocityY": -1.2019102152628651, + "timestamp": 0.4081745641203661 + }, + { + "x": 1.2857914547624776, + "y": 4.194863719874867, + "heading": 2.4408735330108198, + "angularVelocity": 1.165777590125818, + "velocityX": 1.7104390344651783, + "velocityY": -1.4029847408135987, + "timestamp": 0.47620365814042714 + }, + { + "x": 1.4189020134002508, + "y": 4.085711763901469, + "heading": 2.5287912003062516, + "angularVelocity": 1.292353934178601, + "velocityX": 1.956671047221653, + "velocityY": -1.6044893371828546, + "timestamp": 0.5442327521604882 + }, + { + "x": 1.5688122322603035, + "y": 3.9628143203631563, + "heading": 2.6238365711205818, + "angularVelocity": 1.3971282755331538, + "velocityX": 2.2036192164465107, + "velocityY": -1.8065424111347401, + "timestamp": 0.6122618461805491 + }, + { + "x": 1.7355761304311574, + "y": 3.8261286744353766, + "heading": 2.7238718533453037, + "angularVelocity": 1.4704779427934513, + "velocityX": 2.451361444291423, + "velocityY": -2.0092233756262123, + "timestamp": 0.6802909402006101 + }, + { + "x": 1.9192427382156467, + "y": 3.6756195148686888, + "heading": 2.8254629684407537, + "angularVelocity": 1.4933480529006078, + "velocityX": 2.6998243976368146, + "velocityY": -2.2124234011157657, + "timestamp": 0.7483200342206711 + }, + { + "x": 2.1197678234399393, + "y": 3.511336132708068, + "heading": 2.921883740999518, + "angularVelocity": 1.417346121503996, + "velocityX": 2.94763715602563, + "velocityY": -2.41489886830149, + "timestamp": 0.816349128240732 + }, + { + "x": 2.336325957396502, + "y": 3.3346123423953884, + "heading": 2.9945552520993064, + "angularVelocity": 1.0682416420003875, + "velocityX": 3.183316448293462, + "velocityY": -2.5977678059414684, + "timestamp": 0.884378222260793 + }, + { + "x": 2.5671944876290005, + "y": 3.1506814395963123, + "heading": 3.018943844161631, + "angularVelocity": 0.3585023792192819, + "velocityX": 3.3936734504272232, + "velocityY": -2.7037094268054926, + "timestamp": 0.952407316280854 + }, + { + "x": 2.801664380200398, + "y": 2.9585918594844425, + "heading": 3.0189438734994596, + "angularVelocity": 4.312541447677092e-7, + "velocityX": 3.446612011358773, + "velocityY": -2.8236386634110495, + "timestamp": 1.020436410300915 + }, + { + "x": 3.0361342064803303, + "y": 2.766502198455338, + "heading": 3.018943902837169, + "angularVelocity": 4.3125239350876155e-7, + "velocityX": 3.446611036901202, + "velocityY": -2.8236398528614792, + "timestamp": 1.0884655043209759 + }, + { + "x": 3.2706040874248963, + "y": 2.5744126041513797, + "heading": 3.018943932174878, + "angularVelocity": 4.312523803489459e-7, + "velocityX": 3.446611840449086, + "velocityY": -2.8236388720289733, + "timestamp": 1.1564945983410369 + }, + { + "x": 3.50614012111681, + "y": 2.38363180288057, + "heading": 3.0189439616606526, + "angularVelocity": 4.334288912332302e-7, + "velocityX": 3.4622838520009824, + "velocityY": -2.804400147009905, + "timestamp": 1.2245236923610978 + }, + { + "x": 3.746237311752337, + "y": 2.198624029599204, + "heading": 3.0189439927171957, + "angularVelocity": 4.565185470115094e-7, + "velocityX": 3.5293310030664933, + "velocityY": -2.719538984699832, + "timestamp": 1.2925527863811588 + }, + { + "x": 3.986334650538585, + "y": 2.013616448583285, + "heading": 3.0189440237737535, + "angularVelocity": 4.565187647674116e-7, + "velocityX": 3.529333180821812, + "velocityY": -2.719536158475997, + "timestamp": 1.3605818804012197 + }, + { + "x": 4.226432008113782, + "y": 1.8286088919511099, + "heading": 3.0189440548303175, + "angularVelocity": 4.5651885597350814e-7, + "velocityX": 3.529333457011711, + "velocityY": -2.7195358000448815, + "timestamp": 1.4286109744212807 + }, + { + "x": 4.466529379665209, + "y": 1.643601353456878, + "heading": 3.0189440858868863, + "angularVelocity": 4.565189266869802e-7, + "velocityX": 3.5293336624566067, + "velocityY": -2.7195355334244926, + "timestamp": 1.4966400684413417 + }, + { + "x": 4.706629767865023, + "y": 1.4585977299467376, + "heading": 3.0189441169435973, + "angularVelocity": 4.5652101213496753e-7, + "velocityX": 3.5293780059603734, + "velocityY": -2.7194779847514217, + "timestamp": 1.5646691624614026 + }, + { + "x": 4.958832142878476, + "y": 1.2904653444393646, + "heading": 3.0189441488124733, + "angularVelocity": 4.684595064516387e-7, + "velocityX": 3.707272287634489, + "velocityY": -2.4714776512795065, + "timestamp": 1.6326982564814636 + }, + { + "x": 5.22248738866272, + "y": 1.140931240984889, + "heading": 3.0189441823956185, + "angularVelocity": 4.936585621819922e-7, + "velocityX": 3.875624827614127, + "velocityY": -2.1980904730317214, + "timestamp": 1.7007273505015246 + }, + { + "x": 5.4962265319262364, + "y": 1.010772045584542, + "heading": 3.0189442291355726, + "angularVelocity": 6.870583117392439e-7, + "velocityX": 4.0238540172649335, + "velocityY": -1.9132872085870314, + "timestamp": 1.7687564445215855 + }, + { + "x": 5.777018253708926, + "y": 0.9011877519054788, + "heading": 3.0248033325009245, + "angularVelocity": 0.08612643531051656, + "velocityX": 4.127524051693051, + "velocityY": -1.6108445255312074, + "timestamp": 1.8367855385416465 + }, + { + "x": 6.05633409210439, + "y": 0.8114216921379119, + "heading": 3.058880871989868, + "angularVelocity": 0.5009259638074025, + "velocityX": 4.105829166460713, + "velocityY": -1.3195245513793832, + "timestamp": 1.9048146325617075 + }, + { + "x": 6.32467753606711, + "y": 0.7405027635497223, + "heading": 3.0964917239018948, + "angularVelocity": 0.5528642186670262, + "velocityX": 3.9445394331370727, + "velocityY": -1.0424793922329247, + "timestamp": 1.9728437265817684 + }, + { + "x": 6.579380074717612, + "y": 0.6865702377068368, + "heading": 3.129527002941156, + "angularVelocity": 0.48560515930904025, + "velocityX": 3.744023675743692, + "velocityY": -0.7927861839080411, + "timestamp": 2.0408728206018294 + }, + { + "x": 6.819417953491211, + "y": 0.648584246635437, + "heading": 3.1544674706525395, + "angularVelocity": 0.3666147266936806, + "velocityX": 3.528459142831067, + "velocityY": -0.5583786116598611, + "timestamp": 2.1089019146218906 + }, + { + "x": 7.056356249212082, + "y": 0.6255394234040195, + "heading": 3.169663027692689, + "angularVelocity": 0.21114304614497145, + "velocityX": 3.2922697979889413, + "velocityY": -0.3202090033354899, + "timestamp": 2.180869982325699 + }, + { + "x": 7.2727644426179525, + "y": 0.6154361557285976, + "heading": 3.176323379743751, + "angularVelocity": 0.09254593410057531, + "velocityX": 3.0070029710471062, + "velocityY": -0.1403854236715493, + "timestamp": 2.2528380500295073 + }, + { + "x": 7.466589513658325, + "y": 0.6144567823314835, + "heading": 3.176929612086991, + "angularVelocity": 0.008423629570475733, + "velocityX": 2.6932093249756126, + "velocityY": -0.013608443694012455, + "timestamp": 2.3248061177333157 + }, + { + "x": 7.6367535402426725, + "y": 0.6194648455903583, + "heading": 3.1735679082028514, + "angularVelocity": -0.04671104826623231, + "velocityX": 2.3644378960495933, + "velocityY": 0.06958729640325968, + "timestamp": 2.396774185437124 + }, + { + "x": 7.78273930823007, + "y": 0.6279831761643265, + "heading": 3.1678878851398564, + "angularVelocity": -0.07892421242114901, + "velocityX": 2.0284797500499288, + "velocityY": 0.11836264117894969, + "timestamp": 2.4687422531409324 + }, + { + "x": 7.904333300767225, + "y": 0.6380692684622451, + "heading": 3.1611775939336626, + "angularVelocity": -0.09323984122806457, + "velocityX": 1.689554776398703, + "velocityY": 0.1401467709183037, + "timestamp": 2.540710320844741 + }, + { + "x": 8.001482657352598, + "y": 0.6481882157692571, + "heading": 3.1544521899594713, + "angularVelocity": -0.09344983391620998, + "velocityX": 1.3498953033615035, + "velocityY": 0.14060329295844945, + "timestamp": 2.612678388548549 + }, + { + "x": 8.074218176420764, + "y": 0.6571108749361716, + "heading": 3.148524448427661, + "angularVelocity": -0.08236627327840042, + "velocityX": 1.0106637761557824, + "velocityY": 0.12398080776097292, + "timestamp": 2.6846464562523575 + }, + { + "x": 8.122612952729549, + "y": 0.6638382057961907, + "heading": 3.144055791313293, + "angularVelocity": -0.06209222029914048, + "velocityX": 0.6724479043672269, + "velocityY": 0.09347660809382861, + "timestamp": 2.756614523956166 + }, + { + "x": 8.146759986877441, + "y": 0.667546272277832, "heading": 3.141592653589793, - "angularVelocity": 2.5566523025657903, - "velocityX": 0.6231541275550934, - "velocityY": -0.1167157675019664, - "timestamp": 5.234421600642128 + "angularVelocity": -0.034225425276624886, + "velocityX": 0.3355242806750317, + "velocityY": 0.05152377436201764, + "timestamp": 2.8285825916599743 }, { - "x": 7.956855773925781, - "y": 0.7431064248085022, + "x": 8.146759986877441, + "y": 0.667546272277832, "heading": 3.141592653589793, - "angularVelocity": -3.8315823290727504e-21, - "velocityX": 3.876602036733102e-21, - "velocityY": -3.3558838471561212e-21, - "timestamp": 5.327893414939309 - }, - { - "x": 7.9018904258286256, - "y": 0.7580071883715975, - "heading": 2.929746437641678, - "angularVelocity": -2.3660710929042184, - "velocityX": -0.6138977779803758, - "velocityY": 0.166423864457769, - "timestamp": 5.417428432117549 - }, - { - "x": 7.77927283258943, - "y": 0.7936488654478715, - "heading": 2.8587142949291526, - "angularVelocity": -0.7933448269867456, - "velocityX": -1.3694931559023147, - "velocityY": 0.39807528048295576, - "timestamp": 5.506963449295789 - }, - { - "x": 7.643021297144732, - "y": 0.8351292518640833, - "heading": 2.8460714923728077, - "angularVelocity": -0.14120511677768, - "velocityX": -1.5217681275858608, - "velocityY": 0.4632867421428616, - "timestamp": 5.596498466474029 - }, - { - "x": 7.50584253248777, - "y": 0.8779834119507215, - "heading": 2.8388066609223235, - "angularVelocity": -0.08113955499692087, - "velocityX": -1.532124178676125, - "velocityY": 0.4786301654617149, - "timestamp": 5.686033483652269 - }, - { - "x": 7.368255234864718, - "y": 0.9216599007543135, - "heading": 2.8342113751430746, - "angularVelocity": -0.051323894539503874, - "velocityX": -1.536687007600088, - "velocityY": 0.487814602376678, - "timestamp": 5.775568500830508 - }, - { - "x": 7.230614869935536, - "y": 0.9659066250673984, - "heading": 2.8305583662971365, - "angularVelocity": -0.040799778243928046, - "velocityX": -1.5372797064993953, - "velocityY": 0.4941834570155044, - "timestamp": 5.865103518008748 - }, - { - "x": 7.093252862402945, - "y": 1.010681152263652, - "heading": 2.826475143484932, - "angularVelocity": -0.045604758237506364, - "velocityX": -1.5341707843663217, - "velocityY": 0.5000783895212758, - "timestamp": 5.954638535186988 - }, - { - "x": 6.956119170941034, - "y": 1.0557271520364861, - "heading": 2.821845381251112, - "angularVelocity": -0.051708955665952475, - "velocityX": -1.5316207645207218, - "velocityY": 0.5031104163766442, - "timestamp": 6.044173552365228 - }, - { - "x": 6.818992484234107, - "y": 1.100769539311748, - "heading": 2.817204104711705, - "angularVelocity": -0.0518375568093923, - "velocityX": -1.5315425297115504, - "velocityY": 0.5030700690613045, - "timestamp": 6.133708569543468 - }, - { - "x": 6.681872127416794, - "y": 1.1458071900530102, - "heading": 2.8125526096190336, - "angularVelocity": -0.05195168593547435, - "velocityX": -1.5314718323484902, - "velocityY": 0.5030171675915902, - "timestamp": 6.223243586721708 - }, - { - "x": 6.544758118625867, - "y": 1.1908399522969457, - "heading": 2.8078906701771453, - "angularVelocity": -0.052068336934676634, - "velocityX": -1.5314009324192217, - "velocityY": 0.5029625688716561, - "timestamp": 6.312778603899948 - }, - { - "x": 6.407650518790602, - "y": 1.2358677646327403, - "heading": 2.8032180092193197, - "angularVelocity": -0.05218808355755985, - "velocityX": -1.5313293519821691, - "velocityY": 0.5029072842657369, - "timestamp": 6.4023136210781875 - }, - { - "x": 6.270549391355044, - "y": 1.280890584680893, - "heading": 2.7985343668363263, - "angularVelocity": -0.05231073305843581, - "velocityX": -1.531257062950314, - "velocityY": 0.5028515263310278, - "timestamp": 6.491848638256427 - }, - { - "x": 6.1334547976207645, - "y": 1.3259083749515967, - "heading": 2.793839500454244, - "angularVelocity": -0.05243609182244443, - "velocityX": -1.5311840892526187, - "velocityY": 0.5027953496796241, - "timestamp": 6.581383655434667 - }, - { - "x": 5.996366796772198, - "y": 1.3709210989414258, - "heading": 2.7891331795005336, - "angularVelocity": -0.05256402580837642, - "velocityX": -1.5311104545349274, - "velocityY": 0.5027387653282189, - "timestamp": 6.670918672612907 - }, - { - "x": 5.8592854465592445, - "y": 1.4159287198651518, - "heading": 2.784415180924841, - "angularVelocity": -0.052694451002343774, - "velocityX": -1.5310361748193169, - "velocityY": 0.5026817701294257, - "timestamp": 6.760453689791147 - }, - { - "x": 5.722210803960778, - "y": 1.4609312002186288, - "heading": 2.7796852859248706, - "angularVelocity": -0.05282731995856272, - "velocityX": -1.530961258717216, - "velocityY": 0.5026243560537855, - "timestamp": 6.849988706969387 - }, - { - "x": 5.585142925732609, - "y": 1.5059285016384842, - "heading": 2.7749432775568774, - "angularVelocity": -0.052962611919236485, - "velocityX": -1.5308857087200383, - "velocityY": 0.5025665135047443, - "timestamp": 6.939523724147627 - }, - { - "x": 5.448081868853608, - "y": 1.5509205848763696, - "heading": 2.7701889389309033, - "angularVelocity": -0.05310032628362197, - "velocityX": -1.5308095223363887, - "velocityY": 0.502508232598188, - "timestamp": 7.0290587413258665 - }, - { - "x": 5.311027690899557, - "y": 1.5959074098195543, - "heading": 2.7654220517793227, - "angularVelocity": -0.05324047843863795, - "velocityX": -1.5307326928995175, - "velocityY": 0.502449503679975, - "timestamp": 7.118593758504106 - }, - { - "x": 5.173980450370318, - "y": 1.6408889355303347, - "heading": 2.7606423952580355, - "angularVelocity": -0.05338309716043176, - "velocityX": -1.5306552100885358, - "velocityY": 0.5023903175361529, - "timestamp": 7.208128775682346 - }, - { - "x": 5.0369402069873335, - "y": 1.6858651202909567, - "heading": 2.7558497448937236, - "angularVelocity": -0.05352822298309391, - "velocityX": -1.5305770602598503, - "velocityY": 0.5023306654544649, - "timestamp": 7.297663792860586 - }, - { - "x": 4.899907021964721, - "y": 1.7308359216420997, - "heading": 2.751043871646217, - "angularVelocity": -0.05367590691292534, - "velocityX": -1.5304982267420264, - "velocityY": 0.5022705391525029, - "timestamp": 7.387198810038826 - }, - { - "x": 4.762880958231051, - "y": 1.7758012963934744, - "heading": 2.7462245411420234, - "angularVelocity": -0.05382620851683086, - "velocityX": -1.5304186903866774, - "velocityY": 0.5022099304662124, - "timestamp": 7.476733827217066 - }, - { - "x": 4.625862080529766, - "y": 1.8207612005630536, - "heading": 2.7413915132887583, - "angularVelocity": -0.053979191668033985, - "velocityX": -1.5303384309238324, - "velocityY": 0.5021488305528153, - "timestamp": 7.566268844395306 - }, - { - "x": 4.488850455262, - "y": 1.8657155891757824, - "heading": 2.736544542679806, - "angularVelocity": -0.054134915720223636, - "velocityX": -1.5302574298390257, - "velocityY": 0.5020872283213718, - "timestamp": 7.6558038615735455 - }, - { - "x": 4.351846149940031, - "y": 1.910664415870897, - "heading": 2.7316833801907157, - "angularVelocity": -0.05429342219718387, - "velocityX": -1.5301756747221047, - "velocityY": 0.5020251082951581, - "timestamp": 7.745338878751785 - }, - { - "x": 4.214849232355917, - "y": 1.955607632403858, - "heading": 2.7268077754532425, - "angularVelocity": -0.05445472499064026, - "velocityX": -1.530093162448295, - "velocityY": 0.5019624494346333, - "timestamp": 7.834873895930025 - }, - { - "x": 4.077859769965423, - "y": 2.0005451883147503, - "heading": 2.721917478680163, - "angularVelocity": -0.05461881761126215, - "velocityX": -1.5300098967735323, - "velocityY": 0.5018992269966714, - "timestamp": 7.924408913108265 - }, - { - "x": 3.940877826047202, - "y": 2.045477029542233, - "heading": 2.717012252912238, - "angularVelocity": -0.05478555678567875, - "velocityX": -1.5299259243512187, - "velocityY": 0.5018354007576298, - "timestamp": 8.013943930286505 - }, - { - "x": 3.8039034641003533, - "y": 2.0904031002081847, - "heading": 2.712091859898031, - "angularVelocity": -0.05495495694618896, - "velocityX": -1.5298412427192656, - "velocityY": 0.5017709504262021, - "timestamp": 8.103478947464744 - }, - { - "x": 3.66693674900291, - "y": 2.135323343068443, - "heading": 2.707156056414113, - "angularVelocity": -0.05512707362408464, - "velocityX": -1.5297558364765886, - "velocityY": 0.501705860745354, - "timestamp": 8.193013964642983 - }, - { - "x": 3.5299779535424887, - "y": 2.1802377886608806, - "heading": 2.7022039564715556, - "angularVelocity": -0.05530908574796876, - "velocityX": -1.5296673835196157, - "velocityY": 0.5016411121363245, - "timestamp": 8.282548981821222 - }, - { - "x": 3.426502516345769, - "y": 2.233179068985383, - "heading": 2.594355857349509, - "angularVelocity": -1.2045354155386023, - "velocityX": -1.1556979655315056, - "velocityY": 0.5912913404496402, - "timestamp": 8.37208399899946 - }, - { - "x": 3.387338399887085, - "y": 2.2590601444244385, - "heading": 2.3477629859714533, - "angularVelocity": -2.7541500426269785, - "velocityX": -0.43741675260662705, - "velocityY": 0.28906093118330495, - "timestamp": 8.4616190161777 - }, - { - "x": 3.387338399887085, - "y": 2.2590601444244385, - "heading": 2.3477629859714533, - "angularVelocity": -2.400686558954734e-26, - "velocityX": 2.2132770883951947e-26, - "velocityY": 2.6443990744659133e-26, - "timestamp": 8.551154033355939 + "angularVelocity": 6.15835594457233e-23, + "velocityX": 1.29593094240872e-22, + "velocityY": -4.905397235503692e-22, + "timestamp": 2.9005506593637826 + }, + { + "x": 8.116302316915732, + "y": 0.6788007824238615, + "heading": 3.1342355794344465, + "angularVelocity": -0.08856487332455099, + "velocityX": -0.36665114758415307, + "velocityY": 0.13548242743870734, + "timestamp": 2.98362054790059 + }, + { + "x": 8.055384628279041, + "y": 0.7013126982960538, + "heading": 3.119642832006715, + "angularVelocity": -0.17566831597788424, + "velocityX": -0.7333305691110794, + "velocityY": 0.2709997120342536, + "timestamp": 3.0666904364373972 + }, + { + "x": 7.964004124464221, + "y": 0.7350855445347456, + "heading": 3.09796265201497, + "angularVelocity": -0.26098722862904256, + "velocityX": -1.1000436551004955, + "velocityY": 0.40655942644882115, + "timestamp": 3.1497603249742046 + }, + { + "x": 7.842157398019484, + "y": 0.7801237085031743, + "heading": 3.0693814277169014, + "angularVelocity": -0.34406238892934254, + "velocityX": -1.4667977601865743, + "velocityY": 0.542171956189308, + "timestamp": 3.232830213511012 + }, + { + "x": 7.689840193239616, + "y": 0.8364327959906936, + "heading": 3.0341415239312566, + "angularVelocity": -0.424219947881965, + "velocityX": -1.8336030956918719, + "velocityY": 0.6778519687379819, + "timestamp": 3.315900102047819 + }, + { + "x": 7.50704705480538, + "y": 0.904020204266213, + "heading": 2.992571084782186, + "angularVelocity": -0.5004272893739545, + "velocityX": -2.2004741026376022, + "velocityY": 0.8136210300266904, + "timestamp": 3.3989699905846265 + }, + { + "x": 7.293770807150263, + "y": 0.982896119722086, + "heading": 2.9451379766381653, + "angularVelocity": -0.5710024291534157, + "velocityX": -2.567431489457423, + "velocityY": 0.9495127156806547, + "timestamp": 3.4820398791214338 + }, + { + "x": 7.050001837904705, + "y": 1.073075442966285, + "heading": 2.892559152885557, + "angularVelocity": -0.6329468436605835, + "velocityX": -2.934504590523767, + "velocityY": 1.0855837756931817, + "timestamp": 3.565109767658241 + }, + { + "x": 6.775727633239132, + "y": 1.1745820471884034, + "heading": 2.8360637721730617, + "angularVelocity": -0.6800945770796716, + "velocityX": -3.301728333775782, + "velocityY": 1.2219422225075236, + "timestamp": 3.6481796561950484 + }, + { + "x": 6.470938505156052, + "y": 1.2874600105843308, + "heading": 2.7782378900044695, + "angularVelocity": -0.6961112285948248, + "velocityX": -3.6690687016876415, + "velocityY": 1.358831285128218, + "timestamp": 3.7312495447318557 + }, + { + "x": 6.135781167752857, + "y": 1.4117970937816189, + "heading": 2.7282295068076383, + "angularVelocity": -0.6020037353808817, + "velocityX": -4.034642917026322, + "velocityY": 1.4967768151295402, + "timestamp": 3.814319433268663 + }, + { + "x": 5.78853790859181, + "y": 1.5399120528524686, + "heading": 2.728229496122602, + "angularVelocity": -1.2862706865047314e-7, + "velocityX": -4.180133924306256, + "velocityY": 1.542255097815422, + "timestamp": 3.8973893218054703 + }, + { + "x": 5.441294649641329, + "y": 1.6680270124940895, + "heading": 2.728229485437636, + "angularVelocity": -1.286262206803139e-7, + "velocityX": -4.180133921771438, + "velocityY": 1.5422551046863975, + "timestamp": 3.9804592103422776 + }, + { + "x": 5.094051397734534, + "y": 1.7961419912269363, + "heading": 2.7282294747526707, + "angularVelocity": -1.286262181377533e-7, + "velocityX": -4.180133836979181, + "velocityY": 1.5422553345076508, + "timestamp": 4.063529098879084 + }, + { + "x": 4.746808156293299, + "y": 1.9242569983256745, + "heading": 2.7282294640677045, + "angularVelocity": -1.2862622783165573e-7, + "velocityX": -4.180133710994155, + "velocityY": 1.5422556759778505, + "timestamp": 4.146598987415891 + }, + { + "x": 4.399565062124353, + "y": 2.0523724045908915, + "heading": 2.7282294533827316, + "angularVelocity": -1.286263045468436e-7, + "velocityX": -4.180131938122049, + "velocityY": 1.5422604811664191, + "timestamp": 4.229668875952698 + }, + { + "x": 4.059443344314116, + "y": 2.1983468564274284, + "heading": 2.728229437713722, + "angularVelocity": -1.8862441726861322e-7, + "velocityX": -4.094404408157266, + "velocityY": 1.7572486782843997, + "timestamp": 4.312738764489505 + }, + { + "x": 3.7193219629342615, + "y": 2.3443220921470425, + "heading": 2.7282294220445418, + "angularVelocity": -1.8862647326726582e-7, + "velocityX": -4.094400358189376, + "velocityY": 1.7572581147130661, + "timestamp": 4.395808653026312 + }, + { + "x": 3.395818553566254, + "y": 2.4935219406512408, + "heading": 2.6724500576638754, + "angularVelocity": -0.6714751321226504, + "velocityX": -3.89435227452686, + "velocityY": 1.7960761851520912, + "timestamp": 4.478878541563119 + }, + { + "x": 3.101026535812032, + "y": 2.6290188837333135, + "heading": 2.59592195667292, + "angularVelocity": -0.9212495928298584, + "velocityX": -3.548722924114715, + "velocityY": 1.631119837388922, + "timestamp": 4.561948430099926 + }, + { + "x": 2.835772833375676, + "y": 2.750798938045291, + "heading": 2.519084794831671, + "angularVelocity": -0.9249700847642709, + "velocityX": -3.1931390195476896, + "velocityY": 1.4659951572947918, + "timestamp": 4.645018318636732 + }, + { + "x": 2.6000574792867495, + "y": 2.8589475580822414, + "heading": 2.4467191944954414, + "angularVelocity": -0.8711411753509879, + "velocityX": -2.8375547173689126, + "velocityY": 1.301899183228487, + "timestamp": 4.728088207173539 + }, + { + "x": 2.393855398523055, + "y": 2.953514187395407, + "heading": 2.3809962926356145, + "angularVelocity": -0.7911759967115644, + "velocityX": -2.4822722697205672, + "velocityY": 1.1383984134186498, + "timestamp": 4.811158095710346 + }, + { + "x": 2.2171452899215467, + "y": 3.0345296096879184, + "heading": 2.3231458932555134, + "angularVelocity": -0.6964063681687525, + "velocityX": -2.1272462466734887, + "velocityY": 0.9752682195620702, + "timestamp": 4.894227984247153 + }, + { + "x": 2.0699108508844217, + "y": 3.102014617448754, + "heading": 2.273952675680828, + "angularVelocity": -0.592190725678996, + "velocityX": -1.7724164752175795, + "velocityY": 0.8123883268596626, + "timestamp": 4.97729787278396 + }, + { + "x": 1.95213960106252, + "y": 3.155984197182567, + "heading": 2.2339585252158036, + "angularVelocity": -0.4814518373562503, + "velocityX": -1.4177369429082447, + "velocityY": 0.6496888425449069, + "timestamp": 5.060367761320767 + }, + { + "x": 1.8638217687898557, + "y": 3.1964496924625165, + "heading": 2.203560612114051, + "angularVelocity": -0.36593179113613356, + "velocityX": -1.0631750424647786, + "velocityY": 0.48712591280317163, + "timestamp": 5.143437649857574 + }, + { + "x": 1.8049494737197318, + "y": 3.223419994217748, + "heading": 2.1830643911586045, + "angularVelocity": -0.24673466302250652, + "velocityX": -0.7087080060789848, + "velocityY": 0.3246700125675676, + "timestamp": 5.22650753839438 + }, + { + "x": 1.775516152381897, + "y": 3.236902236938477, + "heading": 2.1727145819710088, + "angularVelocity": -0.12459158631240587, + "velocityX": -0.35431998111798746, + "velocityY": 0.16229999772727494, + "timestamp": 5.309577426931187 + }, + { + "x": 1.775516152381897, + "y": 3.236902236938477, + "heading": 2.1727145819710088, + "angularVelocity": 3.884064768338255e-24, + "velocityX": -1.446989921221479e-23, + "velocityY": -2.0280809780864772e-23, + "timestamp": 5.392647315467994 } ], "trajectoryWaypoints": [ @@ -3251,25 +2882,36 @@ "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 57 + "controlIntervalCount": 31 }, { - "timestamp": 5.327893414939309, + "timestamp": 2.1089019146218906, + "isStopPoint": false, + "x": 6.819417953491211, + "y": 0.648584246635437, + "heading": 3.1544674706525395, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "timestamp": 2.9005506593637826, "isStopPoint": true, - "x": 7.956855773925781, - "y": 0.7431064248085022, + "x": 8.146759986877441, + "y": 0.667546272277832, "heading": 3.141592653589793, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, - "controlIntervalCount": 36 + "controlIntervalCount": 30 }, { - "timestamp": 8.551154033355939, + "timestamp": 5.392647315467994, "isStopPoint": true, - "x": 3.387338399887085, - "y": 2.2590601444244385, - "heading": 2.3477629859714533, + "x": 1.775516152381897, + "y": 3.2369022369384766, + "heading": 2.1727145819710088, "isInitialGuess": false, "translationConstrained": true, "headingConstrained": true, @@ -3291,9 +2933,15 @@ }, { "scope": [ - 1 + 2 ], "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "WptZeroVelocity" } ], "usesControlIntervalGuessing": true, @@ -3308,6 +2956,3733 @@ ], "eventMarkers": [], "isTrajectoryStale": false + }, + "SourceWing1Contested1": { + "waypoints": [ + { + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 2.5719215869903564, + "y": 4.099675178527832, + "heading": 3.10770755459014, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 1.7375919818878174, + "y": 4.185004234313965, + "heading": 2.362255080817056, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "x": 5.536012172698975, + "y": 0.7129083275794983, + "heading": 3.1544674706525395, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 8.116830825805664, + "y": 0.5379375219345093, + "heading": 3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 31 + }, + { + "x": 1.8569773435592651, + "y": 3.1621029376983643, + "heading": 2.0621810870916404, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "angularVelocity": -6.880993397933692e-22, + "velocityX": 2.429300243510835e-20, + "velocityY": -7.839141866791818e-21, + "timestamp": 0 + }, + { + "x": 0.8426777300221479, + "y": 4.5704094309279295, + "heading": 2.1231094632130714, + "angularVelocity": 0.17115431232368958, + "velocityX": 0.3112922161505791, + "velocityY": -0.08481597086968504, + "timestamp": 0.06949223203245897 + }, + { + "x": 0.8859417314267117, + "y": 4.558618393899322, + "heading": 2.1468991418417867, + "angularVelocity": 0.34233579686436316, + "velocityX": 0.6225732019135002, + "velocityY": -0.16967417341121008, + "timestamp": 0.13898446406491793 + }, + { + "x": 0.9508356832892374, + "y": 4.540926256854508, + "heading": 2.1825982004444193, + "angularVelocity": 0.5137129368064951, + "velocityX": 0.9338302996544205, + "velocityY": -0.2545915784738378, + "timestamp": 0.2084766960973769 + }, + { + "x": 1.0373567008976705, + "y": 4.517328145491284, + "heading": 2.230241303381707, + "angularVelocity": 0.6855889002821759, + "velocityX": 1.2450458861073865, + "velocityY": -0.3395791252208121, + "timestamp": 0.27796892812983587 + }, + { + "x": 1.145500350674682, + "y": 4.487819157938406, + "heading": 2.2898948322449537, + "angularVelocity": 0.858420101334255, + "velocityX": 1.5561976729499636, + "velocityY": -0.42463721037329055, + "timestamp": 0.34746116016229484 + }, + { + "x": 1.2752605130590804, + "y": 4.452395483406523, + "heading": 2.361665844547121, + "angularVelocity": 1.0327918704445085, + "velocityX": 1.8672613987098476, + "velocityY": -0.5097501331564366, + "timestamp": 0.4169533921947538 + }, + { + "x": 1.4266296582889453, + "y": 4.411055876279096, + "heading": 2.445704537096077, + "angularVelocity": 1.2093249862762028, + "velocityX": 2.178216770460937, + "velocityY": -0.5948809804830839, + "timestamp": 0.48644562422721277 + }, + { + "x": 1.5995997714382222, + "y": 4.363803254246465, + "heading": 2.542194010250006, + "angularVelocity": 1.3884929341290912, + "velocityX": 2.489056806644011, + "velocityY": -0.6799698419610362, + "timestamp": 0.5559378562596717 + }, + { + "x": 1.7941640365139602, + "y": 4.310645922199146, + "heading": 2.651322016971142, + "angularVelocity": 1.5703626654294764, + "velocityX": 2.799798760023414, + "velocityY": -0.7649391952540748, + "timestamp": 0.6254300882921306 + }, + { + "x": 1.9669767586764846, + "y": 4.263729609127996, + "heading": 2.753018473554398, + "angularVelocity": 1.463421933774627, + "velocityX": 2.486791934986428, + "velocityY": -0.6751303231882987, + "timestamp": 0.6949223203245896 + }, + { + "x": 2.1181975297514413, + "y": 4.222696517463293, + "heading": 2.841907068392746, + "angularVelocity": 1.2791155534740832, + "velocityX": 2.176081651893456, + "velocityY": -0.5904701930647012, + "timestamp": 0.7644145523570485 + }, + { + "x": 2.247823798372861, + "y": 4.187537365169347, + "heading": 2.917988709739883, + "angularVelocity": 1.0948222430328662, + "velocityX": 1.8653346543952263, + "velocityY": -0.5059436323404242, + "timestamp": 0.8339067843895074 + }, + { + "x": 2.3558519321956344, + "y": 4.158245220739795, + "heading": 2.981299294871451, + "angularVelocity": 0.9110454978909971, + "velocityX": 1.5545353870964267, + "velocityY": -0.4215168166690949, + "timestamp": 0.9033990164219663 + }, + { + "x": 2.4422782681617843, + "y": 4.134815338806806, + "heading": 3.0318877577047387, + "angularVelocity": 0.7279729165938769, + "velocityX": 1.2436834080358974, + "velocityY": -0.3371582873039003, + "timestamp": 0.9728912484544252 + }, + { + "x": 2.5070997855530313, + "y": 4.117244720393447, + "heading": 3.0698000752586414, + "angularVelocity": 0.545561949085099, + "velocityX": 0.9327879605445623, + "velocityY": -0.25284291350943383, + "timestamp": 1.0423834804868841 + }, + { + "x": 2.5503144871302985, + "y": 4.105531623902024, + "heading": 3.0950688818766996, + "angularVelocity": 0.36362059296434324, + "velocityX": 0.6218637725880274, + "velocityY": -0.16855260147567344, + "timestamp": 1.111875712519343 + }, + { + "x": 2.5719215869903564, + "y": 4.099675178527832, + "heading": 3.10770755459014, + "angularVelocity": 0.18187173362825426, + "velocityX": 0.3109282754073192, + "velocityY": -0.0842748204066407, + "timestamp": 1.181367944551802 + }, + { + "x": 2.5719215869903564, + "y": 4.099675178527832, + "heading": 3.10770755459014, + "angularVelocity": 6.1463734412376215e-22, + "velocityX": -2.4282322719240246e-20, + "velocityY": 7.607758723620182e-21, + "timestamp": 1.2508601765842609 + }, + { + "x": 2.5521511502862917, + "y": 4.1016130613202035, + "heading": 3.0885052062119422, + "angularVelocity": -0.28892914084268495, + "velocityX": -0.29747691159878115, + "velocityY": 0.029158454956973664, + "timestamp": 1.3173205838737183 + }, + { + "x": 2.5126144541789404, + "y": 4.10551220988183, + "heading": 3.050075070321908, + "angularVelocity": -0.578241052942358, + "velocityX": -0.5948909692225571, + "velocityY": 0.05866874310060908, + "timestamp": 1.3837809911631758 + }, + { + "x": 2.453314067800273, + "y": 4.111408158656473, + "heading": 2.9924347557750335, + "angularVelocity": -0.8672880124828585, + "velocityX": -0.892266370267546, + "velocityY": 0.08871370211386269, + "timestamp": 1.4502413984526332 + }, + { + "x": 2.3742451801100097, + "y": 4.119346701366484, + "heading": 2.915722057438157, + "angularVelocity": -1.1542616343406789, + "velocityX": -1.189714160882167, + "velocityY": 0.11944769876953625, + "timestamp": 1.5167018057420907 + }, + { + "x": 2.2753885511312752, + "y": 4.129377042526428, + "heading": 2.820281114601736, + "angularVelocity": -1.4360571463359206, + "velocityX": -1.4874514468167541, + "velocityY": 0.15092205373129233, + "timestamp": 1.5831622130315481 + }, + { + "x": 2.1567048547929897, + "y": 4.141537544068227, + "heading": 2.70671634696271, + "angularVelocity": -1.7087582256968925, + "velocityX": -1.7857804545400682, + "velocityY": 0.18297362351147275, + "timestamp": 1.6496226203210056 + }, + { + "x": 2.0370126884468625, + "y": 4.154030980294255, + "heading": 2.6076428490845873, + "angularVelocity": -1.490714576072696, + "velocityX": -1.8009544513446583, + "velocityY": 0.1879831426794298, + "timestamp": 1.716083027610463 + }, + { + "x": 1.9372285222359857, + "y": 4.164397106788352, + "heading": 2.52557974085617, + "angularVelocity": -1.2347668570703279, + "velocityX": -1.501407684371891, + "velocityY": 0.15597446535271473, + "timestamp": 1.7825434348999205 + }, + { + "x": 1.8573807268012847, + "y": 4.172657809666075, + "heading": 2.4601710748158503, + "angularVelocity": -0.9841749201963685, + "velocityX": -1.2014340370641543, + "velocityY": 0.12429509861029223, + "timestamp": 1.849003842189378 + }, + { + "x": 1.7974869423285986, + "y": 4.178835602778205, + "heading": 2.4112055231539617, + "angularVelocity": -0.7367627382815587, + "velocityX": -0.9011949657760652, + "velocityY": 0.09295448770309593, + "timestamp": 1.9154642494788354 + }, + { + "x": 1.757556356355509, + "y": 4.182947964217865, + "heading": 2.378578032753243, + "angularVelocity": -0.4909312435991425, + "velocityX": -0.6008176537224419, + "velocityY": 0.06187686183969354, + "timestamp": 1.9819246567682929 + }, + { + "x": 1.7375919818878174, + "y": 4.185004234313965, + "heading": 2.362255080817056, + "angularVelocity": -0.24560415143252423, + "velocityX": -0.30039500631917604, + "velocityY": 0.030939775724573536, + "timestamp": 2.0483850640577503 + }, + { + "x": 1.7375919818878174, + "y": 4.185004234313965, + "heading": 2.362255080817056, + "angularVelocity": -3.596507047850015e-24, + "velocityX": -6.256878628501107e-23, + "velocityY": -6.331189922772691e-23, + "timestamp": 2.114845471347208 + }, + { + "x": 1.7500448547508285, + "y": 4.169918910269556, + "heading": 2.3726443658683603, + "angularVelocity": 0.1601015115213246, + "velocityX": 0.19190192186522692, + "velocityY": -0.23246866067994823, + "timestamp": 2.179737332470708 + }, + { + "x": 1.7749616511915172, + "y": 4.139736833623858, + "heading": 2.393148152590508, + "angularVelocity": 0.31596854161919546, + "velocityX": 0.3839741380397122, + "velocityY": -0.4651134383132558, + "timestamp": 2.2446291935942084 + }, + { + "x": 1.8123551092604897, + "y": 4.094444892066208, + "heading": 2.4234406505462944, + "angularVelocity": 0.4668150586424898, + "velocityX": 0.5762426507972481, + "velocityY": -0.6979602799718191, + "timestamp": 2.3095210547177087 + }, + { + "x": 1.862239603437118, + "y": 4.0340277852116175, + "heading": 2.4631338121351902, + "angularVelocity": 0.6116816639509415, + "velocityX": 0.7687326779191858, + "velocityY": -0.9310429044345997, + "timestamp": 2.374412915841209 + }, + { + "x": 1.9246311116449513, + "y": 3.9584675364071735, + "heading": 2.5117637367500465, + "angularVelocity": 0.7493994435188912, + "velocityX": 0.9614689288860394, + "velocityY": -1.164402553667557, + "timestamp": 2.4393047769647094 + }, + { + "x": 1.9995472742646765, + "y": 3.8677431003581373, + "heading": 2.5687734572106, + "angularVelocity": 0.878534217905294, + "velocityX": 1.1544770225829577, + "velocityY": -1.3980865162176925, + "timestamp": 2.5041966380882097 + }, + { + "x": 2.08700767845586, + "y": 3.7618301407634718, + "heading": 2.633488115256641, + "angularVelocity": 0.9972692557372997, + "velocityX": 1.347786959365091, + "velocityY": -1.6321455073247935, + "timestamp": 2.56908849921171 + }, + { + "x": 2.18703451414934, + "y": 3.640701026603113, + "heading": 2.7050730144651673, + "angularVelocity": 1.1031414104811743, + "velocityX": 1.5414388485963126, + "velocityY": -1.8666302994427872, + "timestamp": 2.6339803603352103 + }, + { + "x": 2.2996535810248653, + "y": 3.5043251322158286, + "heading": 2.7824542959291714, + "angularVelocity": 1.1924651277412772, + "velocityX": 1.7354883174207654, + "velocityY": -2.101587040749814, + "timestamp": 2.6988722214587106 + }, + { + "x": 2.4248949182977166, + "y": 3.3526700317076736, + "heading": 2.864159859448204, + "angularVelocity": 1.2591034084156272, + "velocityX": 1.9300006981537456, + "velocityY": -2.3370434732875047, + "timestamp": 2.763764082582211 + }, + { + "x": 2.5627898036818046, + "y": 3.185706945978596, + "heading": 2.947984812199732, + "angularVelocity": 1.2917637327737392, + "velocityX": 2.1249950763725343, + "velocityY": -2.572943399039195, + "timestamp": 2.8286559437057113 + }, + { + "x": 2.7133500598265714, + "y": 3.0034380376889436, + "heading": 3.0302259153482303, + "angularVelocity": 1.2673562096174134, + "velocityX": 2.3201716446107956, + "velocityY": -2.808809997647686, + "timestamp": 2.8935478048292116 + }, + { + "x": 2.8764454665102543, + "y": 2.8060484554493246, + "heading": 3.1036395778244232, + "angularVelocity": 1.1313231151819507, + "velocityX": 2.5133414862810737, + "velocityY": -3.041823409317128, + "timestamp": 2.958439665952712 + }, + { + "x": 3.049983564231236, + "y": 2.59451668989809, + "heading": 3.1503578251858677, + "angularVelocity": 0.7199400133174118, + "velocityX": 2.674265997560302, + "velocityY": -3.2597580326545725, + "timestamp": 3.023331527076212 + }, + { + "x": 3.229588483804468, + "y": 2.3696781969148084, + "heading": 3.1544671382222993, + "angularVelocity": 0.06332555370250754, + "velocityX": 2.7677572574380895, + "velocityY": -3.4648180694860256, + "timestamp": 3.0882233881997125 + }, + { + "x": 3.413445860556505, + "y": 2.1465356197492285, + "heading": 3.154467162773942, + "angularVelocity": 3.783470257874771e-7, + "velocityX": 2.833288698595432, + "velocityY": -3.4386835776046256, + "timestamp": 3.153115249323213 + }, + { + "x": 3.6000973110406074, + "y": 1.9257248846021084, + "heading": 3.1544671873317807, + "angularVelocity": 3.7844251267652267e-7, + "velocityX": 2.876346081812528, + "velocityY": -3.4027493020562343, + "timestamp": 3.218007110446713 + }, + { + "x": 3.8014757154623195, + "y": 1.718257097054393, + "heading": 3.154467212149594, + "angularVelocity": 3.8244877762487134e-7, + "velocityX": 3.1032921684655466, + "velocityY": -3.1971311032807224, + "timestamp": 3.2828989715702135 + }, + { + "x": 4.016630577734113, + "y": 1.5251132914284782, + "heading": 3.154467237801092, + "angularVelocity": 3.952960817114755e-7, + "velocityX": 3.315590869898414, + "velocityY": -2.976394917358426, + "timestamp": 3.347790832693714 + }, + { + "x": 4.2445454275226515, + "y": 1.3472061098038577, + "heading": 3.1544672649477823, + "angularVelocity": 4.1833736664202863e-7, + "velocityX": 3.512225506289282, + "velocityY": -2.7415946860582885, + "timestamp": 3.412682693817214 + }, + { + "x": 4.484143440597044, + "y": 1.1853761509334682, + "heading": 3.1544672944204697, + "angularVelocity": 4.541815744966032e-7, + "velocityX": 3.6922660088049732, + "velocityY": -2.4938406152722283, + "timestamp": 3.4775745549407144 + }, + { + "x": 4.734292576211658, + "y": 1.040388039013423, + "heading": 3.1544673273474686, + "angularVelocity": 5.074133830222093e-7, + "velocityX": 3.8548614769814935, + "velocityY": -2.23430349214531, + "timestamp": 3.5424664160642148 + }, + { + "x": 4.993810935799209, + "y": 0.9129268192433763, + "heading": 3.154467365380173, + "angularVelocity": 5.860936051826097e-7, + "velocityX": 3.9992435891712756, + "velocityY": -1.9642096491494785, + "timestamp": 3.607358277187715 + }, + { + "x": 5.261472350224649, + "y": 0.8035947237152907, + "heading": 3.1544674111329933, + "angularVelocity": 7.050625321396259e-7, + "velocityX": 4.12473012472298, + "velocityY": -1.6848352572290692, + "timestamp": 3.6722501383112154 + }, + { + "x": 5.536012172698975, + "y": 0.7129083275794983, + "heading": 3.1544674706525395, + "angularVelocity": 9.172112660822983e-7, + "velocityX": 4.230728133252795, + "velocityY": -1.3975003115290687, + "timestamp": 3.7371419994347157 + }, + { + "x": 5.851986653314552, + "y": 0.6350614800439672, + "heading": 3.1539640111947476, + "angularVelocity": -0.006887282897011849, + "velocityX": 4.322504230591005, + "velocityY": -1.0649383050015728, + "timestamp": 3.8102418655677273 + }, + { + "x": 6.163084948302816, + "y": 0.5819578289574815, + "heading": 3.153020697079212, + "angularVelocity": -0.012904457496809069, + "velocityX": 4.25579842269744, + "velocityY": -0.7264534655899257, + "timestamp": 3.883341731700739 + }, + { + "x": 6.458568718598011, + "y": 0.5486582997656804, + "heading": 3.1517988719890786, + "angularVelocity": -0.01671446412651432, + "velocityX": 4.042193042563675, + "velocityY": -0.4555347492868147, + "timestamp": 3.9564415978337504 + }, + { + "x": 6.732865281543566, + "y": 0.5290381776313112, + "heading": 3.1504499662020073, + "angularVelocity": -0.018452917336628596, + "velocityX": 3.752353833951604, + "velocityY": -0.26840161510923477, + "timestamp": 4.029541463966762 + }, + { + "x": 6.98366753282154, + "y": 0.5185855599477185, + "heading": 3.149074247230133, + "angularVelocity": -0.018819719442045503, + "velocityX": 3.430953632960416, + "velocityY": -0.14299092784347003, + "timestamp": 4.1026413300997735 + }, + { + "x": 7.210009572737756, + "y": 0.5142749171101598, + "heading": 3.147735302871607, + "angularVelocity": -0.018316645834748266, + "velocityX": 3.096340005662489, + "velocityY": -0.058969230254336036, + "timestamp": 4.175741196232785 + }, + { + "x": 7.411462955935819, + "y": 0.5140361320742359, + "heading": 3.1464751819616255, + "angularVelocity": -0.017238347710359158, + "velocityX": 2.755865282044427, + "velocityY": -0.003266559141017675, + "timestamp": 4.248841062365797 + }, + { + "x": 7.587830777283194, + "y": 0.5163933493371166, + "heading": 3.145323181311359, + "angularVelocity": -0.015759271681428454, + "velocityX": 2.4126969128296243, + "velocityY": 0.03224653323703015, + "timestamp": 4.321940928498808 + }, + { + "x": 7.739023807932044, + "y": 0.5202518916978398, + "heading": 3.144300674526786, + "angularVelocity": -0.013987806526385212, + "velocityX": 2.068307900506172, + "velocityY": 0.052784533882760894, + "timestamp": 4.39504079463182 + }, + { + "x": 7.865006007354459, + "y": 0.5247719050263068, + "heading": 3.1434238563856503, + "angularVelocity": -0.011994798178430731, + "velocityX": 1.7234258568022467, + "velocityY": 0.0618334009017538, + "timestamp": 4.468140660764831 + }, + { + "x": 7.96576870810601, + "y": 0.5292907028384434, + "heading": 3.1427053813238377, + "angularVelocity": -0.009828678215427785, + "velocityX": 1.378425243189977, + "velocityY": 0.06181677274092789, + "timestamp": 4.541240526897843 + }, + { + "x": 8.041317623102847, + "y": 0.5332730840639932, + "heading": 3.1421553895932863, + "angularVelocity": -0.007523840461631992, + "velocityX": 1.0335027817885274, + "velocityY": 0.05447863910316233, + "timestamp": 4.614340393030854 + }, + { + "x": 8.091665990307739, + "y": 0.5362783280705644, + "heading": 3.1417821778172224, + "angularVelocity": -0.005105505602228686, + "velocityX": 0.688761414600654, + "velocityY": 0.04111148440549697, + "timestamp": 4.687440259163866 + }, + { + "x": 8.116830825805664, + "y": 0.5379375219345093, + "heading": 3.141592653589793, + "angularVelocity": -0.0025926754377961075, + "velocityX": 0.3442528260193612, + "velocityY": 0.02269763204389355, + "timestamp": 4.7605401252968775 + }, + { + "x": 8.116830825805664, + "y": 0.5379375219345093, + "heading": 3.141592653589793, + "angularVelocity": 2.6782007527142383e-24, + "velocityX": -3.91857307116395e-24, + "velocityY": 2.7111362097714367e-23, + "timestamp": 4.833639991429889 + }, + { + "x": 8.089255596234777, + "y": 0.54942402960786, + "heading": 3.1321392994576667, + "angularVelocity": -0.11847903719259653, + "velocityX": -0.34560078933473104, + "velocityY": 0.14396058275432985, + "timestamp": 4.913429246610612 + }, + { + "x": 8.034097606978237, + "y": 0.5724008755052853, + "heading": 3.113461638242577, + "angularVelocity": -0.23408742408717345, + "velocityX": -0.6912959537171532, + "velocityY": 0.2879691738616022, + "timestamp": 4.993218501791334 + }, + { + "x": 7.9513480563517875, + "y": 0.606872555226039, + "heading": 3.085834950822301, + "angularVelocity": -0.346245711376821, + "velocityX": -1.0371014297479375, + "velocityY": 0.43203410838559014, + "timestamp": 5.073007756972057 + }, + { + "x": 7.840996433281568, + "y": 0.652844444117696, + "heading": 3.0496002995849607, + "angularVelocity": -0.4541294583496149, + "velocityX": -1.3830386412340105, + "velocityY": 0.5761664122259446, + "timestamp": 5.15279701215278 + }, + { + "x": 7.703029917572737, + "y": 0.7103230512929849, + "heading": 3.005193906243408, + "angularVelocity": -0.5565460316802363, + "velocityX": -1.7291365284252544, + "velocityY": 0.720380294879303, + "timestamp": 5.232586267333502 + }, + { + "x": 7.537432570924173, + "y": 0.7793163153547259, + "heading": 2.9531931917128307, + "angularVelocity": -0.65172577952753, + "velocityX": -2.075434170597116, + "velocityY": 0.8646936721676576, + "timestamp": 5.312375522514225 + }, + { + "x": 7.344184305821202, + "y": 0.8598339479102136, + "heading": 2.8943934690100765, + "angularVelocity": -0.7369378567273606, + "velocityX": -2.421983569908799, + "velocityY": 1.0091287651841792, + "timestamp": 5.392164777694948 + }, + { + "x": 7.123259821783499, + "y": 0.9518877987646011, + "heading": 2.8299481315961454, + "angularVelocity": -0.8076944353968832, + "velocityX": -2.7688500605414945, + "velocityY": 1.1537123719965283, + "timestamp": 5.47195403287567 + }, + { + "x": 6.874628809836458, + "y": 1.055491871977118, + "heading": 2.761663241881283, + "angularVelocity": -0.8558156052490734, + "velocityX": -3.1160964140333447, + "velocityY": 1.2984714919051967, + "timestamp": 5.551743288056393 + }, + { + "x": 6.598266120224734, + "y": 1.1706587451437058, + "heading": 2.692764023482451, + "angularVelocity": -0.8635149963835985, + "velocityX": -3.4636579698076297, + "velocityY": 1.4433882470231743, + "timestamp": 5.631532543237116 + }, + { + "x": 6.294260106065497, + "y": 1.2973571312742034, + "heading": 2.630823871265101, + "angularVelocity": -0.7762969096159158, + "velocityX": -3.810112194563846, + "velocityY": 1.5879128817975097, + "timestamp": 5.711321798417838 + }, + { + "x": 5.9677610677766175, + "y": 1.434232859949075, + "heading": 2.6252073225176806, + "angularVelocity": -0.07039229448500041, + "velocityX": -4.092017622540287, + "velocityY": 1.715465677237474, + "timestamp": 5.791111053598561 + }, + { + "x": 5.6396041489574, + "y": 1.5709726586832762, + "heading": 2.6252073065947137, + "angularVelocity": -1.9956279736518076e-7, + "velocityX": -4.1127958655077475, + "velocityY": 1.7137620651362904, + "timestamp": 5.870900308779284 + }, + { + "x": 5.311447232039342, + "y": 1.7077124619800061, + "heading": 2.6252072906717543, + "angularVelocity": -1.995627014046446e-7, + "velocityX": -4.1127958416804935, + "velocityY": 1.7137621223185358, + "timestamp": 5.950689563960006 + }, + { + "x": 4.9832903234128985, + "y": 1.8444522851754865, + "heading": 2.6252072747487953, + "angularVelocity": -1.9956269204035307e-7, + "velocityX": -4.112795737761537, + "velocityY": 1.713762371709888, + "timestamp": 6.030478819140729 + }, + { + "x": 4.655133427137336, + "y": 1.9811921380113924, + "heading": 2.625207258825836, + "angularVelocity": -1.9956270204696126e-7, + "velocityX": -4.112795582967754, + "velocityY": 1.7137627431938112, + "timestamp": 6.110268074321452 + }, + { + "x": 4.326976566232365, + "y": 2.117932075731836, + "heading": 2.625207242902869, + "angularVelocity": -1.9956279397787226e-7, + "velocityX": -4.112795139667575, + "velocityY": 1.7137638070530719, + "timestamp": 6.190057329502174 + }, + { + "x": 3.999438899364335, + "y": 2.256148618450317, + "heading": 2.6252072265396365, + "angularVelocity": -2.050806574466794e-7, + "velocityX": -4.105034770987135, + "velocityY": 1.7322701209006253, + "timestamp": 6.269846584682897 + }, + { + "x": 3.6745952182551624, + "y": 2.394736220372859, + "heading": 2.616353473366314, + "angularVelocity": -0.11096422887102513, + "velocityX": -4.071271004766277, + "velocityY": 1.7369206117871623, + "timestamp": 6.34963583986362 + }, + { + "x": 3.371360246276191, + "y": 2.5227681355611673, + "heading": 2.54789899304901, + "angularVelocity": -0.8579410869578161, + "velocityX": -3.8004487106960148, + "velocityY": 1.6046260226181663, + "timestamp": 6.429425095044342 + }, + { + "x": 3.09571714689087, + "y": 2.6391416829563425, + "heading": 2.4725529095377854, + "angularVelocity": -0.9443136590329878, + "velocityX": -3.454639334093173, + "velocityY": 1.4585115142582736, + "timestamp": 6.509214350225065 + }, + { + "x": 2.8477516687682445, + "y": 2.7438274798328672, + "heading": 2.3982762432251263, + "angularVelocity": -0.9309106363309555, + "velocityX": -3.1077552680619074, + "velocityY": 1.3120287517336962, + "timestamp": 6.589003605405788 + }, + { + "x": 2.6274310870254833, + "y": 2.83684141662163, + "heading": 2.3284405360388263, + "angularVelocity": -0.875252025202168, + "velocityX": -2.7612813435059107, + "velocityY": 1.1657451442313427, + "timestamp": 6.66879286058651 + }, + { + "x": 2.4347195920645524, + "y": 2.918199144350896, + "heading": 2.2649041443604916, + "angularVelocity": -0.7963026040840354, + "velocityX": -2.4152562212097375, + "velocityY": 1.019657691314338, + "timestamp": 6.748582115767233 + }, + { + "x": 2.269588113206454, + "y": 2.9879129508006694, + "heading": 2.208834388541783, + "angularVelocity": -0.7027231384941546, + "velocityX": -2.0695954421942657, + "velocityY": 0.8737242413389122, + "timestamp": 6.828371370947956 + }, + { + "x": 2.1320138837828355, + "y": 3.0459924554790536, + "heading": 2.1610282462744177, + "angularVelocity": -0.5991551388603021, + "velocityX": -1.7242199981941622, + "velocityY": 0.7279113528110278, + "timestamp": 6.908160626128678 + }, + { + "x": 2.0219789365980305, + "y": 3.092445388285202, + "heading": 2.1220638838563577, + "angularVelocity": -0.4883409718489752, + "velocityX": -1.379069737342154, + "velocityY": 0.5821953432317734, + "timestamp": 6.987949881309401 + }, + { + "x": 1.93946881514082, + "y": 3.1272780949907744, + "heading": 2.09238087210519, + "angularVelocity": -0.37201765681275867, + "velocityX": -1.0341006601744267, + "velocityY": 0.4365588653093307, + "timestamp": 7.067739136490124 + }, + { + "x": 1.884471623102071, + "y": 3.1504958321346646, + "heading": 2.0723259960019775, + "angularVelocity": -0.25134808011164267, + "velocityX": -0.6892806796376343, + "velocityY": 0.29098826767216296, + "timestamp": 7.147528391670846 + }, + { + "x": 1.8569773435592651, + "y": 3.1621029376983643, + "heading": 2.0621810870916404, + "angularVelocity": -0.12714630419044576, + "velocityX": -0.344586241349558, + "velocityY": 0.14547203802578762, + "timestamp": 7.227317646851569 + }, + { + "x": 1.8569773435592651, + "y": 3.1621029376983643, + "heading": 2.0621810870916404, + "angularVelocity": 1.0052435240302537e-24, + "velocityX": -4.289779840395474e-24, + "velocityY": -8.499784651893661e-24, + "timestamp": 7.307106902032292 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "timestamp": 1.2508601765842609, + "isStopPoint": true, + "x": 2.5719215869903564, + "y": 4.099675178527832, + "heading": 3.10770755459014, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "timestamp": 2.114845471347208, + "isStopPoint": true, + "x": 1.7375919818878174, + "y": 4.185004234313965, + "heading": 2.362255080817056, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "timestamp": 3.7371419994347157, + "isStopPoint": false, + "x": 5.536012172698975, + "y": 0.7129083275794983, + "heading": 3.1544674706525395, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "timestamp": 4.833639991429889, + "isStopPoint": true, + "x": 8.116830825805664, + "y": 0.5379375219345093, + "heading": 3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 31 + }, + { + "timestamp": 7.307106902032292, + "isStopPoint": true, + "x": 1.8569773435592651, + "y": 3.1621029376983643, + "heading": 2.0621810870916404, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 2 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 4 + ], + "type": "StopPoint", + "direction": 0 + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [ + { + "x": 4.85899543762207, + "y": 4.113875865936279, + "radius": 1.7227418123199636 + } + ], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "AmpWing3Contested5": { + "waypoints": [ + { + "x": 0.8817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "x": 6.410865783691406, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 8.055591583251953, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "x": 1.9446686506271362, + "y": 6.794289588928223, + "heading": -2.5113986210516552, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.8817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "angularVelocity": -1.2014756905517071e-27, + "velocityX": -1.2223583879961949e-27, + "velocityY": -6.901280117928792e-27, + "timestamp": 0 + }, + { + "x": 0.8869818977350931, + "y": 6.608539823221273, + "heading": -2.1202506201761104, + "angularVelocity": -0.11208980721325, + "velocityX": 0.1531927732427503, + "velocityY": -0.026074847312906376, + "timestamp": 0.033895276864232556 + }, + { + "x": 0.8973773914804737, + "y": 6.606806194813917, + "heading": -2.1277869053789877, + "angularVelocity": -0.22234027569870818, + "velocityX": 0.30669446327344546, + "velocityY": -0.0511466070715543, + "timestamp": 0.06779055372846511 + }, + { + "x": 0.9129875332520139, + "y": 6.6042613695509065, + "heading": -2.1389872005229207, + "angularVelocity": -0.33043822562050607, + "velocityX": 0.46054032348125123, + "velocityY": -0.07507905225863701, + "timestamp": 0.10168583059269767 + }, + { + "x": 0.9338252508983529, + "y": 6.6009496119352935, + "heading": -2.153767308340282, + "angularVelocity": -0.43605213424167905, + "velocityX": 0.6147675892958303, + "velocityY": -0.09770557794461668, + "timestamp": 0.13558110745693022 + }, + { + "x": 0.9599047889558443, + "y": 6.5969221453368565, + "heading": -2.1720310680460675, + "angularVelocity": -0.5388290462692202, + "velocityX": 0.7694151064749511, + "velocityY": -0.1188208792207058, + "timestamp": 0.16947638432116277 + }, + { + "x": 0.9912417390730456, + "y": 6.592238845623578, + "heading": -2.193669425315102, + "angularVelocity": -0.6383885682865823, + "velocityX": 0.9245226183789964, + "velocityY": -0.13816968458577333, + "timestamp": 0.20337166118539532 + }, + { + "x": 1.027853029229915, + "y": 6.586970470643848, + "heading": -2.2185590685016074, + "angularVelocity": -0.7343100717601693, + "velocityX": 1.0801295503062518, + "velocityY": -0.15543094693789433, + "timestamp": 0.23726693804962787 + }, + { + "x": 1.0697568424657102, + "y": 6.581201673061091, + "heading": -2.2465601604436785, + "angularVelocity": -0.8261060104105233, + "velocityX": 1.2362729298138135, + "velocityY": -0.17019473261315557, + "timestamp": 0.27116221491386044 + }, + { + "x": 1.1169724052705505, + "y": 6.575035210978171, + "heading": -2.277512266109695, + "angularVelocity": -0.9131686927944219, + "velocityX": 1.3929835414521643, + "velocityY": -0.18192688343042243, + "timestamp": 0.305057491778093 + }, + { + "x": 1.1695195059195997, + "y": 6.568598078255894, + "heading": -2.3112268138699363, + "angularVelocity": -0.9946680151126935, + "velocityX": 1.5502779593607208, + "velocityY": -0.1899123806558918, + "timestamp": 0.33895276864232554 + }, + { + "x": 1.2274173885480242, + "y": 6.5620508605074, + "heading": -2.3474730638586254, + "angularVelocity": -1.0693599032653842, + "velocityX": 1.708140129975461, + "velocityY": -0.19316017906325608, + "timestamp": 0.3728480455065581 + }, + { + "x": 1.2906820743805074, + "y": 6.555602762480133, + "heading": -2.385952109409252, + "angularVelocity": -1.1352332569742463, + "velocityX": 1.86647496894301, + "velocityY": -0.19023588605263358, + "timestamp": 0.40674332237079064 + }, + { + "x": 1.3593194449788035, + "y": 6.549536996558709, + "heading": -2.426249199917607, + "angularVelocity": -1.1888703747653402, + "velocityX": 2.0249833294834234, + "velocityY": -0.17895608127705168, + "timestamp": 0.4406385992350232 + }, + { + "x": 1.433306250599446, + "y": 6.544255415834808, + "heading": -2.46774838929686, + "angularVelocity": -1.2243354596417166, + "velocityX": 2.182805761315846, + "velocityY": -0.15582055119526933, + "timestamp": 0.47453387609925574 + }, + { + "x": 1.5125352741691294, + "y": 6.540357169258217, + "heading": -2.5094867872592093, + "angularVelocity": -1.231392743287873, + "velocityX": 2.3374650069104015, + "velocityY": -0.11500854801113106, + "timestamp": 0.5084291529634883 + }, + { + "x": 1.5966520622230542, + "y": 6.538761495263786, + "heading": -2.5498872175403693, + "angularVelocity": -1.191919170419624, + "velocityX": 2.481666941115555, + "velocityY": -0.04707658830528645, + "timestamp": 0.5423244298277209 + }, + { + "x": 1.684609840222074, + "y": 6.540735678101622, + "heading": -2.58616040134833, + "angularVelocity": -1.0701545219191795, + "velocityX": 2.5949862675951723, + "velocityY": 0.058243596762563986, + "timestamp": 0.5762197066919534 + }, + { + "x": 1.7742611830095716, + "y": 6.547411083866923, + "heading": -2.61639770388055, + "angularVelocity": -0.8920801164520721, + "velocityX": 2.6449508923203644, + "velocityY": 0.1969420634042785, + "timestamp": 0.610114983556186 + }, + { + "x": 1.8627427547828321, + "y": 6.558330467635622, + "heading": -2.636370991885648, + "angularVelocity": -0.5892646366365757, + "velocityX": 2.610439564416991, + "velocityY": 0.3221505996967478, + "timestamp": 0.6440102604204185 + }, + { + "x": 1.9490348499949164, + "y": 6.573031022742947, + "heading": -2.6456655056562304, + "angularVelocity": -0.27421265233534975, + "velocityX": 2.545844235399729, + "velocityY": 0.4337051196309045, + "timestamp": 0.6779055372846511 + }, + { + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": -2.6456655056562304, + "angularVelocity": -5.472062340179423e-28, + "velocityX": 2.459579916917323, + "velocityY": 0.5379281964379007, + "timestamp": 0.7118008141488836 + }, + { + "x": 2.0935350616513824, + "y": 6.607202674935178, + "heading": -2.6456655056562304, + "angularVelocity": 4.486076522279272e-29, + "velocityX": 2.3668924862008094, + "velocityY": 0.6170990704788266, + "timestamp": 0.7376288015877945 + }, + { + "x": 2.151930892919423, + "y": 6.624698409831654, + "heading": -2.6456655056562304, + "angularVelocity": -1.1967477749725208e-28, + "velocityX": 2.260951667494798, + "velocityY": 0.6773944326036805, + "timestamp": 0.7634567890267054 + }, + { + "x": 2.207364580281582, + "y": 6.643260873915916, + "heading": -2.6456655056562304, + "angularVelocity": -3.73182424325557e-29, + "velocityX": 2.1462642992711727, + "velocityY": 0.7186957221566704, + "timestamp": 0.7892847764656163 + }, + { + "x": 2.2597112074994805, + "y": 6.662441616507122, + "heading": -2.6456655056562304, + "angularVelocity": -7.041879039105387e-29, + "velocityX": 2.0267404629071466, + "velocityY": 0.7426340374592862, + "timestamp": 0.8151127639045272 + }, + { + "x": 2.3089180745864937, + "y": 6.681854987475696, + "heading": -2.6456655056562304, + "angularVelocity": -5.359145584619787e-29, + "velocityX": 1.9051762048204006, + "velocityY": 0.7516408707605028, + "timestamp": 0.8409407513434382 + }, + { + "x": 2.354977814271759, + "y": 6.7011800425592005, + "heading": -2.6456655056562304, + "angularVelocity": -6.820300814682504e-29, + "velocityX": 1.7833267030273643, + "velocityY": 0.7482214837378254, + "timestamp": 0.8667687387823491 + }, + { + "x": 2.3979089062644126, + "y": 6.720153280877432, + "heading": -2.6456655056562304, + "angularVelocity": -1.4852566415875153e-29, + "velocityX": 1.6621926928761115, + "velocityY": 0.7345999514328465, + "timestamp": 0.89259672622126 + }, + { + "x": 2.43774324560733, + "y": 6.738558773488973, + "heading": -2.6456655056562304, + "angularVelocity": -1.0195969895119802e-28, + "velocityX": 1.5422935850938635, + "velocityY": 0.7126181494037732, + "timestamp": 0.9184247136601709 + }, + { + "x": 2.4745187542425713, + "y": 6.756218767940139, + "heading": -2.6456655056562304, + "angularVelocity": -1.4569555248631127e-28, + "velocityX": 1.4238627272924023, + "velocityY": 0.6837541830518434, + "timestamp": 0.9442527010990818 + }, + { + "x": 2.508275186661938, + "y": 6.7729857942130955, + "heading": -2.6456655056562304, + "angularVelocity": 6.354841834267747e-30, + "velocityX": 1.3069710715636813, + "velocityY": 0.6491805183278327, + "timestamp": 0.9700806885379927 + }, + { + "x": 2.5390518441434655, + "y": 6.788736378077184, + "heading": -2.6456655056562304, + "angularVelocity": -4.241645153599559e-29, + "velocityX": 1.191601070517836, + "velocityY": 0.6098262166706413, + "timestamp": 0.9959086759769036 + }, + { + "x": 2.5668863956450756, + "y": 6.803366146756738, + "heading": -2.6456655056562304, + "angularVelocity": -6.05857206571248e-29, + "velocityX": 1.0776895244914062, + "velocityY": 0.5664308422852048, + "timestamp": 1.0217366634158145 + }, + { + "x": 2.5918143285783564, + "y": 6.816786046375137, + "heading": -2.6456655056562304, + "angularVelocity": 4.0217337150651536e-29, + "velocityX": 0.9651519690506631, + "velocityY": 0.5195875075493303, + "timestamp": 1.0475646508547254 + }, + { + "x": 2.6138687520274657, + "y": 6.828919416468381, + "heading": -2.6456655056562304, + "angularVelocity": -1.6748569547134468e-28, + "velocityX": 0.8538963208524588, + "velocityY": 0.46977605676560424, + "timestamp": 1.0733926382936363 + }, + { + "x": 2.633080392488375, + "y": 6.839699715925182, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072902596e-29, + "velocityX": 0.7438303315869891, + "velocityY": 0.41738828789114973, + "timestamp": 1.0992206257325472 + }, + { + "x": 2.649477690401211, + "y": 6.849068742358771, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072816723e-29, + "velocityX": 0.6348654904536816, + "velocityY": 0.36274705707321936, + "timestamp": 1.125048613171458 + }, + { + "x": 2.663086945151386, + "y": 6.856975226116494, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072817978e-29, + "velocityX": 0.5269189007608046, + "velocityY": 0.30612078375921664, + "timestamp": 1.150876600610369 + }, + { + "x": 2.673932478991516, + "y": 6.863373710306793, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072817312e-29, + "velocityX": 0.4199140124944715, + "velocityY": 0.247734524628868, + "timestamp": 1.17670458804928 + }, + { + "x": 2.6820368035241824, + "y": 6.868223650766698, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572073639929e-29, + "velocityX": 0.31378072146871994, + "velocityY": 0.1877784891825456, + "timestamp": 1.2025325754881908 + }, + { + "x": 2.687420780025508, + "y": 6.871488686525517, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572073685113e-29, + "velocityX": 0.20845513085601136, + "velocityY": 0.12641464096035698, + "timestamp": 1.2283605629271017 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": -1.025796443021167e-28, + "velocityX": 0.10387914595382026, + "velocityY": 0.06378185783788577, + "timestamp": 1.2541885503660126 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": -2.636348366286149e-27, + "velocityX": 1.2064225206925918e-26, + "velocityY": -1.2304376584134913e-26, + "timestamp": 1.2800165378049235 + }, + { + "x": 2.7076987449428147, + "y": 6.876455219250757, + "heading": -2.6505612004886614, + "angularVelocity": -0.07931047023704109, + "velocityX": 0.2850393743108695, + "velocityY": 0.053770791429821045, + "timestamp": 1.341744766345227 + }, + { + "x": 2.7428910411738983, + "y": 6.883093308266834, + "heading": -2.660281614006307, + "angularVelocity": -0.1574711237873724, + "velocityX": 0.5701167369173091, + "velocityY": 0.10753733215821971, + "timestamp": 1.4034729948855307 + }, + { + "x": 2.7956833179679075, + "y": 6.893049993467995, + "heading": -2.674745154435654, + "angularVelocity": -0.23430998704108283, + "velocityX": 0.8552371911910641, + "velocityY": 0.16129873538588263, + "timestamp": 1.4652012234258343 + }, + { + "x": 2.866078630848398, + "y": 6.906324892630325, + "heading": -2.6938564282956072, + "angularVelocity": -0.3096034717321421, + "velocityX": 1.1404071450799549, + "velocityY": 0.21505394656938268, + "timestamp": 1.5269294519661378 + }, + { + "x": 2.9540805352004873, + "y": 6.922917544791187, + "heading": -2.7175018943072526, + "angularVelocity": -0.38305758274282437, + "velocityX": 1.4256346963631306, + "velocityY": 0.26880169013805943, + "timestamp": 1.5886576805064414 + }, + { + "x": 3.059693222941866, + "y": 6.942827391558645, + "heading": -2.7455437894569963, + "angularVelocity": -0.4542799269127636, + "velocityX": 1.710930156896079, + "velocityY": 0.32254038773295207, + "timestamp": 1.650385909046745 + }, + { + "x": 3.182921703895506, + "y": 6.966053749056196, + "heading": -2.7778113788413443, + "angularVelocity": -0.522736358184659, + "velocityX": 1.99630677678012, + "velocityY": 0.37626800649861214, + "timestamp": 1.7121141375870486 + }, + { + "x": 3.3237720495046434, + "y": 6.99259576051427, + "heading": -2.81408784809302, + "angularVelocity": -0.5876803872314301, + "velocityX": 2.281781754309916, + "velocityY": 0.42998174555979446, + "timestamp": 1.7738423661273521 + }, + { + "x": 3.482251722494327, + "y": 7.02245230648218, + "heading": -2.8540896383173284, + "angularVelocity": -0.6480307497920543, + "velocityX": 2.567377628311637, + "velocityY": 0.48367734947094865, + "timestamp": 1.8355705946676557 + }, + { + "x": 3.658370014862313, + "y": 7.055621817346754, + "heading": -2.8974315872353036, + "angularVelocity": -0.7021414666010782, + "velocityX": 2.853124033083097, + "velocityY": 0.5373475255800785, + "timestamp": 1.8972988232079593 + }, + { + "x": 3.8521385634468457, + "y": 7.0921018429516485, + "heading": -2.943562541910987, + "angularVelocity": -0.7473234817610797, + "velocityX": 3.1390589551426644, + "velocityY": 0.5909780090493952, + "timestamp": 1.9590270517482629 + }, + { + "x": 4.063571547247647, + "y": 7.131887936757753, + "heading": -2.9916304108767116, + "angularVelocity": -0.7787015779067759, + "velocityX": 3.4252235776173627, + "velocityY": 0.6445364583907961, + "timestamp": 2.0207552802885664 + }, + { + "x": 4.292682637319209, + "y": 7.174970131274871, + "heading": -3.0401399686723773, + "angularVelocity": -0.7858569562545195, + "velocityX": 3.7116096717723086, + "velocityY": 0.697933433955407, + "timestamp": 2.08248350882887 + }, + { + "x": 4.539450280266567, + "y": 7.2213162497869625, + "heading": -3.0857352356627743, + "angularVelocity": -0.7386453178488174, + "velocityX": 3.9976465999868798, + "velocityY": 0.7508091453139826, + "timestamp": 2.1442117373691736 + }, + { + "x": 4.802813151076208, + "y": 7.2704892670136925, + "heading": -3.1119417473367834, + "angularVelocity": -0.4245466343959359, + "velocityX": 4.266490016600536, + "velocityY": 0.796605028032253, + "timestamp": 2.205939965909477 + }, + { + "x": 5.073062376801058, + "y": 7.321569302428323, + "heading": -3.1119417606999895, + "angularVelocity": -2.1648451741594153e-7, + "velocityX": 4.378049267174403, + "velocityY": 0.8274988060167595, + "timestamp": 2.2676681944497807 + }, + { + "x": 5.343311610885675, + "y": 7.372649293614076, + "heading": -3.1119417740630597, + "angularVelocity": -2.164823270096376e-7, + "velocityX": 4.378049402603003, + "velocityY": 0.8274980895070112, + "timestamp": 2.3293964229900843 + }, + { + "x": 5.614008499129137, + "y": 7.421301207393971, + "heading": -3.111941787439452, + "angularVelocity": -2.1669814427337908e-7, + "velocityX": 4.38530141953967, + "velocityY": 0.7881631294202577, + "timestamp": 2.391124651530388 + }, + { + "x": 5.887305956856282, + "y": 7.452160778776353, + "heading": -3.111941805031968, + "angularVelocity": -2.849995312086089e-7, + "velocityX": 4.427430758825403, + "velocityY": 0.49992640501959174, + "timestamp": 2.4528528800706915 + }, + { + "x": 6.157728807203207, + "y": 7.466194314480162, + "heading": -3.1290880401121526, + "angularVelocity": -0.27776975762376865, + "velocityX": 4.380861993639772, + "velocityY": 0.22734389169529218, + "timestamp": 2.514581108610995 + }, + { + "x": 6.410865783691406, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": -0.2025752848143975, + "velocityX": 4.100830081701138, + "velocityY": 0.1512177294179093, + "timestamp": 2.5763093371512986 + }, + { + "x": 6.68474663373147, + "y": 7.480945622329778, + "heading": -3.148801429523152, + "angularVelocity": -0.09918660878674208, + "velocityX": 3.7683669153032966, + "velocityY": 0.0745319969268312, + "timestamp": 2.6489882604609787 + }, + { + "x": 6.933913818291899, + "y": 7.483381950457323, + "heading": -3.152034611955166, + "angularVelocity": -0.04448583282168001, + "velocityX": 3.428327955530431, + "velocityY": 0.033521797195094405, + "timestamp": 2.7216671837706587 + }, + { + "x": 7.158227694750873, + "y": 7.4840407661449, + "heading": -3.1529052531279556, + "angularVelocity": -0.011979280005011805, + "velocityX": 3.086367632376563, + "velocityY": 0.00906474198536178, + "timestamp": 2.794346107080339 + }, + { + "x": 7.357640364981529, + "y": 7.483610410531308, + "heading": -3.1523302695195397, + "angularVelocity": 0.007911284072907336, + "velocityX": 2.7437482718472905, + "velocityY": -0.005921326211146144, + "timestamp": 2.867025030390019 + }, + { + "x": 7.532133153302445, + "y": 7.4825354434077695, + "heading": -3.1509003521726955, + "angularVelocity": 0.019674443177302856, + "velocityX": 2.4008719498693405, + "velocityY": -0.014790630826468705, + "timestamp": 2.939703953699699 + }, + { + "x": 7.681698702311371, + "y": 7.481126333784936, + "heading": -3.149027604334941, + "angularVelocity": 0.02576741306107681, + "velocityX": 2.0578943963112653, + "velocityY": -0.019388146640932385, + "timestamp": 3.012382877009379 + }, + { + "x": 7.806334577335972, + "y": 7.479612058816613, + "heading": -3.1470158430904474, + "angularVelocity": 0.02768011897921896, + "velocityX": 1.7148833437382505, + "velocityY": -0.02083513210385575, + "timestamp": 3.085061800319059 + }, + { + "x": 7.906040631727904, + "y": 7.478168416966872, + "heading": -3.1450983025782047, + "angularVelocity": 0.026383722060277892, + "velocityX": 1.3718702734091426, + "velocityY": -0.01986328063212595, + "timestamp": 3.157740723628739 + }, + { + "x": 7.98081781070478, + "y": 7.476934603107449, + "heading": -3.1434596612761303, + "angularVelocity": 0.022546306789553248, + "velocityX": 1.0288702084682209, + "velocityY": -0.016976226438656095, + "timestamp": 3.230419646938419 + }, + { + "x": 8.03066757049182, + "y": 7.476023550788211, + "heading": -3.142249770303474, + "angularVelocity": 0.016647067919557137, + "velocityX": 0.6858901799443771, + "velocityY": -0.012535302915215497, + "timestamp": 3.3030985702480993 + }, + { + "x": 8.055591583251953, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": 0.009041365553547289, + "velocityX": 0.3429331589563321, + "velocityY": -0.006808490338895578, + "timestamp": 3.3757774935577793 + }, + { + "x": 8.055591583251953, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": -3.3238121669195562e-28, + "velocityX": 1.3302229914992024e-27, + "velocityY": -3.7370579466818436e-26, + "timestamp": 3.4484564168674594 + }, + { + "x": 8.020855812608946, + "y": 7.47166071609439, + "heading": -3.1343435110919597, + "angularVelocity": 0.08413316993333007, + "velocityX": -0.40314154331313246, + "velocityY": -0.04489181734832549, + "timestamp": 3.5346191337076505 + }, + { + "x": 7.951378131703854, + "y": 7.463923825990849, + "heading": -3.1201063976334886, + "angularVelocity": 0.1652351966207958, + "velocityX": -0.8063543427252293, + "velocityY": -0.08979394321896841, + "timestamp": 3.620781850547842 + }, + { + "x": 7.8471512954777065, + "y": 7.452317003930522, + "heading": -3.099202536450615, + "angularVelocity": 0.24260912317382702, + "velocityX": -1.2096512279140474, + "velocityY": -0.13470817177056066, + "timestamp": 3.7069445673880335 + }, + { + "x": 7.708166650380872, + "y": 7.436839026074143, + "heading": -3.0720369478993717, + "angularVelocity": 0.3152824045883838, + "velocityX": -1.6130485457487815, + "velocityY": -0.1796366041368773, + "timestamp": 3.793107284228225 + }, + { + "x": 7.534413751102053, + "y": 7.417488448912146, + "heading": -3.0391360738972004, + "angularVelocity": 0.3818458285524292, + "velocityX": -2.0165670913219236, + "velocityY": -0.22458178979995733, + "timestamp": 3.8792700010684165 + }, + { + "x": 7.325879923024415, + "y": 7.3942635524433395, + "heading": -3.0012114872247055, + "angularVelocity": 0.44015077591895324, + "velocityX": -2.4202327378373227, + "velocityY": -0.26954693770720584, + "timestamp": 3.965432717908608 + }, + { + "x": 7.082549975883412, + "y": 7.367162267336314, + "heading": -2.95927784614972, + "angularVelocity": 0.48667965232295907, + "velocityX": -2.824074681771172, + "velocityY": -0.31453610216690825, + "timestamp": 4.0515954347487995 + }, + { + "x": 6.804407163920078, + "y": 7.336182164967317, + "heading": -2.9149001379339157, + "angularVelocity": 0.5150453681505026, + "velocityX": -3.2281109761106315, + "velocityY": -0.3595534531073048, + "timestamp": 4.137758151588991 + }, + { + "x": 6.491441793733576, + "y": 7.3013211420751505, + "heading": -2.8708180414404794, + "angularVelocity": 0.5116145139108913, + "velocityX": -3.632259771554896, + "velocityY": -0.404595214387502, + "timestamp": 4.2239208684291825 + }, + { + "x": 6.143722098141646, + "y": 7.262584052476802, + "heading": -2.833133271102624, + "angularVelocity": 0.43736748004070747, + "velocityX": -4.035616660473416, + "velocityY": -0.4495806425207707, + "timestamp": 4.310083585269374 + }, + { + "x": 5.763092713732822, + "y": 7.219914368051812, + "heading": -2.829938273384347, + "angularVelocity": 0.03708097696365238, + "velocityX": -4.417564793306024, + "velocityY": -0.49522213307329827, + "timestamp": 4.3962463021095655 + }, + { + "x": 5.381546707630396, + "y": 7.177432729436718, + "heading": -2.8299382625614427, + "angularVelocity": 1.256100666703822e-7, + "velocityX": -4.428203056898617, + "velocityY": -0.4930396832064479, + "timestamp": 4.482409018949757 + }, + { + "x": 5.0000007011662335, + "y": 7.134951094070541, + "heading": -2.8299382517385405, + "angularVelocity": 1.2561003603790461e-7, + "velocityX": -4.428203061096927, + "velocityY": -0.49303964549967616, + "timestamp": 4.5685717357899485 + }, + { + "x": 4.618454694702072, + "y": 7.092469458704354, + "heading": -2.8299382409156384, + "angularVelocity": 1.256100357232878e-7, + "velocityX": -4.428203061096916, + "velocityY": -0.4930396454997818, + "timestamp": 4.65473445263014 + }, + { + "x": 4.236908688689301, + "y": 7.049987819284037, + "heading": -2.8299382300927336, + "angularVelocity": 1.2561006936450142e-7, + "velocityX": -4.428203055858097, + "velocityY": -0.4930396925518196, + "timestamp": 4.7408971694703315 + }, + { + "x": 3.8563823708116955, + "y": 7.0072578552109235, + "heading": -2.8264076724560487, + "angularVelocity": 0.04097546788402064, + "velocityX": -4.41636860851753, + "velocityY": -0.49592173552705077, + "timestamp": 4.827059886310523 + }, + { + "x": 3.5086860797405395, + "y": 6.968518605080416, + "heading": -2.787817768614683, + "angularVelocity": 0.4478724122979906, + "velocityX": -4.035345028825394, + "velocityY": -0.44960571754437684, + "timestamp": 4.9132226031507145 + }, + { + "x": 3.1957416658397833, + "y": 6.933655163283351, + "heading": -2.742718748624896, + "angularVelocity": 0.5234168749974962, + "velocityX": -3.632016553994976, + "velocityY": -0.40462328807165143, + "timestamp": 4.999385319990906 + }, + { + "x": 2.9176198484629934, + "y": 6.902672538405997, + "heading": -2.6973522860047296, + "angularVelocity": 0.5265208002239449, + "velocityX": -3.2278673140336474, + "velocityY": -0.35958272920779544, + "timestamp": 5.0855480368310975 + }, + { + "x": 2.674310160776578, + "y": 6.875568579535636, + "heading": -2.654519075523016, + "angularVelocity": 0.49712000796304645, + "velocityX": -2.8238395515973513, + "velocityY": -0.31456713372480594, + "timestamp": 5.171710753671289 + }, + { + "x": 2.4657949770738905, + "y": 6.852340853729729, + "heading": -2.6158118677401805, + "angularVelocity": 0.44923383572766007, + "velocityX": -2.420016352193573, + "velocityY": -0.269579774846101, + "timestamp": 5.2578734705114805 + }, + { + "x": 2.2920584467479186, + "y": 6.832987389106081, + "heading": -2.582256733700525, + "angularVelocity": 0.38943913644101064, + "velocityX": -2.0163771141084914, + "velocityY": -0.22461530152934878, + "timestamp": 5.344036187351672 + }, + { + "x": 2.1530874313369837, + "y": 6.8175066409515335, + "heading": -2.5545687221827422, + "angularVelocity": 0.321345618304218, + "velocityX": -1.6128903603247504, + "velocityY": -0.17966875607299837, + "timestamp": 5.4301989041918635 + }, + { + "x": 2.0488711542448845, + "y": 6.805897385165772, + "heading": -2.53327431428957, + "angularVelocity": 0.2471417879344249, + "velocityX": -1.209528679154725, + "velocityY": -0.1347364174610917, + "timestamp": 5.516361621032055 + }, + { + "x": 1.9794007109586114, + "y": 6.798158635561578, + "heading": -2.51877750640236, + "angularVelocity": 0.16824919662291424, + "velocityX": -0.8062703432985042, + "velocityY": -0.08981552448662153, + "timestamp": 5.6025243378722465 + }, + { + "x": 1.944668650627136, + "y": 6.794289588928223, + "heading": -2.5113986210516552, + "angularVelocity": 0.085638958720288, + "velocityX": -0.40309848163091144, + "velocityY": -0.044903953534010145, + "timestamp": 5.688687054712438 + }, + { + "x": 1.944668650627136, + "y": 6.794289588928223, + "heading": -2.5113986210516552, + "angularVelocity": -2.4070077255640283e-28, + "velocityX": 3.6388248341051744e-28, + "velocityY": -8.930765034898444e-28, + "timestamp": 5.7748497715526295 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.8817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "timestamp": 0.7118008141488836, + "isStopPoint": false, + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 + }, + { + "timestamp": 1.2800165378049235, + "isStopPoint": true, + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 21 + }, + { + "timestamp": 2.5763093371512986, + "isStopPoint": false, + "x": 6.410865783691406, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "timestamp": 3.4484564168674594, + "isStopPoint": true, + "x": 8.055591583251953, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 27 + }, + { + "timestamp": 5.7748497715526295, + "isStopPoint": true, + "x": 1.9446686506271362, + "y": 6.794289588928223, + "heading": -2.5113986210516552, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 1, + 2 + ], + "type": "ZeroAngularVelocity" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false + }, + "AmpWing321": { + "waypoints": [ + { + "x": 0.3817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 2.0986995697021484, + "y": 6.135313034057617, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "x": 2.6396329402923584, + "y": 5.556640625, + "heading": -3.1349260454664436, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "x": 2.0483803749084473, + "y": 4.827009677886963, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 27 + }, + { + "x": 2.652212619781494, + "y": 4.1854376792907715, + "heading": -3.6607387948340793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.3817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "angularVelocity": 1.8698301864234364e-22, + "velocityX": 4.800440645526857e-21, + "velocityY": 2.9477177393174924e-20, + "timestamp": 0 + }, + { + "x": 0.3883549561387046, + "y": 6.608408421186061, + "heading": -2.1197134410063434, + "angularVelocity": -0.08634729729146294, + "velocityX": 0.17378773726557262, + "velocityY": -0.026872324952015636, + "timestamp": 0.037779247083705426 + }, + { + "x": 0.4014921075538034, + "y": 6.606405430964887, + "heading": -2.126204479988474, + "angularVelocity": -0.17181493764946093, + "velocityX": 0.3477346011156235, + "velocityY": -0.05301826732320674, + "timestamp": 0.07555849416741085 + }, + { + "x": 0.4212075211916924, + "y": 6.603445687185866, + "heading": -2.1358857960695534, + "angularVelocity": -0.25626016473089336, + "velocityX": 0.5218582994574282, + "velocityY": -0.0783431118282232, + "timestamp": 0.11333774125111629 + }, + { + "x": 0.44750862695642823, + "y": 6.599564542552146, + "heading": -2.1487127600714926, + "angularVelocity": -0.3395240771611807, + "velocityX": 0.6961786640814154, + "velocityY": -0.10273218587648632, + "timestamp": 0.1511169883348217 + }, + { + "x": 0.48040369549494544, + "y": 6.594802655593878, + "heading": -2.1646339690222502, + "angularVelocity": -0.42142737560337534, + "velocityX": 0.8707179490802492, + "velocityY": -0.1260450465763573, + "timestamp": 0.18889623541852713 + }, + { + "x": 0.5199019408821504, + "y": 6.589207268746513, + "heading": -2.1835902439067887, + "angularVelocity": -0.5017642316306103, + "velocityX": 1.0455011265758887, + "velocityY": -0.1481074208540784, + "timestamp": 0.22667548250223254 + }, + { + "x": 0.5660136324357566, + "y": 6.582833919675241, + "heading": -2.205513273539415, + "angularVelocity": -0.5802929207152439, + "velocityX": 1.220556128380285, + "velocityY": -0.16869973763992252, + "timestamp": 0.26445472958593796 + }, + { + "x": 0.6187502087281199, + "y": 6.575748787450549, + "heading": -2.2303236919031626, + "angularVelocity": -0.6567208263514764, + "velocityX": 1.3959139041473307, + "velocityY": -0.1875403236323719, + "timestamp": 0.3022339766696434 + }, + { + "x": 0.6781243747498052, + "y": 6.568032000955299, + "heading": -2.2579282145890622, + "angularVelocity": -0.730679534844558, + "velocityX": 1.5716079754085093, + "velocityY": -0.2042599334549938, + "timestamp": 0.34001322375334886 + }, + { + "x": 0.7441501329753766, + "y": 6.559782463812031, + "heading": -2.2882151619643003, + "angularVelocity": -0.8016821327363354, + "velocityX": 1.7476726859930256, + "velocityY": -0.21836160802711913, + "timestamp": 0.3777924708370543 + }, + { + "x": 0.8168426216101802, + "y": 6.55112518097433, + "heading": -2.3210471231149787, + "angularVelocity": -0.8690475243705671, + "velocityX": 1.9241380981931202, + "velocityY": -0.22915445663977885, + "timestamp": 0.41557171792075975 + }, + { + "x": 0.8962174244775805, + "y": 6.54222294081638, + "heading": -2.3562483469235813, + "angularVelocity": -0.9317608614753342, + "velocityX": 2.1010160073209, + "velocityY": -0.23563836881711223, + "timestamp": 0.4533509650044652 + }, + { + "x": 0.9822884113766555, + "y": 6.5332960857123705, + "heading": -2.393581906091161, + "angularVelocity": -0.9882028375224495, + "velocityX": 2.2782610439104056, + "velocityY": -0.2362899155781206, + "timestamp": 0.49113021208817065 + }, + { + "x": 1.0750612253316763, + "y": 6.524658510256875, + "heading": -2.432705739729525, + "angularVelocity": -1.0355906127954124, + "velocityX": 2.455655448862486, + "velocityY": -0.22863281092648347, + "timestamp": 0.528909459171876 + }, + { + "x": 1.1745124212868387, + "y": 6.516789255723791, + "heading": -2.4730818457948223, + "angularVelocity": -1.0687377113640772, + "velocityX": 2.6324292735325403, + "velocityY": -0.20829569513922166, + "timestamp": 0.5666887062555814 + }, + { + "x": 1.2805135717940606, + "y": 6.510488992251309, + "heading": -2.5137740657710497, + "angularVelocity": -1.0771051070993467, + "velocityX": 2.805803680320948, + "velocityY": -0.16676519408961948, + "timestamp": 0.6044679533392868 + }, + { + "x": 1.3925030399539646, + "y": 6.5072291237539, + "heading": -2.5529426361670393, + "angularVelocity": -1.0367747750294287, + "velocityX": 2.9643118062089164, + "velocityY": -0.08628728069154737, + "timestamp": 0.6422472004229922 + }, + { + "x": 1.5081204845079395, + "y": 6.509370790524853, + "heading": -2.586975983570745, + "angularVelocity": -0.9008476883697443, + "velocityX": 3.060342740494668, + "velocityY": 0.05668897440470873, + "timestamp": 0.6800264475066976 + }, + { + "x": 1.622365364792802, + "y": 6.517488893832767, + "heading": -2.612477761707434, + "angularVelocity": -0.6750208144748423, + "velocityX": 3.0240115699429984, + "velocityY": 0.21488261240214016, + "timestamp": 0.717805694590403 + }, + { + "x": 1.7327667460189187, + "y": 6.53045723021325, + "heading": -2.629741912942711, + "angularVelocity": -0.4569744652937502, + "velocityX": 2.9222758458236795, + "velocityY": 0.34326614163931696, + "timestamp": 0.7555849416741084 + }, + { + "x": 1.8382100625927147, + "y": 6.547429672114164, + "heading": -2.6405106169347077, + "angularVelocity": -0.28504284291683973, + "velocityX": 2.7910380622508084, + "velocityY": 0.44925304793238396, + "timestamp": 0.7933641887578138 + }, + { + "x": 1.938178409904931, + "y": 6.567826322651328, + "heading": -2.6456655056562304, + "angularVelocity": -0.13644762983499997, + "velocityX": 2.646118041757629, + "velocityY": 0.5398903395804769, + "timestamp": 0.8311434358415192 + }, + { + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": -2.6456655056562304, + "angularVelocity": 1.8055348625312267e-21, + "velocityX": 2.4940831175072695, + "velocityY": 0.6203915390652321, + "timestamp": 0.8689226829252246 + }, + { + "x": 2.093877892062785, + "y": 6.608590709626909, + "heading": -2.6456655056562304, + "angularVelocity": 4.168304475470088e-23, + "velocityX": 2.3837897196813476, + "velocityY": 0.671861873409428, + "timestamp": 0.8947114088797997 + }, + { + "x": 2.1523659521196383, + "y": 6.626881977306388, + "heading": -2.6456655056562304, + "angularVelocity": 4.168273499118604e-23, + "velocityX": 2.2679701261668037, + "velocityY": 0.7092738009447922, + "timestamp": 0.9205001348343749 + }, + { + "x": 2.2077779049993467, + "y": 6.6457974470281025, + "heading": -2.6456655056562304, + "angularVelocity": 4.16827486833322e-23, + "velocityX": 2.148689042542395, + "velocityY": 0.7334782553910214, + "timestamp": 0.94628886078895 + }, + { + "x": 2.2600669512143567, + "y": 6.665028382936204, + "heading": -2.6456655056562304, + "angularVelocity": 4.168303685702792e-23, + "velocityX": 2.0275932322952634, + "velocityY": 0.7457109723785112, + "timestamp": 0.9720775867435252 + }, + { + "x": 2.309217481860067, + "y": 6.684301584296192, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682770338729235e-23, + "velocityX": 1.9058921612597222, + "velocityY": 0.7473498843667037, + "timestamp": 0.9978663126981003 + }, + { + "x": 2.3552353371432924, + "y": 6.7033787939224325, + "heading": -2.6456655056562304, + "angularVelocity": 4.168287783381093e-23, + "velocityX": 1.7844175537902998, + "velocityY": 0.7397499845392672, + "timestamp": 1.0236550386526755 + }, + { + "x": 2.3981402688351157, + "y": 6.7220537901087445, + "heading": -2.6456655056562304, + "angularVelocity": 4.168284482622447e-23, + "velocityX": 1.663708853527376, + "velocityY": 0.7241535009973167, + "timestamp": 1.0494437646072505 + }, + { + "x": 2.4379604976017806, + "y": 6.740148546732269, + "heading": -2.6456655056562304, + "angularVelocity": 4.168287413318469e-23, + "velocityX": 1.5440944557233214, + "velocityY": 0.7016537635645865, + "timestamp": 1.0752324905618256 + }, + { + "x": 2.474728960142428, + "y": 6.7575092774466885, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682752490293846e-23, + "velocityX": 1.4257572322666272, + "velocityY": 0.6731907091840713, + "timestamp": 1.1010212165164006 + }, + { + "x": 2.508480806878266, + "y": 6.774002770496258, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682856288215976e-23, + "velocityX": 1.3087830238384508, + "velocityY": 0.6395621512623721, + "timestamp": 1.1268099424709757 + }, + { + "x": 2.5392517800354852, + "y": 6.789513173699484, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682970034902383e-23, + "velocityX": 1.1931947786603707, + "velocityY": 0.6014412356126597, + "timestamp": 1.1525986684255507 + }, + { + "x": 2.567077192846392, + "y": 6.80393925778094, + "heading": -2.6456655056562304, + "angularVelocity": 4.168282751801243e-23, + "velocityX": 1.0789758617732326, + "velocityY": 0.5593949893802513, + "timestamp": 1.1783873943801257 + }, + { + "x": 2.591991311058039, + "y": 6.8171921248456435, + "heading": -2.6456655056562304, + "angularVelocity": 4.16831016133047e-23, + "velocityX": 0.9660856552411753, + "velocityY": 0.5139015819585995, + "timestamp": 1.2041761203347008 + }, + { + "x": 2.6140269997935075, + "y": 6.8291933055291585, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682704625493976e-23, + "velocityX": 0.854469847572779, + "velocityY": 0.4653653966747698, + "timestamp": 1.2299648462892758 + }, + { + "x": 2.6332155431922697, + "y": 6.839873184183342, + "heading": -2.6456655056562304, + "angularVelocity": 4.168275058132412e-23, + "velocityX": 0.7440671335423578, + "velocityY": 0.4141297508395471, + "timestamp": 1.2557535722438509 + }, + { + "x": 2.6495865749656664, + "y": 6.849169695790004, + "heading": -2.6456655056562304, + "angularVelocity": 4.168252977577915e-23, + "velocityX": 0.6348135151085763, + "velocityY": 0.3604874324980445, + "timestamp": 1.281542298198426 + }, + { + "x": 2.6631680788200836, + "y": 6.857027245752284, + "heading": -2.6456655056562304, + "angularVelocity": 4.1683540057046367e-23, + "velocityX": 0.5266450106294411, + "velocityY": 0.30468934277736914, + "timestamp": 1.307331024153001 + }, + { + "x": 2.67398643165684, + "y": 6.8633958116591645, + "heading": -2.6456655056562304, + "angularVelocity": 4.1683480067867495e-23, + "velocityX": 0.41949931360441123, + "velocityY": 0.2469515523307539, + "timestamp": 1.333119750107576 + }, + { + "x": 2.682066471763531, + "y": 6.868230193464582, + "heading": -2.6456655056562304, + "angularVelocity": 4.16807726013415e-23, + "velocityX": 0.3133167617887716, + "velocityY": 0.1874610561874916, + "timestamp": 1.358908476062151 + }, + { + "x": 2.6874315804021185, + "y": 6.87148938481805, + "heading": -2.6456655056562304, + "angularVelocity": 4.1684045007313837e-23, + "velocityX": 0.20804085661285734, + "velocityY": 0.12638047181321807, + "timestamp": 1.384697202016726 + }, + { + "x": 2.6901037693023544, + "y": 6.873136043548666, + "heading": -2.6456655056562304, + "angularVelocity": 4.1681852972267e-23, + "velocityX": 0.10361849224262781, + "velocityY": 0.06385188370251901, + "timestamp": 1.4104859279713011 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": 1.876229838959747e-22, + "velocityX": 1.9791825060653877e-20, + "velocityY": -3.163566312793905e-20, + "timestamp": 1.4362746539258762 + }, + { + "x": 2.6855621835413945, + "y": 6.87107725775431, + "heading": -2.6494144391039285, + "angularVelocity": -0.1135887895189958, + "velocityX": -0.13760533129825955, + "velocityY": -0.06237907114466451, + "timestamp": 1.469279085644653 + }, + { + "x": 2.676506807566959, + "y": 6.866895146887751, + "heading": -2.6568904043742765, + "angularVelocity": -0.22651398254783756, + "velocityX": -0.27436848637770506, + "velocityY": -0.1267136153772196, + "timestamp": 1.5022835173634297 + }, + { + "x": 2.662971687717957, + "y": 6.860513477351578, + "heading": -2.6680645143317867, + "angularVelocity": -0.3385639253759127, + "velocityX": -0.4101000727517643, + "velocityY": -0.1933579584266206, + "timestamp": 1.5352879490822064 + }, + { + "x": 2.6449994198187143, + "y": 6.851840728868721, + "heading": -2.6828968756767435, + "angularVelocity": -0.4494051426590254, + "velocityX": -0.544541049892368, + "velocityY": -0.26277527081079527, + "timestamp": 1.5682923808009832 + }, + { + "x": 2.6226447357456646, + "y": 6.8407649649450155, + "heading": -2.7013308201305883, + "angularVelocity": -0.5585293699620854, + "velocityX": -0.6773237080258816, + "velocityY": -0.33558414270177084, + "timestamp": 1.60129681251976 + }, + { + "x": 2.595980364541067, + "y": 6.827146327228181, + "heading": -2.7232846239136888, + "angularVelocity": -0.6651774516272203, + "velocityX": -0.8079027517213173, + "velocityY": -0.4126305773987425, + "timestamp": 1.6343012442385367 + }, + { + "x": 2.5651071249375024, + "y": 6.810805799364362, + "heading": -2.7486396330262775, + "angularVelocity": -0.7682304403430714, + "velocityX": -0.9354270925379659, + "velocityY": -0.49510102167646325, + "timestamp": 1.6673056759573135 + }, + { + "x": 2.5301723586562304, + "y": 6.791508128843289, + "heading": -2.7772234107649822, + "angularVelocity": -0.8660587760534934, + "velocityX": -1.0584871322416465, + "velocityY": -0.5846993726631504, + "timestamp": 1.7003101076760903 + }, + { + "x": 2.491405811821051, + "y": 6.768936135527107, + "heading": -2.808786483180526, + "angularVelocity": -0.9563283102246689, + "velocityX": -1.1745861030270022, + "velocityY": -0.683907952377895, + "timestamp": 1.733314539394867 + }, + { + "x": 2.4491937947918876, + "y": 6.742655604777802, + "heading": -2.8429701361950754, + "angularVelocity": -1.0357291804270654, + "velocityX": -1.278980271160013, + "velocityY": -0.7962727846137726, + "timestamp": 1.7663189711136438 + }, + { + "x": 2.4042373058522646, + "y": 6.712087454564749, + "heading": -2.8792403833872675, + "angularVelocity": -1.0989508167037272, + "velocityX": -1.3621349194159027, + "velocityY": -0.9261832008960693, + "timestamp": 1.7993234028324205 + }, + { + "x": 2.357861726905285, + "y": 6.6765936215423825, + "heading": -2.9166547914513425, + "angularVelocity": -1.1336177027035363, + "velocityX": -1.405131872656832, + "velocityY": -1.0754262738047007, + "timestamp": 1.8323278345511973 + }, + { + "x": 2.3122631013668973, + "y": 6.636020424738222, + "heading": -2.9535956195097754, + "angularVelocity": -1.119268720431154, + "velocityX": -1.3815909913833053, + "velocityY": -1.229325720554031, + "timestamp": 1.865332266269974 + }, + { + "x": 2.2696422032639783, + "y": 6.591320309416235, + "heading": -2.9884057432125344, + "angularVelocity": -1.0547105915765655, + "velocityX": -1.2913689429977981, + "velocityY": -1.3543670650918083, + "timestamp": 1.8983366979887508 + }, + { + "x": 2.23120231957765, + "y": 6.543808933743509, + "heading": -3.0202463024819033, + "angularVelocity": -0.9647358736752334, + "velocityX": -1.1646885489156587, + "velocityY": -1.4395453337164417, + "timestamp": 1.9313411297075276 + }, + { + "x": 2.197428425363642, + "y": 6.494494224568626, + "heading": -3.048607569176528, + "angularVelocity": -0.8593169225358813, + "velocityX": -1.0233139143793606, + "velocityY": -1.494184465743377, + "timestamp": 1.9643455614263043 + }, + { + "x": 2.1685016928598695, + "y": 6.444043742872585, + "heading": -3.0731583606612194, + "angularVelocity": -0.7438634815434175, + "velocityX": -0.8764499492144375, + "velocityY": -1.5285971934290237, + "timestamp": 1.997349993145081 + }, + { + "x": 2.1444919046238304, + "y": 6.392902243224364, + "heading": -3.0937182784350274, + "angularVelocity": -0.6229441533486862, + "velocityX": -0.72747164503908, + "velocityY": -1.5495343196327538, + "timestamp": 2.030354424863858 + }, + { + "x": 2.125426328721103, + "y": 6.341380152546792, + "heading": -3.1102000784524315, + "angularVelocity": -0.49938142119341705, + "velocityX": -0.5776671467995586, + "velocityY": -1.5610658325093172, + "timestamp": 2.0633588565826346 + }, + { + "x": 2.1113144979598757, + "y": 6.289704629326546, + "heading": -3.122567100672372, + "angularVelocity": -0.3747079278721404, + "velocityX": -0.42757381437354175, + "velocityY": -1.5657146791850087, + "timestamp": 2.0963632883014114 + }, + { + "x": 2.1021579392057013, + "y": 6.238048635898547, + "heading": -3.1308085711190277, + "angularVelocity": -0.24970799427420023, + "velocityX": -0.2774342195070909, + "velocityY": -1.5651229467650507, + "timestamp": 2.129367720020188 + }, + { + "x": 2.0979543563247316, + "y": 6.186548177449108, + "heading": -3.1349260454664436, + "angularVelocity": -0.12475519598398203, + "velocityX": -0.127364195111383, + "velocityY": -1.5604103984660276, + "timestamp": 2.162372151738965 + }, + { + "x": 2.0986995697021484, + "y": 6.135313034057617, + "heading": -3.1349260454664436, + "angularVelocity": 3.0970432324121202e-22, + "velocityX": 0.02257919129671586, + "velocityY": -1.552371627787794, + "timestamp": 2.1953765834577417 + }, + { + "x": 2.1033897121209892, + "y": 6.090187708264656, + "heading": -3.1349260454664436, + "angularVelocity": -5.717605117434922e-23, + "velocityX": 0.16029945836581586, + "velocityY": -1.5422911795098355, + "timestamp": 2.224635212692289 + }, + { + "x": 2.1121017027787423, + "y": 6.045447928244468, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623983844657e-23, + "velocityX": 0.29775799091431626, + "velocityY": -1.5291140149276428, + "timestamp": 2.253893841926836 + }, + { + "x": 2.1248234606724927, + "y": 6.00120380324005, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623190409668e-23, + "velocityX": 0.43480361953307217, + "velocityY": -1.5121735420265516, + "timestamp": 2.283152471161383 + }, + { + "x": 2.141535772219987, + "y": 5.957591648743971, + "heading": -3.1349260454664436, + "angularVelocity": -5.717624354569263e-23, + "velocityX": 0.5711925672765078, + "velocityY": -1.4905740848795832, + "timestamp": 2.3124111003959302 + }, + { + "x": 2.1622074822868833, + "y": 5.91478389840203, + "heading": -3.1349260454664436, + "angularVelocity": -5.717622820098084e-23, + "velocityX": 0.706516696362894, + "velocityY": -1.4630811990108465, + "timestamp": 2.3416697296304774 + }, + { + "x": 2.1867865150959807, + "y": 5.873004121037691, + "heading": -3.1349260454664436, + "angularVelocity": -5.717608261879809e-23, + "velocityX": 0.8400609820801516, + "velocityY": -1.4279471888253283, + "timestamp": 2.3709283588650245 + }, + { + "x": 2.2151821291704135, + "y": 5.832550089747363, + "heading": -3.1349260454664436, + "angularVelocity": -5.717636301748886e-23, + "velocityX": 0.9705039100359978, + "velocityY": -1.3826359043014271, + "timestamp": 2.4001869880995716 + }, + { + "x": 2.247227781288673, + "y": 5.793828430179212, + "heading": -3.1349260454664436, + "angularVelocity": -5.717627842193595e-23, + "velocityX": 1.0952547319085686, + "velocityY": -1.3234269882491958, + "timestamp": 2.429445617334119 + }, + { + "x": 2.2826005084616283, + "y": 5.757399030074356, + "heading": -3.1349260454664436, + "angularVelocity": -5.717621943290754e-23, + "velocityX": 1.2089673405192627, + "velocityY": -1.245082256342438, + "timestamp": 2.458704246568666 + }, + { + "x": 2.320655607594778, + "y": 5.7239910126155, + "heading": -3.1349260454664436, + "angularVelocity": -5.717628005263719e-23, + "velocityX": 1.3006453182787074, + "velocityY": -1.141817587932542, + "timestamp": 2.487962875803213 + }, + { + "x": 2.360204689273028, + "y": 5.694336910489396, + "heading": -3.1349260454664436, + "angularVelocity": -5.717616763256594e-23, + "velocityX": 1.351706580688257, + "velocityY": -1.013516453161909, + "timestamp": 2.51722150503776 + }, + { + "x": 2.3996260680339816, + "y": 5.668721069816042, + "heading": -3.1349260454664436, + "angularVelocity": -5.717628968554313e-23, + "velocityX": 1.34734195662094, + "velocityY": -0.8754969505913973, + "timestamp": 2.5464801342723073 + }, + { + "x": 2.4375100420911338, + "y": 5.6468415720185074, + "heading": -3.1349260454664436, + "angularVelocity": -5.717625457879064e-23, + "velocityX": 1.2947966137939357, + "velocityY": -0.7477964063905799, + "timestamp": 2.5757387635068545 + }, + { + "x": 2.4729703851854588, + "y": 5.62819470377097, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623609342389e-23, + "velocityX": 1.2119618731994621, + "velocityY": -0.6373117516233145, + "timestamp": 2.6049973927414016 + }, + { + "x": 2.5055011746844404, + "y": 5.612330209413261, + "heading": -3.1349260454664436, + "angularVelocity": -5.717628902265798e-23, + "velocityX": 1.1118357335957716, + "velocityY": -0.5422159128005899, + "timestamp": 2.6342560219759488 + }, + { + "x": 2.5348060457698836, + "y": 5.598898274411973, + "heading": -3.1349260454664436, + "angularVelocity": -5.717627045197986e-23, + "velocityX": 1.0015804517181697, + "velocityY": -0.4590760180043621, + "timestamp": 2.663514651210496 + }, + { + "x": 2.5607008711079886, + "y": 5.587633246659649, + "heading": -3.1349260454664436, + "angularVelocity": -5.717608754941009e-23, + "velocityX": 0.8850320748298349, + "velocityY": -0.38501556795704556, + "timestamp": 2.692773280445043 + }, + { + "x": 2.5830643099643584, + "y": 5.578331178882631, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623485224611e-23, + "velocityX": 0.7643365202491328, + "velocityY": -0.31792561785481754, + "timestamp": 2.72203190967959 + }, + { + "x": 2.601812219563059, + "y": 5.570832400631334, + "heading": -3.1349260454664436, + "angularVelocity": -5.717639538428928e-23, + "velocityX": 0.6407651379834913, + "velocityY": -0.2562928765800323, + "timestamp": 2.7512905389141373 + }, + { + "x": 2.6168837285238395, + "y": 5.565009326805624, + "heading": -3.1349260454664436, + "angularVelocity": -5.717612764732363e-23, + "velocityX": 0.5151132966602103, + "velocityY": -0.1990207326189503, + "timestamp": 2.7805491681486845 + }, + { + "x": 2.62823323421208, + "y": 5.560758067303841, + "heading": -3.1349260454664436, + "angularVelocity": -5.717624354345976e-23, + "velocityX": 0.3879028507202083, + "velocityY": -0.1452993394780887, + "timestamp": 2.8098077973832316 + }, + { + "x": 2.635825567663263, + "y": 5.557992596069733, + "heading": -3.1349260454664436, + "angularVelocity": -5.717624354345825e-23, + "velocityX": 0.2594904016290358, + "velocityY": -0.09451814067963153, + "timestamp": 2.8390664266177787 + }, + { + "x": 2.6396329402923584, + "y": 5.556640625, + "heading": -3.1349260454664436, + "angularVelocity": -5.717626024583988e-23, + "velocityX": 0.13012819563669478, + "velocityY": -0.04620760114590083, + "timestamp": 2.868325055852326 + }, + { + "x": 2.6396329402923584, + "y": 5.556640625, + "heading": -3.1349260454664436, + "angularVelocity": 6.493546095227337e-22, + "velocityX": 1.881202272196025e-21, + "velocityY": 2.989774730921632e-20, + "timestamp": 2.897583685086873 + }, + { + "x": 2.63578986878673, + "y": 5.555006216645995, + "heading": -3.138403367676192, + "angularVelocity": -0.11468866986671536, + "velocityX": -0.12675177409426172, + "velocityY": -0.053905881835223775, + "timestamp": 2.927903351482776 + }, + { + "x": 2.6281241915651026, + "y": 5.55168270192059, + "heading": -3.145319654992211, + "angularVelocity": -0.22811224984168904, + "velocityX": -0.25282854770026447, + "velocityY": -0.10961580783856312, + "timestamp": 2.958223017878679 + }, + { + "x": 2.616660760682821, + "y": 5.546605937980688, + "heading": -3.155626430554535, + "angularVelocity": -0.33993697119690924, + "velocityX": -0.37808565346951006, + "velocityY": -0.1674412862474532, + "timestamp": 2.9885426842745817 + }, + { + "x": 2.6014305324024507, + "y": 5.539699833963852, + "heading": -3.1692644048210545, + "angularVelocity": -0.4498062112042979, + "velocityX": -0.502321763092811, + "velocityY": -0.22777638535441594, + "timestamp": 3.0188623506704846 + }, + { + "x": 2.5824731253801407, + "y": 5.530872998614334, + "heading": -3.1861630308768345, + "angularVelocity": -0.5573486803952303, + "velocityX": -0.6252511744282022, + "velocityY": -0.2911257411039302, + "timestamp": 3.0491820170663875 + }, + { + "x": 2.5598407273275248, + "y": 5.520014195669526, + "heading": -3.2062405203859172, + "angularVelocity": -0.6621936154217003, + "velocityX": -0.7464593362305498, + "velocityY": -0.35814387938840914, + "timestamp": 3.0795016834622904 + }, + { + "x": 2.5336042337816798, + "y": 5.506986103666438, + "heading": -3.2294044879426838, + "angularVelocity": -0.7639915048635432, + "velocityX": -0.8653292289980953, + "velocityY": -0.4296911395064041, + "timestamp": 3.1098213498581933 + }, + { + "x": 2.503863218826844, + "y": 5.49161664330363, + "heading": -3.255552933908256, + "angularVelocity": -0.8624252531058615, + "velocityX": -0.9809149799500774, + "velocityY": -0.5069139007700986, + "timestamp": 3.140141016254096 + }, + { + "x": 2.470762785739885, + "y": 5.4736868205948594, + "heading": -3.2845732996383643, + "angularVelocity": -0.9571466041601998, + "velocityX": -1.0917149501167551, + "velocityY": -0.5913594983085235, + "timestamp": 3.170460682649999 + }, + { + "x": 2.4345235301066466, + "y": 5.452913761348259, + "heading": -3.3163303487290055, + "angularVelocity": -1.047407602576138, + "velocityX": -1.1952392602228237, + "velocityY": -0.6851348222423583, + "timestamp": 3.200780349045902 + }, + { + "x": 2.3954986328534775, + "y": 5.428928619205951, + "heading": -3.3506130763033335, + "angularVelocity": -1.1307092606718314, + "velocityX": -1.2871149947230278, + "velocityY": -0.7910753973717586, + "timestamp": 3.231100015441805 + }, + { + "x": 2.354290847908129, + "y": 5.401261287885048, + "heading": -3.3869757124641584, + "angularVelocity": -1.1993085836108863, + "velocityX": -1.3591107635311952, + "velocityY": -0.9125209677319055, + "timestamp": 3.2614196818377077 + }, + { + "x": 2.3119700393450096, + "y": 5.369418962959644, + "heading": -3.424488739130058, + "angularVelocity": -1.237250640428174, + "velocityX": -1.3958203896611634, + "velocityY": -1.050220161054137, + "timestamp": 3.2917393482336106 + }, + { + "x": 2.2701943697940816, + "y": 5.333273315136478, + "heading": -3.4617422361668626, + "angularVelocity": -1.2286908619099843, + "velocityX": -1.3778406729622434, + "velocityY": -1.1921518974243095, + "timestamp": 3.3220590146295135 + }, + { + "x": 2.2306531953186166, + "y": 5.2934548415358735, + "heading": -3.497295099309754, + "angularVelocity": -1.1726007363885618, + "velocityX": -1.3041427949485624, + "velocityY": -1.3132886450881536, + "timestamp": 3.3523786810254164 + }, + { + "x": 2.194384797808916, + "y": 5.250947089639415, + "heading": -3.5300729098031467, + "angularVelocity": -1.0810742461804137, + "velocityX": -1.1962004144809404, + "velocityY": -1.4019861347226739, + "timestamp": 3.3826983474213193 + }, + { + "x": 2.1618561434717347, + "y": 5.206562904098581, + "heading": -3.559395458180096, + "angularVelocity": -0.9671131599558667, + "velocityX": -1.0728566044374948, + "velocityY": -1.463874468844275, + "timestamp": 3.413018013817222 + }, + { + "x": 2.1332734403753273, + "y": 5.160862427241999, + "heading": -3.5849957373685863, + "angularVelocity": -0.8443456749890059, + "velocityX": -0.9427116619022493, + "velocityY": -1.5072882484862653, + "timestamp": 3.443337680213125 + }, + { + "x": 2.1087386455620734, + "y": 5.114234766629067, + "heading": -3.606823401382331, + "angularVelocity": -0.7199176840776125, + "velocityX": -0.8092039830811812, + "velocityY": -1.5378685241486245, + "timestamp": 3.473657346609028 + }, + { + "x": 2.0883065892590618, + "y": 5.0669625612384435, + "heading": -3.624907841959223, + "angularVelocity": -0.5964590883274463, + "velocityX": -0.6738878995637272, + "velocityY": -1.5591268311907651, + "timestamp": 3.503977013004931 + }, + { + "x": 2.0720078142934666, + "y": 5.019259378769025, + "heading": -3.6392985462841363, + "angularVelocity": -0.4746326736252551, + "velocityX": -0.5375644557817244, + "velocityY": -1.5733412711912018, + "timestamp": 3.5342966794008337 + }, + { + "x": 2.059859229705669, + "y": 4.97129159857966, + "heading": -3.6500430565181534, + "angularVelocity": -0.35437428940409044, + "velocityX": -0.4006833198346745, + "velocityY": -1.5820682049415709, + "timestamp": 3.5646163457967366 + }, + { + "x": 2.0518696849147733, + "y": 4.923191943386821, + "heading": -3.657180162870296, + "angularVelocity": -0.2353952797154437, + "velocityX": -0.26351031329218877, + "velocityY": -1.5864176922246054, + "timestamp": 3.5949360121926395 + }, + { + "x": 2.0480431107826926, + "y": 4.875068293638133, + "heading": -3.6607387948340793, + "angularVelocity": -0.11737041949327015, + "velocityX": -0.12620765948148724, + "velocityY": -1.5872090781053572, + "timestamp": 3.6252556785885424 + }, + { + "x": 2.0483803749084473, + "y": 4.827009677886963, + "heading": -3.6607387948340793, + "angularVelocity": 1.5097575312240631e-21, + "velocityX": 0.011123609387845542, + "velocityY": -1.5850641337420555, + "timestamp": 3.6555753449844453 + }, + { + "x": 2.052256902501467, + "y": 4.7835396477807635, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300342434097995e-22, + "velocityX": 0.14088183500646628, + "velocityY": -1.5797998239936404, + "timestamp": 3.683091508390709 + }, + { + "x": 2.0597005743510723, + "y": 4.740280724780923, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300332898125506e-22, + "velocityX": 0.2705199754676443, + "velocityY": -1.5721277113074286, + "timestamp": 3.710607671796973 + }, + { + "x": 2.070705854466647, + "y": 4.69731168064185, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300341717459463e-22, + "velocityX": 0.39995692542870404, + "velocityY": -1.5615928537944752, + "timestamp": 3.738123835203237 + }, + { + "x": 2.0852637266783254, + "y": 4.654727566618508, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300339024855326e-22, + "velocityX": 0.52906620725952, + "velocityY": -1.5476036173578895, + "timestamp": 3.765639998609501 + }, + { + "x": 2.1033596031452872, + "y": 4.612645056001655, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300340767321184e-22, + "velocityX": 0.6576453337547263, + "velocityY": -1.529374208007212, + "timestamp": 3.793156162015765 + }, + { + "x": 2.124969679557945, + "y": 4.571210215598577, + "heading": -3.6607387948340793, + "angularVelocity": 1.030034450352898e-22, + "velocityX": 0.7853593574649949, + "velocityY": -1.5058363984587355, + "timestamp": 3.820672325422029 + }, + { + "x": 2.1500542877457156, + "y": 4.530610063288077, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300340617397168e-22, + "velocityX": 0.9116317495799002, + "velocityY": -1.4755019335743174, + "timestamp": 3.848188488828293 + }, + { + "x": 2.178545140467357, + "y": 4.491089989654474, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300319327454968e-22, + "velocityX": 1.0354224279373974, + "velocityY": -1.4362494163925468, + "timestamp": 3.875704652234557 + }, + { + "x": 2.21031955626508, + "y": 4.452979625452379, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300351516928058e-22, + "velocityX": 1.1547545829188868, + "velocityY": -1.3850173674076385, + "timestamp": 3.903220815640821 + }, + { + "x": 2.245146390808226, + "y": 4.416727163520435, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300321637684974e-22, + "velocityX": 1.2656864268803893, + "velocityY": -1.317496970659786, + "timestamp": 3.9307369790470847 + }, + { + "x": 2.282575631489174, + "y": 4.382923484071245, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300349373445067e-22, + "velocityX": 1.3602637885358229, + "velocityY": -1.2285026422508498, + "timestamp": 3.9582531424533487 + }, + { + "x": 2.321764544418595, + "y": 4.352229884876951, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300341066851439e-22, + "velocityX": 1.4242142827403603, + "velocityY": -1.1154752478066263, + "timestamp": 3.9857693058596126 + }, + { + "x": 2.3614329153831446, + "y": 4.325077353660923, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300333549099457e-22, + "velocityX": 1.4416388788967012, + "velocityY": -0.9867847786456208, + "timestamp": 4.013285469265877 + }, + { + "x": 2.4002641272952094, + "y": 4.301398755593006, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300340140508024e-22, + "velocityX": 1.4112146137068142, + "velocityY": -0.8605341419991839, + "timestamp": 4.0408016326721405 + }, + { + "x": 2.4373189641856667, + "y": 4.28082072949791, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300338693239079e-22, + "velocityX": 1.3466571027123986, + "velocityY": -0.7478523001647727, + "timestamp": 4.0683177960784045 + }, + { + "x": 2.472031674195157, + "y": 4.262941500153538, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300337163704035e-22, + "velocityX": 1.2615388816018003, + "velocityY": -0.649771884270625, + "timestamp": 4.095833959484668 + }, + { + "x": 2.504066706947767, + "y": 4.247428515210111, + "heading": -3.6607387948340793, + "angularVelocity": 1.030033511464939e-22, + "velocityX": 1.1642259961762114, + "velocityY": -0.5637771775946641, + "timestamp": 4.123350122890932 + }, + { + "x": 2.5332162501951645, + "y": 4.234023378166114, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300349841571229e-22, + "velocityX": 1.0593607407040928, + "velocityY": -0.4871731878490917, + "timestamp": 4.150866286297196 + }, + { + "x": 2.5593444084951305, + "y": 4.222526200447489, + "heading": -3.6607387948340793, + "angularVelocity": 1.030032428740828e-22, + "velocityX": 0.9495567355890002, + "velocityY": -0.4178336037939713, + "timestamp": 4.17838244970346 + }, + { + "x": 2.582357728690195, + "y": 4.212780268556745, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300353293396209e-22, + "velocityX": 0.8363564300460482, + "velocityY": -0.35418934488933373, + "timestamp": 4.205898613109724 + }, + { + "x": 2.6021891216432027, + "y": 4.204660503766347, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300328136841497e-22, + "velocityX": 0.7207179525806894, + "velocityY": -0.2950907316007032, + "timestamp": 4.233414776515988 + }, + { + "x": 2.6187886631305672, + "y": 4.198065277647973, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300337513905507e-22, + "velocityX": 0.6032651152076492, + "velocityY": -0.23968552668501003, + "timestamp": 4.260930939922252 + }, + { + "x": 2.632118072440845, + "y": 4.192910648265756, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300328350956759e-22, + "velocityX": 0.48442106966299653, + "velocityY": -0.1873309627548638, + "timestamp": 4.288447103328516 + }, + { + "x": 2.6421472503261962, + "y": 4.189126258673718, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300343744876286e-22, + "velocityX": 0.36448314895028716, + "velocityY": -0.1375333303617742, + "timestamp": 4.31596326673478 + }, + { + "x": 2.648852022726871, + "y": 4.186652370386277, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300326335422924e-22, + "velocityX": 0.2436666878916012, + "velocityY": -0.08990673048827487, + "timestamp": 4.343479430141044 + }, + { + "x": 2.652212619781494, + "y": 4.1854376792907715, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300339610415066e-22, + "velocityX": 0.12213174507630833, + "velocityY": -0.04414463882810916, + "timestamp": 4.370995593547308 + }, + { + "x": 2.652212619781494, + "y": 4.1854376792907715, + "heading": -3.6607387948340793, + "angularVelocity": 3.4334501124721124e-23, + "velocityX": 3.058092897395823e-21, + "velocityY": 8.734750320006533e-21, + "timestamp": 4.398511756953572 + } + ], + "trajectoryWaypoints": [ + { + "timestamp": 0, + "isStopPoint": true, + "x": 0.3817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 0.8689226829252246, + "isStopPoint": false, + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 22 + }, + { + "timestamp": 1.4362746539258762, + "isStopPoint": true, + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "timestamp": 2.1953765834577417, + "isStopPoint": false, + "x": 2.0986995697021484, + "y": 6.135313034057617, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 24 + }, + { + "timestamp": 2.897583685086873, + "isStopPoint": true, + "x": 2.6396329402923584, + "y": 5.556640625, + "heading": -3.1349260454664436, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 25 + }, + { + "timestamp": 3.6555753449844453, + "isStopPoint": false, + "x": 2.0483803749084473, + "y": 4.827009677886963, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 27 + }, + { + "timestamp": 4.398511756953572, + "isStopPoint": true, + "x": 2.652212619781494, + "y": 4.1854376792907715, + "heading": -3.6607387948340793, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + "last" + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 2 + ], + "type": "StopPoint", + "direction": 0 + }, + { + "scope": [ + 1, + 2 + ], + "type": "ZeroAngularVelocity", + "direction": 0 + }, + { + "scope": [ + 3, + 4 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 5, + 6 + ], + "type": "ZeroAngularVelocity" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [], + "eventMarkers": [], + "isTrajectoryStale": false } }, "splitTrajectoriesAtStopPoints": true, diff --git a/src/main/deploy/choreo/AmpWing3.1.traj b/src/main/deploy/choreo/AmpWing3.1.traj index 7fc5db33..9110e39b 100644 --- a/src/main/deploy/choreo/AmpWing3.1.traj +++ b/src/main/deploy/choreo/AmpWing3.1.traj @@ -4,172 +4,172 @@ "x": 0.875186562538147, "y": 6.525386810302734, "heading": -2.143762813465651, - "angularVelocity": -5.355945535245033e-29, - "velocityX": 2.43610406010345e-28, - "velocityY": -8.419270817143307e-28, + "angularVelocity": 7.837232067421535e-25, + "velocityX": -1.3869968870640015e-24, + "velocityY": -1.5166283413174535e-25, "timestamp": 0 }, { - "x": 0.8966416033945684, - "y": 6.537208607893962, - "heading": -2.364630778366916, - "angularVelocity": -2.838662406313846, - "velocityX": 0.27574672466546707, - "velocityY": 0.15193734597170733, - "timestamp": 0.07780705603103885 - }, - { - "x": 0.9545363231559146, - "y": 6.574628260062144, - "heading": -2.5677938220576824, - "angularVelocity": -2.611113362388633, - "velocityX": 0.7440805849054467, - "velocityY": 0.48092877531896794, - "timestamp": 0.1556141120620777 - }, - { - "x": 1.0367704734566419, - "y": 6.613390261480878, - "heading": -2.7011163551758717, - "angularVelocity": -1.7135018328543843, - "velocityX": 1.0568983649493484, - "velocityY": 0.49818105704026555, - "timestamp": 0.23342116809311655 - }, - { - "x": 1.1598322354696735, - "y": 6.642210050914376, - "heading": -2.701117699890818, - "angularVelocity": -0.000017282686365776702, - "velocityX": 1.5816272750885192, - "velocityY": 0.37040071818167397, - "timestamp": 0.3112282241241554 - }, - { - "x": 1.2828941160720575, - "y": 6.671029334799358, - "heading": -2.7011190439216617, - "angularVelocity": -0.00001727389406811502, - "velocityX": 1.5816287992350209, - "velocityY": 0.37039422071804506, - "timestamp": 0.3890352801551943 - }, - { - "x": 1.4059559966738584, - "y": 6.699848618684591, - "heading": -2.7011203879554953, - "angularVelocity": -0.000017273932497369342, - "velocityX": 1.5816287992275255, - "velocityY": 0.3703942207212755, - "timestamp": 0.46684233618623316 - }, - { - "x": 1.5290178772749528, - "y": 6.728667902570602, - "heading": -2.7011217319923193, - "angularVelocity": -0.00001727397092835146, - "velocityX": 1.5816287992184466, - "velocityY": 0.3703942207312666, - "timestamp": 0.544649392217272 - }, - { - "x": 1.6520797578753408, - "y": 6.75748718645739, - "heading": -2.7011230760321334, - "angularVelocity": -0.000017274009359396956, - "velocityX": 1.5816287992093674, - "velocityY": 0.37039422074125794, - "timestamp": 0.6224564482483109 - }, - { - "x": 1.7751416384750223, - "y": 6.786306470344956, - "heading": -2.701124420074938, - "angularVelocity": -0.000017274047791505146, - "velocityX": 1.5816287992002884, - "velocityY": 0.37039422075124934, - "timestamp": 0.7002635042793498 - }, - { - "x": 1.898203519073992, - "y": 6.81512575423332, - "heading": -2.7011257641207327, - "angularVelocity": -0.000017274086223033065, - "velocityX": 1.581628799191142, - "velocityY": 0.3703942207615267, - "timestamp": 0.7780705603103887 - }, - { - "x": 2.021265394654385, - "y": 6.843945059515562, - "heading": -2.7011271081971797, - "angularVelocity": -0.000017274480179424693, - "velocityX": 1.581628734690864, - "velocityY": 0.37039449572214483, - "timestamp": 0.8558776163414276 - }, - { - "x": 2.1197062132467392, - "y": 6.880631402807114, - "heading": -2.784861574958977, - "angularVelocity": -1.0761808894092335, - "velocityX": 1.2651914056879914, - "velocityY": 0.4715040661211618, - "timestamp": 0.9336846723724664 - }, - { - "x": 2.228161206604935, - "y": 6.914841049698727, - "heading": -2.8360417692393636, - "angularVelocity": -0.6577834568110343, - "velocityX": 1.393896632137462, - "velocityY": 0.43967280908258105, - "timestamp": 1.0114917284035052 - }, - { - "x": 2.342602667471001, - "y": 6.947096322596154, - "heading": -2.866867411656533, - "angularVelocity": -0.39618055211949504, - "velocityX": 1.4708365372468657, - "velocityY": 0.4145545988086245, - "timestamp": 1.089298784434544 - }, - { - "x": 2.460575781873871, - "y": 6.9780078455456644, - "heading": -2.885328334561652, - "angularVelocity": -0.23726540813669628, - "velocityX": 1.5162264249634116, - "velocityY": 0.39728431489785143, - "timestamp": 1.167105840465583 - }, - { - "x": 2.563184602851941, - "y": 6.995518176666575, - "heading": -2.9725697719134425, - "angularVelocity": -1.121253544369907, - "velocityX": 1.3187598427723222, - "velocityY": 0.22504811278202433, - "timestamp": 1.2449128964966218 + "x": 0.8969727039190931, + "y": 6.531365834673865, + "heading": -2.166346916414201, + "angularVelocity": -0.3175047636593087, + "velocityX": 0.3062864035806097, + "velocityY": 0.08405774292624062, + "timestamp": 0.07112996569961132 + }, + { + "x": 0.9406148218819743, + "y": 6.543392626223336, + "heading": -2.210869956131151, + "angularVelocity": -0.6259392828189381, + "velocityX": 0.6135546043588166, + "velocityY": 0.16908192533455152, + "timestamp": 0.14225993139922263 + }, + { + "x": 1.0062172991621152, + "y": 6.561523867966903, + "heading": -2.2764288840652025, + "angularVelocity": -0.9216780479118013, + "velocityX": 0.9222902982575069, + "velocityY": 0.25490300136143174, + "timestamp": 0.21338989709883394 + }, + { + "x": 1.093969891095599, + "y": 6.585785930725148, + "heading": -2.36133395682105, + "angularVelocity": -1.1936610951621995, + "velocityX": 1.2336937192416417, + "velocityY": 0.3410948187534151, + "timestamp": 0.28451986279844527 + }, + { + "x": 1.2042232731861569, + "y": 6.6162032972801414, + "heading": -2.461705007120923, + "angularVelocity": -1.4110937537035995, + "velocityX": 1.5500272073259296, + "velocityY": 0.42763083400670887, + "timestamp": 0.3556498284980566 + }, + { + "x": 1.3374731837843992, + "y": 6.652906460035935, + "heading": -2.567532529869651, + "angularVelocity": -1.4878050580770381, + "velocityX": 1.8733301680612304, + "velocityY": 0.5160014122710946, + "timestamp": 0.4267797941976679 + }, + { + "x": 1.4887376373930754, + "y": 6.6960750532748765, + "heading": -2.6261176444741485, + "angularVelocity": -0.8236347934133412, + "velocityX": 2.1265925284918663, + "velocityY": 0.6068974280297831, + "timestamp": 0.49790975989727926 + }, + { + "x": 1.6559451101368896, + "y": 6.7447998856875095, + "heading": -2.6266322508442816, + "angularVelocity": -0.007234733843487786, + "velocityX": 2.3507318061975107, + "velocityY": 0.685011330082775, + "timestamp": 0.5690397255968905 + }, + { + "x": 1.8243976661229355, + "y": 6.7896009456539, + "heading": -2.626632286559664, + "angularVelocity": -5.021144321983056e-7, + "velocityX": 2.368236148143772, + "velocityY": 0.6298479062339171, + "timestamp": 0.6401696912965018 + }, + { + "x": 1.9921751026428776, + "y": 6.83686703905938, + "heading": -2.6266327970809837, + "angularVelocity": -0.000007177303057357513, + "velocityX": 2.358744797213626, + "velocityY": 0.6645032503613142, + "timestamp": 0.7112996569961131 + }, + { + "x": 2.144093876666576, + "y": 6.878851502304941, + "heading": -2.6858581481034687, + "angularVelocity": -0.8326357315087016, + "velocityX": 2.135791470296306, + "velocityY": 0.5902500139373705, + "timestamp": 0.7824296226957244 + }, + { + "x": 2.277702419689923, + "y": 6.914840816688254, + "heading": -2.79218877780944, + "angularVelocity": -1.4948781242917446, + "velocityX": 1.8783720997081357, + "velocityY": 0.505965580460125, + "timestamp": 0.8535595883953356 + }, + { + "x": 2.3883482703840326, + "y": 6.944322663737028, + "heading": -2.894178661554579, + "angularVelocity": -1.4338525646961797, + "velocityX": 1.5555448341051872, + "velocityY": 0.41447857817447836, + "timestamp": 0.9246895540949469 + }, + { + "x": 2.4764066005629384, + "y": 6.967574702744683, + "heading": -2.9819793987887278, + "angularVelocity": -1.2343705830667766, + "velocityX": 1.2379920236540676, + "velocityY": 0.3268951247052539, + "timestamp": 0.9958195197945582 + }, + { + "x": 2.5421906012565776, + "y": 6.984807873214321, + "heading": -3.0509427511393503, + "angularVelocity": -0.9695400760048395, + "velocityX": 0.9248422946167467, + "velocityY": 0.24227722170451613, + "timestamp": 1.0669494854941695 + }, + { + "x": 2.5859077991162414, + "y": 6.996185319772124, + "heading": -3.098476979135992, + "angularVelocity": -0.6682728935563261, + "velocityX": 0.6146101355409838, + "velocityY": 0.1599529318747363, + "timestamp": 1.1380794511937808 }, { "x": 2.6077051162719727, "y": 7.001829624176025, "heading": -3.122903212305612, - "angularVelocity": -1.9321311981293672, - "velocityX": 0.5721912084974853, - "velocityY": 0.08111664714486487, - "timestamp": 1.3227199525276607 + "angularVelocity": -0.34340285320499864, + "velocityX": 0.30644352125493224, + "velocityY": 0.0793519910825985, + "timestamp": 1.2092094168933922 }, { "x": 2.6077051162719727, "y": 7.001829624176025, "heading": -3.122903212305612, - "angularVelocity": -5.850460431476152e-29, - "velocityX": -4.4073165345897844e-27, - "velocityY": 1.309519926897454e-26, - "timestamp": 1.4005270085586996 + "angularVelocity": -1.2711795884725035e-24, + "velocityX": 1.0813970849471872e-24, + "velocityY": 4.939088142936104e-25, + "timestamp": 1.2803393825930036 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/AmpWing3.2.traj b/src/main/deploy/choreo/AmpWing3.2.traj index 70c64142..85e64e95 100644 --- a/src/main/deploy/choreo/AmpWing3.2.traj +++ b/src/main/deploy/choreo/AmpWing3.2.traj @@ -4,91 +4,109 @@ "x": 2.6077051162719727, "y": 7.001829624176025, "heading": -3.122903212305612, - "angularVelocity": -5.850460431476152e-29, - "velocityX": -4.4073165345897844e-27, - "velocityY": 1.309519926897454e-26, + "angularVelocity": -1.2711795884725035e-24, + "velocityX": 1.0813970849471872e-24, + "velocityY": 4.939088142936104e-25, "timestamp": 0 }, { - "x": 2.581564908929706, - "y": 6.99268752896088, - "heading": -2.9753776937459495, - "angularVelocity": 2.140746378487904, - "velocityX": -0.37932118285182687, - "velocityY": -0.1326611654355847, - "timestamp": 0.068913123031354 + "x": 2.588497605107588, + "y": 6.99102525005929, + "heading": -3.105999231961874, + "angularVelocity": 0.2434967604471089, + "velocityX": -0.27667843014925436, + "velocityY": -0.15563376450908756, + "timestamp": 0.06942178743035177 }, { - "x": 2.510121983919206, - "y": 6.965703523715206, - "heading": -2.8481100670568136, - "angularVelocity": 1.846783618139497, - "velocityX": -1.0367100178872335, - "velocityY": -0.3915655546969944, - "timestamp": 0.137826246062708 + "x": 2.550087465438114, + "y": 6.9694228225612935, + "heading": -3.0720638130010824, + "angularVelocity": 0.4888295190445427, + "velocityX": -0.5532865270576512, + "velocityY": -0.31117648072184184, + "timestamp": 0.13884357486070353 }, { - "x": 2.4183443604796597, - "y": 6.911820320186112, - "heading": -2.82871535814287, - "angularVelocity": 0.2814370915263766, - "velocityX": -1.3317873200694863, - "velocityY": -0.7819004735074551, - "timestamp": 0.206739369094062 + "x": 2.4924860648996834, + "y": 6.937030311414511, + "heading": -3.0208719743457277, + "angularVelocity": 0.7374030624998452, + "velocityX": -0.8297308765813584, + "velocityY": -0.46660439533166076, + "timestamp": 0.2082653622910553 }, { - "x": 2.324659626384285, - "y": 6.850545155910229, - "heading": -2.828714867091888, - "angularVelocity": 0.000007125652715720525, - "velocityX": -1.3594614490588457, - "velocityY": -0.8891653952180393, - "timestamp": 0.275652492125416 + "x": 2.415713178984927, + "y": 6.893852911543239, + "heading": -2.9521086467399438, + "angularVelocity": 0.9905150839680044, + "velocityX": -1.1058903660724633, + "velocityY": -0.621957478617077, + "timestamp": 0.27768714972140707 }, { - "x": 2.2309748959640565, - "y": 6.7892699860148875, - "heading": -2.8287143760416544, - "angularVelocity": 0.000007125641845880462, - "velocityX": -1.359461395728709, - "velocityY": -0.8891654767621404, - "timestamp": 0.34456561515677 + "x": 2.3197977520281423, + "y": 6.8398868515705145, + "heading": -2.865405613995347, + "angularVelocity": 1.248931149051482, + "velocityX": -1.3816329211202274, + "velocityY": -0.7773648874550575, + "timestamp": 0.34710893715175883 }, { - "x": 2.136919509671741, - "y": 6.728565387611802, - "heading": -2.828713709965057, - "angularVelocity": 0.000009665453660450363, - "velocityX": -1.3648399920799144, - "velocityY": -0.8808858999970965, - "timestamp": 0.413478738188124 + "x": 2.2244467831783776, + "y": 6.785620081909038, + "heading": -2.773269561572217, + "angularVelocity": 1.327192165940218, + "velocityX": -1.3735020716000237, + "velocityY": -0.781696520215938, + "timestamp": 0.4165307245821106 }, { - "x": 2.062260898631176, - "y": 6.694685917301612, - "heading": -2.7155906116912156, - "angularVelocity": 1.6415320231876958, - "velocityX": -1.0833729158754979, - "velocityY": -0.491625815518109, - "timestamp": 0.48239186121947797 + "x": 2.14818394170963, + "y": 6.7421795937838125, + "heading": -2.6995256157206273, + "angularVelocity": 1.0622593940781788, + "velocityX": -1.0985433289982525, + "velocityY": -0.6257471859077053, + "timestamp": 0.48595251201246237 + }, + { + "x": 2.090994438671986, + "y": 6.709584746202325, + "heading": -2.6442247517444515, + "angularVelocity": 0.7965923382721457, + "velocityX": -0.8237976167787333, + "velocityY": -0.46951899091028315, + "timestamp": 0.5553742994428141 + }, + { + "x": 2.052870310008216, + "y": 6.687849855649161, + "heading": -2.607365454456659, + "angularVelocity": 0.5309471082802596, + "velocityX": -0.5491666244119513, + "velocityY": -0.3130845712517861, + "timestamp": 0.6247960868731659 }, { "x": 2.03380823135376, "y": 6.676982402801514, "heading": -2.588935989400422, - "angularVelocity": 1.8378882964449172, - "velocityX": -0.4128773450663434, - "velocityY": -0.256896128362137, - "timestamp": 0.551304984250832 + "angularVelocity": 0.2654709096150401, + "velocityX": -0.2745835185183043, + "velocityY": -0.1565423946848157, + "timestamp": 0.6942178743035177 }, { "x": 2.03380823135376, "y": 6.676982402801514, "heading": -2.588935989400422, - "angularVelocity": -3.9702743259995845e-28, - "velocityX": -1.31352865617337e-26, - "velocityY": 2.3602776479280726e-26, - "timestamp": 0.620218107282186 + "angularVelocity": -5.507504748429065e-28, + "velocityX": -9.813305241361081e-27, + "velocityY": 1.6191757102806756e-26, + "timestamp": 0.7636396617338694 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/AmpWing3.traj b/src/main/deploy/choreo/AmpWing3.traj index e637d35f..a7e02769 100644 --- a/src/main/deploy/choreo/AmpWing3.traj +++ b/src/main/deploy/choreo/AmpWing3.traj @@ -4,253 +4,271 @@ "x": 0.875186562538147, "y": 6.525386810302734, "heading": -2.143762813465651, - "angularVelocity": -5.355945535245033e-29, - "velocityX": 2.43610406010345e-28, - "velocityY": -8.419270817143307e-28, + "angularVelocity": 7.837232067421535e-25, + "velocityX": -1.3869968870640015e-24, + "velocityY": -1.5166283413174535e-25, "timestamp": 0 }, { - "x": 0.8966416033945684, - "y": 6.537208607893962, - "heading": -2.364630778366916, - "angularVelocity": -2.838662406313846, - "velocityX": 0.27574672466546707, - "velocityY": 0.15193734597170733, - "timestamp": 0.07780705603103885 + "x": 0.8969727039190931, + "y": 6.531365834673865, + "heading": -2.166346916414201, + "angularVelocity": -0.3175047636593087, + "velocityX": 0.3062864035806097, + "velocityY": 0.08405774292624062, + "timestamp": 0.07112996569961132 }, { - "x": 0.9545363231559146, - "y": 6.574628260062144, - "heading": -2.5677938220576824, - "angularVelocity": -2.611113362388633, - "velocityX": 0.7440805849054467, - "velocityY": 0.48092877531896794, - "timestamp": 0.1556141120620777 + "x": 0.9406148218819743, + "y": 6.543392626223336, + "heading": -2.210869956131151, + "angularVelocity": -0.6259392828189381, + "velocityX": 0.6135546043588166, + "velocityY": 0.16908192533455152, + "timestamp": 0.14225993139922263 }, { - "x": 1.0367704734566419, - "y": 6.613390261480878, - "heading": -2.7011163551758717, - "angularVelocity": -1.7135018328543843, - "velocityX": 1.0568983649493484, - "velocityY": 0.49818105704026555, - "timestamp": 0.23342116809311655 + "x": 1.0062172991621152, + "y": 6.561523867966903, + "heading": -2.2764288840652025, + "angularVelocity": -0.9216780479118013, + "velocityX": 0.9222902982575069, + "velocityY": 0.25490300136143174, + "timestamp": 0.21338989709883394 }, { - "x": 1.1598322354696735, - "y": 6.642210050914376, - "heading": -2.701117699890818, - "angularVelocity": -0.000017282686365776702, - "velocityX": 1.5816272750885192, - "velocityY": 0.37040071818167397, - "timestamp": 0.3112282241241554 + "x": 1.093969891095599, + "y": 6.585785930725148, + "heading": -2.36133395682105, + "angularVelocity": -1.1936610951621995, + "velocityX": 1.2336937192416417, + "velocityY": 0.3410948187534151, + "timestamp": 0.28451986279844527 }, { - "x": 1.2828941160720575, - "y": 6.671029334799358, - "heading": -2.7011190439216617, - "angularVelocity": -0.00001727389406811502, - "velocityX": 1.5816287992350209, - "velocityY": 0.37039422071804506, - "timestamp": 0.3890352801551943 + "x": 1.2042232731861569, + "y": 6.6162032972801414, + "heading": -2.461705007120923, + "angularVelocity": -1.4110937537035995, + "velocityX": 1.5500272073259296, + "velocityY": 0.42763083400670887, + "timestamp": 0.3556498284980566 }, { - "x": 1.4059559966738584, - "y": 6.699848618684591, - "heading": -2.7011203879554953, - "angularVelocity": -0.000017273932497369342, - "velocityX": 1.5816287992275255, - "velocityY": 0.3703942207212755, - "timestamp": 0.46684233618623316 + "x": 1.3374731837843992, + "y": 6.652906460035935, + "heading": -2.567532529869651, + "angularVelocity": -1.4878050580770381, + "velocityX": 1.8733301680612304, + "velocityY": 0.5160014122710946, + "timestamp": 0.4267797941976679 }, { - "x": 1.5290178772749528, - "y": 6.728667902570602, - "heading": -2.7011217319923193, - "angularVelocity": -0.00001727397092835146, - "velocityX": 1.5816287992184466, - "velocityY": 0.3703942207312666, - "timestamp": 0.544649392217272 + "x": 1.4887376373930754, + "y": 6.6960750532748765, + "heading": -2.6261176444741485, + "angularVelocity": -0.8236347934133412, + "velocityX": 2.1265925284918663, + "velocityY": 0.6068974280297831, + "timestamp": 0.49790975989727926 }, { - "x": 1.6520797578753408, - "y": 6.75748718645739, - "heading": -2.7011230760321334, - "angularVelocity": -0.000017274009359396956, - "velocityX": 1.5816287992093674, - "velocityY": 0.37039422074125794, - "timestamp": 0.6224564482483109 + "x": 1.6559451101368896, + "y": 6.7447998856875095, + "heading": -2.6266322508442816, + "angularVelocity": -0.007234733843487786, + "velocityX": 2.3507318061975107, + "velocityY": 0.685011330082775, + "timestamp": 0.5690397255968905 }, { - "x": 1.7751416384750223, - "y": 6.786306470344956, - "heading": -2.701124420074938, - "angularVelocity": -0.000017274047791505146, - "velocityX": 1.5816287992002884, - "velocityY": 0.37039422075124934, - "timestamp": 0.7002635042793498 + "x": 1.8243976661229355, + "y": 6.7896009456539, + "heading": -2.626632286559664, + "angularVelocity": -5.021144321983056e-7, + "velocityX": 2.368236148143772, + "velocityY": 0.6298479062339171, + "timestamp": 0.6401696912965018 }, { - "x": 1.898203519073992, - "y": 6.81512575423332, - "heading": -2.7011257641207327, - "angularVelocity": -0.000017274086223033065, - "velocityX": 1.581628799191142, - "velocityY": 0.3703942207615267, - "timestamp": 0.7780705603103887 + "x": 1.9921751026428776, + "y": 6.83686703905938, + "heading": -2.6266327970809837, + "angularVelocity": -0.000007177303057357513, + "velocityX": 2.358744797213626, + "velocityY": 0.6645032503613142, + "timestamp": 0.7112996569961131 }, { - "x": 2.021265394654385, - "y": 6.843945059515562, - "heading": -2.7011271081971797, - "angularVelocity": -0.000017274480179424693, - "velocityX": 1.581628734690864, - "velocityY": 0.37039449572214483, - "timestamp": 0.8558776163414276 + "x": 2.144093876666576, + "y": 6.878851502304941, + "heading": -2.6858581481034687, + "angularVelocity": -0.8326357315087016, + "velocityX": 2.135791470296306, + "velocityY": 0.5902500139373705, + "timestamp": 0.7824296226957244 }, { - "x": 2.1197062132467392, - "y": 6.880631402807114, - "heading": -2.784861574958977, - "angularVelocity": -1.0761808894092335, - "velocityX": 1.2651914056879914, - "velocityY": 0.4715040661211618, - "timestamp": 0.9336846723724664 + "x": 2.277702419689923, + "y": 6.914840816688254, + "heading": -2.79218877780944, + "angularVelocity": -1.4948781242917446, + "velocityX": 1.8783720997081357, + "velocityY": 0.505965580460125, + "timestamp": 0.8535595883953356 }, { - "x": 2.228161206604935, - "y": 6.914841049698727, - "heading": -2.8360417692393636, - "angularVelocity": -0.6577834568110343, - "velocityX": 1.393896632137462, - "velocityY": 0.43967280908258105, - "timestamp": 1.0114917284035052 + "x": 2.3883482703840326, + "y": 6.944322663737028, + "heading": -2.894178661554579, + "angularVelocity": -1.4338525646961797, + "velocityX": 1.5555448341051872, + "velocityY": 0.41447857817447836, + "timestamp": 0.9246895540949469 }, { - "x": 2.342602667471001, - "y": 6.947096322596154, - "heading": -2.866867411656533, - "angularVelocity": -0.39618055211949504, - "velocityX": 1.4708365372468657, - "velocityY": 0.4145545988086245, - "timestamp": 1.089298784434544 + "x": 2.4764066005629384, + "y": 6.967574702744683, + "heading": -2.9819793987887278, + "angularVelocity": -1.2343705830667766, + "velocityX": 1.2379920236540676, + "velocityY": 0.3268951247052539, + "timestamp": 0.9958195197945582 }, { - "x": 2.460575781873871, - "y": 6.9780078455456644, - "heading": -2.885328334561652, - "angularVelocity": -0.23726540813669628, - "velocityX": 1.5162264249634116, - "velocityY": 0.39728431489785143, - "timestamp": 1.167105840465583 + "x": 2.5421906012565776, + "y": 6.984807873214321, + "heading": -3.0509427511393503, + "angularVelocity": -0.9695400760048395, + "velocityX": 0.9248422946167467, + "velocityY": 0.24227722170451613, + "timestamp": 1.0669494854941695 }, { - "x": 2.563184602851941, - "y": 6.995518176666575, - "heading": -2.9725697719134425, - "angularVelocity": -1.121253544369907, - "velocityX": 1.3187598427723222, - "velocityY": 0.22504811278202433, - "timestamp": 1.2449128964966218 + "x": 2.5859077991162414, + "y": 6.996185319772124, + "heading": -3.098476979135992, + "angularVelocity": -0.6682728935563261, + "velocityX": 0.6146101355409838, + "velocityY": 0.1599529318747363, + "timestamp": 1.1380794511937808 }, { "x": 2.6077051162719727, "y": 7.001829624176025, "heading": -3.122903212305612, - "angularVelocity": -1.9321311981293672, - "velocityX": 0.5721912084974853, - "velocityY": 0.08111664714486487, - "timestamp": 1.3227199525276607 + "angularVelocity": -0.34340285320499864, + "velocityX": 0.30644352125493224, + "velocityY": 0.0793519910825985, + "timestamp": 1.2092094168933922 }, { "x": 2.6077051162719727, "y": 7.001829624176025, "heading": -3.122903212305612, - "angularVelocity": -5.850460431476152e-29, - "velocityX": -4.4073165345897844e-27, - "velocityY": 1.309519926897454e-26, - "timestamp": 1.4005270085586996 + "angularVelocity": -1.2711795884725035e-24, + "velocityX": 1.0813970849471872e-24, + "velocityY": 4.939088142936104e-25, + "timestamp": 1.2803393825930036 }, { - "x": 2.581564908929706, - "y": 6.99268752896088, - "heading": -2.9753776937459495, - "angularVelocity": 2.140746378487904, - "velocityX": -0.37932118285182687, - "velocityY": -0.1326611654355847, - "timestamp": 1.4694401315900536 + "x": 2.588497605107588, + "y": 6.99102525005929, + "heading": -3.105999231961874, + "angularVelocity": 0.2434967604471089, + "velocityX": -0.27667843014925436, + "velocityY": -0.15563376450908756, + "timestamp": 1.3497611700233554 }, { - "x": 2.510121983919206, - "y": 6.965703523715206, - "heading": -2.8481100670568136, - "angularVelocity": 1.846783618139497, - "velocityX": -1.0367100178872335, - "velocityY": -0.3915655546969944, - "timestamp": 1.5383532546214076 + "x": 2.550087465438114, + "y": 6.9694228225612935, + "heading": -3.0720638130010824, + "angularVelocity": 0.4888295190445427, + "velocityX": -0.5532865270576512, + "velocityY": -0.31117648072184184, + "timestamp": 1.4191829574537072 }, { - "x": 2.4183443604796597, - "y": 6.911820320186112, - "heading": -2.82871535814287, - "angularVelocity": 0.2814370915263766, - "velocityX": -1.3317873200694863, - "velocityY": -0.7819004735074551, - "timestamp": 1.6072663776527616 + "x": 2.4924860648996834, + "y": 6.937030311414511, + "heading": -3.0208719743457277, + "angularVelocity": 0.7374030624998452, + "velocityX": -0.8297308765813584, + "velocityY": -0.46660439533166076, + "timestamp": 1.488604744884059 }, { - "x": 2.324659626384285, - "y": 6.850545155910229, - "heading": -2.828714867091888, - "angularVelocity": 0.000007125652715720525, - "velocityX": -1.3594614490588457, - "velocityY": -0.8891653952180393, - "timestamp": 1.6761795006841156 + "x": 2.415713178984927, + "y": 6.893852911543239, + "heading": -2.9521086467399438, + "angularVelocity": 0.9905150839680044, + "velocityX": -1.1058903660724633, + "velocityY": -0.621957478617077, + "timestamp": 1.5580265323144107 }, { - "x": 2.2309748959640565, - "y": 6.7892699860148875, - "heading": -2.8287143760416544, - "angularVelocity": 0.000007125641845880462, - "velocityX": -1.359461395728709, - "velocityY": -0.8891654767621404, - "timestamp": 1.7450926237154696 + "x": 2.3197977520281423, + "y": 6.8398868515705145, + "heading": -2.865405613995347, + "angularVelocity": 1.248931149051482, + "velocityX": -1.3816329211202274, + "velocityY": -0.7773648874550575, + "timestamp": 1.6274483197447625 }, { - "x": 2.136919509671741, - "y": 6.728565387611802, - "heading": -2.828713709965057, - "angularVelocity": 0.000009665453660450363, - "velocityX": -1.3648399920799144, - "velocityY": -0.8808858999970965, - "timestamp": 1.8140057467468236 + "x": 2.2244467831783776, + "y": 6.785620081909038, + "heading": -2.773269561572217, + "angularVelocity": 1.327192165940218, + "velocityX": -1.3735020716000237, + "velocityY": -0.781696520215938, + "timestamp": 1.6968701071751142 }, { - "x": 2.062260898631176, - "y": 6.694685917301612, - "heading": -2.7155906116912156, - "angularVelocity": 1.6415320231876958, - "velocityX": -1.0833729158754979, - "velocityY": -0.491625815518109, - "timestamp": 1.8829188697781776 + "x": 2.14818394170963, + "y": 6.7421795937838125, + "heading": -2.6995256157206273, + "angularVelocity": 1.0622593940781788, + "velocityX": -1.0985433289982525, + "velocityY": -0.6257471859077053, + "timestamp": 1.766291894605466 + }, + { + "x": 2.090994438671986, + "y": 6.709584746202325, + "heading": -2.6442247517444515, + "angularVelocity": 0.7965923382721457, + "velocityX": -0.8237976167787333, + "velocityY": -0.46951899091028315, + "timestamp": 1.8357136820358178 + }, + { + "x": 2.052870310008216, + "y": 6.687849855649161, + "heading": -2.607365454456659, + "angularVelocity": 0.5309471082802596, + "velocityX": -0.5491666244119513, + "velocityY": -0.3130845712517861, + "timestamp": 1.9051354694661695 }, { "x": 2.03380823135376, "y": 6.676982402801514, "heading": -2.588935989400422, - "angularVelocity": 1.8378882964449172, - "velocityX": -0.4128773450663434, - "velocityY": -0.256896128362137, - "timestamp": 1.9518319928095316 + "angularVelocity": 0.2654709096150401, + "velocityX": -0.2745835185183043, + "velocityY": -0.1565423946848157, + "timestamp": 1.9745572568965213 }, { "x": 2.03380823135376, "y": 6.676982402801514, "heading": -2.588935989400422, - "angularVelocity": -3.9702743259995845e-28, - "velocityX": -1.31352865617337e-26, - "velocityY": 2.3602776479280726e-26, - "timestamp": 2.0207451158408856 + "angularVelocity": -5.507504748429065e-28, + "velocityX": -9.813305241361081e-27, + "velocityY": 1.6191757102806756e-26, + "timestamp": 2.043979044326873 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/AmpWing321.1.traj b/src/main/deploy/choreo/AmpWing321.1.traj new file mode 100644 index 00000000..c061357d --- /dev/null +++ b/src/main/deploy/choreo/AmpWing321.1.traj @@ -0,0 +1,419 @@ +{ + "samples": [ + { + "x": 0.3817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "angularVelocity": 1.8698301864234364e-22, + "velocityX": 4.800440645526857e-21, + "velocityY": 2.9477177393174924e-20, + "timestamp": 0 + }, + { + "x": 0.3883549561387046, + "y": 6.608408421186061, + "heading": -2.1197134410063434, + "angularVelocity": -0.08634729729146294, + "velocityX": 0.17378773726557262, + "velocityY": -0.026872324952015636, + "timestamp": 0.037779247083705426 + }, + { + "x": 0.4014921075538034, + "y": 6.606405430964887, + "heading": -2.126204479988474, + "angularVelocity": -0.17181493764946093, + "velocityX": 0.3477346011156235, + "velocityY": -0.05301826732320674, + "timestamp": 0.07555849416741085 + }, + { + "x": 0.4212075211916924, + "y": 6.603445687185866, + "heading": -2.1358857960695534, + "angularVelocity": -0.25626016473089336, + "velocityX": 0.5218582994574282, + "velocityY": -0.0783431118282232, + "timestamp": 0.11333774125111629 + }, + { + "x": 0.44750862695642823, + "y": 6.599564542552146, + "heading": -2.1487127600714926, + "angularVelocity": -0.3395240771611807, + "velocityX": 0.6961786640814154, + "velocityY": -0.10273218587648632, + "timestamp": 0.1511169883348217 + }, + { + "x": 0.48040369549494544, + "y": 6.594802655593878, + "heading": -2.1646339690222502, + "angularVelocity": -0.42142737560337534, + "velocityX": 0.8707179490802492, + "velocityY": -0.1260450465763573, + "timestamp": 0.18889623541852713 + }, + { + "x": 0.5199019408821504, + "y": 6.589207268746513, + "heading": -2.1835902439067887, + "angularVelocity": -0.5017642316306103, + "velocityX": 1.0455011265758887, + "velocityY": -0.1481074208540784, + "timestamp": 0.22667548250223254 + }, + { + "x": 0.5660136324357566, + "y": 6.582833919675241, + "heading": -2.205513273539415, + "angularVelocity": -0.5802929207152439, + "velocityX": 1.220556128380285, + "velocityY": -0.16869973763992252, + "timestamp": 0.26445472958593796 + }, + { + "x": 0.6187502087281199, + "y": 6.575748787450549, + "heading": -2.2303236919031626, + "angularVelocity": -0.6567208263514764, + "velocityX": 1.3959139041473307, + "velocityY": -0.1875403236323719, + "timestamp": 0.3022339766696434 + }, + { + "x": 0.6781243747498052, + "y": 6.568032000955299, + "heading": -2.2579282145890622, + "angularVelocity": -0.730679534844558, + "velocityX": 1.5716079754085093, + "velocityY": -0.2042599334549938, + "timestamp": 0.34001322375334886 + }, + { + "x": 0.7441501329753766, + "y": 6.559782463812031, + "heading": -2.2882151619643003, + "angularVelocity": -0.8016821327363354, + "velocityX": 1.7476726859930256, + "velocityY": -0.21836160802711913, + "timestamp": 0.3777924708370543 + }, + { + "x": 0.8168426216101802, + "y": 6.55112518097433, + "heading": -2.3210471231149787, + "angularVelocity": -0.8690475243705671, + "velocityX": 1.9241380981931202, + "velocityY": -0.22915445663977885, + "timestamp": 0.41557171792075975 + }, + { + "x": 0.8962174244775805, + "y": 6.54222294081638, + "heading": -2.3562483469235813, + "angularVelocity": -0.9317608614753342, + "velocityX": 2.1010160073209, + "velocityY": -0.23563836881711223, + "timestamp": 0.4533509650044652 + }, + { + "x": 0.9822884113766555, + "y": 6.5332960857123705, + "heading": -2.393581906091161, + "angularVelocity": -0.9882028375224495, + "velocityX": 2.2782610439104056, + "velocityY": -0.2362899155781206, + "timestamp": 0.49113021208817065 + }, + { + "x": 1.0750612253316763, + "y": 6.524658510256875, + "heading": -2.432705739729525, + "angularVelocity": -1.0355906127954124, + "velocityX": 2.455655448862486, + "velocityY": -0.22863281092648347, + "timestamp": 0.528909459171876 + }, + { + "x": 1.1745124212868387, + "y": 6.516789255723791, + "heading": -2.4730818457948223, + "angularVelocity": -1.0687377113640772, + "velocityX": 2.6324292735325403, + "velocityY": -0.20829569513922166, + "timestamp": 0.5666887062555814 + }, + { + "x": 1.2805135717940606, + "y": 6.510488992251309, + "heading": -2.5137740657710497, + "angularVelocity": -1.0771051070993467, + "velocityX": 2.805803680320948, + "velocityY": -0.16676519408961948, + "timestamp": 0.6044679533392868 + }, + { + "x": 1.3925030399539646, + "y": 6.5072291237539, + "heading": -2.5529426361670393, + "angularVelocity": -1.0367747750294287, + "velocityX": 2.9643118062089164, + "velocityY": -0.08628728069154737, + "timestamp": 0.6422472004229922 + }, + { + "x": 1.5081204845079395, + "y": 6.509370790524853, + "heading": -2.586975983570745, + "angularVelocity": -0.9008476883697443, + "velocityX": 3.060342740494668, + "velocityY": 0.05668897440470873, + "timestamp": 0.6800264475066976 + }, + { + "x": 1.622365364792802, + "y": 6.517488893832767, + "heading": -2.612477761707434, + "angularVelocity": -0.6750208144748423, + "velocityX": 3.0240115699429984, + "velocityY": 0.21488261240214016, + "timestamp": 0.717805694590403 + }, + { + "x": 1.7327667460189187, + "y": 6.53045723021325, + "heading": -2.629741912942711, + "angularVelocity": -0.4569744652937502, + "velocityX": 2.9222758458236795, + "velocityY": 0.34326614163931696, + "timestamp": 0.7555849416741084 + }, + { + "x": 1.8382100625927147, + "y": 6.547429672114164, + "heading": -2.6405106169347077, + "angularVelocity": -0.28504284291683973, + "velocityX": 2.7910380622508084, + "velocityY": 0.44925304793238396, + "timestamp": 0.7933641887578138 + }, + { + "x": 1.938178409904931, + "y": 6.567826322651328, + "heading": -2.6456655056562304, + "angularVelocity": -0.13644762983499997, + "velocityX": 2.646118041757629, + "velocityY": 0.5398903395804769, + "timestamp": 0.8311434358415192 + }, + { + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": -2.6456655056562304, + "angularVelocity": 1.8055348625312267e-21, + "velocityX": 2.4940831175072695, + "velocityY": 0.6203915390652321, + "timestamp": 0.8689226829252246 + }, + { + "x": 2.093877892062785, + "y": 6.608590709626909, + "heading": -2.6456655056562304, + "angularVelocity": 4.168304475470088e-23, + "velocityX": 2.3837897196813476, + "velocityY": 0.671861873409428, + "timestamp": 0.8947114088797997 + }, + { + "x": 2.1523659521196383, + "y": 6.626881977306388, + "heading": -2.6456655056562304, + "angularVelocity": 4.168273499118604e-23, + "velocityX": 2.2679701261668037, + "velocityY": 0.7092738009447922, + "timestamp": 0.9205001348343749 + }, + { + "x": 2.2077779049993467, + "y": 6.6457974470281025, + "heading": -2.6456655056562304, + "angularVelocity": 4.16827486833322e-23, + "velocityX": 2.148689042542395, + "velocityY": 0.7334782553910214, + "timestamp": 0.94628886078895 + }, + { + "x": 2.2600669512143567, + "y": 6.665028382936204, + "heading": -2.6456655056562304, + "angularVelocity": 4.168303685702792e-23, + "velocityX": 2.0275932322952634, + "velocityY": 0.7457109723785112, + "timestamp": 0.9720775867435252 + }, + { + "x": 2.309217481860067, + "y": 6.684301584296192, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682770338729235e-23, + "velocityX": 1.9058921612597222, + "velocityY": 0.7473498843667037, + "timestamp": 0.9978663126981003 + }, + { + "x": 2.3552353371432924, + "y": 6.7033787939224325, + "heading": -2.6456655056562304, + "angularVelocity": 4.168287783381093e-23, + "velocityX": 1.7844175537902998, + "velocityY": 0.7397499845392672, + "timestamp": 1.0236550386526755 + }, + { + "x": 2.3981402688351157, + "y": 6.7220537901087445, + "heading": -2.6456655056562304, + "angularVelocity": 4.168284482622447e-23, + "velocityX": 1.663708853527376, + "velocityY": 0.7241535009973167, + "timestamp": 1.0494437646072505 + }, + { + "x": 2.4379604976017806, + "y": 6.740148546732269, + "heading": -2.6456655056562304, + "angularVelocity": 4.168287413318469e-23, + "velocityX": 1.5440944557233214, + "velocityY": 0.7016537635645865, + "timestamp": 1.0752324905618256 + }, + { + "x": 2.474728960142428, + "y": 6.7575092774466885, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682752490293846e-23, + "velocityX": 1.4257572322666272, + "velocityY": 0.6731907091840713, + "timestamp": 1.1010212165164006 + }, + { + "x": 2.508480806878266, + "y": 6.774002770496258, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682856288215976e-23, + "velocityX": 1.3087830238384508, + "velocityY": 0.6395621512623721, + "timestamp": 1.1268099424709757 + }, + { + "x": 2.5392517800354852, + "y": 6.789513173699484, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682970034902383e-23, + "velocityX": 1.1931947786603707, + "velocityY": 0.6014412356126597, + "timestamp": 1.1525986684255507 + }, + { + "x": 2.567077192846392, + "y": 6.80393925778094, + "heading": -2.6456655056562304, + "angularVelocity": 4.168282751801243e-23, + "velocityX": 1.0789758617732326, + "velocityY": 0.5593949893802513, + "timestamp": 1.1783873943801257 + }, + { + "x": 2.591991311058039, + "y": 6.8171921248456435, + "heading": -2.6456655056562304, + "angularVelocity": 4.16831016133047e-23, + "velocityX": 0.9660856552411753, + "velocityY": 0.5139015819585995, + "timestamp": 1.2041761203347008 + }, + { + "x": 2.6140269997935075, + "y": 6.8291933055291585, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682704625493976e-23, + "velocityX": 0.854469847572779, + "velocityY": 0.4653653966747698, + "timestamp": 1.2299648462892758 + }, + { + "x": 2.6332155431922697, + "y": 6.839873184183342, + "heading": -2.6456655056562304, + "angularVelocity": 4.168275058132412e-23, + "velocityX": 0.7440671335423578, + "velocityY": 0.4141297508395471, + "timestamp": 1.2557535722438509 + }, + { + "x": 2.6495865749656664, + "y": 6.849169695790004, + "heading": -2.6456655056562304, + "angularVelocity": 4.168252977577915e-23, + "velocityX": 0.6348135151085763, + "velocityY": 0.3604874324980445, + "timestamp": 1.281542298198426 + }, + { + "x": 2.6631680788200836, + "y": 6.857027245752284, + "heading": -2.6456655056562304, + "angularVelocity": 4.1683540057046367e-23, + "velocityX": 0.5266450106294411, + "velocityY": 0.30468934277736914, + "timestamp": 1.307331024153001 + }, + { + "x": 2.67398643165684, + "y": 6.8633958116591645, + "heading": -2.6456655056562304, + "angularVelocity": 4.1683480067867495e-23, + "velocityX": 0.41949931360441123, + "velocityY": 0.2469515523307539, + "timestamp": 1.333119750107576 + }, + { + "x": 2.682066471763531, + "y": 6.868230193464582, + "heading": -2.6456655056562304, + "angularVelocity": 4.16807726013415e-23, + "velocityX": 0.3133167617887716, + "velocityY": 0.1874610561874916, + "timestamp": 1.358908476062151 + }, + { + "x": 2.6874315804021185, + "y": 6.87148938481805, + "heading": -2.6456655056562304, + "angularVelocity": 4.1684045007313837e-23, + "velocityX": 0.20804085661285734, + "velocityY": 0.12638047181321807, + "timestamp": 1.384697202016726 + }, + { + "x": 2.6901037693023544, + "y": 6.873136043548666, + "heading": -2.6456655056562304, + "angularVelocity": 4.1681852972267e-23, + "velocityX": 0.10361849224262781, + "velocityY": 0.06385188370251901, + "timestamp": 1.4104859279713011 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": 1.876229838959747e-22, + "velocityX": 1.9791825060653877e-20, + "velocityY": -3.163566312793905e-20, + "timestamp": 1.4362746539258762 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing321.2.traj b/src/main/deploy/choreo/AmpWing321.2.traj new file mode 100644 index 00000000..713f0145 --- /dev/null +++ b/src/main/deploy/choreo/AmpWing321.2.traj @@ -0,0 +1,437 @@ +{ + "samples": [ + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": 1.876229838959747e-22, + "velocityX": 1.9791825060653877e-20, + "velocityY": -3.163566312793905e-20, + "timestamp": 0 + }, + { + "x": 2.6855621835413945, + "y": 6.87107725775431, + "heading": -2.6494144391039285, + "angularVelocity": -0.1135887895189958, + "velocityX": -0.13760533129825955, + "velocityY": -0.06237907114466451, + "timestamp": 0.03300443171877676 + }, + { + "x": 2.676506807566959, + "y": 6.866895146887751, + "heading": -2.6568904043742765, + "angularVelocity": -0.22651398254783756, + "velocityX": -0.27436848637770506, + "velocityY": -0.1267136153772196, + "timestamp": 0.06600886343755352 + }, + { + "x": 2.662971687717957, + "y": 6.860513477351578, + "heading": -2.6680645143317867, + "angularVelocity": -0.3385639253759127, + "velocityX": -0.4101000727517643, + "velocityY": -0.1933579584266206, + "timestamp": 0.09901329515633028 + }, + { + "x": 2.6449994198187143, + "y": 6.851840728868721, + "heading": -2.6828968756767435, + "angularVelocity": -0.4494051426590254, + "velocityX": -0.544541049892368, + "velocityY": -0.26277527081079527, + "timestamp": 0.13201772687510704 + }, + { + "x": 2.6226447357456646, + "y": 6.8407649649450155, + "heading": -2.7013308201305883, + "angularVelocity": -0.5585293699620854, + "velocityX": -0.6773237080258816, + "velocityY": -0.33558414270177084, + "timestamp": 0.1650221585938838 + }, + { + "x": 2.595980364541067, + "y": 6.827146327228181, + "heading": -2.7232846239136888, + "angularVelocity": -0.6651774516272203, + "velocityX": -0.8079027517213173, + "velocityY": -0.4126305773987425, + "timestamp": 0.19802659031266057 + }, + { + "x": 2.5651071249375024, + "y": 6.810805799364362, + "heading": -2.7486396330262775, + "angularVelocity": -0.7682304403430714, + "velocityX": -0.9354270925379659, + "velocityY": -0.49510102167646325, + "timestamp": 0.23103102203143733 + }, + { + "x": 2.5301723586562304, + "y": 6.791508128843289, + "heading": -2.7772234107649822, + "angularVelocity": -0.8660587760534934, + "velocityX": -1.0584871322416465, + "velocityY": -0.5846993726631504, + "timestamp": 0.2640354537502141 + }, + { + "x": 2.491405811821051, + "y": 6.768936135527107, + "heading": -2.808786483180526, + "angularVelocity": -0.9563283102246689, + "velocityX": -1.1745861030270022, + "velocityY": -0.683907952377895, + "timestamp": 0.29703988546899085 + }, + { + "x": 2.4491937947918876, + "y": 6.742655604777802, + "heading": -2.8429701361950754, + "angularVelocity": -1.0357291804270654, + "velocityX": -1.278980271160013, + "velocityY": -0.7962727846137726, + "timestamp": 0.3300443171877676 + }, + { + "x": 2.4042373058522646, + "y": 6.712087454564749, + "heading": -2.8792403833872675, + "angularVelocity": -1.0989508167037272, + "velocityX": -1.3621349194159027, + "velocityY": -0.9261832008960693, + "timestamp": 0.36304874890654437 + }, + { + "x": 2.357861726905285, + "y": 6.6765936215423825, + "heading": -2.9166547914513425, + "angularVelocity": -1.1336177027035363, + "velocityX": -1.405131872656832, + "velocityY": -1.0754262738047007, + "timestamp": 0.39605318062532113 + }, + { + "x": 2.3122631013668973, + "y": 6.636020424738222, + "heading": -2.9535956195097754, + "angularVelocity": -1.119268720431154, + "velocityX": -1.3815909913833053, + "velocityY": -1.229325720554031, + "timestamp": 0.4290576123440979 + }, + { + "x": 2.2696422032639783, + "y": 6.591320309416235, + "heading": -2.9884057432125344, + "angularVelocity": -1.0547105915765655, + "velocityX": -1.2913689429977981, + "velocityY": -1.3543670650918083, + "timestamp": 0.46206204406287466 + }, + { + "x": 2.23120231957765, + "y": 6.543808933743509, + "heading": -3.0202463024819033, + "angularVelocity": -0.9647358736752334, + "velocityX": -1.1646885489156587, + "velocityY": -1.4395453337164417, + "timestamp": 0.4950664757816514 + }, + { + "x": 2.197428425363642, + "y": 6.494494224568626, + "heading": -3.048607569176528, + "angularVelocity": -0.8593169225358813, + "velocityX": -1.0233139143793606, + "velocityY": -1.494184465743377, + "timestamp": 0.5280709075004282 + }, + { + "x": 2.1685016928598695, + "y": 6.444043742872585, + "heading": -3.0731583606612194, + "angularVelocity": -0.7438634815434175, + "velocityX": -0.8764499492144375, + "velocityY": -1.5285971934290237, + "timestamp": 0.5610753392192049 + }, + { + "x": 2.1444919046238304, + "y": 6.392902243224364, + "heading": -3.0937182784350274, + "angularVelocity": -0.6229441533486862, + "velocityX": -0.72747164503908, + "velocityY": -1.5495343196327538, + "timestamp": 0.5940797709379817 + }, + { + "x": 2.125426328721103, + "y": 6.341380152546792, + "heading": -3.1102000784524315, + "angularVelocity": -0.49938142119341705, + "velocityX": -0.5776671467995586, + "velocityY": -1.5610658325093172, + "timestamp": 0.6270842026567585 + }, + { + "x": 2.1113144979598757, + "y": 6.289704629326546, + "heading": -3.122567100672372, + "angularVelocity": -0.3747079278721404, + "velocityX": -0.42757381437354175, + "velocityY": -1.5657146791850087, + "timestamp": 0.6600886343755352 + }, + { + "x": 2.1021579392057013, + "y": 6.238048635898547, + "heading": -3.1308085711190277, + "angularVelocity": -0.24970799427420023, + "velocityX": -0.2774342195070909, + "velocityY": -1.5651229467650507, + "timestamp": 0.693093066094312 + }, + { + "x": 2.0979543563247316, + "y": 6.186548177449108, + "heading": -3.1349260454664436, + "angularVelocity": -0.12475519598398203, + "velocityX": -0.127364195111383, + "velocityY": -1.5604103984660276, + "timestamp": 0.7260974978130887 + }, + { + "x": 2.0986995697021484, + "y": 6.135313034057617, + "heading": -3.1349260454664436, + "angularVelocity": 3.0970432324121202e-22, + "velocityX": 0.02257919129671586, + "velocityY": -1.552371627787794, + "timestamp": 0.7591019295318655 + }, + { + "x": 2.1033897121209892, + "y": 6.090187708264656, + "heading": -3.1349260454664436, + "angularVelocity": -5.717605117434922e-23, + "velocityX": 0.16029945836581586, + "velocityY": -1.5422911795098355, + "timestamp": 0.7883605587664126 + }, + { + "x": 2.1121017027787423, + "y": 6.045447928244468, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623983844657e-23, + "velocityX": 0.29775799091431626, + "velocityY": -1.5291140149276428, + "timestamp": 0.8176191880009598 + }, + { + "x": 2.1248234606724927, + "y": 6.00120380324005, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623190409668e-23, + "velocityX": 0.43480361953307217, + "velocityY": -1.5121735420265516, + "timestamp": 0.8468778172355069 + }, + { + "x": 2.141535772219987, + "y": 5.957591648743971, + "heading": -3.1349260454664436, + "angularVelocity": -5.717624354569263e-23, + "velocityX": 0.5711925672765078, + "velocityY": -1.4905740848795832, + "timestamp": 0.8761364464700541 + }, + { + "x": 2.1622074822868833, + "y": 5.91478389840203, + "heading": -3.1349260454664436, + "angularVelocity": -5.717622820098084e-23, + "velocityX": 0.706516696362894, + "velocityY": -1.4630811990108465, + "timestamp": 0.9053950757046012 + }, + { + "x": 2.1867865150959807, + "y": 5.873004121037691, + "heading": -3.1349260454664436, + "angularVelocity": -5.717608261879809e-23, + "velocityX": 0.8400609820801516, + "velocityY": -1.4279471888253283, + "timestamp": 0.9346537049391483 + }, + { + "x": 2.2151821291704135, + "y": 5.832550089747363, + "heading": -3.1349260454664436, + "angularVelocity": -5.717636301748886e-23, + "velocityX": 0.9705039100359978, + "velocityY": -1.3826359043014271, + "timestamp": 0.9639123341736955 + }, + { + "x": 2.247227781288673, + "y": 5.793828430179212, + "heading": -3.1349260454664436, + "angularVelocity": -5.717627842193595e-23, + "velocityX": 1.0952547319085686, + "velocityY": -1.3234269882491958, + "timestamp": 0.9931709634082426 + }, + { + "x": 2.2826005084616283, + "y": 5.757399030074356, + "heading": -3.1349260454664436, + "angularVelocity": -5.717621943290754e-23, + "velocityX": 1.2089673405192627, + "velocityY": -1.245082256342438, + "timestamp": 1.0224295926427898 + }, + { + "x": 2.320655607594778, + "y": 5.7239910126155, + "heading": -3.1349260454664436, + "angularVelocity": -5.717628005263719e-23, + "velocityX": 1.3006453182787074, + "velocityY": -1.141817587932542, + "timestamp": 1.051688221877337 + }, + { + "x": 2.360204689273028, + "y": 5.694336910489396, + "heading": -3.1349260454664436, + "angularVelocity": -5.717616763256594e-23, + "velocityX": 1.351706580688257, + "velocityY": -1.013516453161909, + "timestamp": 1.080946851111884 + }, + { + "x": 2.3996260680339816, + "y": 5.668721069816042, + "heading": -3.1349260454664436, + "angularVelocity": -5.717628968554313e-23, + "velocityX": 1.34734195662094, + "velocityY": -0.8754969505913973, + "timestamp": 1.1102054803464312 + }, + { + "x": 2.4375100420911338, + "y": 5.6468415720185074, + "heading": -3.1349260454664436, + "angularVelocity": -5.717625457879064e-23, + "velocityX": 1.2947966137939357, + "velocityY": -0.7477964063905799, + "timestamp": 1.1394641095809783 + }, + { + "x": 2.4729703851854588, + "y": 5.62819470377097, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623609342389e-23, + "velocityX": 1.2119618731994621, + "velocityY": -0.6373117516233145, + "timestamp": 1.1687227388155255 + }, + { + "x": 2.5055011746844404, + "y": 5.612330209413261, + "heading": -3.1349260454664436, + "angularVelocity": -5.717628902265798e-23, + "velocityX": 1.1118357335957716, + "velocityY": -0.5422159128005899, + "timestamp": 1.1979813680500726 + }, + { + "x": 2.5348060457698836, + "y": 5.598898274411973, + "heading": -3.1349260454664436, + "angularVelocity": -5.717627045197986e-23, + "velocityX": 1.0015804517181697, + "velocityY": -0.4590760180043621, + "timestamp": 1.2272399972846197 + }, + { + "x": 2.5607008711079886, + "y": 5.587633246659649, + "heading": -3.1349260454664436, + "angularVelocity": -5.717608754941009e-23, + "velocityX": 0.8850320748298349, + "velocityY": -0.38501556795704556, + "timestamp": 1.2564986265191669 + }, + { + "x": 2.5830643099643584, + "y": 5.578331178882631, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623485224611e-23, + "velocityX": 0.7643365202491328, + "velocityY": -0.31792561785481754, + "timestamp": 1.285757255753714 + }, + { + "x": 2.601812219563059, + "y": 5.570832400631334, + "heading": -3.1349260454664436, + "angularVelocity": -5.717639538428928e-23, + "velocityX": 0.6407651379834913, + "velocityY": -0.2562928765800323, + "timestamp": 1.3150158849882612 + }, + { + "x": 2.6168837285238395, + "y": 5.565009326805624, + "heading": -3.1349260454664436, + "angularVelocity": -5.717612764732363e-23, + "velocityX": 0.5151132966602103, + "velocityY": -0.1990207326189503, + "timestamp": 1.3442745142228083 + }, + { + "x": 2.62823323421208, + "y": 5.560758067303841, + "heading": -3.1349260454664436, + "angularVelocity": -5.717624354345976e-23, + "velocityX": 0.3879028507202083, + "velocityY": -0.1452993394780887, + "timestamp": 1.3735331434573554 + }, + { + "x": 2.635825567663263, + "y": 5.557992596069733, + "heading": -3.1349260454664436, + "angularVelocity": -5.717624354345825e-23, + "velocityX": 0.2594904016290358, + "velocityY": -0.09451814067963153, + "timestamp": 1.4027917726919026 + }, + { + "x": 2.6396329402923584, + "y": 5.556640625, + "heading": -3.1349260454664436, + "angularVelocity": -5.717626024583988e-23, + "velocityX": 0.13012819563669478, + "velocityY": -0.04620760114590083, + "timestamp": 1.4320504019264497 + }, + { + "x": 2.6396329402923584, + "y": 5.556640625, + "heading": -3.1349260454664436, + "angularVelocity": 6.493546095227337e-22, + "velocityX": 1.881202272196025e-21, + "velocityY": 2.989774730921632e-20, + "timestamp": 1.4613090311609969 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing321.3.traj b/src/main/deploy/choreo/AmpWing321.3.traj new file mode 100644 index 00000000..28de315c --- /dev/null +++ b/src/main/deploy/choreo/AmpWing321.3.traj @@ -0,0 +1,482 @@ +{ + "samples": [ + { + "x": 2.6396329402923584, + "y": 5.556640625, + "heading": -3.1349260454664436, + "angularVelocity": 6.493546095227337e-22, + "velocityX": 1.881202272196025e-21, + "velocityY": 2.989774730921632e-20, + "timestamp": 0 + }, + { + "x": 2.63578986878673, + "y": 5.555006216645995, + "heading": -3.138403367676192, + "angularVelocity": -0.11468866986671536, + "velocityX": -0.12675177409426172, + "velocityY": -0.053905881835223775, + "timestamp": 0.03031966639590289 + }, + { + "x": 2.6281241915651026, + "y": 5.55168270192059, + "heading": -3.145319654992211, + "angularVelocity": -0.22811224984168904, + "velocityX": -0.25282854770026447, + "velocityY": -0.10961580783856312, + "timestamp": 0.06063933279180578 + }, + { + "x": 2.616660760682821, + "y": 5.546605937980688, + "heading": -3.155626430554535, + "angularVelocity": -0.33993697119690924, + "velocityX": -0.37808565346951006, + "velocityY": -0.1674412862474532, + "timestamp": 0.09095899918770867 + }, + { + "x": 2.6014305324024507, + "y": 5.539699833963852, + "heading": -3.1692644048210545, + "angularVelocity": -0.4498062112042979, + "velocityX": -0.502321763092811, + "velocityY": -0.22777638535441594, + "timestamp": 0.12127866558361156 + }, + { + "x": 2.5824731253801407, + "y": 5.530872998614334, + "heading": -3.1861630308768345, + "angularVelocity": -0.5573486803952303, + "velocityX": -0.6252511744282022, + "velocityY": -0.2911257411039302, + "timestamp": 0.15159833197951444 + }, + { + "x": 2.5598407273275248, + "y": 5.520014195669526, + "heading": -3.2062405203859172, + "angularVelocity": -0.6621936154217003, + "velocityX": -0.7464593362305498, + "velocityY": -0.35814387938840914, + "timestamp": 0.18191799837541733 + }, + { + "x": 2.5336042337816798, + "y": 5.506986103666438, + "heading": -3.2294044879426838, + "angularVelocity": -0.7639915048635432, + "velocityX": -0.8653292289980953, + "velocityY": -0.4296911395064041, + "timestamp": 0.21223766477132022 + }, + { + "x": 2.503863218826844, + "y": 5.49161664330363, + "heading": -3.255552933908256, + "angularVelocity": -0.8624252531058615, + "velocityX": -0.9809149799500774, + "velocityY": -0.5069139007700986, + "timestamp": 0.2425573311672231 + }, + { + "x": 2.470762785739885, + "y": 5.4736868205948594, + "heading": -3.2845732996383643, + "angularVelocity": -0.9571466041601998, + "velocityX": -1.0917149501167551, + "velocityY": -0.5913594983085235, + "timestamp": 0.272876997563126 + }, + { + "x": 2.4345235301066466, + "y": 5.452913761348259, + "heading": -3.3163303487290055, + "angularVelocity": -1.047407602576138, + "velocityX": -1.1952392602228237, + "velocityY": -0.6851348222423583, + "timestamp": 0.3031966639590289 + }, + { + "x": 2.3954986328534775, + "y": 5.428928619205951, + "heading": -3.3506130763033335, + "angularVelocity": -1.1307092606718314, + "velocityX": -1.2871149947230278, + "velocityY": -0.7910753973717586, + "timestamp": 0.3335163303549318 + }, + { + "x": 2.354290847908129, + "y": 5.401261287885048, + "heading": -3.3869757124641584, + "angularVelocity": -1.1993085836108863, + "velocityX": -1.3591107635311952, + "velocityY": -0.9125209677319055, + "timestamp": 0.36383599675083467 + }, + { + "x": 2.3119700393450096, + "y": 5.369418962959644, + "heading": -3.424488739130058, + "angularVelocity": -1.237250640428174, + "velocityX": -1.3958203896611634, + "velocityY": -1.050220161054137, + "timestamp": 0.39415566314673756 + }, + { + "x": 2.2701943697940816, + "y": 5.333273315136478, + "heading": -3.4617422361668626, + "angularVelocity": -1.2286908619099843, + "velocityX": -1.3778406729622434, + "velocityY": -1.1921518974243095, + "timestamp": 0.42447532954264044 + }, + { + "x": 2.2306531953186166, + "y": 5.2934548415358735, + "heading": -3.497295099309754, + "angularVelocity": -1.1726007363885618, + "velocityX": -1.3041427949485624, + "velocityY": -1.3132886450881536, + "timestamp": 0.45479499593854333 + }, + { + "x": 2.194384797808916, + "y": 5.250947089639415, + "heading": -3.5300729098031467, + "angularVelocity": -1.0810742461804137, + "velocityX": -1.1962004144809404, + "velocityY": -1.4019861347226739, + "timestamp": 0.4851146623344462 + }, + { + "x": 2.1618561434717347, + "y": 5.206562904098581, + "heading": -3.559395458180096, + "angularVelocity": -0.9671131599558667, + "velocityX": -1.0728566044374948, + "velocityY": -1.463874468844275, + "timestamp": 0.5154343287303491 + }, + { + "x": 2.1332734403753273, + "y": 5.160862427241999, + "heading": -3.5849957373685863, + "angularVelocity": -0.8443456749890059, + "velocityX": -0.9427116619022493, + "velocityY": -1.5072882484862653, + "timestamp": 0.545753995126252 + }, + { + "x": 2.1087386455620734, + "y": 5.114234766629067, + "heading": -3.606823401382331, + "angularVelocity": -0.7199176840776125, + "velocityX": -0.8092039830811812, + "velocityY": -1.5378685241486245, + "timestamp": 0.5760736615221549 + }, + { + "x": 2.0883065892590618, + "y": 5.0669625612384435, + "heading": -3.624907841959223, + "angularVelocity": -0.5964590883274463, + "velocityX": -0.6738878995637272, + "velocityY": -1.5591268311907651, + "timestamp": 0.6063933279180578 + }, + { + "x": 2.0720078142934666, + "y": 5.019259378769025, + "heading": -3.6392985462841363, + "angularVelocity": -0.4746326736252551, + "velocityX": -0.5375644557817244, + "velocityY": -1.5733412711912018, + "timestamp": 0.6367129943139607 + }, + { + "x": 2.059859229705669, + "y": 4.97129159857966, + "heading": -3.6500430565181534, + "angularVelocity": -0.35437428940409044, + "velocityX": -0.4006833198346745, + "velocityY": -1.5820682049415709, + "timestamp": 0.6670326607098636 + }, + { + "x": 2.0518696849147733, + "y": 4.923191943386821, + "heading": -3.657180162870296, + "angularVelocity": -0.2353952797154437, + "velocityX": -0.26351031329218877, + "velocityY": -1.5864176922246054, + "timestamp": 0.6973523271057664 + }, + { + "x": 2.0480431107826926, + "y": 4.875068293638133, + "heading": -3.6607387948340793, + "angularVelocity": -0.11737041949327015, + "velocityX": -0.12620765948148724, + "velocityY": -1.5872090781053572, + "timestamp": 0.7276719935016693 + }, + { + "x": 2.0483803749084473, + "y": 4.827009677886963, + "heading": -3.6607387948340793, + "angularVelocity": 1.5097575312240631e-21, + "velocityX": 0.011123609387845542, + "velocityY": -1.5850641337420555, + "timestamp": 0.7579916598975722 + }, + { + "x": 2.052256902501467, + "y": 4.7835396477807635, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300342434097995e-22, + "velocityX": 0.14088183500646628, + "velocityY": -1.5797998239936404, + "timestamp": 0.7855078233038362 + }, + { + "x": 2.0597005743510723, + "y": 4.740280724780923, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300332898125506e-22, + "velocityX": 0.2705199754676443, + "velocityY": -1.5721277113074286, + "timestamp": 0.8130239867101001 + }, + { + "x": 2.070705854466647, + "y": 4.69731168064185, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300341717459463e-22, + "velocityX": 0.39995692542870404, + "velocityY": -1.5615928537944752, + "timestamp": 0.8405401501163641 + }, + { + "x": 2.0852637266783254, + "y": 4.654727566618508, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300339024855326e-22, + "velocityX": 0.52906620725952, + "velocityY": -1.5476036173578895, + "timestamp": 0.868056313522628 + }, + { + "x": 2.1033596031452872, + "y": 4.612645056001655, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300340767321184e-22, + "velocityX": 0.6576453337547263, + "velocityY": -1.529374208007212, + "timestamp": 0.895572476928892 + }, + { + "x": 2.124969679557945, + "y": 4.571210215598577, + "heading": -3.6607387948340793, + "angularVelocity": 1.030034450352898e-22, + "velocityX": 0.7853593574649949, + "velocityY": -1.5058363984587355, + "timestamp": 0.9230886403351559 + }, + { + "x": 2.1500542877457156, + "y": 4.530610063288077, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300340617397168e-22, + "velocityX": 0.9116317495799002, + "velocityY": -1.4755019335743174, + "timestamp": 0.9506048037414199 + }, + { + "x": 2.178545140467357, + "y": 4.491089989654474, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300319327454968e-22, + "velocityX": 1.0354224279373974, + "velocityY": -1.4362494163925468, + "timestamp": 0.9781209671476838 + }, + { + "x": 2.21031955626508, + "y": 4.452979625452379, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300351516928058e-22, + "velocityX": 1.1547545829188868, + "velocityY": -1.3850173674076385, + "timestamp": 1.0056371305539478 + }, + { + "x": 2.245146390808226, + "y": 4.416727163520435, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300321637684974e-22, + "velocityX": 1.2656864268803893, + "velocityY": -1.317496970659786, + "timestamp": 1.0331532939602117 + }, + { + "x": 2.282575631489174, + "y": 4.382923484071245, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300349373445067e-22, + "velocityX": 1.3602637885358229, + "velocityY": -1.2285026422508498, + "timestamp": 1.0606694573664757 + }, + { + "x": 2.321764544418595, + "y": 4.352229884876951, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300341066851439e-22, + "velocityX": 1.4242142827403603, + "velocityY": -1.1154752478066263, + "timestamp": 1.0881856207727396 + }, + { + "x": 2.3614329153831446, + "y": 4.325077353660923, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300333549099457e-22, + "velocityX": 1.4416388788967012, + "velocityY": -0.9867847786456208, + "timestamp": 1.1157017841790036 + }, + { + "x": 2.4002641272952094, + "y": 4.301398755593006, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300340140508024e-22, + "velocityX": 1.4112146137068142, + "velocityY": -0.8605341419991839, + "timestamp": 1.1432179475852675 + }, + { + "x": 2.4373189641856667, + "y": 4.28082072949791, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300338693239079e-22, + "velocityX": 1.3466571027123986, + "velocityY": -0.7478523001647727, + "timestamp": 1.1707341109915315 + }, + { + "x": 2.472031674195157, + "y": 4.262941500153538, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300337163704035e-22, + "velocityX": 1.2615388816018003, + "velocityY": -0.649771884270625, + "timestamp": 1.1982502743977954 + }, + { + "x": 2.504066706947767, + "y": 4.247428515210111, + "heading": -3.6607387948340793, + "angularVelocity": 1.030033511464939e-22, + "velocityX": 1.1642259961762114, + "velocityY": -0.5637771775946641, + "timestamp": 1.2257664378040594 + }, + { + "x": 2.5332162501951645, + "y": 4.234023378166114, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300349841571229e-22, + "velocityX": 1.0593607407040928, + "velocityY": -0.4871731878490917, + "timestamp": 1.2532826012103233 + }, + { + "x": 2.5593444084951305, + "y": 4.222526200447489, + "heading": -3.6607387948340793, + "angularVelocity": 1.030032428740828e-22, + "velocityX": 0.9495567355890002, + "velocityY": -0.4178336037939713, + "timestamp": 1.2807987646165873 + }, + { + "x": 2.582357728690195, + "y": 4.212780268556745, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300353293396209e-22, + "velocityX": 0.8363564300460482, + "velocityY": -0.35418934488933373, + "timestamp": 1.3083149280228512 + }, + { + "x": 2.6021891216432027, + "y": 4.204660503766347, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300328136841497e-22, + "velocityX": 0.7207179525806894, + "velocityY": -0.2950907316007032, + "timestamp": 1.3358310914291152 + }, + { + "x": 2.6187886631305672, + "y": 4.198065277647973, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300337513905507e-22, + "velocityX": 0.6032651152076492, + "velocityY": -0.23968552668501003, + "timestamp": 1.363347254835379 + }, + { + "x": 2.632118072440845, + "y": 4.192910648265756, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300328350956759e-22, + "velocityX": 0.48442106966299653, + "velocityY": -0.1873309627548638, + "timestamp": 1.390863418241643 + }, + { + "x": 2.6421472503261962, + "y": 4.189126258673718, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300343744876286e-22, + "velocityX": 0.36448314895028716, + "velocityY": -0.1375333303617742, + "timestamp": 1.418379581647907 + }, + { + "x": 2.648852022726871, + "y": 4.186652370386277, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300326335422924e-22, + "velocityX": 0.2436666878916012, + "velocityY": -0.08990673048827487, + "timestamp": 1.445895745054171 + }, + { + "x": 2.652212619781494, + "y": 4.1854376792907715, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300339610415066e-22, + "velocityX": 0.12213174507630833, + "velocityY": -0.04414463882810916, + "timestamp": 1.473411908460435 + }, + { + "x": 2.652212619781494, + "y": 4.1854376792907715, + "heading": -3.6607387948340793, + "angularVelocity": 3.4334501124721124e-23, + "velocityX": 3.058092897395823e-21, + "velocityY": 8.734750320006533e-21, + "timestamp": 1.5009280718666989 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing321.traj b/src/main/deploy/choreo/AmpWing321.traj new file mode 100644 index 00000000..972b5495 --- /dev/null +++ b/src/main/deploy/choreo/AmpWing321.traj @@ -0,0 +1,1310 @@ +{ + "samples": [ + { + "x": 0.3817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "angularVelocity": 1.8698301864234364e-22, + "velocityX": 4.800440645526857e-21, + "velocityY": 2.9477177393174924e-20, + "timestamp": 0 + }, + { + "x": 0.3883549561387046, + "y": 6.608408421186061, + "heading": -2.1197134410063434, + "angularVelocity": -0.08634729729146294, + "velocityX": 0.17378773726557262, + "velocityY": -0.026872324952015636, + "timestamp": 0.037779247083705426 + }, + { + "x": 0.4014921075538034, + "y": 6.606405430964887, + "heading": -2.126204479988474, + "angularVelocity": -0.17181493764946093, + "velocityX": 0.3477346011156235, + "velocityY": -0.05301826732320674, + "timestamp": 0.07555849416741085 + }, + { + "x": 0.4212075211916924, + "y": 6.603445687185866, + "heading": -2.1358857960695534, + "angularVelocity": -0.25626016473089336, + "velocityX": 0.5218582994574282, + "velocityY": -0.0783431118282232, + "timestamp": 0.11333774125111629 + }, + { + "x": 0.44750862695642823, + "y": 6.599564542552146, + "heading": -2.1487127600714926, + "angularVelocity": -0.3395240771611807, + "velocityX": 0.6961786640814154, + "velocityY": -0.10273218587648632, + "timestamp": 0.1511169883348217 + }, + { + "x": 0.48040369549494544, + "y": 6.594802655593878, + "heading": -2.1646339690222502, + "angularVelocity": -0.42142737560337534, + "velocityX": 0.8707179490802492, + "velocityY": -0.1260450465763573, + "timestamp": 0.18889623541852713 + }, + { + "x": 0.5199019408821504, + "y": 6.589207268746513, + "heading": -2.1835902439067887, + "angularVelocity": -0.5017642316306103, + "velocityX": 1.0455011265758887, + "velocityY": -0.1481074208540784, + "timestamp": 0.22667548250223254 + }, + { + "x": 0.5660136324357566, + "y": 6.582833919675241, + "heading": -2.205513273539415, + "angularVelocity": -0.5802929207152439, + "velocityX": 1.220556128380285, + "velocityY": -0.16869973763992252, + "timestamp": 0.26445472958593796 + }, + { + "x": 0.6187502087281199, + "y": 6.575748787450549, + "heading": -2.2303236919031626, + "angularVelocity": -0.6567208263514764, + "velocityX": 1.3959139041473307, + "velocityY": -0.1875403236323719, + "timestamp": 0.3022339766696434 + }, + { + "x": 0.6781243747498052, + "y": 6.568032000955299, + "heading": -2.2579282145890622, + "angularVelocity": -0.730679534844558, + "velocityX": 1.5716079754085093, + "velocityY": -0.2042599334549938, + "timestamp": 0.34001322375334886 + }, + { + "x": 0.7441501329753766, + "y": 6.559782463812031, + "heading": -2.2882151619643003, + "angularVelocity": -0.8016821327363354, + "velocityX": 1.7476726859930256, + "velocityY": -0.21836160802711913, + "timestamp": 0.3777924708370543 + }, + { + "x": 0.8168426216101802, + "y": 6.55112518097433, + "heading": -2.3210471231149787, + "angularVelocity": -0.8690475243705671, + "velocityX": 1.9241380981931202, + "velocityY": -0.22915445663977885, + "timestamp": 0.41557171792075975 + }, + { + "x": 0.8962174244775805, + "y": 6.54222294081638, + "heading": -2.3562483469235813, + "angularVelocity": -0.9317608614753342, + "velocityX": 2.1010160073209, + "velocityY": -0.23563836881711223, + "timestamp": 0.4533509650044652 + }, + { + "x": 0.9822884113766555, + "y": 6.5332960857123705, + "heading": -2.393581906091161, + "angularVelocity": -0.9882028375224495, + "velocityX": 2.2782610439104056, + "velocityY": -0.2362899155781206, + "timestamp": 0.49113021208817065 + }, + { + "x": 1.0750612253316763, + "y": 6.524658510256875, + "heading": -2.432705739729525, + "angularVelocity": -1.0355906127954124, + "velocityX": 2.455655448862486, + "velocityY": -0.22863281092648347, + "timestamp": 0.528909459171876 + }, + { + "x": 1.1745124212868387, + "y": 6.516789255723791, + "heading": -2.4730818457948223, + "angularVelocity": -1.0687377113640772, + "velocityX": 2.6324292735325403, + "velocityY": -0.20829569513922166, + "timestamp": 0.5666887062555814 + }, + { + "x": 1.2805135717940606, + "y": 6.510488992251309, + "heading": -2.5137740657710497, + "angularVelocity": -1.0771051070993467, + "velocityX": 2.805803680320948, + "velocityY": -0.16676519408961948, + "timestamp": 0.6044679533392868 + }, + { + "x": 1.3925030399539646, + "y": 6.5072291237539, + "heading": -2.5529426361670393, + "angularVelocity": -1.0367747750294287, + "velocityX": 2.9643118062089164, + "velocityY": -0.08628728069154737, + "timestamp": 0.6422472004229922 + }, + { + "x": 1.5081204845079395, + "y": 6.509370790524853, + "heading": -2.586975983570745, + "angularVelocity": -0.9008476883697443, + "velocityX": 3.060342740494668, + "velocityY": 0.05668897440470873, + "timestamp": 0.6800264475066976 + }, + { + "x": 1.622365364792802, + "y": 6.517488893832767, + "heading": -2.612477761707434, + "angularVelocity": -0.6750208144748423, + "velocityX": 3.0240115699429984, + "velocityY": 0.21488261240214016, + "timestamp": 0.717805694590403 + }, + { + "x": 1.7327667460189187, + "y": 6.53045723021325, + "heading": -2.629741912942711, + "angularVelocity": -0.4569744652937502, + "velocityX": 2.9222758458236795, + "velocityY": 0.34326614163931696, + "timestamp": 0.7555849416741084 + }, + { + "x": 1.8382100625927147, + "y": 6.547429672114164, + "heading": -2.6405106169347077, + "angularVelocity": -0.28504284291683973, + "velocityX": 2.7910380622508084, + "velocityY": 0.44925304793238396, + "timestamp": 0.7933641887578138 + }, + { + "x": 1.938178409904931, + "y": 6.567826322651328, + "heading": -2.6456655056562304, + "angularVelocity": -0.13644762983499997, + "velocityX": 2.646118041757629, + "velocityY": 0.5398903395804769, + "timestamp": 0.8311434358415192 + }, + { + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": -2.6456655056562304, + "angularVelocity": 1.8055348625312267e-21, + "velocityX": 2.4940831175072695, + "velocityY": 0.6203915390652321, + "timestamp": 0.8689226829252246 + }, + { + "x": 2.093877892062785, + "y": 6.608590709626909, + "heading": -2.6456655056562304, + "angularVelocity": 4.168304475470088e-23, + "velocityX": 2.3837897196813476, + "velocityY": 0.671861873409428, + "timestamp": 0.8947114088797997 + }, + { + "x": 2.1523659521196383, + "y": 6.626881977306388, + "heading": -2.6456655056562304, + "angularVelocity": 4.168273499118604e-23, + "velocityX": 2.2679701261668037, + "velocityY": 0.7092738009447922, + "timestamp": 0.9205001348343749 + }, + { + "x": 2.2077779049993467, + "y": 6.6457974470281025, + "heading": -2.6456655056562304, + "angularVelocity": 4.16827486833322e-23, + "velocityX": 2.148689042542395, + "velocityY": 0.7334782553910214, + "timestamp": 0.94628886078895 + }, + { + "x": 2.2600669512143567, + "y": 6.665028382936204, + "heading": -2.6456655056562304, + "angularVelocity": 4.168303685702792e-23, + "velocityX": 2.0275932322952634, + "velocityY": 0.7457109723785112, + "timestamp": 0.9720775867435252 + }, + { + "x": 2.309217481860067, + "y": 6.684301584296192, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682770338729235e-23, + "velocityX": 1.9058921612597222, + "velocityY": 0.7473498843667037, + "timestamp": 0.9978663126981003 + }, + { + "x": 2.3552353371432924, + "y": 6.7033787939224325, + "heading": -2.6456655056562304, + "angularVelocity": 4.168287783381093e-23, + "velocityX": 1.7844175537902998, + "velocityY": 0.7397499845392672, + "timestamp": 1.0236550386526755 + }, + { + "x": 2.3981402688351157, + "y": 6.7220537901087445, + "heading": -2.6456655056562304, + "angularVelocity": 4.168284482622447e-23, + "velocityX": 1.663708853527376, + "velocityY": 0.7241535009973167, + "timestamp": 1.0494437646072505 + }, + { + "x": 2.4379604976017806, + "y": 6.740148546732269, + "heading": -2.6456655056562304, + "angularVelocity": 4.168287413318469e-23, + "velocityX": 1.5440944557233214, + "velocityY": 0.7016537635645865, + "timestamp": 1.0752324905618256 + }, + { + "x": 2.474728960142428, + "y": 6.7575092774466885, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682752490293846e-23, + "velocityX": 1.4257572322666272, + "velocityY": 0.6731907091840713, + "timestamp": 1.1010212165164006 + }, + { + "x": 2.508480806878266, + "y": 6.774002770496258, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682856288215976e-23, + "velocityX": 1.3087830238384508, + "velocityY": 0.6395621512623721, + "timestamp": 1.1268099424709757 + }, + { + "x": 2.5392517800354852, + "y": 6.789513173699484, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682970034902383e-23, + "velocityX": 1.1931947786603707, + "velocityY": 0.6014412356126597, + "timestamp": 1.1525986684255507 + }, + { + "x": 2.567077192846392, + "y": 6.80393925778094, + "heading": -2.6456655056562304, + "angularVelocity": 4.168282751801243e-23, + "velocityX": 1.0789758617732326, + "velocityY": 0.5593949893802513, + "timestamp": 1.1783873943801257 + }, + { + "x": 2.591991311058039, + "y": 6.8171921248456435, + "heading": -2.6456655056562304, + "angularVelocity": 4.16831016133047e-23, + "velocityX": 0.9660856552411753, + "velocityY": 0.5139015819585995, + "timestamp": 1.2041761203347008 + }, + { + "x": 2.6140269997935075, + "y": 6.8291933055291585, + "heading": -2.6456655056562304, + "angularVelocity": 4.1682704625493976e-23, + "velocityX": 0.854469847572779, + "velocityY": 0.4653653966747698, + "timestamp": 1.2299648462892758 + }, + { + "x": 2.6332155431922697, + "y": 6.839873184183342, + "heading": -2.6456655056562304, + "angularVelocity": 4.168275058132412e-23, + "velocityX": 0.7440671335423578, + "velocityY": 0.4141297508395471, + "timestamp": 1.2557535722438509 + }, + { + "x": 2.6495865749656664, + "y": 6.849169695790004, + "heading": -2.6456655056562304, + "angularVelocity": 4.168252977577915e-23, + "velocityX": 0.6348135151085763, + "velocityY": 0.3604874324980445, + "timestamp": 1.281542298198426 + }, + { + "x": 2.6631680788200836, + "y": 6.857027245752284, + "heading": -2.6456655056562304, + "angularVelocity": 4.1683540057046367e-23, + "velocityX": 0.5266450106294411, + "velocityY": 0.30468934277736914, + "timestamp": 1.307331024153001 + }, + { + "x": 2.67398643165684, + "y": 6.8633958116591645, + "heading": -2.6456655056562304, + "angularVelocity": 4.1683480067867495e-23, + "velocityX": 0.41949931360441123, + "velocityY": 0.2469515523307539, + "timestamp": 1.333119750107576 + }, + { + "x": 2.682066471763531, + "y": 6.868230193464582, + "heading": -2.6456655056562304, + "angularVelocity": 4.16807726013415e-23, + "velocityX": 0.3133167617887716, + "velocityY": 0.1874610561874916, + "timestamp": 1.358908476062151 + }, + { + "x": 2.6874315804021185, + "y": 6.87148938481805, + "heading": -2.6456655056562304, + "angularVelocity": 4.1684045007313837e-23, + "velocityX": 0.20804085661285734, + "velocityY": 0.12638047181321807, + "timestamp": 1.384697202016726 + }, + { + "x": 2.6901037693023544, + "y": 6.873136043548666, + "heading": -2.6456655056562304, + "angularVelocity": 4.1681852972267e-23, + "velocityX": 0.10361849224262781, + "velocityY": 0.06385188370251901, + "timestamp": 1.4104859279713011 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": 1.876229838959747e-22, + "velocityX": 1.9791825060653877e-20, + "velocityY": -3.163566312793905e-20, + "timestamp": 1.4362746539258762 + }, + { + "x": 2.6855621835413945, + "y": 6.87107725775431, + "heading": -2.6494144391039285, + "angularVelocity": -0.1135887895189958, + "velocityX": -0.13760533129825955, + "velocityY": -0.06237907114466451, + "timestamp": 1.469279085644653 + }, + { + "x": 2.676506807566959, + "y": 6.866895146887751, + "heading": -2.6568904043742765, + "angularVelocity": -0.22651398254783756, + "velocityX": -0.27436848637770506, + "velocityY": -0.1267136153772196, + "timestamp": 1.5022835173634297 + }, + { + "x": 2.662971687717957, + "y": 6.860513477351578, + "heading": -2.6680645143317867, + "angularVelocity": -0.3385639253759127, + "velocityX": -0.4101000727517643, + "velocityY": -0.1933579584266206, + "timestamp": 1.5352879490822064 + }, + { + "x": 2.6449994198187143, + "y": 6.851840728868721, + "heading": -2.6828968756767435, + "angularVelocity": -0.4494051426590254, + "velocityX": -0.544541049892368, + "velocityY": -0.26277527081079527, + "timestamp": 1.5682923808009832 + }, + { + "x": 2.6226447357456646, + "y": 6.8407649649450155, + "heading": -2.7013308201305883, + "angularVelocity": -0.5585293699620854, + "velocityX": -0.6773237080258816, + "velocityY": -0.33558414270177084, + "timestamp": 1.60129681251976 + }, + { + "x": 2.595980364541067, + "y": 6.827146327228181, + "heading": -2.7232846239136888, + "angularVelocity": -0.6651774516272203, + "velocityX": -0.8079027517213173, + "velocityY": -0.4126305773987425, + "timestamp": 1.6343012442385367 + }, + { + "x": 2.5651071249375024, + "y": 6.810805799364362, + "heading": -2.7486396330262775, + "angularVelocity": -0.7682304403430714, + "velocityX": -0.9354270925379659, + "velocityY": -0.49510102167646325, + "timestamp": 1.6673056759573135 + }, + { + "x": 2.5301723586562304, + "y": 6.791508128843289, + "heading": -2.7772234107649822, + "angularVelocity": -0.8660587760534934, + "velocityX": -1.0584871322416465, + "velocityY": -0.5846993726631504, + "timestamp": 1.7003101076760903 + }, + { + "x": 2.491405811821051, + "y": 6.768936135527107, + "heading": -2.808786483180526, + "angularVelocity": -0.9563283102246689, + "velocityX": -1.1745861030270022, + "velocityY": -0.683907952377895, + "timestamp": 1.733314539394867 + }, + { + "x": 2.4491937947918876, + "y": 6.742655604777802, + "heading": -2.8429701361950754, + "angularVelocity": -1.0357291804270654, + "velocityX": -1.278980271160013, + "velocityY": -0.7962727846137726, + "timestamp": 1.7663189711136438 + }, + { + "x": 2.4042373058522646, + "y": 6.712087454564749, + "heading": -2.8792403833872675, + "angularVelocity": -1.0989508167037272, + "velocityX": -1.3621349194159027, + "velocityY": -0.9261832008960693, + "timestamp": 1.7993234028324205 + }, + { + "x": 2.357861726905285, + "y": 6.6765936215423825, + "heading": -2.9166547914513425, + "angularVelocity": -1.1336177027035363, + "velocityX": -1.405131872656832, + "velocityY": -1.0754262738047007, + "timestamp": 1.8323278345511973 + }, + { + "x": 2.3122631013668973, + "y": 6.636020424738222, + "heading": -2.9535956195097754, + "angularVelocity": -1.119268720431154, + "velocityX": -1.3815909913833053, + "velocityY": -1.229325720554031, + "timestamp": 1.865332266269974 + }, + { + "x": 2.2696422032639783, + "y": 6.591320309416235, + "heading": -2.9884057432125344, + "angularVelocity": -1.0547105915765655, + "velocityX": -1.2913689429977981, + "velocityY": -1.3543670650918083, + "timestamp": 1.8983366979887508 + }, + { + "x": 2.23120231957765, + "y": 6.543808933743509, + "heading": -3.0202463024819033, + "angularVelocity": -0.9647358736752334, + "velocityX": -1.1646885489156587, + "velocityY": -1.4395453337164417, + "timestamp": 1.9313411297075276 + }, + { + "x": 2.197428425363642, + "y": 6.494494224568626, + "heading": -3.048607569176528, + "angularVelocity": -0.8593169225358813, + "velocityX": -1.0233139143793606, + "velocityY": -1.494184465743377, + "timestamp": 1.9643455614263043 + }, + { + "x": 2.1685016928598695, + "y": 6.444043742872585, + "heading": -3.0731583606612194, + "angularVelocity": -0.7438634815434175, + "velocityX": -0.8764499492144375, + "velocityY": -1.5285971934290237, + "timestamp": 1.997349993145081 + }, + { + "x": 2.1444919046238304, + "y": 6.392902243224364, + "heading": -3.0937182784350274, + "angularVelocity": -0.6229441533486862, + "velocityX": -0.72747164503908, + "velocityY": -1.5495343196327538, + "timestamp": 2.030354424863858 + }, + { + "x": 2.125426328721103, + "y": 6.341380152546792, + "heading": -3.1102000784524315, + "angularVelocity": -0.49938142119341705, + "velocityX": -0.5776671467995586, + "velocityY": -1.5610658325093172, + "timestamp": 2.0633588565826346 + }, + { + "x": 2.1113144979598757, + "y": 6.289704629326546, + "heading": -3.122567100672372, + "angularVelocity": -0.3747079278721404, + "velocityX": -0.42757381437354175, + "velocityY": -1.5657146791850087, + "timestamp": 2.0963632883014114 + }, + { + "x": 2.1021579392057013, + "y": 6.238048635898547, + "heading": -3.1308085711190277, + "angularVelocity": -0.24970799427420023, + "velocityX": -0.2774342195070909, + "velocityY": -1.5651229467650507, + "timestamp": 2.129367720020188 + }, + { + "x": 2.0979543563247316, + "y": 6.186548177449108, + "heading": -3.1349260454664436, + "angularVelocity": -0.12475519598398203, + "velocityX": -0.127364195111383, + "velocityY": -1.5604103984660276, + "timestamp": 2.162372151738965 + }, + { + "x": 2.0986995697021484, + "y": 6.135313034057617, + "heading": -3.1349260454664436, + "angularVelocity": 3.0970432324121202e-22, + "velocityX": 0.02257919129671586, + "velocityY": -1.552371627787794, + "timestamp": 2.1953765834577417 + }, + { + "x": 2.1033897121209892, + "y": 6.090187708264656, + "heading": -3.1349260454664436, + "angularVelocity": -5.717605117434922e-23, + "velocityX": 0.16029945836581586, + "velocityY": -1.5422911795098355, + "timestamp": 2.224635212692289 + }, + { + "x": 2.1121017027787423, + "y": 6.045447928244468, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623983844657e-23, + "velocityX": 0.29775799091431626, + "velocityY": -1.5291140149276428, + "timestamp": 2.253893841926836 + }, + { + "x": 2.1248234606724927, + "y": 6.00120380324005, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623190409668e-23, + "velocityX": 0.43480361953307217, + "velocityY": -1.5121735420265516, + "timestamp": 2.283152471161383 + }, + { + "x": 2.141535772219987, + "y": 5.957591648743971, + "heading": -3.1349260454664436, + "angularVelocity": -5.717624354569263e-23, + "velocityX": 0.5711925672765078, + "velocityY": -1.4905740848795832, + "timestamp": 2.3124111003959302 + }, + { + "x": 2.1622074822868833, + "y": 5.91478389840203, + "heading": -3.1349260454664436, + "angularVelocity": -5.717622820098084e-23, + "velocityX": 0.706516696362894, + "velocityY": -1.4630811990108465, + "timestamp": 2.3416697296304774 + }, + { + "x": 2.1867865150959807, + "y": 5.873004121037691, + "heading": -3.1349260454664436, + "angularVelocity": -5.717608261879809e-23, + "velocityX": 0.8400609820801516, + "velocityY": -1.4279471888253283, + "timestamp": 2.3709283588650245 + }, + { + "x": 2.2151821291704135, + "y": 5.832550089747363, + "heading": -3.1349260454664436, + "angularVelocity": -5.717636301748886e-23, + "velocityX": 0.9705039100359978, + "velocityY": -1.3826359043014271, + "timestamp": 2.4001869880995716 + }, + { + "x": 2.247227781288673, + "y": 5.793828430179212, + "heading": -3.1349260454664436, + "angularVelocity": -5.717627842193595e-23, + "velocityX": 1.0952547319085686, + "velocityY": -1.3234269882491958, + "timestamp": 2.429445617334119 + }, + { + "x": 2.2826005084616283, + "y": 5.757399030074356, + "heading": -3.1349260454664436, + "angularVelocity": -5.717621943290754e-23, + "velocityX": 1.2089673405192627, + "velocityY": -1.245082256342438, + "timestamp": 2.458704246568666 + }, + { + "x": 2.320655607594778, + "y": 5.7239910126155, + "heading": -3.1349260454664436, + "angularVelocity": -5.717628005263719e-23, + "velocityX": 1.3006453182787074, + "velocityY": -1.141817587932542, + "timestamp": 2.487962875803213 + }, + { + "x": 2.360204689273028, + "y": 5.694336910489396, + "heading": -3.1349260454664436, + "angularVelocity": -5.717616763256594e-23, + "velocityX": 1.351706580688257, + "velocityY": -1.013516453161909, + "timestamp": 2.51722150503776 + }, + { + "x": 2.3996260680339816, + "y": 5.668721069816042, + "heading": -3.1349260454664436, + "angularVelocity": -5.717628968554313e-23, + "velocityX": 1.34734195662094, + "velocityY": -0.8754969505913973, + "timestamp": 2.5464801342723073 + }, + { + "x": 2.4375100420911338, + "y": 5.6468415720185074, + "heading": -3.1349260454664436, + "angularVelocity": -5.717625457879064e-23, + "velocityX": 1.2947966137939357, + "velocityY": -0.7477964063905799, + "timestamp": 2.5757387635068545 + }, + { + "x": 2.4729703851854588, + "y": 5.62819470377097, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623609342389e-23, + "velocityX": 1.2119618731994621, + "velocityY": -0.6373117516233145, + "timestamp": 2.6049973927414016 + }, + { + "x": 2.5055011746844404, + "y": 5.612330209413261, + "heading": -3.1349260454664436, + "angularVelocity": -5.717628902265798e-23, + "velocityX": 1.1118357335957716, + "velocityY": -0.5422159128005899, + "timestamp": 2.6342560219759488 + }, + { + "x": 2.5348060457698836, + "y": 5.598898274411973, + "heading": -3.1349260454664436, + "angularVelocity": -5.717627045197986e-23, + "velocityX": 1.0015804517181697, + "velocityY": -0.4590760180043621, + "timestamp": 2.663514651210496 + }, + { + "x": 2.5607008711079886, + "y": 5.587633246659649, + "heading": -3.1349260454664436, + "angularVelocity": -5.717608754941009e-23, + "velocityX": 0.8850320748298349, + "velocityY": -0.38501556795704556, + "timestamp": 2.692773280445043 + }, + { + "x": 2.5830643099643584, + "y": 5.578331178882631, + "heading": -3.1349260454664436, + "angularVelocity": -5.717623485224611e-23, + "velocityX": 0.7643365202491328, + "velocityY": -0.31792561785481754, + "timestamp": 2.72203190967959 + }, + { + "x": 2.601812219563059, + "y": 5.570832400631334, + "heading": -3.1349260454664436, + "angularVelocity": -5.717639538428928e-23, + "velocityX": 0.6407651379834913, + "velocityY": -0.2562928765800323, + "timestamp": 2.7512905389141373 + }, + { + "x": 2.6168837285238395, + "y": 5.565009326805624, + "heading": -3.1349260454664436, + "angularVelocity": -5.717612764732363e-23, + "velocityX": 0.5151132966602103, + "velocityY": -0.1990207326189503, + "timestamp": 2.7805491681486845 + }, + { + "x": 2.62823323421208, + "y": 5.560758067303841, + "heading": -3.1349260454664436, + "angularVelocity": -5.717624354345976e-23, + "velocityX": 0.3879028507202083, + "velocityY": -0.1452993394780887, + "timestamp": 2.8098077973832316 + }, + { + "x": 2.635825567663263, + "y": 5.557992596069733, + "heading": -3.1349260454664436, + "angularVelocity": -5.717624354345825e-23, + "velocityX": 0.2594904016290358, + "velocityY": -0.09451814067963153, + "timestamp": 2.8390664266177787 + }, + { + "x": 2.6396329402923584, + "y": 5.556640625, + "heading": -3.1349260454664436, + "angularVelocity": -5.717626024583988e-23, + "velocityX": 0.13012819563669478, + "velocityY": -0.04620760114590083, + "timestamp": 2.868325055852326 + }, + { + "x": 2.6396329402923584, + "y": 5.556640625, + "heading": -3.1349260454664436, + "angularVelocity": 6.493546095227337e-22, + "velocityX": 1.881202272196025e-21, + "velocityY": 2.989774730921632e-20, + "timestamp": 2.897583685086873 + }, + { + "x": 2.63578986878673, + "y": 5.555006216645995, + "heading": -3.138403367676192, + "angularVelocity": -0.11468866986671536, + "velocityX": -0.12675177409426172, + "velocityY": -0.053905881835223775, + "timestamp": 2.927903351482776 + }, + { + "x": 2.6281241915651026, + "y": 5.55168270192059, + "heading": -3.145319654992211, + "angularVelocity": -0.22811224984168904, + "velocityX": -0.25282854770026447, + "velocityY": -0.10961580783856312, + "timestamp": 2.958223017878679 + }, + { + "x": 2.616660760682821, + "y": 5.546605937980688, + "heading": -3.155626430554535, + "angularVelocity": -0.33993697119690924, + "velocityX": -0.37808565346951006, + "velocityY": -0.1674412862474532, + "timestamp": 2.9885426842745817 + }, + { + "x": 2.6014305324024507, + "y": 5.539699833963852, + "heading": -3.1692644048210545, + "angularVelocity": -0.4498062112042979, + "velocityX": -0.502321763092811, + "velocityY": -0.22777638535441594, + "timestamp": 3.0188623506704846 + }, + { + "x": 2.5824731253801407, + "y": 5.530872998614334, + "heading": -3.1861630308768345, + "angularVelocity": -0.5573486803952303, + "velocityX": -0.6252511744282022, + "velocityY": -0.2911257411039302, + "timestamp": 3.0491820170663875 + }, + { + "x": 2.5598407273275248, + "y": 5.520014195669526, + "heading": -3.2062405203859172, + "angularVelocity": -0.6621936154217003, + "velocityX": -0.7464593362305498, + "velocityY": -0.35814387938840914, + "timestamp": 3.0795016834622904 + }, + { + "x": 2.5336042337816798, + "y": 5.506986103666438, + "heading": -3.2294044879426838, + "angularVelocity": -0.7639915048635432, + "velocityX": -0.8653292289980953, + "velocityY": -0.4296911395064041, + "timestamp": 3.1098213498581933 + }, + { + "x": 2.503863218826844, + "y": 5.49161664330363, + "heading": -3.255552933908256, + "angularVelocity": -0.8624252531058615, + "velocityX": -0.9809149799500774, + "velocityY": -0.5069139007700986, + "timestamp": 3.140141016254096 + }, + { + "x": 2.470762785739885, + "y": 5.4736868205948594, + "heading": -3.2845732996383643, + "angularVelocity": -0.9571466041601998, + "velocityX": -1.0917149501167551, + "velocityY": -0.5913594983085235, + "timestamp": 3.170460682649999 + }, + { + "x": 2.4345235301066466, + "y": 5.452913761348259, + "heading": -3.3163303487290055, + "angularVelocity": -1.047407602576138, + "velocityX": -1.1952392602228237, + "velocityY": -0.6851348222423583, + "timestamp": 3.200780349045902 + }, + { + "x": 2.3954986328534775, + "y": 5.428928619205951, + "heading": -3.3506130763033335, + "angularVelocity": -1.1307092606718314, + "velocityX": -1.2871149947230278, + "velocityY": -0.7910753973717586, + "timestamp": 3.231100015441805 + }, + { + "x": 2.354290847908129, + "y": 5.401261287885048, + "heading": -3.3869757124641584, + "angularVelocity": -1.1993085836108863, + "velocityX": -1.3591107635311952, + "velocityY": -0.9125209677319055, + "timestamp": 3.2614196818377077 + }, + { + "x": 2.3119700393450096, + "y": 5.369418962959644, + "heading": -3.424488739130058, + "angularVelocity": -1.237250640428174, + "velocityX": -1.3958203896611634, + "velocityY": -1.050220161054137, + "timestamp": 3.2917393482336106 + }, + { + "x": 2.2701943697940816, + "y": 5.333273315136478, + "heading": -3.4617422361668626, + "angularVelocity": -1.2286908619099843, + "velocityX": -1.3778406729622434, + "velocityY": -1.1921518974243095, + "timestamp": 3.3220590146295135 + }, + { + "x": 2.2306531953186166, + "y": 5.2934548415358735, + "heading": -3.497295099309754, + "angularVelocity": -1.1726007363885618, + "velocityX": -1.3041427949485624, + "velocityY": -1.3132886450881536, + "timestamp": 3.3523786810254164 + }, + { + "x": 2.194384797808916, + "y": 5.250947089639415, + "heading": -3.5300729098031467, + "angularVelocity": -1.0810742461804137, + "velocityX": -1.1962004144809404, + "velocityY": -1.4019861347226739, + "timestamp": 3.3826983474213193 + }, + { + "x": 2.1618561434717347, + "y": 5.206562904098581, + "heading": -3.559395458180096, + "angularVelocity": -0.9671131599558667, + "velocityX": -1.0728566044374948, + "velocityY": -1.463874468844275, + "timestamp": 3.413018013817222 + }, + { + "x": 2.1332734403753273, + "y": 5.160862427241999, + "heading": -3.5849957373685863, + "angularVelocity": -0.8443456749890059, + "velocityX": -0.9427116619022493, + "velocityY": -1.5072882484862653, + "timestamp": 3.443337680213125 + }, + { + "x": 2.1087386455620734, + "y": 5.114234766629067, + "heading": -3.606823401382331, + "angularVelocity": -0.7199176840776125, + "velocityX": -0.8092039830811812, + "velocityY": -1.5378685241486245, + "timestamp": 3.473657346609028 + }, + { + "x": 2.0883065892590618, + "y": 5.0669625612384435, + "heading": -3.624907841959223, + "angularVelocity": -0.5964590883274463, + "velocityX": -0.6738878995637272, + "velocityY": -1.5591268311907651, + "timestamp": 3.503977013004931 + }, + { + "x": 2.0720078142934666, + "y": 5.019259378769025, + "heading": -3.6392985462841363, + "angularVelocity": -0.4746326736252551, + "velocityX": -0.5375644557817244, + "velocityY": -1.5733412711912018, + "timestamp": 3.5342966794008337 + }, + { + "x": 2.059859229705669, + "y": 4.97129159857966, + "heading": -3.6500430565181534, + "angularVelocity": -0.35437428940409044, + "velocityX": -0.4006833198346745, + "velocityY": -1.5820682049415709, + "timestamp": 3.5646163457967366 + }, + { + "x": 2.0518696849147733, + "y": 4.923191943386821, + "heading": -3.657180162870296, + "angularVelocity": -0.2353952797154437, + "velocityX": -0.26351031329218877, + "velocityY": -1.5864176922246054, + "timestamp": 3.5949360121926395 + }, + { + "x": 2.0480431107826926, + "y": 4.875068293638133, + "heading": -3.6607387948340793, + "angularVelocity": -0.11737041949327015, + "velocityX": -0.12620765948148724, + "velocityY": -1.5872090781053572, + "timestamp": 3.6252556785885424 + }, + { + "x": 2.0483803749084473, + "y": 4.827009677886963, + "heading": -3.6607387948340793, + "angularVelocity": 1.5097575312240631e-21, + "velocityX": 0.011123609387845542, + "velocityY": -1.5850641337420555, + "timestamp": 3.6555753449844453 + }, + { + "x": 2.052256902501467, + "y": 4.7835396477807635, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300342434097995e-22, + "velocityX": 0.14088183500646628, + "velocityY": -1.5797998239936404, + "timestamp": 3.683091508390709 + }, + { + "x": 2.0597005743510723, + "y": 4.740280724780923, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300332898125506e-22, + "velocityX": 0.2705199754676443, + "velocityY": -1.5721277113074286, + "timestamp": 3.710607671796973 + }, + { + "x": 2.070705854466647, + "y": 4.69731168064185, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300341717459463e-22, + "velocityX": 0.39995692542870404, + "velocityY": -1.5615928537944752, + "timestamp": 3.738123835203237 + }, + { + "x": 2.0852637266783254, + "y": 4.654727566618508, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300339024855326e-22, + "velocityX": 0.52906620725952, + "velocityY": -1.5476036173578895, + "timestamp": 3.765639998609501 + }, + { + "x": 2.1033596031452872, + "y": 4.612645056001655, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300340767321184e-22, + "velocityX": 0.6576453337547263, + "velocityY": -1.529374208007212, + "timestamp": 3.793156162015765 + }, + { + "x": 2.124969679557945, + "y": 4.571210215598577, + "heading": -3.6607387948340793, + "angularVelocity": 1.030034450352898e-22, + "velocityX": 0.7853593574649949, + "velocityY": -1.5058363984587355, + "timestamp": 3.820672325422029 + }, + { + "x": 2.1500542877457156, + "y": 4.530610063288077, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300340617397168e-22, + "velocityX": 0.9116317495799002, + "velocityY": -1.4755019335743174, + "timestamp": 3.848188488828293 + }, + { + "x": 2.178545140467357, + "y": 4.491089989654474, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300319327454968e-22, + "velocityX": 1.0354224279373974, + "velocityY": -1.4362494163925468, + "timestamp": 3.875704652234557 + }, + { + "x": 2.21031955626508, + "y": 4.452979625452379, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300351516928058e-22, + "velocityX": 1.1547545829188868, + "velocityY": -1.3850173674076385, + "timestamp": 3.903220815640821 + }, + { + "x": 2.245146390808226, + "y": 4.416727163520435, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300321637684974e-22, + "velocityX": 1.2656864268803893, + "velocityY": -1.317496970659786, + "timestamp": 3.9307369790470847 + }, + { + "x": 2.282575631489174, + "y": 4.382923484071245, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300349373445067e-22, + "velocityX": 1.3602637885358229, + "velocityY": -1.2285026422508498, + "timestamp": 3.9582531424533487 + }, + { + "x": 2.321764544418595, + "y": 4.352229884876951, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300341066851439e-22, + "velocityX": 1.4242142827403603, + "velocityY": -1.1154752478066263, + "timestamp": 3.9857693058596126 + }, + { + "x": 2.3614329153831446, + "y": 4.325077353660923, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300333549099457e-22, + "velocityX": 1.4416388788967012, + "velocityY": -0.9867847786456208, + "timestamp": 4.013285469265877 + }, + { + "x": 2.4002641272952094, + "y": 4.301398755593006, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300340140508024e-22, + "velocityX": 1.4112146137068142, + "velocityY": -0.8605341419991839, + "timestamp": 4.0408016326721405 + }, + { + "x": 2.4373189641856667, + "y": 4.28082072949791, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300338693239079e-22, + "velocityX": 1.3466571027123986, + "velocityY": -0.7478523001647727, + "timestamp": 4.0683177960784045 + }, + { + "x": 2.472031674195157, + "y": 4.262941500153538, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300337163704035e-22, + "velocityX": 1.2615388816018003, + "velocityY": -0.649771884270625, + "timestamp": 4.095833959484668 + }, + { + "x": 2.504066706947767, + "y": 4.247428515210111, + "heading": -3.6607387948340793, + "angularVelocity": 1.030033511464939e-22, + "velocityX": 1.1642259961762114, + "velocityY": -0.5637771775946641, + "timestamp": 4.123350122890932 + }, + { + "x": 2.5332162501951645, + "y": 4.234023378166114, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300349841571229e-22, + "velocityX": 1.0593607407040928, + "velocityY": -0.4871731878490917, + "timestamp": 4.150866286297196 + }, + { + "x": 2.5593444084951305, + "y": 4.222526200447489, + "heading": -3.6607387948340793, + "angularVelocity": 1.030032428740828e-22, + "velocityX": 0.9495567355890002, + "velocityY": -0.4178336037939713, + "timestamp": 4.17838244970346 + }, + { + "x": 2.582357728690195, + "y": 4.212780268556745, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300353293396209e-22, + "velocityX": 0.8363564300460482, + "velocityY": -0.35418934488933373, + "timestamp": 4.205898613109724 + }, + { + "x": 2.6021891216432027, + "y": 4.204660503766347, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300328136841497e-22, + "velocityX": 0.7207179525806894, + "velocityY": -0.2950907316007032, + "timestamp": 4.233414776515988 + }, + { + "x": 2.6187886631305672, + "y": 4.198065277647973, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300337513905507e-22, + "velocityX": 0.6032651152076492, + "velocityY": -0.23968552668501003, + "timestamp": 4.260930939922252 + }, + { + "x": 2.632118072440845, + "y": 4.192910648265756, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300328350956759e-22, + "velocityX": 0.48442106966299653, + "velocityY": -0.1873309627548638, + "timestamp": 4.288447103328516 + }, + { + "x": 2.6421472503261962, + "y": 4.189126258673718, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300343744876286e-22, + "velocityX": 0.36448314895028716, + "velocityY": -0.1375333303617742, + "timestamp": 4.31596326673478 + }, + { + "x": 2.648852022726871, + "y": 4.186652370386277, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300326335422924e-22, + "velocityX": 0.2436666878916012, + "velocityY": -0.08990673048827487, + "timestamp": 4.343479430141044 + }, + { + "x": 2.652212619781494, + "y": 4.1854376792907715, + "heading": -3.6607387948340793, + "angularVelocity": 1.0300339610415066e-22, + "velocityX": 0.12213174507630833, + "velocityY": -0.04414463882810916, + "timestamp": 4.370995593547308 + }, + { + "x": 2.652212619781494, + "y": 4.1854376792907715, + "heading": -3.6607387948340793, + "angularVelocity": 3.4334501124721124e-23, + "velocityX": 3.058092897395823e-21, + "velocityY": 8.734750320006533e-21, + "timestamp": 4.398511756953572 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing3Contested5.1.traj b/src/main/deploy/choreo/AmpWing3Contested5.1.traj new file mode 100644 index 00000000..2b206aff --- /dev/null +++ b/src/main/deploy/choreo/AmpWing3Contested5.1.traj @@ -0,0 +1,401 @@ +{ + "samples": [ + { + "x": 0.8817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "angularVelocity": -1.2014756905517071e-27, + "velocityX": -1.2223583879961949e-27, + "velocityY": -6.901280117928792e-27, + "timestamp": 0 + }, + { + "x": 0.8869818977350931, + "y": 6.608539823221273, + "heading": -2.1202506201761104, + "angularVelocity": -0.11208980721325, + "velocityX": 0.1531927732427503, + "velocityY": -0.026074847312906376, + "timestamp": 0.033895276864232556 + }, + { + "x": 0.8973773914804737, + "y": 6.606806194813917, + "heading": -2.1277869053789877, + "angularVelocity": -0.22234027569870818, + "velocityX": 0.30669446327344546, + "velocityY": -0.0511466070715543, + "timestamp": 0.06779055372846511 + }, + { + "x": 0.9129875332520139, + "y": 6.6042613695509065, + "heading": -2.1389872005229207, + "angularVelocity": -0.33043822562050607, + "velocityX": 0.46054032348125123, + "velocityY": -0.07507905225863701, + "timestamp": 0.10168583059269767 + }, + { + "x": 0.9338252508983529, + "y": 6.6009496119352935, + "heading": -2.153767308340282, + "angularVelocity": -0.43605213424167905, + "velocityX": 0.6147675892958303, + "velocityY": -0.09770557794461668, + "timestamp": 0.13558110745693022 + }, + { + "x": 0.9599047889558443, + "y": 6.5969221453368565, + "heading": -2.1720310680460675, + "angularVelocity": -0.5388290462692202, + "velocityX": 0.7694151064749511, + "velocityY": -0.1188208792207058, + "timestamp": 0.16947638432116277 + }, + { + "x": 0.9912417390730456, + "y": 6.592238845623578, + "heading": -2.193669425315102, + "angularVelocity": -0.6383885682865823, + "velocityX": 0.9245226183789964, + "velocityY": -0.13816968458577333, + "timestamp": 0.20337166118539532 + }, + { + "x": 1.027853029229915, + "y": 6.586970470643848, + "heading": -2.2185590685016074, + "angularVelocity": -0.7343100717601693, + "velocityX": 1.0801295503062518, + "velocityY": -0.15543094693789433, + "timestamp": 0.23726693804962787 + }, + { + "x": 1.0697568424657102, + "y": 6.581201673061091, + "heading": -2.2465601604436785, + "angularVelocity": -0.8261060104105233, + "velocityX": 1.2362729298138135, + "velocityY": -0.17019473261315557, + "timestamp": 0.27116221491386044 + }, + { + "x": 1.1169724052705505, + "y": 6.575035210978171, + "heading": -2.277512266109695, + "angularVelocity": -0.9131686927944219, + "velocityX": 1.3929835414521643, + "velocityY": -0.18192688343042243, + "timestamp": 0.305057491778093 + }, + { + "x": 1.1695195059195997, + "y": 6.568598078255894, + "heading": -2.3112268138699363, + "angularVelocity": -0.9946680151126935, + "velocityX": 1.5502779593607208, + "velocityY": -0.1899123806558918, + "timestamp": 0.33895276864232554 + }, + { + "x": 1.2274173885480242, + "y": 6.5620508605074, + "heading": -2.3474730638586254, + "angularVelocity": -1.0693599032653842, + "velocityX": 1.708140129975461, + "velocityY": -0.19316017906325608, + "timestamp": 0.3728480455065581 + }, + { + "x": 1.2906820743805074, + "y": 6.555602762480133, + "heading": -2.385952109409252, + "angularVelocity": -1.1352332569742463, + "velocityX": 1.86647496894301, + "velocityY": -0.19023588605263358, + "timestamp": 0.40674332237079064 + }, + { + "x": 1.3593194449788035, + "y": 6.549536996558709, + "heading": -2.426249199917607, + "angularVelocity": -1.1888703747653402, + "velocityX": 2.0249833294834234, + "velocityY": -0.17895608127705168, + "timestamp": 0.4406385992350232 + }, + { + "x": 1.433306250599446, + "y": 6.544255415834808, + "heading": -2.46774838929686, + "angularVelocity": -1.2243354596417166, + "velocityX": 2.182805761315846, + "velocityY": -0.15582055119526933, + "timestamp": 0.47453387609925574 + }, + { + "x": 1.5125352741691294, + "y": 6.540357169258217, + "heading": -2.5094867872592093, + "angularVelocity": -1.231392743287873, + "velocityX": 2.3374650069104015, + "velocityY": -0.11500854801113106, + "timestamp": 0.5084291529634883 + }, + { + "x": 1.5966520622230542, + "y": 6.538761495263786, + "heading": -2.5498872175403693, + "angularVelocity": -1.191919170419624, + "velocityX": 2.481666941115555, + "velocityY": -0.04707658830528645, + "timestamp": 0.5423244298277209 + }, + { + "x": 1.684609840222074, + "y": 6.540735678101622, + "heading": -2.58616040134833, + "angularVelocity": -1.0701545219191795, + "velocityX": 2.5949862675951723, + "velocityY": 0.058243596762563986, + "timestamp": 0.5762197066919534 + }, + { + "x": 1.7742611830095716, + "y": 6.547411083866923, + "heading": -2.61639770388055, + "angularVelocity": -0.8920801164520721, + "velocityX": 2.6449508923203644, + "velocityY": 0.1969420634042785, + "timestamp": 0.610114983556186 + }, + { + "x": 1.8627427547828321, + "y": 6.558330467635622, + "heading": -2.636370991885648, + "angularVelocity": -0.5892646366365757, + "velocityX": 2.610439564416991, + "velocityY": 0.3221505996967478, + "timestamp": 0.6440102604204185 + }, + { + "x": 1.9490348499949164, + "y": 6.573031022742947, + "heading": -2.6456655056562304, + "angularVelocity": -0.27421265233534975, + "velocityX": 2.545844235399729, + "velocityY": 0.4337051196309045, + "timestamp": 0.6779055372846511 + }, + { + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": -2.6456655056562304, + "angularVelocity": -5.472062340179423e-28, + "velocityX": 2.459579916917323, + "velocityY": 0.5379281964379007, + "timestamp": 0.7118008141488836 + }, + { + "x": 2.0935350616513824, + "y": 6.607202674935178, + "heading": -2.6456655056562304, + "angularVelocity": 4.486076522279272e-29, + "velocityX": 2.3668924862008094, + "velocityY": 0.6170990704788266, + "timestamp": 0.7376288015877945 + }, + { + "x": 2.151930892919423, + "y": 6.624698409831654, + "heading": -2.6456655056562304, + "angularVelocity": -1.1967477749725208e-28, + "velocityX": 2.260951667494798, + "velocityY": 0.6773944326036805, + "timestamp": 0.7634567890267054 + }, + { + "x": 2.207364580281582, + "y": 6.643260873915916, + "heading": -2.6456655056562304, + "angularVelocity": -3.73182424325557e-29, + "velocityX": 2.1462642992711727, + "velocityY": 0.7186957221566704, + "timestamp": 0.7892847764656163 + }, + { + "x": 2.2597112074994805, + "y": 6.662441616507122, + "heading": -2.6456655056562304, + "angularVelocity": -7.041879039105387e-29, + "velocityX": 2.0267404629071466, + "velocityY": 0.7426340374592862, + "timestamp": 0.8151127639045272 + }, + { + "x": 2.3089180745864937, + "y": 6.681854987475696, + "heading": -2.6456655056562304, + "angularVelocity": -5.359145584619787e-29, + "velocityX": 1.9051762048204006, + "velocityY": 0.7516408707605028, + "timestamp": 0.8409407513434382 + }, + { + "x": 2.354977814271759, + "y": 6.7011800425592005, + "heading": -2.6456655056562304, + "angularVelocity": -6.820300814682504e-29, + "velocityX": 1.7833267030273643, + "velocityY": 0.7482214837378254, + "timestamp": 0.8667687387823491 + }, + { + "x": 2.3979089062644126, + "y": 6.720153280877432, + "heading": -2.6456655056562304, + "angularVelocity": -1.4852566415875153e-29, + "velocityX": 1.6621926928761115, + "velocityY": 0.7345999514328465, + "timestamp": 0.89259672622126 + }, + { + "x": 2.43774324560733, + "y": 6.738558773488973, + "heading": -2.6456655056562304, + "angularVelocity": -1.0195969895119802e-28, + "velocityX": 1.5422935850938635, + "velocityY": 0.7126181494037732, + "timestamp": 0.9184247136601709 + }, + { + "x": 2.4745187542425713, + "y": 6.756218767940139, + "heading": -2.6456655056562304, + "angularVelocity": -1.4569555248631127e-28, + "velocityX": 1.4238627272924023, + "velocityY": 0.6837541830518434, + "timestamp": 0.9442527010990818 + }, + { + "x": 2.508275186661938, + "y": 6.7729857942130955, + "heading": -2.6456655056562304, + "angularVelocity": 6.354841834267747e-30, + "velocityX": 1.3069710715636813, + "velocityY": 0.6491805183278327, + "timestamp": 0.9700806885379927 + }, + { + "x": 2.5390518441434655, + "y": 6.788736378077184, + "heading": -2.6456655056562304, + "angularVelocity": -4.241645153599559e-29, + "velocityX": 1.191601070517836, + "velocityY": 0.6098262166706413, + "timestamp": 0.9959086759769036 + }, + { + "x": 2.5668863956450756, + "y": 6.803366146756738, + "heading": -2.6456655056562304, + "angularVelocity": -6.05857206571248e-29, + "velocityX": 1.0776895244914062, + "velocityY": 0.5664308422852048, + "timestamp": 1.0217366634158145 + }, + { + "x": 2.5918143285783564, + "y": 6.816786046375137, + "heading": -2.6456655056562304, + "angularVelocity": 4.0217337150651536e-29, + "velocityX": 0.9651519690506631, + "velocityY": 0.5195875075493303, + "timestamp": 1.0475646508547254 + }, + { + "x": 2.6138687520274657, + "y": 6.828919416468381, + "heading": -2.6456655056562304, + "angularVelocity": -1.6748569547134468e-28, + "velocityX": 0.8538963208524588, + "velocityY": 0.46977605676560424, + "timestamp": 1.0733926382936363 + }, + { + "x": 2.633080392488375, + "y": 6.839699715925182, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072902596e-29, + "velocityX": 0.7438303315869891, + "velocityY": 0.41738828789114973, + "timestamp": 1.0992206257325472 + }, + { + "x": 2.649477690401211, + "y": 6.849068742358771, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072816723e-29, + "velocityX": 0.6348654904536816, + "velocityY": 0.36274705707321936, + "timestamp": 1.125048613171458 + }, + { + "x": 2.663086945151386, + "y": 6.856975226116494, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072817978e-29, + "velocityX": 0.5269189007608046, + "velocityY": 0.30612078375921664, + "timestamp": 1.150876600610369 + }, + { + "x": 2.673932478991516, + "y": 6.863373710306793, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072817312e-29, + "velocityX": 0.4199140124944715, + "velocityY": 0.247734524628868, + "timestamp": 1.17670458804928 + }, + { + "x": 2.6820368035241824, + "y": 6.868223650766698, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572073639929e-29, + "velocityX": 0.31378072146871994, + "velocityY": 0.1877784891825456, + "timestamp": 1.2025325754881908 + }, + { + "x": 2.687420780025508, + "y": 6.871488686525517, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572073685113e-29, + "velocityX": 0.20845513085601136, + "velocityY": 0.12641464096035698, + "timestamp": 1.2283605629271017 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": -1.025796443021167e-28, + "velocityX": 0.10387914595382026, + "velocityY": 0.06378185783788577, + "timestamp": 1.2541885503660126 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": -2.636348366286149e-27, + "velocityX": 1.2064225206925918e-26, + "velocityY": -1.2304376584134913e-26, + "timestamp": 1.2800165378049235 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing3Contested5.2.traj b/src/main/deploy/choreo/AmpWing3Contested5.2.traj new file mode 100644 index 00000000..cd55be13 --- /dev/null +++ b/src/main/deploy/choreo/AmpWing3Contested5.2.traj @@ -0,0 +1,311 @@ +{ + "samples": [ + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": -2.636348366286149e-27, + "velocityX": 1.2064225206925918e-26, + "velocityY": -1.2304376584134913e-26, + "timestamp": 0 + }, + { + "x": 2.7076987449428147, + "y": 6.876455219250757, + "heading": -2.6505612004886614, + "angularVelocity": -0.07931047023704109, + "velocityX": 0.2850393743108695, + "velocityY": 0.053770791429821045, + "timestamp": 0.061728228540303576 + }, + { + "x": 2.7428910411738983, + "y": 6.883093308266834, + "heading": -2.660281614006307, + "angularVelocity": -0.1574711237873724, + "velocityX": 0.5701167369173091, + "velocityY": 0.10753733215821971, + "timestamp": 0.12345645708060715 + }, + { + "x": 2.7956833179679075, + "y": 6.893049993467995, + "heading": -2.674745154435654, + "angularVelocity": -0.23430998704108283, + "velocityX": 0.8552371911910641, + "velocityY": 0.16129873538588263, + "timestamp": 0.18518468562091073 + }, + { + "x": 2.866078630848398, + "y": 6.906324892630325, + "heading": -2.6938564282956072, + "angularVelocity": -0.3096034717321421, + "velocityX": 1.1404071450799549, + "velocityY": 0.21505394656938268, + "timestamp": 0.2469129141612143 + }, + { + "x": 2.9540805352004873, + "y": 6.922917544791187, + "heading": -2.7175018943072526, + "angularVelocity": -0.38305758274282437, + "velocityX": 1.4256346963631306, + "velocityY": 0.26880169013805943, + "timestamp": 0.3086411427015179 + }, + { + "x": 3.059693222941866, + "y": 6.942827391558645, + "heading": -2.7455437894569963, + "angularVelocity": -0.4542799269127636, + "velocityX": 1.710930156896079, + "velocityY": 0.32254038773295207, + "timestamp": 0.37036937124182145 + }, + { + "x": 3.182921703895506, + "y": 6.966053749056196, + "heading": -2.7778113788413443, + "angularVelocity": -0.522736358184659, + "velocityX": 1.99630677678012, + "velocityY": 0.37626800649861214, + "timestamp": 0.43209759978212503 + }, + { + "x": 3.3237720495046434, + "y": 6.99259576051427, + "heading": -2.81408784809302, + "angularVelocity": -0.5876803872314301, + "velocityX": 2.281781754309916, + "velocityY": 0.42998174555979446, + "timestamp": 0.4938258283224286 + }, + { + "x": 3.482251722494327, + "y": 7.02245230648218, + "heading": -2.8540896383173284, + "angularVelocity": -0.6480307497920543, + "velocityX": 2.567377628311637, + "velocityY": 0.48367734947094865, + "timestamp": 0.5555540568627322 + }, + { + "x": 3.658370014862313, + "y": 7.055621817346754, + "heading": -2.8974315872353036, + "angularVelocity": -0.7021414666010782, + "velocityX": 2.853124033083097, + "velocityY": 0.5373475255800785, + "timestamp": 0.6172822854030358 + }, + { + "x": 3.8521385634468457, + "y": 7.0921018429516485, + "heading": -2.943562541910987, + "angularVelocity": -0.7473234817610797, + "velocityX": 3.1390589551426644, + "velocityY": 0.5909780090493952, + "timestamp": 0.6790105139433393 + }, + { + "x": 4.063571547247647, + "y": 7.131887936757753, + "heading": -2.9916304108767116, + "angularVelocity": -0.7787015779067759, + "velocityX": 3.4252235776173627, + "velocityY": 0.6445364583907961, + "timestamp": 0.7407387424836429 + }, + { + "x": 4.292682637319209, + "y": 7.174970131274871, + "heading": -3.0401399686723773, + "angularVelocity": -0.7858569562545195, + "velocityX": 3.7116096717723086, + "velocityY": 0.697933433955407, + "timestamp": 0.8024669710239465 + }, + { + "x": 4.539450280266567, + "y": 7.2213162497869625, + "heading": -3.0857352356627743, + "angularVelocity": -0.7386453178488174, + "velocityX": 3.9976465999868798, + "velocityY": 0.7508091453139826, + "timestamp": 0.8641951995642501 + }, + { + "x": 4.802813151076208, + "y": 7.2704892670136925, + "heading": -3.1119417473367834, + "angularVelocity": -0.4245466343959359, + "velocityX": 4.266490016600536, + "velocityY": 0.796605028032253, + "timestamp": 0.9259234281045536 + }, + { + "x": 5.073062376801058, + "y": 7.321569302428323, + "heading": -3.1119417606999895, + "angularVelocity": -2.1648451741594153e-7, + "velocityX": 4.378049267174403, + "velocityY": 0.8274988060167595, + "timestamp": 0.9876516566448572 + }, + { + "x": 5.343311610885675, + "y": 7.372649293614076, + "heading": -3.1119417740630597, + "angularVelocity": -2.164823270096376e-7, + "velocityX": 4.378049402603003, + "velocityY": 0.8274980895070112, + "timestamp": 1.0493798851851608 + }, + { + "x": 5.614008499129137, + "y": 7.421301207393971, + "heading": -3.111941787439452, + "angularVelocity": -2.1669814427337908e-7, + "velocityX": 4.38530141953967, + "velocityY": 0.7881631294202577, + "timestamp": 1.1111081137254644 + }, + { + "x": 5.887305956856282, + "y": 7.452160778776353, + "heading": -3.111941805031968, + "angularVelocity": -2.849995312086089e-7, + "velocityX": 4.427430758825403, + "velocityY": 0.49992640501959174, + "timestamp": 1.172836342265768 + }, + { + "x": 6.157728807203207, + "y": 7.466194314480162, + "heading": -3.1290880401121526, + "angularVelocity": -0.27776975762376865, + "velocityX": 4.380861993639772, + "velocityY": 0.22734389169529218, + "timestamp": 1.2345645708060715 + }, + { + "x": 6.410865783691406, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": -0.2025752848143975, + "velocityX": 4.100830081701138, + "velocityY": 0.1512177294179093, + "timestamp": 1.296292799346375 + }, + { + "x": 6.68474663373147, + "y": 7.480945622329778, + "heading": -3.148801429523152, + "angularVelocity": -0.09918660878674208, + "velocityX": 3.7683669153032966, + "velocityY": 0.0745319969268312, + "timestamp": 1.3689717226560552 + }, + { + "x": 6.933913818291899, + "y": 7.483381950457323, + "heading": -3.152034611955166, + "angularVelocity": -0.04448583282168001, + "velocityX": 3.428327955530431, + "velocityY": 0.033521797195094405, + "timestamp": 1.4416506459657352 + }, + { + "x": 7.158227694750873, + "y": 7.4840407661449, + "heading": -3.1529052531279556, + "angularVelocity": -0.011979280005011805, + "velocityX": 3.086367632376563, + "velocityY": 0.00906474198536178, + "timestamp": 1.5143295692754153 + }, + { + "x": 7.357640364981529, + "y": 7.483610410531308, + "heading": -3.1523302695195397, + "angularVelocity": 0.007911284072907336, + "velocityX": 2.7437482718472905, + "velocityY": -0.005921326211146144, + "timestamp": 1.5870084925850954 + }, + { + "x": 7.532133153302445, + "y": 7.4825354434077695, + "heading": -3.1509003521726955, + "angularVelocity": 0.019674443177302856, + "velocityX": 2.4008719498693405, + "velocityY": -0.014790630826468705, + "timestamp": 1.6596874158947754 + }, + { + "x": 7.681698702311371, + "y": 7.481126333784936, + "heading": -3.149027604334941, + "angularVelocity": 0.02576741306107681, + "velocityX": 2.0578943963112653, + "velocityY": -0.019388146640932385, + "timestamp": 1.7323663392044555 + }, + { + "x": 7.806334577335972, + "y": 7.479612058816613, + "heading": -3.1470158430904474, + "angularVelocity": 0.02768011897921896, + "velocityX": 1.7148833437382505, + "velocityY": -0.02083513210385575, + "timestamp": 1.8050452625141356 + }, + { + "x": 7.906040631727904, + "y": 7.478168416966872, + "heading": -3.1450983025782047, + "angularVelocity": 0.026383722060277892, + "velocityX": 1.3718702734091426, + "velocityY": -0.01986328063212595, + "timestamp": 1.8777241858238156 + }, + { + "x": 7.98081781070478, + "y": 7.476934603107449, + "heading": -3.1434596612761303, + "angularVelocity": 0.022546306789553248, + "velocityX": 1.0288702084682209, + "velocityY": -0.016976226438656095, + "timestamp": 1.9504031091334957 + }, + { + "x": 8.03066757049182, + "y": 7.476023550788211, + "heading": -3.142249770303474, + "angularVelocity": 0.016647067919557137, + "velocityX": 0.6858901799443771, + "velocityY": -0.012535302915215497, + "timestamp": 2.0230820324431757 + }, + { + "x": 8.055591583251953, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": 0.009041365553547289, + "velocityX": 0.3429331589563321, + "velocityY": -0.006808490338895578, + "timestamp": 2.095760955752856 + }, + { + "x": 8.055591583251953, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": -3.3238121669195562e-28, + "velocityX": 1.3302229914992024e-27, + "velocityY": -3.7370579466818436e-26, + "timestamp": 2.168439879062536 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing3Contested5.3.traj b/src/main/deploy/choreo/AmpWing3Contested5.3.traj new file mode 100644 index 00000000..179c1c78 --- /dev/null +++ b/src/main/deploy/choreo/AmpWing3Contested5.3.traj @@ -0,0 +1,257 @@ +{ + "samples": [ + { + "x": 8.055591583251953, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": -3.3238121669195562e-28, + "velocityX": 1.3302229914992024e-27, + "velocityY": -3.7370579466818436e-26, + "timestamp": 0 + }, + { + "x": 8.020855812608946, + "y": 7.47166071609439, + "heading": -3.1343435110919597, + "angularVelocity": 0.08413316993333007, + "velocityX": -0.40314154331313246, + "velocityY": -0.04489181734832549, + "timestamp": 0.08616271684019106 + }, + { + "x": 7.951378131703854, + "y": 7.463923825990849, + "heading": -3.1201063976334886, + "angularVelocity": 0.1652351966207958, + "velocityX": -0.8063543427252293, + "velocityY": -0.08979394321896841, + "timestamp": 0.17232543368038256 + }, + { + "x": 7.8471512954777065, + "y": 7.452317003930522, + "heading": -3.099202536450615, + "angularVelocity": 0.24260912317382702, + "velocityX": -1.2096512279140474, + "velocityY": -0.13470817177056066, + "timestamp": 0.25848815052057406 + }, + { + "x": 7.708166650380872, + "y": 7.436839026074143, + "heading": -3.0720369478993717, + "angularVelocity": 0.3152824045883838, + "velocityX": -1.6130485457487815, + "velocityY": -0.1796366041368773, + "timestamp": 0.34465086736076556 + }, + { + "x": 7.534413751102053, + "y": 7.417488448912146, + "heading": -3.0391360738972004, + "angularVelocity": 0.3818458285524292, + "velocityX": -2.0165670913219236, + "velocityY": -0.22458178979995733, + "timestamp": 0.43081358420095706 + }, + { + "x": 7.325879923024415, + "y": 7.3942635524433395, + "heading": -3.0012114872247055, + "angularVelocity": 0.44015077591895324, + "velocityX": -2.4202327378373227, + "velocityY": -0.26954693770720584, + "timestamp": 0.5169763010411486 + }, + { + "x": 7.082549975883412, + "y": 7.367162267336314, + "heading": -2.95927784614972, + "angularVelocity": 0.48667965232295907, + "velocityX": -2.824074681771172, + "velocityY": -0.31453610216690825, + "timestamp": 0.6031390178813401 + }, + { + "x": 6.804407163920078, + "y": 7.336182164967317, + "heading": -2.9149001379339157, + "angularVelocity": 0.5150453681505026, + "velocityX": -3.2281109761106315, + "velocityY": -0.3595534531073048, + "timestamp": 0.6893017347215316 + }, + { + "x": 6.491441793733576, + "y": 7.3013211420751505, + "heading": -2.8708180414404794, + "angularVelocity": 0.5116145139108913, + "velocityX": -3.632259771554896, + "velocityY": -0.404595214387502, + "timestamp": 0.7754644515617231 + }, + { + "x": 6.143722098141646, + "y": 7.262584052476802, + "heading": -2.833133271102624, + "angularVelocity": 0.43736748004070747, + "velocityX": -4.035616660473416, + "velocityY": -0.4495806425207707, + "timestamp": 0.8616271684019146 + }, + { + "x": 5.763092713732822, + "y": 7.219914368051812, + "heading": -2.829938273384347, + "angularVelocity": 0.03708097696365238, + "velocityX": -4.417564793306024, + "velocityY": -0.49522213307329827, + "timestamp": 0.9477898852421061 + }, + { + "x": 5.381546707630396, + "y": 7.177432729436718, + "heading": -2.8299382625614427, + "angularVelocity": 1.256100666703822e-7, + "velocityX": -4.428203056898617, + "velocityY": -0.4930396832064479, + "timestamp": 1.0339526020822976 + }, + { + "x": 5.0000007011662335, + "y": 7.134951094070541, + "heading": -2.8299382517385405, + "angularVelocity": 1.2561003603790461e-7, + "velocityX": -4.428203061096927, + "velocityY": -0.49303964549967616, + "timestamp": 1.120115318922489 + }, + { + "x": 4.618454694702072, + "y": 7.092469458704354, + "heading": -2.8299382409156384, + "angularVelocity": 1.256100357232878e-7, + "velocityX": -4.428203061096916, + "velocityY": -0.4930396454997818, + "timestamp": 1.2062780357626806 + }, + { + "x": 4.236908688689301, + "y": 7.049987819284037, + "heading": -2.8299382300927336, + "angularVelocity": 1.2561006936450142e-7, + "velocityX": -4.428203055858097, + "velocityY": -0.4930396925518196, + "timestamp": 1.292440752602872 + }, + { + "x": 3.8563823708116955, + "y": 7.0072578552109235, + "heading": -2.8264076724560487, + "angularVelocity": 0.04097546788402064, + "velocityX": -4.41636860851753, + "velocityY": -0.49592173552705077, + "timestamp": 1.3786034694430636 + }, + { + "x": 3.5086860797405395, + "y": 6.968518605080416, + "heading": -2.787817768614683, + "angularVelocity": 0.4478724122979906, + "velocityX": -4.035345028825394, + "velocityY": -0.44960571754437684, + "timestamp": 1.464766186283255 + }, + { + "x": 3.1957416658397833, + "y": 6.933655163283351, + "heading": -2.742718748624896, + "angularVelocity": 0.5234168749974962, + "velocityX": -3.632016553994976, + "velocityY": -0.40462328807165143, + "timestamp": 1.5509289031234466 + }, + { + "x": 2.9176198484629934, + "y": 6.902672538405997, + "heading": -2.6973522860047296, + "angularVelocity": 0.5265208002239449, + "velocityX": -3.2278673140336474, + "velocityY": -0.35958272920779544, + "timestamp": 1.637091619963638 + }, + { + "x": 2.674310160776578, + "y": 6.875568579535636, + "heading": -2.654519075523016, + "angularVelocity": 0.49712000796304645, + "velocityX": -2.8238395515973513, + "velocityY": -0.31456713372480594, + "timestamp": 1.7232543368038296 + }, + { + "x": 2.4657949770738905, + "y": 6.852340853729729, + "heading": -2.6158118677401805, + "angularVelocity": 0.44923383572766007, + "velocityX": -2.420016352193573, + "velocityY": -0.269579774846101, + "timestamp": 1.809417053644021 + }, + { + "x": 2.2920584467479186, + "y": 6.832987389106081, + "heading": -2.582256733700525, + "angularVelocity": 0.38943913644101064, + "velocityX": -2.0163771141084914, + "velocityY": -0.22461530152934878, + "timestamp": 1.8955797704842126 + }, + { + "x": 2.1530874313369837, + "y": 6.8175066409515335, + "heading": -2.5545687221827422, + "angularVelocity": 0.321345618304218, + "velocityX": -1.6128903603247504, + "velocityY": -0.17966875607299837, + "timestamp": 1.981742487324404 + }, + { + "x": 2.0488711542448845, + "y": 6.805897385165772, + "heading": -2.53327431428957, + "angularVelocity": 0.2471417879344249, + "velocityX": -1.209528679154725, + "velocityY": -0.1347364174610917, + "timestamp": 2.0679052041645956 + }, + { + "x": 1.9794007109586114, + "y": 6.798158635561578, + "heading": -2.51877750640236, + "angularVelocity": 0.16824919662291424, + "velocityX": -0.8062703432985042, + "velocityY": -0.08981552448662153, + "timestamp": 2.154067921004787 + }, + { + "x": 1.944668650627136, + "y": 6.794289588928223, + "heading": -2.5113986210516552, + "angularVelocity": 0.085638958720288, + "velocityX": -0.40309848163091144, + "velocityY": -0.044903953534010145, + "timestamp": 2.2402306378449786 + }, + { + "x": 1.944668650627136, + "y": 6.794289588928223, + "heading": -2.5113986210516552, + "angularVelocity": -2.4070077255640283e-28, + "velocityX": 3.6388248341051744e-28, + "velocityY": -8.930765034898444e-28, + "timestamp": 2.32639335468517 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/AmpWing3Contested5.traj b/src/main/deploy/choreo/AmpWing3Contested5.traj new file mode 100644 index 00000000..cac999f1 --- /dev/null +++ b/src/main/deploy/choreo/AmpWing3Contested5.traj @@ -0,0 +1,941 @@ +{ + "samples": [ + { + "x": 0.8817893862724304, + "y": 6.609423637390137, + "heading": -2.116451305126959, + "angularVelocity": -1.2014756905517071e-27, + "velocityX": -1.2223583879961949e-27, + "velocityY": -6.901280117928792e-27, + "timestamp": 0 + }, + { + "x": 0.8869818977350931, + "y": 6.608539823221273, + "heading": -2.1202506201761104, + "angularVelocity": -0.11208980721325, + "velocityX": 0.1531927732427503, + "velocityY": -0.026074847312906376, + "timestamp": 0.033895276864232556 + }, + { + "x": 0.8973773914804737, + "y": 6.606806194813917, + "heading": -2.1277869053789877, + "angularVelocity": -0.22234027569870818, + "velocityX": 0.30669446327344546, + "velocityY": -0.0511466070715543, + "timestamp": 0.06779055372846511 + }, + { + "x": 0.9129875332520139, + "y": 6.6042613695509065, + "heading": -2.1389872005229207, + "angularVelocity": -0.33043822562050607, + "velocityX": 0.46054032348125123, + "velocityY": -0.07507905225863701, + "timestamp": 0.10168583059269767 + }, + { + "x": 0.9338252508983529, + "y": 6.6009496119352935, + "heading": -2.153767308340282, + "angularVelocity": -0.43605213424167905, + "velocityX": 0.6147675892958303, + "velocityY": -0.09770557794461668, + "timestamp": 0.13558110745693022 + }, + { + "x": 0.9599047889558443, + "y": 6.5969221453368565, + "heading": -2.1720310680460675, + "angularVelocity": -0.5388290462692202, + "velocityX": 0.7694151064749511, + "velocityY": -0.1188208792207058, + "timestamp": 0.16947638432116277 + }, + { + "x": 0.9912417390730456, + "y": 6.592238845623578, + "heading": -2.193669425315102, + "angularVelocity": -0.6383885682865823, + "velocityX": 0.9245226183789964, + "velocityY": -0.13816968458577333, + "timestamp": 0.20337166118539532 + }, + { + "x": 1.027853029229915, + "y": 6.586970470643848, + "heading": -2.2185590685016074, + "angularVelocity": -0.7343100717601693, + "velocityX": 1.0801295503062518, + "velocityY": -0.15543094693789433, + "timestamp": 0.23726693804962787 + }, + { + "x": 1.0697568424657102, + "y": 6.581201673061091, + "heading": -2.2465601604436785, + "angularVelocity": -0.8261060104105233, + "velocityX": 1.2362729298138135, + "velocityY": -0.17019473261315557, + "timestamp": 0.27116221491386044 + }, + { + "x": 1.1169724052705505, + "y": 6.575035210978171, + "heading": -2.277512266109695, + "angularVelocity": -0.9131686927944219, + "velocityX": 1.3929835414521643, + "velocityY": -0.18192688343042243, + "timestamp": 0.305057491778093 + }, + { + "x": 1.1695195059195997, + "y": 6.568598078255894, + "heading": -2.3112268138699363, + "angularVelocity": -0.9946680151126935, + "velocityX": 1.5502779593607208, + "velocityY": -0.1899123806558918, + "timestamp": 0.33895276864232554 + }, + { + "x": 1.2274173885480242, + "y": 6.5620508605074, + "heading": -2.3474730638586254, + "angularVelocity": -1.0693599032653842, + "velocityX": 1.708140129975461, + "velocityY": -0.19316017906325608, + "timestamp": 0.3728480455065581 + }, + { + "x": 1.2906820743805074, + "y": 6.555602762480133, + "heading": -2.385952109409252, + "angularVelocity": -1.1352332569742463, + "velocityX": 1.86647496894301, + "velocityY": -0.19023588605263358, + "timestamp": 0.40674332237079064 + }, + { + "x": 1.3593194449788035, + "y": 6.549536996558709, + "heading": -2.426249199917607, + "angularVelocity": -1.1888703747653402, + "velocityX": 2.0249833294834234, + "velocityY": -0.17895608127705168, + "timestamp": 0.4406385992350232 + }, + { + "x": 1.433306250599446, + "y": 6.544255415834808, + "heading": -2.46774838929686, + "angularVelocity": -1.2243354596417166, + "velocityX": 2.182805761315846, + "velocityY": -0.15582055119526933, + "timestamp": 0.47453387609925574 + }, + { + "x": 1.5125352741691294, + "y": 6.540357169258217, + "heading": -2.5094867872592093, + "angularVelocity": -1.231392743287873, + "velocityX": 2.3374650069104015, + "velocityY": -0.11500854801113106, + "timestamp": 0.5084291529634883 + }, + { + "x": 1.5966520622230542, + "y": 6.538761495263786, + "heading": -2.5498872175403693, + "angularVelocity": -1.191919170419624, + "velocityX": 2.481666941115555, + "velocityY": -0.04707658830528645, + "timestamp": 0.5423244298277209 + }, + { + "x": 1.684609840222074, + "y": 6.540735678101622, + "heading": -2.58616040134833, + "angularVelocity": -1.0701545219191795, + "velocityX": 2.5949862675951723, + "velocityY": 0.058243596762563986, + "timestamp": 0.5762197066919534 + }, + { + "x": 1.7742611830095716, + "y": 6.547411083866923, + "heading": -2.61639770388055, + "angularVelocity": -0.8920801164520721, + "velocityX": 2.6449508923203644, + "velocityY": 0.1969420634042785, + "timestamp": 0.610114983556186 + }, + { + "x": 1.8627427547828321, + "y": 6.558330467635622, + "heading": -2.636370991885648, + "angularVelocity": -0.5892646366365757, + "velocityX": 2.610439564416991, + "velocityY": 0.3221505996967478, + "timestamp": 0.6440102604204185 + }, + { + "x": 1.9490348499949164, + "y": 6.573031022742947, + "heading": -2.6456655056562304, + "angularVelocity": -0.27421265233534975, + "velocityX": 2.545844235399729, + "velocityY": 0.4337051196309045, + "timestamp": 0.6779055372846511 + }, + { + "x": 2.032402992248535, + "y": 6.591264247894287, + "heading": -2.6456655056562304, + "angularVelocity": -5.472062340179423e-28, + "velocityX": 2.459579916917323, + "velocityY": 0.5379281964379007, + "timestamp": 0.7118008141488836 + }, + { + "x": 2.0935350616513824, + "y": 6.607202674935178, + "heading": -2.6456655056562304, + "angularVelocity": 4.486076522279272e-29, + "velocityX": 2.3668924862008094, + "velocityY": 0.6170990704788266, + "timestamp": 0.7376288015877945 + }, + { + "x": 2.151930892919423, + "y": 6.624698409831654, + "heading": -2.6456655056562304, + "angularVelocity": -1.1967477749725208e-28, + "velocityX": 2.260951667494798, + "velocityY": 0.6773944326036805, + "timestamp": 0.7634567890267054 + }, + { + "x": 2.207364580281582, + "y": 6.643260873915916, + "heading": -2.6456655056562304, + "angularVelocity": -3.73182424325557e-29, + "velocityX": 2.1462642992711727, + "velocityY": 0.7186957221566704, + "timestamp": 0.7892847764656163 + }, + { + "x": 2.2597112074994805, + "y": 6.662441616507122, + "heading": -2.6456655056562304, + "angularVelocity": -7.041879039105387e-29, + "velocityX": 2.0267404629071466, + "velocityY": 0.7426340374592862, + "timestamp": 0.8151127639045272 + }, + { + "x": 2.3089180745864937, + "y": 6.681854987475696, + "heading": -2.6456655056562304, + "angularVelocity": -5.359145584619787e-29, + "velocityX": 1.9051762048204006, + "velocityY": 0.7516408707605028, + "timestamp": 0.8409407513434382 + }, + { + "x": 2.354977814271759, + "y": 6.7011800425592005, + "heading": -2.6456655056562304, + "angularVelocity": -6.820300814682504e-29, + "velocityX": 1.7833267030273643, + "velocityY": 0.7482214837378254, + "timestamp": 0.8667687387823491 + }, + { + "x": 2.3979089062644126, + "y": 6.720153280877432, + "heading": -2.6456655056562304, + "angularVelocity": -1.4852566415875153e-29, + "velocityX": 1.6621926928761115, + "velocityY": 0.7345999514328465, + "timestamp": 0.89259672622126 + }, + { + "x": 2.43774324560733, + "y": 6.738558773488973, + "heading": -2.6456655056562304, + "angularVelocity": -1.0195969895119802e-28, + "velocityX": 1.5422935850938635, + "velocityY": 0.7126181494037732, + "timestamp": 0.9184247136601709 + }, + { + "x": 2.4745187542425713, + "y": 6.756218767940139, + "heading": -2.6456655056562304, + "angularVelocity": -1.4569555248631127e-28, + "velocityX": 1.4238627272924023, + "velocityY": 0.6837541830518434, + "timestamp": 0.9442527010990818 + }, + { + "x": 2.508275186661938, + "y": 6.7729857942130955, + "heading": -2.6456655056562304, + "angularVelocity": 6.354841834267747e-30, + "velocityX": 1.3069710715636813, + "velocityY": 0.6491805183278327, + "timestamp": 0.9700806885379927 + }, + { + "x": 2.5390518441434655, + "y": 6.788736378077184, + "heading": -2.6456655056562304, + "angularVelocity": -4.241645153599559e-29, + "velocityX": 1.191601070517836, + "velocityY": 0.6098262166706413, + "timestamp": 0.9959086759769036 + }, + { + "x": 2.5668863956450756, + "y": 6.803366146756738, + "heading": -2.6456655056562304, + "angularVelocity": -6.05857206571248e-29, + "velocityX": 1.0776895244914062, + "velocityY": 0.5664308422852048, + "timestamp": 1.0217366634158145 + }, + { + "x": 2.5918143285783564, + "y": 6.816786046375137, + "heading": -2.6456655056562304, + "angularVelocity": 4.0217337150651536e-29, + "velocityX": 0.9651519690506631, + "velocityY": 0.5195875075493303, + "timestamp": 1.0475646508547254 + }, + { + "x": 2.6138687520274657, + "y": 6.828919416468381, + "heading": -2.6456655056562304, + "angularVelocity": -1.6748569547134468e-28, + "velocityX": 0.8538963208524588, + "velocityY": 0.46977605676560424, + "timestamp": 1.0733926382936363 + }, + { + "x": 2.633080392488375, + "y": 6.839699715925182, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072902596e-29, + "velocityX": 0.7438303315869891, + "velocityY": 0.41738828789114973, + "timestamp": 1.0992206257325472 + }, + { + "x": 2.649477690401211, + "y": 6.849068742358771, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072816723e-29, + "velocityX": 0.6348654904536816, + "velocityY": 0.36274705707321936, + "timestamp": 1.125048613171458 + }, + { + "x": 2.663086945151386, + "y": 6.856975226116494, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072817978e-29, + "velocityX": 0.5269189007608046, + "velocityY": 0.30612078375921664, + "timestamp": 1.150876600610369 + }, + { + "x": 2.673932478991516, + "y": 6.863373710306793, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572072817312e-29, + "velocityX": 0.4199140124944715, + "velocityY": 0.247734524628868, + "timestamp": 1.17670458804928 + }, + { + "x": 2.6820368035241824, + "y": 6.868223650766698, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572073639929e-29, + "velocityX": 0.31378072146871994, + "velocityY": 0.1877784891825456, + "timestamp": 1.2025325754881908 + }, + { + "x": 2.687420780025508, + "y": 6.871488686525517, + "heading": -2.6456655056562304, + "angularVelocity": -6.058572073685113e-29, + "velocityX": 0.20845513085601136, + "velocityY": 0.12641464096035698, + "timestamp": 1.2283605629271017 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": -1.025796443021167e-28, + "velocityX": 0.10387914595382026, + "velocityY": 0.06378185783788577, + "timestamp": 1.2541885503660126 + }, + { + "x": 2.690103769302368, + "y": 6.873136043548584, + "heading": -2.6456655056562304, + "angularVelocity": -2.636348366286149e-27, + "velocityX": 1.2064225206925918e-26, + "velocityY": -1.2304376584134913e-26, + "timestamp": 1.2800165378049235 + }, + { + "x": 2.7076987449428147, + "y": 6.876455219250757, + "heading": -2.6505612004886614, + "angularVelocity": -0.07931047023704109, + "velocityX": 0.2850393743108695, + "velocityY": 0.053770791429821045, + "timestamp": 1.341744766345227 + }, + { + "x": 2.7428910411738983, + "y": 6.883093308266834, + "heading": -2.660281614006307, + "angularVelocity": -0.1574711237873724, + "velocityX": 0.5701167369173091, + "velocityY": 0.10753733215821971, + "timestamp": 1.4034729948855307 + }, + { + "x": 2.7956833179679075, + "y": 6.893049993467995, + "heading": -2.674745154435654, + "angularVelocity": -0.23430998704108283, + "velocityX": 0.8552371911910641, + "velocityY": 0.16129873538588263, + "timestamp": 1.4652012234258343 + }, + { + "x": 2.866078630848398, + "y": 6.906324892630325, + "heading": -2.6938564282956072, + "angularVelocity": -0.3096034717321421, + "velocityX": 1.1404071450799549, + "velocityY": 0.21505394656938268, + "timestamp": 1.5269294519661378 + }, + { + "x": 2.9540805352004873, + "y": 6.922917544791187, + "heading": -2.7175018943072526, + "angularVelocity": -0.38305758274282437, + "velocityX": 1.4256346963631306, + "velocityY": 0.26880169013805943, + "timestamp": 1.5886576805064414 + }, + { + "x": 3.059693222941866, + "y": 6.942827391558645, + "heading": -2.7455437894569963, + "angularVelocity": -0.4542799269127636, + "velocityX": 1.710930156896079, + "velocityY": 0.32254038773295207, + "timestamp": 1.650385909046745 + }, + { + "x": 3.182921703895506, + "y": 6.966053749056196, + "heading": -2.7778113788413443, + "angularVelocity": -0.522736358184659, + "velocityX": 1.99630677678012, + "velocityY": 0.37626800649861214, + "timestamp": 1.7121141375870486 + }, + { + "x": 3.3237720495046434, + "y": 6.99259576051427, + "heading": -2.81408784809302, + "angularVelocity": -0.5876803872314301, + "velocityX": 2.281781754309916, + "velocityY": 0.42998174555979446, + "timestamp": 1.7738423661273521 + }, + { + "x": 3.482251722494327, + "y": 7.02245230648218, + "heading": -2.8540896383173284, + "angularVelocity": -0.6480307497920543, + "velocityX": 2.567377628311637, + "velocityY": 0.48367734947094865, + "timestamp": 1.8355705946676557 + }, + { + "x": 3.658370014862313, + "y": 7.055621817346754, + "heading": -2.8974315872353036, + "angularVelocity": -0.7021414666010782, + "velocityX": 2.853124033083097, + "velocityY": 0.5373475255800785, + "timestamp": 1.8972988232079593 + }, + { + "x": 3.8521385634468457, + "y": 7.0921018429516485, + "heading": -2.943562541910987, + "angularVelocity": -0.7473234817610797, + "velocityX": 3.1390589551426644, + "velocityY": 0.5909780090493952, + "timestamp": 1.9590270517482629 + }, + { + "x": 4.063571547247647, + "y": 7.131887936757753, + "heading": -2.9916304108767116, + "angularVelocity": -0.7787015779067759, + "velocityX": 3.4252235776173627, + "velocityY": 0.6445364583907961, + "timestamp": 2.0207552802885664 + }, + { + "x": 4.292682637319209, + "y": 7.174970131274871, + "heading": -3.0401399686723773, + "angularVelocity": -0.7858569562545195, + "velocityX": 3.7116096717723086, + "velocityY": 0.697933433955407, + "timestamp": 2.08248350882887 + }, + { + "x": 4.539450280266567, + "y": 7.2213162497869625, + "heading": -3.0857352356627743, + "angularVelocity": -0.7386453178488174, + "velocityX": 3.9976465999868798, + "velocityY": 0.7508091453139826, + "timestamp": 2.1442117373691736 + }, + { + "x": 4.802813151076208, + "y": 7.2704892670136925, + "heading": -3.1119417473367834, + "angularVelocity": -0.4245466343959359, + "velocityX": 4.266490016600536, + "velocityY": 0.796605028032253, + "timestamp": 2.205939965909477 + }, + { + "x": 5.073062376801058, + "y": 7.321569302428323, + "heading": -3.1119417606999895, + "angularVelocity": -2.1648451741594153e-7, + "velocityX": 4.378049267174403, + "velocityY": 0.8274988060167595, + "timestamp": 2.2676681944497807 + }, + { + "x": 5.343311610885675, + "y": 7.372649293614076, + "heading": -3.1119417740630597, + "angularVelocity": -2.164823270096376e-7, + "velocityX": 4.378049402603003, + "velocityY": 0.8274980895070112, + "timestamp": 2.3293964229900843 + }, + { + "x": 5.614008499129137, + "y": 7.421301207393971, + "heading": -3.111941787439452, + "angularVelocity": -2.1669814427337908e-7, + "velocityX": 4.38530141953967, + "velocityY": 0.7881631294202577, + "timestamp": 2.391124651530388 + }, + { + "x": 5.887305956856282, + "y": 7.452160778776353, + "heading": -3.111941805031968, + "angularVelocity": -2.849995312086089e-7, + "velocityX": 4.427430758825403, + "velocityY": 0.49992640501959174, + "timestamp": 2.4528528800706915 + }, + { + "x": 6.157728807203207, + "y": 7.466194314480162, + "heading": -3.1290880401121526, + "angularVelocity": -0.27776975762376865, + "velocityX": 4.380861993639772, + "velocityY": 0.22734389169529218, + "timestamp": 2.514581108610995 + }, + { + "x": 6.410865783691406, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": -0.2025752848143975, + "velocityX": 4.100830081701138, + "velocityY": 0.1512177294179093, + "timestamp": 2.5763093371512986 + }, + { + "x": 6.68474663373147, + "y": 7.480945622329778, + "heading": -3.148801429523152, + "angularVelocity": -0.09918660878674208, + "velocityX": 3.7683669153032966, + "velocityY": 0.0745319969268312, + "timestamp": 2.6489882604609787 + }, + { + "x": 6.933913818291899, + "y": 7.483381950457323, + "heading": -3.152034611955166, + "angularVelocity": -0.04448583282168001, + "velocityX": 3.428327955530431, + "velocityY": 0.033521797195094405, + "timestamp": 2.7216671837706587 + }, + { + "x": 7.158227694750873, + "y": 7.4840407661449, + "heading": -3.1529052531279556, + "angularVelocity": -0.011979280005011805, + "velocityX": 3.086367632376563, + "velocityY": 0.00906474198536178, + "timestamp": 2.794346107080339 + }, + { + "x": 7.357640364981529, + "y": 7.483610410531308, + "heading": -3.1523302695195397, + "angularVelocity": 0.007911284072907336, + "velocityX": 2.7437482718472905, + "velocityY": -0.005921326211146144, + "timestamp": 2.867025030390019 + }, + { + "x": 7.532133153302445, + "y": 7.4825354434077695, + "heading": -3.1509003521726955, + "angularVelocity": 0.019674443177302856, + "velocityX": 2.4008719498693405, + "velocityY": -0.014790630826468705, + "timestamp": 2.939703953699699 + }, + { + "x": 7.681698702311371, + "y": 7.481126333784936, + "heading": -3.149027604334941, + "angularVelocity": 0.02576741306107681, + "velocityX": 2.0578943963112653, + "velocityY": -0.019388146640932385, + "timestamp": 3.012382877009379 + }, + { + "x": 7.806334577335972, + "y": 7.479612058816613, + "heading": -3.1470158430904474, + "angularVelocity": 0.02768011897921896, + "velocityX": 1.7148833437382505, + "velocityY": -0.02083513210385575, + "timestamp": 3.085061800319059 + }, + { + "x": 7.906040631727904, + "y": 7.478168416966872, + "heading": -3.1450983025782047, + "angularVelocity": 0.026383722060277892, + "velocityX": 1.3718702734091426, + "velocityY": -0.01986328063212595, + "timestamp": 3.157740723628739 + }, + { + "x": 7.98081781070478, + "y": 7.476934603107449, + "heading": -3.1434596612761303, + "angularVelocity": 0.022546306789553248, + "velocityX": 1.0288702084682209, + "velocityY": -0.016976226438656095, + "timestamp": 3.230419646938419 + }, + { + "x": 8.03066757049182, + "y": 7.476023550788211, + "heading": -3.142249770303474, + "angularVelocity": 0.016647067919557137, + "velocityX": 0.6858901799443771, + "velocityY": -0.012535302915215497, + "timestamp": 3.3030985702480993 + }, + { + "x": 8.055591583251953, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": 0.009041365553547289, + "velocityX": 0.3429331589563321, + "velocityY": -0.006808490338895578, + "timestamp": 3.3757774935577793 + }, + { + "x": 8.055591583251953, + "y": 7.475528717041016, + "heading": -3.141592653589793, + "angularVelocity": -3.3238121669195562e-28, + "velocityX": 1.3302229914992024e-27, + "velocityY": -3.7370579466818436e-26, + "timestamp": 3.4484564168674594 + }, + { + "x": 8.020855812608946, + "y": 7.47166071609439, + "heading": -3.1343435110919597, + "angularVelocity": 0.08413316993333007, + "velocityX": -0.40314154331313246, + "velocityY": -0.04489181734832549, + "timestamp": 3.5346191337076505 + }, + { + "x": 7.951378131703854, + "y": 7.463923825990849, + "heading": -3.1201063976334886, + "angularVelocity": 0.1652351966207958, + "velocityX": -0.8063543427252293, + "velocityY": -0.08979394321896841, + "timestamp": 3.620781850547842 + }, + { + "x": 7.8471512954777065, + "y": 7.452317003930522, + "heading": -3.099202536450615, + "angularVelocity": 0.24260912317382702, + "velocityX": -1.2096512279140474, + "velocityY": -0.13470817177056066, + "timestamp": 3.7069445673880335 + }, + { + "x": 7.708166650380872, + "y": 7.436839026074143, + "heading": -3.0720369478993717, + "angularVelocity": 0.3152824045883838, + "velocityX": -1.6130485457487815, + "velocityY": -0.1796366041368773, + "timestamp": 3.793107284228225 + }, + { + "x": 7.534413751102053, + "y": 7.417488448912146, + "heading": -3.0391360738972004, + "angularVelocity": 0.3818458285524292, + "velocityX": -2.0165670913219236, + "velocityY": -0.22458178979995733, + "timestamp": 3.8792700010684165 + }, + { + "x": 7.325879923024415, + "y": 7.3942635524433395, + "heading": -3.0012114872247055, + "angularVelocity": 0.44015077591895324, + "velocityX": -2.4202327378373227, + "velocityY": -0.26954693770720584, + "timestamp": 3.965432717908608 + }, + { + "x": 7.082549975883412, + "y": 7.367162267336314, + "heading": -2.95927784614972, + "angularVelocity": 0.48667965232295907, + "velocityX": -2.824074681771172, + "velocityY": -0.31453610216690825, + "timestamp": 4.0515954347487995 + }, + { + "x": 6.804407163920078, + "y": 7.336182164967317, + "heading": -2.9149001379339157, + "angularVelocity": 0.5150453681505026, + "velocityX": -3.2281109761106315, + "velocityY": -0.3595534531073048, + "timestamp": 4.137758151588991 + }, + { + "x": 6.491441793733576, + "y": 7.3013211420751505, + "heading": -2.8708180414404794, + "angularVelocity": 0.5116145139108913, + "velocityX": -3.632259771554896, + "velocityY": -0.404595214387502, + "timestamp": 4.2239208684291825 + }, + { + "x": 6.143722098141646, + "y": 7.262584052476802, + "heading": -2.833133271102624, + "angularVelocity": 0.43736748004070747, + "velocityX": -4.035616660473416, + "velocityY": -0.4495806425207707, + "timestamp": 4.310083585269374 + }, + { + "x": 5.763092713732822, + "y": 7.219914368051812, + "heading": -2.829938273384347, + "angularVelocity": 0.03708097696365238, + "velocityX": -4.417564793306024, + "velocityY": -0.49522213307329827, + "timestamp": 4.3962463021095655 + }, + { + "x": 5.381546707630396, + "y": 7.177432729436718, + "heading": -2.8299382625614427, + "angularVelocity": 1.256100666703822e-7, + "velocityX": -4.428203056898617, + "velocityY": -0.4930396832064479, + "timestamp": 4.482409018949757 + }, + { + "x": 5.0000007011662335, + "y": 7.134951094070541, + "heading": -2.8299382517385405, + "angularVelocity": 1.2561003603790461e-7, + "velocityX": -4.428203061096927, + "velocityY": -0.49303964549967616, + "timestamp": 4.5685717357899485 + }, + { + "x": 4.618454694702072, + "y": 7.092469458704354, + "heading": -2.8299382409156384, + "angularVelocity": 1.256100357232878e-7, + "velocityX": -4.428203061096916, + "velocityY": -0.4930396454997818, + "timestamp": 4.65473445263014 + }, + { + "x": 4.236908688689301, + "y": 7.049987819284037, + "heading": -2.8299382300927336, + "angularVelocity": 1.2561006936450142e-7, + "velocityX": -4.428203055858097, + "velocityY": -0.4930396925518196, + "timestamp": 4.7408971694703315 + }, + { + "x": 3.8563823708116955, + "y": 7.0072578552109235, + "heading": -2.8264076724560487, + "angularVelocity": 0.04097546788402064, + "velocityX": -4.41636860851753, + "velocityY": -0.49592173552705077, + "timestamp": 4.827059886310523 + }, + { + "x": 3.5086860797405395, + "y": 6.968518605080416, + "heading": -2.787817768614683, + "angularVelocity": 0.4478724122979906, + "velocityX": -4.035345028825394, + "velocityY": -0.44960571754437684, + "timestamp": 4.9132226031507145 + }, + { + "x": 3.1957416658397833, + "y": 6.933655163283351, + "heading": -2.742718748624896, + "angularVelocity": 0.5234168749974962, + "velocityX": -3.632016553994976, + "velocityY": -0.40462328807165143, + "timestamp": 4.999385319990906 + }, + { + "x": 2.9176198484629934, + "y": 6.902672538405997, + "heading": -2.6973522860047296, + "angularVelocity": 0.5265208002239449, + "velocityX": -3.2278673140336474, + "velocityY": -0.35958272920779544, + "timestamp": 5.0855480368310975 + }, + { + "x": 2.674310160776578, + "y": 6.875568579535636, + "heading": -2.654519075523016, + "angularVelocity": 0.49712000796304645, + "velocityX": -2.8238395515973513, + "velocityY": -0.31456713372480594, + "timestamp": 5.171710753671289 + }, + { + "x": 2.4657949770738905, + "y": 6.852340853729729, + "heading": -2.6158118677401805, + "angularVelocity": 0.44923383572766007, + "velocityX": -2.420016352193573, + "velocityY": -0.269579774846101, + "timestamp": 5.2578734705114805 + }, + { + "x": 2.2920584467479186, + "y": 6.832987389106081, + "heading": -2.582256733700525, + "angularVelocity": 0.38943913644101064, + "velocityX": -2.0163771141084914, + "velocityY": -0.22461530152934878, + "timestamp": 5.344036187351672 + }, + { + "x": 2.1530874313369837, + "y": 6.8175066409515335, + "heading": -2.5545687221827422, + "angularVelocity": 0.321345618304218, + "velocityX": -1.6128903603247504, + "velocityY": -0.17966875607299837, + "timestamp": 5.4301989041918635 + }, + { + "x": 2.0488711542448845, + "y": 6.805897385165772, + "heading": -2.53327431428957, + "angularVelocity": 0.2471417879344249, + "velocityX": -1.209528679154725, + "velocityY": -0.1347364174610917, + "timestamp": 5.516361621032055 + }, + { + "x": 1.9794007109586114, + "y": 6.798158635561578, + "heading": -2.51877750640236, + "angularVelocity": 0.16824919662291424, + "velocityX": -0.8062703432985042, + "velocityY": -0.08981552448662153, + "timestamp": 5.6025243378722465 + }, + { + "x": 1.944668650627136, + "y": 6.794289588928223, + "heading": -2.5113986210516552, + "angularVelocity": 0.085638958720288, + "velocityX": -0.40309848163091144, + "velocityY": -0.044903953534010145, + "timestamp": 5.688687054712438 + }, + { + "x": 1.944668650627136, + "y": 6.794289588928223, + "heading": -2.5113986210516552, + "angularVelocity": -2.4070077255640283e-28, + "velocityX": 3.6388248341051744e-28, + "velocityY": -8.930765034898444e-28, + "timestamp": 5.7748497715526295 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/FrontWing1.1.traj b/src/main/deploy/choreo/FrontWing1.1.traj index 990bbd1d..33307025 100644 --- a/src/main/deploy/choreo/FrontWing1.1.traj +++ b/src/main/deploy/choreo/FrontWing1.1.traj @@ -4,154 +4,154 @@ "x": 1.2263859510421753, "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": 4.442897870116303e-27, - "velocityX": 5.36651543276403e-26, - "velocityY": -1.1808751253669755e-25, + "angularVelocity": 8.13803032196424e-26, + "velocityX": -1.9489451876882144e-24, + "velocityY": 8.026375946046321e-25, "timestamp": 0 }, { - "x": 1.2664719468759371, - "y": 5.574445984484986, - "heading": 2.936921539881242, - "angularVelocity": -2.425035961622917, - "velocityX": 0.4763363139734096, - "velocityY": -0.2336705712390479, - "timestamp": 0.08415481805151542 - }, - { - "x": 1.353066220057362, - "y": 5.511048868833387, - "heading": 2.8414585534340224, - "angularVelocity": -1.1343733924869426, - "velocityX": 1.028987706068311, - "velocityY": -0.753339109031048, - "timestamp": 0.16830963610303085 - }, - { - "x": 1.4537891107085041, - "y": 5.418622426082042, - "heading": 2.841458332012736, - "angularVelocity": -0.0000026311183569063204, - "velocityX": 1.196876102678809, - "velocityY": -1.0982905660228062, - "timestamp": 0.25246445415454627 - }, - { - "x": 1.554511990655445, - "y": 5.326195971665319, - "heading": 2.841458110592174, - "angularVelocity": -0.000002631109743829199, - "velocityX": 1.1968759754822746, - "velocityY": -1.0982907046408665, - "timestamp": 0.3366192722060617 - }, - { - "x": 1.6552348706023827, - "y": 5.233769517248598, - "heading": 2.8414578891715823, - "angularVelocity": -0.0000026311100983002847, - "velocityX": 1.196875975482236, - "velocityY": -1.098290704640839, - "timestamp": 0.4207740902575771 - }, - { - "x": 1.7559577505493258, - "y": 5.141343062831889, - "heading": 2.841457667750961, - "angularVelocity": -0.0000026311104525941368, - "velocityX": 1.1968759754823024, - "velocityY": -1.0982907046406973, - "timestamp": 0.5049289083090925 - }, - { - "x": 1.8566806304962746, - "y": 5.048916608415192, - "heading": 2.8414574463303097, - "angularVelocity": -0.0000026311108067721062, - "velocityX": 1.1968759754823688, - "velocityY": -1.0982907046405554, - "timestamp": 0.5890837263606079 - }, - { - "x": 1.957403510443229, - "y": 4.956490153998508, - "heading": 2.841457224909629, - "angularVelocity": -0.000002631111160911123, - "velocityX": 1.1968759754824354, - "velocityY": -1.0982907046404133, - "timestamp": 0.6732385444121234 - }, - { - "x": 2.058126390390189, - "y": 4.864063699581835, - "heading": 2.841457003488918, - "angularVelocity": -0.000002631111515369215, - "velocityX": 1.1968759754825018, - "velocityY": -1.0982907046402715, - "timestamp": 0.7573933624636389 - }, - { - "x": 2.158849270337155, - "y": 4.7716372451651745, - "heading": 2.8414567820681773, - "angularVelocity": -0.0000026311118694940932, - "velocityX": 1.1968759754825686, - "velocityY": -1.0982907046401296, - "timestamp": 0.8415481805151543 - }, - { - "x": 2.2595721502841255, - "y": 4.679210790748526, - "heading": 2.8414565606474067, - "angularVelocity": -0.000002631112223792794, - "velocityX": 1.196875975482635, - "velocityY": -1.0982907046399877, - "timestamp": 0.9257029985666698 - }, - { - "x": 2.3602950302311068, - "y": 4.586784336331894, - "heading": 2.8414563392266063, - "angularVelocity": -0.00000263111257823472, - "velocityX": 1.1968759754827565, - "velocityY": -1.098290704639786, - "timestamp": 1.0098578166181853 - }, - { - "x": 2.4610179157680534, - "y": 4.494357888007175, - "heading": 2.841456117805399, - "angularVelocity": -0.0000026311174148596253, - "velocityX": 1.1968760419075375, - "velocityY": -1.0982906322504282, - "timestamp": 1.0940126346697008 - }, - { - "x": 2.552458083373214, - "y": 4.421585489236337, - "heading": 2.775799514245796, - "angularVelocity": -0.7801882896284313, - "velocityX": 1.0865707956160633, - "velocityY": -0.8647442945725389, - "timestamp": 1.1781674527212163 - }, - { - "x": 2.593973159790039, - "y": 4.3895583152771, + "x": 1.2482482072967283, + "y": 5.573952919421757, + "heading": 3.124773295402233, + "angularVelocity": -0.20280711940248144, + "velocityX": 0.27324224631752764, + "velocityY": -0.2519364652079374, + "timestamp": 0.0800105274685353 + }, + { + "x": 1.2920158355867717, + "y": 5.53361430645858, + "heading": 3.093062693754785, + "angularVelocity": -0.396330366149863, + "velocityX": 0.547023368984232, + "velocityY": -0.504166317101698, + "timestamp": 0.1600210549370706 + }, + { + "x": 1.3577502186593637, + "y": 5.473058444102122, + "heading": 3.04703030236452, + "angularVelocity": -0.5753291828799363, + "velocityX": 0.821571675032918, + "velocityY": -0.7568486831969985, + "timestamp": 0.2400315824056059 + }, + { + "x": 1.4455473443213254, + "y": 5.392224990475853, + "heading": 2.988789648390865, + "angularVelocity": -0.7279123862364011, + "velocityX": 1.0973196707956758, + "velocityY": -1.0102852235045834, + "timestamp": 0.3200421098741412 + }, + { + "x": 1.5555774477752369, + "y": 5.2910218114000225, + "heading": 2.923252756085671, + "angularVelocity": -0.8191033652536196, + "velocityX": 1.3751953266047607, + "velocityY": -1.2648732895259254, + "timestamp": 0.40005263734267654 + }, + { + "x": 1.6878271047063615, + "y": 5.1697931195328435, + "heading": 2.871035349733247, + "angularVelocity": -0.6526316974095567, + "velocityX": 1.6529032005586097, + "velocityY": -1.515159263446336, + "timestamp": 0.48006316481121186 + }, + { + "x": 1.831736570260106, + "y": 5.036624867836762, + "heading": 2.8710353148498213, + "angularVelocity": -4.3598544731556554e-7, + "velocityX": 1.798631631447972, + "velocityY": -1.6643841243072752, + "timestamp": 0.5600736922797471 + }, + { + "x": 1.975646009225953, + "y": 4.903456587408076, + "heading": 2.871035279966796, + "angularVelocity": -4.359804455802179e-7, + "velocityX": 1.7986312991429831, + "velocityY": -1.6643844834175752, + "timestamp": 0.6400842197482824 + }, + { + "x": 2.119555478641212, + "y": 4.770288339884976, + "heading": 2.8710352450833696, + "angularVelocity": -4.359854554729099e-7, + "velocityX": 1.7986316797105584, + "velocityY": -1.6643840721518786, + "timestamp": 0.7200947472168178 + }, + { + "x": 2.2519584690825245, + "y": 4.649115765781956, + "heading": 2.818147172105817, + "angularVelocity": -0.6610139271779065, + "velocityX": 1.6548196172482494, + "velocityY": -1.5144578836912652, + "timestamp": 0.8001052746853531 + }, + { + "x": 2.3620785943033495, + "y": 4.547955940973104, + "heading": 2.7526679608031075, + "angularVelocity": -0.8183824475905371, + "velocityX": 1.3763204506323357, + "velocityY": -1.2643314324934865, + "timestamp": 0.8801158021538884 + }, + { + "x": 2.4499444602008893, + "y": 4.4671542394648975, + "heading": 2.6942149976329213, + "angularVelocity": -0.7305659020079995, + "velocityX": 1.098178810683297, + "velocityY": -1.0098883742515268, + "timestamp": 0.9601263296224237 + }, + { + "x": 2.5157266326233696, + "y": 4.40663000080001, + "heading": 2.6477299996969115, + "angularVelocity": -0.580986020299514, + "velocityX": 0.822168963307353, + "velocityY": -0.756453439063869, + "timestamp": 1.040136857090959 + }, + { + "x": 2.559524861701357, + "y": 4.366320347788332, + "heading": 2.615543117832924, + "angularVelocity": -0.4022830855182806, + "velocityX": 0.5474058285043993, + "velocityY": -0.5038043653384214, + "timestamp": 1.1201473845594943 + }, + { + "x": 2.5814027786254883, + "y": 4.346181392669678, "heading": 2.599013995570559, - "angularVelocity": -2.1007177339153333, - "velocityX": 0.4933178798082817, - "velocityY": -0.38057445433049264, - "timestamp": 1.2623222707727317 + "angularVelocity": -0.20658684282346457, + "velocityX": 0.27343797893014415, + "velocityY": -0.25170381643308454, + "timestamp": 1.2001579120280295 }, { - "x": 2.593973159790039, - "y": 4.3895583152771, + "x": 2.5814027786254883, + "y": 4.346181392669678, "heading": 2.599013995570559, - "angularVelocity": -1.7281844842695858e-24, - "velocityX": -1.1064710362675692e-24, - "velocityY": 7.502253930064392e-24, - "timestamp": 1.3464770888242472 + "angularVelocity": -6.228298358184451e-25, + "velocityX": 4.332162235643869e-24, + "velocityY": -9.3329937041195e-25, + "timestamp": 1.2801684394965647 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/FrontWing1.2.traj b/src/main/deploy/choreo/FrontWing1.2.traj index b50ede37..1212ce04 100644 --- a/src/main/deploy/choreo/FrontWing1.2.traj +++ b/src/main/deploy/choreo/FrontWing1.2.traj @@ -1,85 +1,103 @@ { "samples": [ { - "x": 2.593973159790039, - "y": 4.3895583152771, + "x": 2.5814027786254883, + "y": 4.346181392669678, "heading": 2.599013995570559, - "angularVelocity": -1.7281844842695858e-24, - "velocityX": -1.1064710362675692e-24, - "velocityY": 7.502253930064392e-24, + "angularVelocity": -6.228298358184451e-25, + "velocityX": 4.332162235643869e-24, + "velocityY": -9.3329937041195e-25, "timestamp": 0 }, { - "x": 2.5399319472382507, - "y": 4.43667284651752, - "heading": 2.699289039214539, - "angularVelocity": 1.1496592855145864, - "velocityX": -0.6195856870550529, - "velocityY": 0.5401708775667837, - "timestamp": 0.08722153155062529 + "x": 2.5566960314145897, + "y": 4.370853223600912, + "heading": 2.6069134460708416, + "angularVelocity": 0.0917703358537092, + "velocityX": -0.287025849369625, + "velocityY": 0.28662021625483314, + "timestamp": 0.0860784743435492 }, { - "x": 2.4355875318293467, - "y": 4.532520831999065, - "heading": 2.6992890864875387, - "angularVelocity": 5.419877275969795e-7, - "velocityX": -1.19631487264518, - "velocityY": 1.0989028027547665, - "timestamp": 0.17444306310125057 + "x": 2.5072824665913997, + "y": 4.4201968838581225, + "heading": 2.6227096530109684, + "angularVelocity": 0.18350937398218814, + "velocityX": -0.5740525166137891, + "velocityY": 0.5732404138608942, + "timestamp": 0.1721569486870984 }, { - "x": 2.331243168301696, - "y": 4.628368873961615, - "heading": 2.6992891337586675, - "angularVelocity": 5.419662778006892e-7, - "velocityX": -1.1963142778235591, - "velocityY": 1.0989034503128068, - "timestamp": 0.26166459465187586 + "x": 2.433161972783442, + "y": 4.494212390724074, + "heading": 2.6463978103323864, + "angularVelocity": 0.2751925786564924, + "velocityX": -0.8610804777061288, + "velocityY": 0.8598608122461178, + "timestamp": 0.2582354230306476 }, { - "x": 2.2268988047740805, - "y": 4.724216915924203, - "heading": 2.6992891810297954, - "angularVelocity": 5.419662665035014e-7, - "velocityX": -1.1963142778231501, - "velocityY": 1.098903450313253, - "timestamp": 0.34888612620250115 + "x": 2.3343343934772123, + "y": 4.5928997989282285, + "heading": 2.6779701614467033, + "angularVelocity": 0.3667856726678033, + "velocityX": -1.148110257063775, + "velocityY": 1.146481846440282, + "timestamp": 0.3443138973741968 }, { - "x": 2.122554441246435, - "y": 4.82006495788676, - "heading": 2.699289228300922, - "angularVelocity": 5.419662547194978e-7, - "velocityX": -1.1963142778234912, - "velocityY": 1.0989034503128838, - "timestamp": 0.43610765775312643 + "x": 2.210797624410119, + "y": 4.716260426889983, + "heading": 2.7172978240148544, + "angularVelocity": 0.45688150107296166, + "velocityX": -1.4351644822844225, + "velocityY": 1.4331181971163696, + "timestamp": 0.430392371717746 }, { - "x": 2.0182100346281864, - "y": 4.915912952938407, - "heading": 2.699289275573439, - "angularVelocity": 5.419821924710352e-7, - "velocityX": -1.1963147718597973, - "velocityY": 1.0989029124765504, - "timestamp": 0.5233291893037517 + "x": 2.1119680830208534, + "y": 4.81494906749223, + "heading": 2.7487514884444098, + "angularVelocity": 0.3654068531003482, + "velocityX": -1.1481330511833399, + "velocityY": 1.1464961635863742, + "timestamp": 0.5164708460612952 + }, + { + "x": 2.037845879093483, + "y": 4.888965610568293, + "heading": 2.772338013759402, + "angularVelocity": 0.27401188851065944, + "velocityX": -0.8611003446871089, + "velocityY": 0.859872850216351, + "timestamp": 0.6025493204048444 + }, + { + "x": 1.9884310645624592, + "y": 4.938309990590957, + "heading": 2.7880613204490317, + "angularVelocity": 0.18266246944475417, + "velocityX": -0.574067034852447, + "velocityY": 0.5732487755966209, + "timestamp": 0.6886277947483936 }, { "x": 1.9637236595153809, "y": 4.962982177734375, "heading": 2.79592312968437, - "angularVelocity": 1.1079128329091787, - "velocityX": -0.6246895020546673, - "velocityY": 0.5396514365108126, - "timestamp": 0.610550720854377 + "angularVelocity": 0.09133304575033727, + "velocityX": -0.28703349165400144, + "velocityY": 0.28662435448086987, + "timestamp": 0.7747062690919426 }, { "x": 1.9637236595153809, "y": 4.962982177734375, "heading": 2.79592312968437, - "angularVelocity": 3.9745579613293646e-24, - "velocityX": 5.1209291302608005e-24, - "velocityY": -1.0346712550534795e-24, - "timestamp": 0.6977722524050023 + "angularVelocity": 1.375760922880761e-26, + "velocityX": -3.7767209080452686e-25, + "velocityY": 4.039448182062531e-25, + "timestamp": 0.8607847434354916 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/FrontWing1.traj b/src/main/deploy/choreo/FrontWing1.traj index c2bb5840..4adf66a6 100644 --- a/src/main/deploy/choreo/FrontWing1.traj +++ b/src/main/deploy/choreo/FrontWing1.traj @@ -4,226 +4,244 @@ "x": 1.2263859510421753, "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": 4.442897870116303e-27, - "velocityX": 5.36651543276403e-26, - "velocityY": -1.1808751253669755e-25, + "angularVelocity": 8.13803032196424e-26, + "velocityX": -1.9489451876882144e-24, + "velocityY": 8.026375946046321e-25, "timestamp": 0 }, { - "x": 1.2664719468759371, - "y": 5.574445984484986, - "heading": 2.936921539881242, - "angularVelocity": -2.425035961622917, - "velocityX": 0.4763363139734096, - "velocityY": -0.2336705712390479, - "timestamp": 0.08415481805151542 - }, - { - "x": 1.353066220057362, - "y": 5.511048868833387, - "heading": 2.8414585534340224, - "angularVelocity": -1.1343733924869426, - "velocityX": 1.028987706068311, - "velocityY": -0.753339109031048, - "timestamp": 0.16830963610303085 - }, - { - "x": 1.4537891107085041, - "y": 5.418622426082042, - "heading": 2.841458332012736, - "angularVelocity": -0.0000026311183569063204, - "velocityX": 1.196876102678809, - "velocityY": -1.0982905660228062, - "timestamp": 0.25246445415454627 - }, - { - "x": 1.554511990655445, - "y": 5.326195971665319, - "heading": 2.841458110592174, - "angularVelocity": -0.000002631109743829199, - "velocityX": 1.1968759754822746, - "velocityY": -1.0982907046408665, - "timestamp": 0.3366192722060617 - }, - { - "x": 1.6552348706023827, - "y": 5.233769517248598, - "heading": 2.8414578891715823, - "angularVelocity": -0.0000026311100983002847, - "velocityX": 1.196875975482236, - "velocityY": -1.098290704640839, - "timestamp": 0.4207740902575771 - }, - { - "x": 1.7559577505493258, - "y": 5.141343062831889, - "heading": 2.841457667750961, - "angularVelocity": -0.0000026311104525941368, - "velocityX": 1.1968759754823024, - "velocityY": -1.0982907046406973, - "timestamp": 0.5049289083090925 - }, - { - "x": 1.8566806304962746, - "y": 5.048916608415192, - "heading": 2.8414574463303097, - "angularVelocity": -0.0000026311108067721062, - "velocityX": 1.1968759754823688, - "velocityY": -1.0982907046405554, - "timestamp": 0.5890837263606079 - }, - { - "x": 1.957403510443229, - "y": 4.956490153998508, - "heading": 2.841457224909629, - "angularVelocity": -0.000002631111160911123, - "velocityX": 1.1968759754824354, - "velocityY": -1.0982907046404133, - "timestamp": 0.6732385444121234 - }, - { - "x": 2.058126390390189, - "y": 4.864063699581835, - "heading": 2.841457003488918, - "angularVelocity": -0.000002631111515369215, - "velocityX": 1.1968759754825018, - "velocityY": -1.0982907046402715, - "timestamp": 0.7573933624636389 - }, - { - "x": 2.158849270337155, - "y": 4.7716372451651745, - "heading": 2.8414567820681773, - "angularVelocity": -0.0000026311118694940932, - "velocityX": 1.1968759754825686, - "velocityY": -1.0982907046401296, - "timestamp": 0.8415481805151543 - }, - { - "x": 2.2595721502841255, - "y": 4.679210790748526, - "heading": 2.8414565606474067, - "angularVelocity": -0.000002631112223792794, - "velocityX": 1.196875975482635, - "velocityY": -1.0982907046399877, - "timestamp": 0.9257029985666698 - }, - { - "x": 2.3602950302311068, - "y": 4.586784336331894, - "heading": 2.8414563392266063, - "angularVelocity": -0.00000263111257823472, - "velocityX": 1.1968759754827565, - "velocityY": -1.098290704639786, - "timestamp": 1.0098578166181853 - }, - { - "x": 2.4610179157680534, - "y": 4.494357888007175, - "heading": 2.841456117805399, - "angularVelocity": -0.0000026311174148596253, - "velocityX": 1.1968760419075375, - "velocityY": -1.0982906322504282, - "timestamp": 1.0940126346697008 - }, - { - "x": 2.552458083373214, - "y": 4.421585489236337, - "heading": 2.775799514245796, - "angularVelocity": -0.7801882896284313, - "velocityX": 1.0865707956160633, - "velocityY": -0.8647442945725389, - "timestamp": 1.1781674527212163 - }, - { - "x": 2.593973159790039, - "y": 4.3895583152771, + "x": 1.2482482072967283, + "y": 5.573952919421757, + "heading": 3.124773295402233, + "angularVelocity": -0.20280711940248144, + "velocityX": 0.27324224631752764, + "velocityY": -0.2519364652079374, + "timestamp": 0.0800105274685353 + }, + { + "x": 1.2920158355867717, + "y": 5.53361430645858, + "heading": 3.093062693754785, + "angularVelocity": -0.396330366149863, + "velocityX": 0.547023368984232, + "velocityY": -0.504166317101698, + "timestamp": 0.1600210549370706 + }, + { + "x": 1.3577502186593637, + "y": 5.473058444102122, + "heading": 3.04703030236452, + "angularVelocity": -0.5753291828799363, + "velocityX": 0.821571675032918, + "velocityY": -0.7568486831969985, + "timestamp": 0.2400315824056059 + }, + { + "x": 1.4455473443213254, + "y": 5.392224990475853, + "heading": 2.988789648390865, + "angularVelocity": -0.7279123862364011, + "velocityX": 1.0973196707956758, + "velocityY": -1.0102852235045834, + "timestamp": 0.3200421098741412 + }, + { + "x": 1.5555774477752369, + "y": 5.2910218114000225, + "heading": 2.923252756085671, + "angularVelocity": -0.8191033652536196, + "velocityX": 1.3751953266047607, + "velocityY": -1.2648732895259254, + "timestamp": 0.40005263734267654 + }, + { + "x": 1.6878271047063615, + "y": 5.1697931195328435, + "heading": 2.871035349733247, + "angularVelocity": -0.6526316974095567, + "velocityX": 1.6529032005586097, + "velocityY": -1.515159263446336, + "timestamp": 0.48006316481121186 + }, + { + "x": 1.831736570260106, + "y": 5.036624867836762, + "heading": 2.8710353148498213, + "angularVelocity": -4.3598544731556554e-7, + "velocityX": 1.798631631447972, + "velocityY": -1.6643841243072752, + "timestamp": 0.5600736922797471 + }, + { + "x": 1.975646009225953, + "y": 4.903456587408076, + "heading": 2.871035279966796, + "angularVelocity": -4.359804455802179e-7, + "velocityX": 1.7986312991429831, + "velocityY": -1.6643844834175752, + "timestamp": 0.6400842197482824 + }, + { + "x": 2.119555478641212, + "y": 4.770288339884976, + "heading": 2.8710352450833696, + "angularVelocity": -4.359854554729099e-7, + "velocityX": 1.7986316797105584, + "velocityY": -1.6643840721518786, + "timestamp": 0.7200947472168178 + }, + { + "x": 2.2519584690825245, + "y": 4.649115765781956, + "heading": 2.818147172105817, + "angularVelocity": -0.6610139271779065, + "velocityX": 1.6548196172482494, + "velocityY": -1.5144578836912652, + "timestamp": 0.8001052746853531 + }, + { + "x": 2.3620785943033495, + "y": 4.547955940973104, + "heading": 2.7526679608031075, + "angularVelocity": -0.8183824475905371, + "velocityX": 1.3763204506323357, + "velocityY": -1.2643314324934865, + "timestamp": 0.8801158021538884 + }, + { + "x": 2.4499444602008893, + "y": 4.4671542394648975, + "heading": 2.6942149976329213, + "angularVelocity": -0.7305659020079995, + "velocityX": 1.098178810683297, + "velocityY": -1.0098883742515268, + "timestamp": 0.9601263296224237 + }, + { + "x": 2.5157266326233696, + "y": 4.40663000080001, + "heading": 2.6477299996969115, + "angularVelocity": -0.580986020299514, + "velocityX": 0.822168963307353, + "velocityY": -0.756453439063869, + "timestamp": 1.040136857090959 + }, + { + "x": 2.559524861701357, + "y": 4.366320347788332, + "heading": 2.615543117832924, + "angularVelocity": -0.4022830855182806, + "velocityX": 0.5474058285043993, + "velocityY": -0.5038043653384214, + "timestamp": 1.1201473845594943 + }, + { + "x": 2.5814027786254883, + "y": 4.346181392669678, "heading": 2.599013995570559, - "angularVelocity": -2.1007177339153333, - "velocityX": 0.4933178798082817, - "velocityY": -0.38057445433049264, - "timestamp": 1.2623222707727317 + "angularVelocity": -0.20658684282346457, + "velocityX": 0.27343797893014415, + "velocityY": -0.25170381643308454, + "timestamp": 1.2001579120280295 }, { - "x": 2.593973159790039, - "y": 4.3895583152771, + "x": 2.5814027786254883, + "y": 4.346181392669678, "heading": 2.599013995570559, - "angularVelocity": -1.7281844842695858e-24, - "velocityX": -1.1064710362675692e-24, - "velocityY": 7.502253930064392e-24, - "timestamp": 1.3464770888242472 - }, - { - "x": 2.5399319472382507, - "y": 4.43667284651752, - "heading": 2.699289039214539, - "angularVelocity": 1.1496592855145864, - "velocityX": -0.6195856870550529, - "velocityY": 0.5401708775667837, - "timestamp": 1.4336986203748725 - }, - { - "x": 2.4355875318293467, - "y": 4.532520831999065, - "heading": 2.6992890864875387, - "angularVelocity": 5.419877275969795e-7, - "velocityX": -1.19631487264518, - "velocityY": 1.0989028027547665, - "timestamp": 1.5209201519254978 - }, - { - "x": 2.331243168301696, - "y": 4.628368873961615, - "heading": 2.6992891337586675, - "angularVelocity": 5.419662778006892e-7, - "velocityX": -1.1963142778235591, - "velocityY": 1.0989034503128068, - "timestamp": 1.608141683476123 - }, - { - "x": 2.2268988047740805, - "y": 4.724216915924203, - "heading": 2.6992891810297954, - "angularVelocity": 5.419662665035014e-7, - "velocityX": -1.1963142778231501, - "velocityY": 1.098903450313253, - "timestamp": 1.6953632150267484 - }, - { - "x": 2.122554441246435, - "y": 4.82006495788676, - "heading": 2.699289228300922, - "angularVelocity": 5.419662547194978e-7, - "velocityX": -1.1963142778234912, - "velocityY": 1.0989034503128838, - "timestamp": 1.7825847465773736 - }, - { - "x": 2.0182100346281864, - "y": 4.915912952938407, - "heading": 2.699289275573439, - "angularVelocity": 5.419821924710352e-7, - "velocityX": -1.1963147718597973, - "velocityY": 1.0989029124765504, - "timestamp": 1.869806278127999 + "angularVelocity": -6.228298358184451e-25, + "velocityX": 4.332162235643869e-24, + "velocityY": -9.3329937041195e-25, + "timestamp": 1.2801684394965647 + }, + { + "x": 2.5566960314145897, + "y": 4.370853223600912, + "heading": 2.6069134460708416, + "angularVelocity": 0.0917703358537092, + "velocityX": -0.287025849369625, + "velocityY": 0.28662021625483314, + "timestamp": 1.3662469138401139 + }, + { + "x": 2.5072824665913997, + "y": 4.4201968838581225, + "heading": 2.6227096530109684, + "angularVelocity": 0.18350937398218814, + "velocityX": -0.5740525166137891, + "velocityY": 0.5732404138608942, + "timestamp": 1.452325388183663 + }, + { + "x": 2.433161972783442, + "y": 4.494212390724074, + "heading": 2.6463978103323864, + "angularVelocity": 0.2751925786564924, + "velocityX": -0.8610804777061288, + "velocityY": 0.8598608122461178, + "timestamp": 1.5384038625272123 + }, + { + "x": 2.3343343934772123, + "y": 4.5928997989282285, + "heading": 2.6779701614467033, + "angularVelocity": 0.3667856726678033, + "velocityX": -1.148110257063775, + "velocityY": 1.146481846440282, + "timestamp": 1.6244823368707615 + }, + { + "x": 2.210797624410119, + "y": 4.716260426889983, + "heading": 2.7172978240148544, + "angularVelocity": 0.45688150107296166, + "velocityX": -1.4351644822844225, + "velocityY": 1.4331181971163696, + "timestamp": 1.7105608112143107 + }, + { + "x": 2.1119680830208534, + "y": 4.81494906749223, + "heading": 2.7487514884444098, + "angularVelocity": 0.3654068531003482, + "velocityX": -1.1481330511833399, + "velocityY": 1.1464961635863742, + "timestamp": 1.79663928555786 + }, + { + "x": 2.037845879093483, + "y": 4.888965610568293, + "heading": 2.772338013759402, + "angularVelocity": 0.27401188851065944, + "velocityX": -0.8611003446871089, + "velocityY": 0.859872850216351, + "timestamp": 1.882717759901409 + }, + { + "x": 1.9884310645624592, + "y": 4.938309990590957, + "heading": 2.7880613204490317, + "angularVelocity": 0.18266246944475417, + "velocityX": -0.574067034852447, + "velocityY": 0.5732487755966209, + "timestamp": 1.9687962342449583 }, { "x": 1.9637236595153809, "y": 4.962982177734375, "heading": 2.79592312968437, - "angularVelocity": 1.1079128329091787, - "velocityX": -0.6246895020546673, - "velocityY": 0.5396514365108126, - "timestamp": 1.9570278096786242 + "angularVelocity": 0.09133304575033727, + "velocityX": -0.28703349165400144, + "velocityY": 0.28662435448086987, + "timestamp": 2.0548747085885073 }, { "x": 1.9637236595153809, "y": 4.962982177734375, "heading": 2.79592312968437, - "angularVelocity": 3.9745579613293646e-24, - "velocityX": 5.1209291302608005e-24, - "velocityY": -1.0346712550534795e-24, - "timestamp": 2.0442493412292495 + "angularVelocity": 1.375760922880761e-26, + "velocityX": -3.7767209080452686e-25, + "velocityY": 4.039448182062531e-25, + "timestamp": 2.1409531829320563 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/FrontWing2.1.traj b/src/main/deploy/choreo/FrontWing2.1.traj index 217c3a7b..9d149283 100644 --- a/src/main/deploy/choreo/FrontWing2.1.traj +++ b/src/main/deploy/choreo/FrontWing2.1.traj @@ -4,109 +4,109 @@ "x": 1.2263859510421753, "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": -3.740256292064187e-22, - "velocityX": -5.996486579954623e-19, - "velocityY": 3.7218569679078715e-21, + "angularVelocity": 2.58414356962129e-20, + "velocityX": 2.577520988220736e-18, + "velocityY": -1.6028256561920876e-20, "timestamp": 0 }, { - "x": 1.3163279724628432, - "y": 5.593551858929642, - "heading": 3.140264869702983, - "angularVelocity": -0.007887547277553473, - "velocityX": 0.9650288514473437, - "velocityY": -0.0059937949143052385, - "timestamp": 0.0932013807522681 + "x": 1.272754287158708, + "y": 5.593822495121423, + "heading": 3.1409509910936695, + "angularVelocity": -0.0004947956205997497, + "velocityX": 0.4681363319992983, + "velocityY": -0.0029075951069526523, + "timestamp": 0.09904878760104914 }, { - "x": 1.467723714747352, - "y": 5.592611539861448, - "heading": 3.1402648694385524, - "angularVelocity": -2.83720041018159e-9, - "velocityX": 1.6243937703766675, - "velocityY": -0.010089110918776865, - "timestamp": 0.1864027615045362 + "x": 1.3654909581138261, + "y": 5.593246507588987, + "heading": 3.140852973337843, + "angularVelocity": -0.0009895906673899658, + "velocityX": 0.9362726510782554, + "velocityY": -0.005815190133657064, + "timestamp": 0.19809757520209828 }, { - "x": 1.6191194570316485, - "y": 5.591671220793256, - "heading": 3.1402648691741217, - "angularVelocity": -2.837195579018617e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777356, - "timestamp": 0.2796041422568043 + "x": 1.5045959617651867, + "y": 5.592382526307659, + "heading": 3.140705946827063, + "angularVelocity": -0.001484384759681857, + "velocityX": 1.4044089486233002, + "velocityY": -0.008722785026614378, + "timestamp": 0.29714636280314743 }, { - "x": 1.7705151993162385, - "y": 5.590730901725062, - "heading": 3.1402648689096915, - "angularVelocity": -2.83719540339334e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777356, - "timestamp": 0.3728055230090724 + "x": 1.6900692938625956, + "y": 5.591230551303714, + "heading": 3.140509911748851, + "angularVelocity": -0.001979176958749809, + "velocityX": 1.8725452031004766, + "velocityY": -0.011630379652077128, + "timestamp": 0.39619515040419656 }, { - "x": 1.9219109416013613, - "y": 5.589790582656871, - "heading": 3.1402648686452608, - "angularVelocity": -2.837195245417577e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777356, - "timestamp": 0.46600690376134046 + "x": 1.9219109416008546, + "y": 5.589790582656859, + "heading": 3.1402648686451275, + "angularVelocity": -0.0024739636865663884, + "velocityX": 2.3406813283735364, + "velocityY": -0.014537973475053038, + "timestamp": 0.4952439380052457 }, { - "x": 2.0733066838851757, - "y": 5.588850263588676, - "heading": 3.14026486838083, - "angularVelocity": -2.837195862058808e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777355, - "timestamp": 0.5592082845136086 + "x": 2.1537525893397524, + "y": 5.588350614009982, + "heading": 3.1400198255413954, + "angularVelocity": -0.0024739636866560476, + "velocityX": 2.3406813283735377, + "velocityY": -0.014537973475053236, + "timestamp": 0.5942927256062949 }, { - "x": 2.224702426169452, - "y": 5.58790994452049, - "heading": 3.1402648681163994, - "angularVelocity": -2.8371952366306506e-9, - "velocityX": 1.6243937703767442, - "velocityY": -0.010089110918777355, - "timestamp": 0.6524096652658767 + "x": 2.339225921431913, + "y": 5.587198639006089, + "heading": 3.1398237904631805, + "angularVelocity": -0.0019791769587811803, + "velocityX": 1.8725452031004775, + "velocityY": -0.011630379652077307, + "timestamp": 0.693341513207344 }, { - "x": 2.3760981684546945, - "y": 5.586969625452291, - "heading": 3.140264867851969, - "angularVelocity": -2.8371952652077662e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777358, - "timestamp": 0.7456110460181449 + "x": 2.4783309250888186, + "y": 5.586334657724714, + "heading": 3.1396767639523975, + "angularVelocity": -0.001484384759709258, + "velocityX": 1.4044089486233011, + "velocityY": -0.008722785026614523, + "timestamp": 0.7923903008083932 }, { - "x": 2.5274939107386767, - "y": 5.5860293063840984, - "heading": 3.1402648675875384, - "angularVelocity": -2.837200309142864e-9, - "velocityX": 1.624393770376668, - "velocityY": -0.010089110918776867, - "timestamp": 0.838812426770413 + "x": 2.571067596041391, + "y": 5.585758670192281, + "heading": 3.1395787461965696, + "angularVelocity": -0.000989590667408484, + "velocityX": 0.9362726510782562, + "velocityY": -0.005815190133657172, + "timestamp": 0.8914390884094424 }, { "x": 2.617435932159424, "y": 5.585470676422119, "heading": 3.139529737290238, - "angularVelocity": -0.007887547280595224, - "velocityX": 0.9650288514473356, - "velocityY": -0.0059937949145238275, - "timestamp": 0.9320138075226811 + "angularVelocity": -0.0004947956206057336, + "velocityX": 0.4681363319992992, + "velocityY": -0.002907595106952706, + "timestamp": 0.9904878760104916 }, { "x": 2.617435932159424, "y": 5.585470676422119, "heading": 3.139529737290238, - "angularVelocity": 3.1865843751901534e-21, - "velocityX": -4.755464983211591e-19, - "velocityY": 2.948079481483161e-21, - "timestamp": 1.0252151882749492 + "angularVelocity": 3.961776114386473e-20, + "velocityX": 6.7461684746138455e-19, + "velocityY": -4.213391536884766e-21, + "timestamp": 1.0895366636115407 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/FrontWing2.traj b/src/main/deploy/choreo/FrontWing2.traj index 217c3a7b..9d149283 100644 --- a/src/main/deploy/choreo/FrontWing2.traj +++ b/src/main/deploy/choreo/FrontWing2.traj @@ -4,109 +4,109 @@ "x": 1.2263859510421753, "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": -3.740256292064187e-22, - "velocityX": -5.996486579954623e-19, - "velocityY": 3.7218569679078715e-21, + "angularVelocity": 2.58414356962129e-20, + "velocityX": 2.577520988220736e-18, + "velocityY": -1.6028256561920876e-20, "timestamp": 0 }, { - "x": 1.3163279724628432, - "y": 5.593551858929642, - "heading": 3.140264869702983, - "angularVelocity": -0.007887547277553473, - "velocityX": 0.9650288514473437, - "velocityY": -0.0059937949143052385, - "timestamp": 0.0932013807522681 + "x": 1.272754287158708, + "y": 5.593822495121423, + "heading": 3.1409509910936695, + "angularVelocity": -0.0004947956205997497, + "velocityX": 0.4681363319992983, + "velocityY": -0.0029075951069526523, + "timestamp": 0.09904878760104914 }, { - "x": 1.467723714747352, - "y": 5.592611539861448, - "heading": 3.1402648694385524, - "angularVelocity": -2.83720041018159e-9, - "velocityX": 1.6243937703766675, - "velocityY": -0.010089110918776865, - "timestamp": 0.1864027615045362 + "x": 1.3654909581138261, + "y": 5.593246507588987, + "heading": 3.140852973337843, + "angularVelocity": -0.0009895906673899658, + "velocityX": 0.9362726510782554, + "velocityY": -0.005815190133657064, + "timestamp": 0.19809757520209828 }, { - "x": 1.6191194570316485, - "y": 5.591671220793256, - "heading": 3.1402648691741217, - "angularVelocity": -2.837195579018617e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777356, - "timestamp": 0.2796041422568043 + "x": 1.5045959617651867, + "y": 5.592382526307659, + "heading": 3.140705946827063, + "angularVelocity": -0.001484384759681857, + "velocityX": 1.4044089486233002, + "velocityY": -0.008722785026614378, + "timestamp": 0.29714636280314743 }, { - "x": 1.7705151993162385, - "y": 5.590730901725062, - "heading": 3.1402648689096915, - "angularVelocity": -2.83719540339334e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777356, - "timestamp": 0.3728055230090724 + "x": 1.6900692938625956, + "y": 5.591230551303714, + "heading": 3.140509911748851, + "angularVelocity": -0.001979176958749809, + "velocityX": 1.8725452031004766, + "velocityY": -0.011630379652077128, + "timestamp": 0.39619515040419656 }, { - "x": 1.9219109416013613, - "y": 5.589790582656871, - "heading": 3.1402648686452608, - "angularVelocity": -2.837195245417577e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777356, - "timestamp": 0.46600690376134046 + "x": 1.9219109416008546, + "y": 5.589790582656859, + "heading": 3.1402648686451275, + "angularVelocity": -0.0024739636865663884, + "velocityX": 2.3406813283735364, + "velocityY": -0.014537973475053038, + "timestamp": 0.4952439380052457 }, { - "x": 2.0733066838851757, - "y": 5.588850263588676, - "heading": 3.14026486838083, - "angularVelocity": -2.837195862058808e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777355, - "timestamp": 0.5592082845136086 + "x": 2.1537525893397524, + "y": 5.588350614009982, + "heading": 3.1400198255413954, + "angularVelocity": -0.0024739636866560476, + "velocityX": 2.3406813283735377, + "velocityY": -0.014537973475053236, + "timestamp": 0.5942927256062949 }, { - "x": 2.224702426169452, - "y": 5.58790994452049, - "heading": 3.1402648681163994, - "angularVelocity": -2.8371952366306506e-9, - "velocityX": 1.6243937703767442, - "velocityY": -0.010089110918777355, - "timestamp": 0.6524096652658767 + "x": 2.339225921431913, + "y": 5.587198639006089, + "heading": 3.1398237904631805, + "angularVelocity": -0.0019791769587811803, + "velocityX": 1.8725452031004775, + "velocityY": -0.011630379652077307, + "timestamp": 0.693341513207344 }, { - "x": 2.3760981684546945, - "y": 5.586969625452291, - "heading": 3.140264867851969, - "angularVelocity": -2.8371952652077662e-9, - "velocityX": 1.6243937703767444, - "velocityY": -0.010089110918777358, - "timestamp": 0.7456110460181449 + "x": 2.4783309250888186, + "y": 5.586334657724714, + "heading": 3.1396767639523975, + "angularVelocity": -0.001484384759709258, + "velocityX": 1.4044089486233011, + "velocityY": -0.008722785026614523, + "timestamp": 0.7923903008083932 }, { - "x": 2.5274939107386767, - "y": 5.5860293063840984, - "heading": 3.1402648675875384, - "angularVelocity": -2.837200309142864e-9, - "velocityX": 1.624393770376668, - "velocityY": -0.010089110918776867, - "timestamp": 0.838812426770413 + "x": 2.571067596041391, + "y": 5.585758670192281, + "heading": 3.1395787461965696, + "angularVelocity": -0.000989590667408484, + "velocityX": 0.9362726510782562, + "velocityY": -0.005815190133657172, + "timestamp": 0.8914390884094424 }, { "x": 2.617435932159424, "y": 5.585470676422119, "heading": 3.139529737290238, - "angularVelocity": -0.007887547280595224, - "velocityX": 0.9650288514473356, - "velocityY": -0.0059937949145238275, - "timestamp": 0.9320138075226811 + "angularVelocity": -0.0004947956206057336, + "velocityX": 0.4681363319992992, + "velocityY": -0.002907595106952706, + "timestamp": 0.9904878760104916 }, { "x": 2.617435932159424, "y": 5.585470676422119, "heading": 3.139529737290238, - "angularVelocity": 3.1865843751901534e-21, - "velocityX": -4.755464983211591e-19, - "velocityY": 2.948079481483161e-21, - "timestamp": 1.0252151882749492 + "angularVelocity": 3.961776114386473e-20, + "velocityX": 6.7461684746138455e-19, + "velocityY": -4.213391536884766e-21, + "timestamp": 1.0895366636115407 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/FrontWing3.1.traj b/src/main/deploy/choreo/FrontWing3.1.traj index 261ddd63..5ca51de5 100644 --- a/src/main/deploy/choreo/FrontWing3.1.traj +++ b/src/main/deploy/choreo/FrontWing3.1.traj @@ -4,154 +4,154 @@ "x": 1.2650032043457031, "y": 5.615814685821533, "heading": 3.141, - "angularVelocity": -9.961906884794862e-27, - "velocityX": 6.399458285179746e-26, - "velocityY": 1.0314201039068904e-25, + "angularVelocity": -2.847593507618242e-26, + "velocityX": 3.1541489300939427e-25, + "velocityY": -2.217525612259016e-25, "timestamp": 0 }, { - "x": 1.310864623992567, - "y": 5.640347561603675, - "heading": 3.338413018068366, - "angularVelocity": 2.2732552294766206, - "velocityX": 0.5281045447942572, - "velocityY": 0.28250157315633995, - "timestamp": 0.08684155457274238 - }, - { - "x": 1.4026758531664765, - "y": 5.712962256076443, - "heading": 3.4150278612810783, - "angularVelocity": 0.8822371224197347, - "velocityX": 1.0572269189055614, - "velocityY": 0.8361745115000444, - "timestamp": 0.17368310914548477 - }, - { - "x": 1.505326244994725, - "y": 5.809724596960637, - "heading": 3.4150280522977696, - "angularVelocity": 0.0000021996000891052408, - "velocityX": 1.1820423106574365, - "velocityY": 1.1142400819545595, - "timestamp": 0.26052466371822713 - }, - { - "x": 1.6079766300540685, - "y": 5.906486945025769, - "heading": 3.4150282433141586, - "angularVelocity": 0.0000021995966111503638, - "velocityX": 1.182042232711982, - "velocityY": 1.1142401646446862, - "timestamp": 0.34736621829096953 - }, - { - "x": 1.7106270151134115, - "y": 6.003249293090899, - "heading": 3.4150284343305666, - "angularVelocity": 0.0000021995968268735563, - "velocityX": 1.182042232711972, - "velocityY": 1.1142401646446563, - "timestamp": 0.4342077728637119 - }, - { - "x": 1.8132774001727587, - "y": 6.100011641156021, - "heading": 3.4150286253469933, - "angularVelocity": 0.0000021995970422363537, - "velocityX": 1.1820422327120208, - "velocityY": 1.1142401646445645, - "timestamp": 0.5210493274364543 - }, - { - "x": 1.9159277852321102, - "y": 6.1967739892211355, - "heading": 3.4150288163634386, - "angularVelocity": 0.000002199597258109715, - "velocityX": 1.1820422327120694, - "velocityY": 1.1142401646444724, - "timestamp": 0.6078908820091966 - }, - { - "x": 2.018578170291466, - "y": 6.293536337286241, - "heading": 3.4150290073799026, - "angularVelocity": 0.0000021995974740409558, - "velocityX": 1.1820422327121183, - "velocityY": 1.1142401646443802, - "timestamp": 0.694732436581939 - }, - { - "x": 2.121228555350826, - "y": 6.390298685351339, - "heading": 3.415029198396385, - "angularVelocity": 0.000002199597689726547, - "velocityX": 1.182042232712167, - "velocityY": 1.1142401646442885, - "timestamp": 0.7815739911546813 - }, - { - "x": 2.22387894041019, - "y": 6.487061033416429, - "heading": 3.4150293894128865, - "angularVelocity": 0.0000021995979051884085, - "velocityX": 1.1820422327122158, - "velocityY": 1.1142401646441964, - "timestamp": 0.8684155457274236 - }, - { - "x": 2.3265293254695587, - "y": 6.583823381481512, - "heading": 3.415029580429407, - "angularVelocity": 0.000002199598120574345, - "velocityX": 1.1820422327122646, - "velocityY": 1.1142401646441045, - "timestamp": 0.955257100300166 - }, - { - "x": 2.4291797105289343, - "y": 6.680585729546584, - "heading": 3.415029771445946, - "angularVelocity": 0.0000021995983364688408, - "velocityX": 1.1820422327123468, - "velocityY": 1.114240164643977, - "timestamp": 1.0420986548729083 - }, - { - "x": 2.5318300994211365, - "y": 6.777348073545511, - "heading": 3.4150299624626883, - "angularVelocity": 0.0000021996006785706527, - "velocityX": 1.1820422768482035, - "velocityY": 1.1142401178214183, - "timestamp": 1.1289402094456507 - }, - { - "x": 2.6271100513248182, - "y": 6.858057139717029, - "heading": 3.467307103119733, - "angularVelocity": 0.6019830127897501, - "velocityX": 1.0971700399935989, - "velocityY": 0.9293830190926906, - "timestamp": 1.215781764018393 + "x": 1.2878690502136103, + "y": 5.636590724334488, + "heading": 3.156944146176203, + "angularVelocity": 0.1956833117689311, + "velocityX": 0.2806336818780737, + "velocityY": 0.2549853706008959, + "timestamp": 0.08147933532027608 + }, + { + "x": 1.3336452622258208, + "y": 5.6781705965802045, + "heading": 3.18797373905128, + "angularVelocity": 0.38082776145764136, + "velocityX": 0.5618137633582192, + "velocityY": 0.5103118733391074, + "timestamp": 0.16295867064055217 + }, + { + "x": 1.4023952169052167, + "y": 5.740596168815711, + "heading": 3.232737562480919, + "angularVelocity": 0.5493886671225605, + "velocityX": 0.8437716681064715, + "velocityY": 0.7661522027656068, + "timestamp": 0.24443800596082826 + }, + { + "x": 1.4942170903836536, + "y": 5.82393396269605, + "heading": 3.2887738175604304, + "angularVelocity": 0.6877357904215389, + "velocityX": 1.1269344934823864, + "velocityY": 1.0228089558261442, + "timestamp": 0.32591734128110433 + }, + { + "x": 1.6092733760907945, + "y": 5.928274751901453, + "heading": 3.3503605428052587, + "angularVelocity": 0.7558569912572904, + "velocityX": 1.4120916089322733, + "velocityY": 1.2805797788514484, + "timestamp": 0.4073966766013804 + }, + { + "x": 1.7472843038638464, + "y": 6.053006887878574, + "heading": 3.393001703956897, + "angularVelocity": 0.523337125714468, + "velocityX": 1.6938150910358227, + "velocityY": 1.5308438082714806, + "timestamp": 0.4888760119216565 + }, + { + "x": 1.8948651009567858, + "y": 6.187498734607365, + "heading": 3.3930017381142776, + "angularVelocity": 4.1921525430622926e-7, + "velocityX": 1.8112665808187325, + "velocityY": 1.650625231540434, + "timestamp": 0.5703553472419326 + }, + { + "x": 2.0424458885331562, + "y": 6.321990591778967, + "heading": 3.3930017722714747, + "angularVelocity": 4.1921300750866336e-7, + "velocityX": 1.811266464021397, + "velocityY": 1.650625359705577, + "timestamp": 0.6518346825622087 + }, + { + "x": 2.190026686851105, + "y": 6.45648243716353, + "heading": 3.3930018064288547, + "angularVelocity": 4.19215254408376e-7, + "velocityX": 1.811266595853337, + "velocityY": 1.650625215042643, + "timestamp": 0.7333140178824847 + }, + { + "x": 2.3281712431329007, + "y": 6.581195834448366, + "heading": 3.4359493310429845, + "angularVelocity": 0.5270971399719079, + "velocityX": 1.6954551204766442, + "velocityY": 1.5306138273541086, + "timestamp": 0.8147933532027608 + }, + { + "x": 2.443305156721716, + "y": 6.685507263891603, + "heading": 3.4972346317576646, + "angularVelocity": 0.7521575927660913, + "velocityX": 1.4130443398470416, + "velocityY": 1.280219445006684, + "timestamp": 0.8962726885230369 + }, + { + "x": 2.5351903686585104, + "y": 6.768824405464408, + "heading": 3.5531988851388214, + "angularVelocity": 0.6868521099389763, + "velocityX": 1.1277118495826628, + "velocityY": 1.0225554890121025, + "timestamp": 0.977752023843313 + }, + { + "x": 2.6039864921588998, + "y": 6.831227820433193, + "heading": 3.598162002435054, + "angularVelocity": 0.5518346108187143, + "velocityX": 0.8443383003796903, + "velocityY": 0.7658802655113843, + "timestamp": 1.0592313591635891 + }, + { + "x": 2.6497929927743678, + "y": 6.872785794141501, + "heading": 3.6294863595083644, + "angularVelocity": 0.384445417358672, + "velocityX": 0.5621854969166529, + "velocityY": 0.5100431114828528, + "timestamp": 1.1407106944838652 }, { "x": 2.6726744174957275, "y": 6.893547058105469, "heading": 3.6456400220863263, - "angularVelocity": 2.0535436041418818, - "velocityX": 0.5246839073193058, - "velocityY": 0.40867437902337306, - "timestamp": 1.3026233185911353 + "angularVelocity": 0.1982547171557739, + "velocityX": 0.28082488193378885, + "velocityY": 0.2548040418145309, + "timestamp": 1.2221900298041413 }, { "x": 2.6726744174957275, "y": 6.893547058105469, "heading": 3.6456400220863263, - "angularVelocity": 9.465973134511326e-26, - "velocityX": -9.092672379053801e-26, - "velocityY": -8.734024503832287e-26, - "timestamp": 1.3894648731638777 + "angularVelocity": 4.786100698151895e-26, + "velocityX": 2.8420295041943254e-25, + "velocityY": -2.9253363934820537e-25, + "timestamp": 1.3036693651244173 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/FrontWing3.traj b/src/main/deploy/choreo/FrontWing3.traj index 261ddd63..5ca51de5 100644 --- a/src/main/deploy/choreo/FrontWing3.traj +++ b/src/main/deploy/choreo/FrontWing3.traj @@ -4,154 +4,154 @@ "x": 1.2650032043457031, "y": 5.615814685821533, "heading": 3.141, - "angularVelocity": -9.961906884794862e-27, - "velocityX": 6.399458285179746e-26, - "velocityY": 1.0314201039068904e-25, + "angularVelocity": -2.847593507618242e-26, + "velocityX": 3.1541489300939427e-25, + "velocityY": -2.217525612259016e-25, "timestamp": 0 }, { - "x": 1.310864623992567, - "y": 5.640347561603675, - "heading": 3.338413018068366, - "angularVelocity": 2.2732552294766206, - "velocityX": 0.5281045447942572, - "velocityY": 0.28250157315633995, - "timestamp": 0.08684155457274238 - }, - { - "x": 1.4026758531664765, - "y": 5.712962256076443, - "heading": 3.4150278612810783, - "angularVelocity": 0.8822371224197347, - "velocityX": 1.0572269189055614, - "velocityY": 0.8361745115000444, - "timestamp": 0.17368310914548477 - }, - { - "x": 1.505326244994725, - "y": 5.809724596960637, - "heading": 3.4150280522977696, - "angularVelocity": 0.0000021996000891052408, - "velocityX": 1.1820423106574365, - "velocityY": 1.1142400819545595, - "timestamp": 0.26052466371822713 - }, - { - "x": 1.6079766300540685, - "y": 5.906486945025769, - "heading": 3.4150282433141586, - "angularVelocity": 0.0000021995966111503638, - "velocityX": 1.182042232711982, - "velocityY": 1.1142401646446862, - "timestamp": 0.34736621829096953 - }, - { - "x": 1.7106270151134115, - "y": 6.003249293090899, - "heading": 3.4150284343305666, - "angularVelocity": 0.0000021995968268735563, - "velocityX": 1.182042232711972, - "velocityY": 1.1142401646446563, - "timestamp": 0.4342077728637119 - }, - { - "x": 1.8132774001727587, - "y": 6.100011641156021, - "heading": 3.4150286253469933, - "angularVelocity": 0.0000021995970422363537, - "velocityX": 1.1820422327120208, - "velocityY": 1.1142401646445645, - "timestamp": 0.5210493274364543 - }, - { - "x": 1.9159277852321102, - "y": 6.1967739892211355, - "heading": 3.4150288163634386, - "angularVelocity": 0.000002199597258109715, - "velocityX": 1.1820422327120694, - "velocityY": 1.1142401646444724, - "timestamp": 0.6078908820091966 - }, - { - "x": 2.018578170291466, - "y": 6.293536337286241, - "heading": 3.4150290073799026, - "angularVelocity": 0.0000021995974740409558, - "velocityX": 1.1820422327121183, - "velocityY": 1.1142401646443802, - "timestamp": 0.694732436581939 - }, - { - "x": 2.121228555350826, - "y": 6.390298685351339, - "heading": 3.415029198396385, - "angularVelocity": 0.000002199597689726547, - "velocityX": 1.182042232712167, - "velocityY": 1.1142401646442885, - "timestamp": 0.7815739911546813 - }, - { - "x": 2.22387894041019, - "y": 6.487061033416429, - "heading": 3.4150293894128865, - "angularVelocity": 0.0000021995979051884085, - "velocityX": 1.1820422327122158, - "velocityY": 1.1142401646441964, - "timestamp": 0.8684155457274236 - }, - { - "x": 2.3265293254695587, - "y": 6.583823381481512, - "heading": 3.415029580429407, - "angularVelocity": 0.000002199598120574345, - "velocityX": 1.1820422327122646, - "velocityY": 1.1142401646441045, - "timestamp": 0.955257100300166 - }, - { - "x": 2.4291797105289343, - "y": 6.680585729546584, - "heading": 3.415029771445946, - "angularVelocity": 0.0000021995983364688408, - "velocityX": 1.1820422327123468, - "velocityY": 1.114240164643977, - "timestamp": 1.0420986548729083 - }, - { - "x": 2.5318300994211365, - "y": 6.777348073545511, - "heading": 3.4150299624626883, - "angularVelocity": 0.0000021996006785706527, - "velocityX": 1.1820422768482035, - "velocityY": 1.1142401178214183, - "timestamp": 1.1289402094456507 - }, - { - "x": 2.6271100513248182, - "y": 6.858057139717029, - "heading": 3.467307103119733, - "angularVelocity": 0.6019830127897501, - "velocityX": 1.0971700399935989, - "velocityY": 0.9293830190926906, - "timestamp": 1.215781764018393 + "x": 1.2878690502136103, + "y": 5.636590724334488, + "heading": 3.156944146176203, + "angularVelocity": 0.1956833117689311, + "velocityX": 0.2806336818780737, + "velocityY": 0.2549853706008959, + "timestamp": 0.08147933532027608 + }, + { + "x": 1.3336452622258208, + "y": 5.6781705965802045, + "heading": 3.18797373905128, + "angularVelocity": 0.38082776145764136, + "velocityX": 0.5618137633582192, + "velocityY": 0.5103118733391074, + "timestamp": 0.16295867064055217 + }, + { + "x": 1.4023952169052167, + "y": 5.740596168815711, + "heading": 3.232737562480919, + "angularVelocity": 0.5493886671225605, + "velocityX": 0.8437716681064715, + "velocityY": 0.7661522027656068, + "timestamp": 0.24443800596082826 + }, + { + "x": 1.4942170903836536, + "y": 5.82393396269605, + "heading": 3.2887738175604304, + "angularVelocity": 0.6877357904215389, + "velocityX": 1.1269344934823864, + "velocityY": 1.0228089558261442, + "timestamp": 0.32591734128110433 + }, + { + "x": 1.6092733760907945, + "y": 5.928274751901453, + "heading": 3.3503605428052587, + "angularVelocity": 0.7558569912572904, + "velocityX": 1.4120916089322733, + "velocityY": 1.2805797788514484, + "timestamp": 0.4073966766013804 + }, + { + "x": 1.7472843038638464, + "y": 6.053006887878574, + "heading": 3.393001703956897, + "angularVelocity": 0.523337125714468, + "velocityX": 1.6938150910358227, + "velocityY": 1.5308438082714806, + "timestamp": 0.4888760119216565 + }, + { + "x": 1.8948651009567858, + "y": 6.187498734607365, + "heading": 3.3930017381142776, + "angularVelocity": 4.1921525430622926e-7, + "velocityX": 1.8112665808187325, + "velocityY": 1.650625231540434, + "timestamp": 0.5703553472419326 + }, + { + "x": 2.0424458885331562, + "y": 6.321990591778967, + "heading": 3.3930017722714747, + "angularVelocity": 4.1921300750866336e-7, + "velocityX": 1.811266464021397, + "velocityY": 1.650625359705577, + "timestamp": 0.6518346825622087 + }, + { + "x": 2.190026686851105, + "y": 6.45648243716353, + "heading": 3.3930018064288547, + "angularVelocity": 4.19215254408376e-7, + "velocityX": 1.811266595853337, + "velocityY": 1.650625215042643, + "timestamp": 0.7333140178824847 + }, + { + "x": 2.3281712431329007, + "y": 6.581195834448366, + "heading": 3.4359493310429845, + "angularVelocity": 0.5270971399719079, + "velocityX": 1.6954551204766442, + "velocityY": 1.5306138273541086, + "timestamp": 0.8147933532027608 + }, + { + "x": 2.443305156721716, + "y": 6.685507263891603, + "heading": 3.4972346317576646, + "angularVelocity": 0.7521575927660913, + "velocityX": 1.4130443398470416, + "velocityY": 1.280219445006684, + "timestamp": 0.8962726885230369 + }, + { + "x": 2.5351903686585104, + "y": 6.768824405464408, + "heading": 3.5531988851388214, + "angularVelocity": 0.6868521099389763, + "velocityX": 1.1277118495826628, + "velocityY": 1.0225554890121025, + "timestamp": 0.977752023843313 + }, + { + "x": 2.6039864921588998, + "y": 6.831227820433193, + "heading": 3.598162002435054, + "angularVelocity": 0.5518346108187143, + "velocityX": 0.8443383003796903, + "velocityY": 0.7658802655113843, + "timestamp": 1.0592313591635891 + }, + { + "x": 2.6497929927743678, + "y": 6.872785794141501, + "heading": 3.6294863595083644, + "angularVelocity": 0.384445417358672, + "velocityX": 0.5621854969166529, + "velocityY": 0.5100431114828528, + "timestamp": 1.1407106944838652 }, { "x": 2.6726744174957275, "y": 6.893547058105469, "heading": 3.6456400220863263, - "angularVelocity": 2.0535436041418818, - "velocityX": 0.5246839073193058, - "velocityY": 0.40867437902337306, - "timestamp": 1.3026233185911353 + "angularVelocity": 0.1982547171557739, + "velocityX": 0.28082488193378885, + "velocityY": 0.2548040418145309, + "timestamp": 1.2221900298041413 }, { "x": 2.6726744174957275, "y": 6.893547058105469, "heading": 3.6456400220863263, - "angularVelocity": 9.465973134511326e-26, - "velocityX": -9.092672379053801e-26, - "velocityY": -8.734024503832287e-26, - "timestamp": 1.3894648731638777 + "angularVelocity": 4.786100698151895e-26, + "velocityX": 2.8420295041943254e-25, + "velocityY": -2.9253363934820537e-25, + "timestamp": 1.3036693651244173 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/FrontWing3Contested5.1.traj b/src/main/deploy/choreo/FrontWing3Contested5.1.traj index b82d664d..aa587dd4 100644 --- a/src/main/deploy/choreo/FrontWing3Contested5.1.traj +++ b/src/main/deploy/choreo/FrontWing3Contested5.1.traj @@ -4,163 +4,154 @@ "x": 1.2263859510421753, "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": -9.716627282364513e-28, - "velocityX": -2.1006296109192272e-27, - "velocityY": 1.6816897032610772e-27, + "angularVelocity": 1.2684261528823466e-28, + "velocityX": -8.90285455606977e-27, + "velocityY": -9.123583685606214e-27, "timestamp": 0 }, { - "x": 1.2629372172985613, - "y": 5.611868166522511, - "heading": 3.3611684637424437, - "angularVelocity": 2.6151040269567423, - "velocityX": 0.4341464801664794, - "velocityY": 0.21092109874697887, - "timestamp": 0.08419109200740245 - }, - { - "x": 1.3467908643065862, - "y": 5.672396849103953, - "heading": 3.4705725495976436, - "angularVelocity": 1.2994734151397, - "velocityX": 0.9959919156370148, - "velocityY": 0.7189440252909496, - "timestamp": 0.1683821840148049 - }, - { - "x": 1.4464012359618026, - "y": 5.766107214252463, - "heading": 3.4705728088664487, - "angularVelocity": 0.0000030795277620559296, - "velocityX": 1.1831462127425316, - "velocityY": 1.1130674625323809, - "timestamp": 0.25257327602220736 - }, - { - "x": 1.5460115925515412, - "y": 5.859817595415609, - "heading": 3.4705730681338594, - "angularVelocity": 0.0000030795111997424536, - "velocityX": 1.1831460337986872, - "velocityY": 1.1130676527500878, - "timestamp": 0.3367643680296098 - }, - { - "x": 1.6456219491412756, - "y": 5.9535279765787505, - "heading": 3.4705733274013144, - "angularVelocity": 0.000003079511724050569, - "velocityX": 1.183146033798638, - "velocityY": 1.1130676527500365, - "timestamp": 0.42095546003701223 - }, - { - "x": 1.7452323057310184, - "y": 6.0472383577418745, - "heading": 3.4705735866688134, - "angularVelocity": 0.000003079512247611481, - "velocityX": 1.1831460337987365, - "velocityY": 1.113067652749828, - "timestamp": 0.5051465520444147 - }, - { - "x": 1.8448426623207694, - "y": 6.1409487389049815, - "heading": 3.4705738459363564, - "angularVelocity": 0.0000030795127711126187, - "velocityX": 1.1831460337988353, - "velocityY": 1.1130676527496197, - "timestamp": 0.5893376440518172 - }, - { - "x": 1.9444530189105287, - "y": 6.234659120068071, - "heading": 3.470574105203944, - "angularVelocity": 0.000003079513294842049, - "velocityX": 1.183146033798934, - "velocityY": 1.1130676527494117, - "timestamp": 0.6735287360592197 - }, - { - "x": 2.0440633755002966, - "y": 6.328369501231142, - "heading": 3.470574364471575, - "angularVelocity": 0.0000030795138187518754, - "velocityX": 1.1831460337990323, - "velocityY": 1.1130676527492032, - "timestamp": 0.7577198280666222 - }, - { - "x": 2.1436737320900723, - "y": 6.422079882394196, - "heading": 3.4705746237392505, - "angularVelocity": 0.0000030795143424110884, - "velocityX": 1.1831460337991306, - "velocityY": 1.113067652748995, - "timestamp": 0.8419109200740247 - }, - { - "x": 2.2432840886798564, - "y": 6.515790263557233, - "heading": 3.47057488300697, - "angularVelocity": 0.0000030795148666763967, - "velocityX": 1.1831460337992294, - "velocityY": 1.1130676527487866, - "timestamp": 0.9261020120814272 - }, - { - "x": 2.342894445269649, - "y": 6.609500644720252, - "heading": 3.4705751422747335, - "angularVelocity": 0.0000030795153898968674, - "velocityX": 1.1831460337993278, - "velocityY": 1.1130676527485786, - "timestamp": 1.0102931040888297 - }, - { - "x": 2.4425048018594557, - "y": 6.703211025883247, - "heading": 3.470575401542541, - "angularVelocity": 0.000003079515913944498, - "velocityX": 1.1831460337994937, - "velocityY": 1.1130676527482986, - "timestamp": 1.0944841960962322 - }, - { - "x": 2.5421151652954292, - "y": 6.796921399768766, - "heading": 3.4705756608109883, - "angularVelocity": 0.0000030795235106850793, - "velocityX": 1.183146115116498, - "velocityY": 1.1130675663083123, - "timestamp": 1.1786752881036346 - }, - { - "x": 2.632005364209705, - "y": 6.868564037074326, - "heading": 3.5437695190039196, - "angularVelocity": 0.8693777031244078, - "velocityX": 1.0676925167614182, - "velocityY": 0.8509527029208858, - "timestamp": 1.2628663801110371 + "x": 1.2489662072831478, + "y": 5.614521121433257, + "heading": 3.1501023791424476, + "angularVelocity": 0.11304779998591061, + "velocityX": 0.2804374824661162, + "velocityY": 0.2534916497154969, + "timestamp": 0.08051796800629471 + }, + { + "x": 1.2941269040941792, + "y": 5.65534214931359, + "heading": 3.1683083464792072, + "angularVelocity": 0.22611061589800957, + "velocityX": 0.5608772542235648, + "velocityY": 0.506980353467706, + "timestamp": 0.16103593601258942 + }, + { + "x": 1.3618682728307967, + "y": 5.716573194164052, + "heading": 3.1956213802625175, + "angularVelocity": 0.33921663027033977, + "velocityX": 0.8413198993213754, + "velocityY": 0.7604643580383921, + "timestamp": 0.24155390401888413 + }, + { + "x": 1.4521905498783176, + "y": 5.798213731151769, + "heading": 3.2320485280043223, + "angularVelocity": 0.4524101718383757, + "velocityX": 1.121765480227465, + "velocityY": 1.0139418444009154, + "timestamp": 0.32207187202517884 + }, + { + "x": 1.5650939157362251, + "y": 5.900263093874502, + "heading": 3.2776019617285486, + "angularVelocity": 0.5657548849303424, + "velocityX": 1.4022133028627977, + "velocityY": 1.2674110543221202, + "timestamp": 0.4025898400314736 + }, + { + "x": 1.700578414867027, + "y": 6.022720499588671, + "heading": 3.3323005252490514, + "angularVelocity": 0.6793336304292585, + "velocityX": 1.6826616777041643, + "velocityY": 1.5208705428904563, + "timestamp": 0.4831078080377683 + }, + { + "x": 1.8586438590589127, + "y": 6.165585103627761, + "heading": 3.3961709105895657, + "angularVelocity": 0.7932438798694066, + "velocityX": 1.9631077150323568, + "velocityY": 1.7743195410485293, + "timestamp": 0.563625776044063 + }, + { + "x": 2.039289707486661, + "y": 6.328856070494732, + "heading": 3.469248537641403, + "angularVelocity": 0.9075940297713951, + "velocityX": 2.2435470355340517, + "velocityY": 2.027758162677519, + "timestamp": 0.6441437440503578 + }, + { + "x": 2.1973352722504775, + "y": 6.471697816178928, + "heading": 3.5342798000329774, + "angularVelocity": 0.8076614947174351, + "velocityX": 1.9628608207234048, + "velocityY": 1.774035649695346, + "timestamp": 0.7246617120566525 + }, + { + "x": 2.3328015579338945, + "y": 6.594133674846713, + "heading": 3.590056917522341, + "angularVelocity": 0.6927288265024656, + "velocityX": 1.682435474189146, + "velocityY": 1.520602937448862, + "timestamp": 0.8051796800629473 + }, + { + "x": 2.4456892468776004, + "y": 6.6961637450440366, + "heading": 3.6365583693082697, + "angularVelocity": 0.5775288787999897, + "velocityX": 1.4020186020452068, + "velocityY": 1.2671714491024832, + "timestamp": 0.885697648069242 + }, + { + "x": 2.5359988834189062, + "y": 6.777787981324914, + "heading": 3.67376995651372, + "angularVelocity": 0.4621525868926695, + "velocityX": 1.121608490346475, + "velocityY": 1.013739396335684, + "timestamp": 0.9662156160755367 + }, + { + "x": 2.603730869861698, + "y": 6.839006281819378, + "heading": 3.701682776944854, + "angularVelocity": 0.3466657334044004, + "velocityX": 0.8412033750963059, + "velocityY": 0.7603060783858558, + "timestamp": 1.0467335840818315 + }, + { + "x": 2.648885462775646, + "y": 6.879818541544654, + "heading": 3.720292007909602, + "angularVelocity": 0.23111898406692608, + "velocityX": 0.5608014463357825, + "velocityY": 0.5068714565932112, + "timestamp": 1.127251552088126 }, { "x": 2.6714627742767334, "y": 6.900224685668945, "heading": 3.729596046054495, - "angularVelocity": 2.207199391525135, - "velocityX": 0.46866490416300804, - "velocityY": 0.3760569894002088, - "timestamp": 1.3470574721184396 + "angularVelocity": 0.11555232173973205, + "velocityX": 0.28040091000958045, + "velocityY": 0.25343590542047173, + "timestamp": 1.2077695200944207 }, { "x": 2.6714627742767334, "y": 6.900224685668945, "heading": 3.729596046054495, - "angularVelocity": -1.0211590332128658e-23, - "velocityX": -1.1480030002254912e-23, - "velocityY": -4.344519998520169e-23, - "timestamp": 1.4312485641258421 + "angularVelocity": 7.708394912303633e-28, + "velocityX": 2.52399716725713e-26, + "velocityY": 2.1878261081032532e-26, + "timestamp": 1.2882874881007154 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/FrontWing3Contested5.2.traj b/src/main/deploy/choreo/FrontWing3Contested5.2.traj index 424aea1d..0416f584 100644 --- a/src/main/deploy/choreo/FrontWing3Contested5.2.traj +++ b/src/main/deploy/choreo/FrontWing3Contested5.2.traj @@ -4,676 +4,451 @@ "x": 2.6714627742767334, "y": 6.900224685668945, "heading": 3.729596046054495, - "angularVelocity": -1.0211590332128658e-23, - "velocityX": -1.1480030002254912e-23, - "velocityY": -4.344519998520169e-23, + "angularVelocity": 7.708394912303633e-28, + "velocityX": 2.52399716725713e-26, + "velocityY": 2.1878261081032532e-26, "timestamp": 0 }, { - "x": 2.720827723501713, - "y": 6.91216585910853, - "heading": 3.486294732820543, - "angularVelocity": -2.69475141570759, - "velocityX": 0.5467552354822697, - "velocityY": 0.13225779017707837, - "timestamp": 0.0902871084196335 - }, - { - "x": 2.8392095989605575, - "y": 6.942185818091124, - "heading": 3.389947764111763, - "angularVelocity": -1.0671176693463396, - "velocityX": 1.311171412297561, - "velocityY": 0.33249441150564496, - "timestamp": 0.180574216839267 - }, - { - "x": 2.985101373007063, - "y": 6.9572215101019514, - "heading": 3.3899472630700718, - "angularVelocity": -0.000005549426712336868, - "velocityX": 1.615864951266793, - "velocityY": 0.16653199192676998, - "timestamp": 0.2708613252589005 - }, - { - "x": 3.1309931487591207, - "y": 6.972257185567622, - "heading": 3.3899467620302377, - "angularVelocity": -0.000005549406141075235, - "velocityX": 1.6158649701561478, - "velocityY": 0.16653180870033996, - "timestamp": 0.361148433678534 - }, - { - "x": 3.276884924511046, - "y": 6.987292861034595, - "heading": 3.389946260989866, - "angularVelocity": -0.000005549412101088006, - "velocityX": 1.615864970155008, - "velocityY": 0.16653180870111933, - "timestamp": 0.4514355420981675 - }, - { - "x": 3.4227767002625384, - "y": 7.002328536502587, - "heading": 3.3899457599489553, - "angularVelocity": -0.000005549418061933105, - "velocityX": 1.6158649701538554, - "velocityY": 0.16653180870202178, - "timestamp": 0.541722650517801 - }, - { - "x": 3.5686684760143703, - "y": 7.017364211969376, - "heading": 3.389945258907507, - "angularVelocity": -0.000005549424021960657, - "velocityX": 1.6158649701527033, - "velocityY": 0.16653180870292417, - "timestamp": 0.6320097589374343 - }, - { - "x": 3.7145602517660743, - "y": 7.032399887436731, - "heading": 3.3899447578655204, - "angularVelocity": -0.000005549429982841055, - "velocityX": 1.6158649701515504, - "velocityY": 0.16653180870382667, - "timestamp": 0.7222968673570676 - }, - { - "x": 3.8604520275173986, - "y": 7.04743556290717, - "heading": 3.3899442568229956, - "angularVelocity": -0.000005549435944010941, - "velocityX": 1.615864970150398, - "velocityY": 0.1665318087047291, - "timestamp": 0.8125839757767008 - }, - { - "x": 4.00634380326868, - "y": 7.062471238372739, - "heading": 3.389943755779933, - "angularVelocity": -0.000005549441903913974, - "velocityX": 1.6158649701492453, - "velocityY": 0.1665318087056315, - "timestamp": 0.9028710841963341 - }, - { - "x": 4.1522355790200765, - "y": 7.077506913841867, - "heading": 3.389943254736332, - "angularVelocity": -0.0000055494478640805644, - "velocityX": 1.615864970148093, - "velocityY": 0.16653180870653383, - "timestamp": 0.9931581926159674 - }, - { - "x": 4.298127354771182, - "y": 7.0925425893096525, - "heading": 3.389942753692193, - "angularVelocity": -0.000005549453823846852, - "velocityX": 1.6158649701469403, - "velocityY": 0.16653180870743617, - "timestamp": 1.0834453010356007 - }, - { - "x": 4.444019130522188, - "y": 7.10757826477934, - "heading": 3.389942252647516, - "angularVelocity": -0.000005549459785648991, - "velocityX": 1.6158649701457874, - "velocityY": 0.16653180870833867, - "timestamp": 1.173732409455234 - }, - { - "x": 4.5899109062731025, - "y": 7.122613940245511, - "heading": 3.3899417516023003, - "angularVelocity": -0.000005549465746228024, - "velocityX": 1.6158649701446353, - "velocityY": 0.1665318087092411, - "timestamp": 1.2640195178748672 - }, - { - "x": 4.735802682023898, - "y": 7.137649615713241, - "heading": 3.389941250556547, - "angularVelocity": -0.00000554947170730299, - "velocityX": 1.6158649701434824, - "velocityY": 0.16653180871014361, - "timestamp": 1.3543066262945005 - }, - { - "x": 4.881694457774774, - "y": 7.152685291182225, - "heading": 3.389940749510255, - "angularVelocity": -0.000005549477667769927, - "velocityX": 1.61586497014233, - "velocityY": 0.1665318087110459, - "timestamp": 1.4445937347141338 - }, - { - "x": 5.027586233525358, - "y": 7.167720966650761, - "heading": 3.3899402484634247, - "angularVelocity": -0.000005549483629157811, - "velocityX": 1.6158649701411771, - "velocityY": 0.16653180871194842, - "timestamp": 1.534880843133767 - }, - { - "x": 5.173478009275799, - "y": 7.182756642119004, - "heading": 3.3899397474160566, - "angularVelocity": -0.000005549489589198961, - "velocityX": 1.6158649701400245, - "velocityY": 0.1665318087128507, - "timestamp": 1.6251679515534003 - }, - { - "x": 5.319369785026301, - "y": 7.197792317586818, - "heading": 3.38993924636815, - "angularVelocity": -0.0000055494955497599425, - "velocityX": 1.615864970138872, - "velocityY": 0.16653180871375314, - "timestamp": 1.7154550599730336 - }, - { - "x": 5.4652615607764785, - "y": 7.212827993058087, - "heading": 3.3899387453197054, - "angularVelocity": -0.000005549501510730972, - "velocityX": 1.6158649701377192, - "velocityY": 0.16653180871465564, - "timestamp": 1.8057421683926669 - }, - { - "x": 5.611153336526806, - "y": 7.227863668525338, - "heading": 3.3899382442707227, - "angularVelocity": -0.000005549507472025305, - "velocityX": 1.6158649701365668, - "velocityY": 0.16653180871555806, - "timestamp": 1.8960292768123002 - }, - { - "x": 5.757045112276785, - "y": 7.242899343993592, - "heading": 3.3899377432212017, - "angularVelocity": -0.000005549513433122579, - "velocityX": 1.6158649701354142, - "velocityY": 0.16653180871646053, - "timestamp": 1.9863163852319334 - }, - { - "x": 5.902936888026919, - "y": 7.257935019462863, - "heading": 3.3899372421711425, - "angularVelocity": -0.000005549519393768537, - "velocityX": 1.6158649701342616, - "velocityY": 0.1665318087173629, - "timestamp": 2.0766034936515667 - }, - { - "x": 6.04882866377676, - "y": 7.272970694932001, - "heading": 3.389936741120545, - "angularVelocity": -0.000005549525355144424, - "velocityX": 1.6158649701331087, - "velocityY": 0.16653180871826534, - "timestamp": 2.1668906020712 - }, - { - "x": 6.194720439526491, - "y": 7.288006370401304, - "heading": 3.3899362400694093, - "angularVelocity": -0.000005549531315809206, - "velocityX": 1.615864970131956, - "velocityY": 0.16653180871916778, - "timestamp": 2.2571777104908333 - }, - { - "x": 6.34061221527629, - "y": 7.30304204587104, - "heading": 3.3899357390177354, - "angularVelocity": -0.000005549537277178968, - "velocityX": 1.6158649701308037, - "velocityY": 0.1665318087200703, - "timestamp": 2.3474648189104665 - }, - { - "x": 6.486503991025816, - "y": 7.318077721337987, - "heading": 3.389935237965523, - "angularVelocity": -0.0000055495432383876534, - "velocityX": 1.6158649701296508, - "velocityY": 0.16653180872097262, - "timestamp": 2.4377519273301 - }, - { - "x": 6.632395766775363, - "y": 7.333113396808981, - "heading": 3.389934736912773, - "angularVelocity": -0.000005549549199340779, - "velocityX": 1.615864970128498, - "velocityY": 0.16653180872187498, - "timestamp": 2.528039035749733 - }, - { - "x": 6.778287542524652, - "y": 7.3481490722785985, - "heading": 3.389934235859484, - "angularVelocity": -0.00000554955516023689, - "velocityX": 1.6158649701273455, - "velocityY": 0.16653180872277748, - "timestamp": 2.618326144169367 - }, - { - "x": 6.924179318273876, - "y": 7.363184747747038, - "heading": 3.3899337348056577, - "angularVelocity": -0.000005549561121255593, - "velocityX": 1.6158649701261931, - "velocityY": 0.16653180872368004, - "timestamp": 2.708613252589 - }, - { - "x": 7.070071094023151, - "y": 7.378220423218671, - "heading": 3.389933233751293, - "angularVelocity": -0.0000055495670825865805, - "velocityX": 1.6158649701250403, - "velocityY": 0.1665318087245824, - "timestamp": 2.7989003610086334 - }, - { - "x": 7.215962869772269, - "y": 7.393256098685687, - "heading": 3.38993273269639, - "angularVelocity": -0.000005549573043968301, - "velocityX": 1.6158649701238874, - "velocityY": 0.16653180872548476, - "timestamp": 2.8891874694282667 - }, - { - "x": 7.361854645521179, - "y": 7.4082917741551615, - "heading": 3.389932231640949, - "angularVelocity": -0.00000554957900552128, - "velocityX": 1.6158649701227348, - "velocityY": 0.16653180872638726, - "timestamp": 2.9794745778479 - }, - { - "x": 7.507746421269845, - "y": 7.423327449627381, - "heading": 3.3899317305849697, - "angularVelocity": -0.0000055495849662374065, - "velocityX": 1.615864970121582, - "velocityY": 0.16653180872728968, - "timestamp": 3.069761686267533 - }, - { - "x": 7.653638197018768, - "y": 7.4383631250955204, - "heading": 3.389931229528452, - "angularVelocity": -0.000005549590927964199, - "velocityX": 1.615864970120429, - "velocityY": 0.16653180872819684, - "timestamp": 3.1600487946871665 - }, - { - "x": 7.79952997270157, - "y": 7.45339880119461, - "heading": 3.389930728467707, - "angularVelocity": -0.000005549637747689064, - "velocityX": 1.6158649693899627, - "velocityY": 0.16653181569922934, - "timestamp": 3.2503359031067998 - }, - { - "x": 7.908273186523536, - "y": 7.465055949439882, - "heading": 3.2483837688222263, - "angularVelocity": -1.567742750023664, - "velocityX": 1.2044157324947478, - "velocityY": 0.1291119900382222, - "timestamp": 3.340623011526433 + "x": 2.7043659064830394, + "y": 6.904431852524175, + "heading": 3.7255178406599083, + "angularVelocity": -0.04862383285360791, + "velocityX": 0.39229912325731986, + "velocityY": 0.05016142105727286, + "timestamp": 0.08387256115462738 + }, + { + "x": 2.770172988802701, + "y": 6.912846664761324, + "heading": 3.7174222347433226, + "angularVelocity": -0.09652269830726562, + "velocityX": 0.7846079982980328, + "velocityY": 0.10032854751670894, + "timestamp": 0.16774512230925476 + }, + { + "x": 2.868885003911175, + "y": 6.925469706226451, + "heading": 3.7053835964173025, + "angularVelocity": -0.1435348838796717, + "velocityX": 1.1769285896312263, + "velocityY": 0.15050263508533468, + "timestamp": 0.25161768346388214 + }, + { + "x": 3.000503155089929, + "y": 6.942301704976706, + "heading": 3.689495079575308, + "angularVelocity": -0.1894364095154122, + "velocityX": 1.5692635275093452, + "velocityY": 0.2006854031704579, + "timestamp": 0.3354902446185095 + }, + { + "x": 3.1650289492172274, + "y": 6.963343594326816, + "heading": 3.669876864825881, + "angularVelocity": -0.23390503973354093, + "velocityX": 1.9616164316716036, + "velocityY": 0.250879299027448, + "timestamp": 0.4193628057731369 + }, + { + "x": 3.362464324784135, + "y": 6.988596614292936, + "heading": 3.64668996545627, + "angularVelocity": -0.2764539326140709, + "velocityX": 2.3539924481728387, + "velocityY": 0.301087979411566, + "timestamp": 0.5032353669277643 + }, + { + "x": 3.592811857209642, + "y": 7.0180624930033995, + "heading": 3.620161246344741, + "angularVelocity": -0.316297949488161, + "velocityX": 2.7463991710094446, + "velocityY": 0.35131726401129654, + "timestamp": 0.5871079280823917 + }, + { + "x": 3.8560750973941316, + "y": 7.05174380499803, + "heading": 3.5906339439313264, + "angularVelocity": -0.3520496096330931, + "velocityX": 3.1388482307001198, + "velocityY": 0.40157724446419313, + "timestamp": 0.670980489237019 + }, + { + "x": 4.152259097249652, + "y": 7.089644787764795, + "heading": 3.558687346907366, + "angularVelocity": -0.38089449736802783, + "velocityX": 3.5313575235824133, + "velocityY": 0.4518877478522514, + "timestamp": 0.7548530503916462 + }, + { + "x": 4.481370515300424, + "y": 7.131773671322726, + "heading": 3.52550603083455, + "angularVelocity": -0.39561586788368536, + "velocityX": 3.9239462050529617, + "velocityY": 0.5022963765260808, + "timestamp": 0.8387256115462733 + }, + { + "x": 4.843397664915269, + "y": 7.178152540283051, + "heading": 3.4948677816779656, + "angularVelocity": -0.3652952614634004, + "velocityX": 4.316395548568172, + "velocityY": 0.5529683167158834, + "timestamp": 0.9225981727009005 + }, + { + "x": 5.214084962853593, + "y": 7.225506645206567, + "heading": 3.4948677687882737, + "angularVelocity": -1.5368186922063484e-7, + "velocityX": 4.419649201541919, + "velocityY": 0.5645959092177173, + "timestamp": 1.0064707338555277 + }, + { + "x": 5.584772271414454, + "y": 7.2728606669772775, + "heading": 3.4948677558987455, + "angularVelocity": -1.5367991215657251e-7, + "velocityX": 4.419649328192821, + "velocityY": 0.5645949177992666, + "timestamp": 1.0903432950101548 + }, + { + "x": 5.955780374663183, + "y": 7.317631980849406, + "heading": 3.4948677392182756, + "angularVelocity": -1.9887875297845992e-7, + "velocityX": 4.423474115268035, + "velocityY": 0.5338016778763695, + "timestamp": 1.174215856164782 + }, + { + "x": 6.300300548190503, + "y": 7.351378145453142, + "heading": 3.441189014071279, + "angularVelocity": -0.6400034100310172, + "velocityX": 4.107662491576513, + "velocityY": 0.40235047242115085, + "timestamp": 1.2580884173194091 + }, + { + "x": 6.611937161645573, + "y": 7.380701076408195, + "heading": 3.3909317361493, + "angularVelocity": -0.5992100065875497, + "velocityX": 3.7155967239457195, + "velocityY": 0.3496129193073305, + "timestamp": 1.3419609784740363 + }, + { + "x": 6.890683200327295, + "y": 7.405709842435669, + "heading": 3.344663622374181, + "angularVelocity": -0.551647799210751, + "velocityX": 3.323447321083073, + "velocityY": 0.29817577623948227, + "timestamp": 1.4258335396286634 + }, + { + "x": 7.136536630210955, + "y": 7.426441407681926, + "heading": 3.302577269433113, + "angularVelocity": -0.5017892903434469, + "velocityX": 2.931273666848028, + "velocityY": 0.2471793511591556, + "timestamp": 1.5097061007832906 + }, + { + "x": 7.349496491178017, + "y": 7.442914379961346, + "heading": 3.264769514381459, + "angularVelocity": -0.45077620775108873, + "velocityX": 2.539088565263299, + "velocityY": 0.196404784266102, + "timestamp": 1.5935786619379178 + }, + { + "x": 7.529562223309363, + "y": 7.455139968848337, + "heading": 3.2312984812041172, + "angularVelocity": -0.39907012158165334, + "velocityX": 2.1468967878467162, + "velocityY": 0.14576386745185102, + "timestamp": 1.677451223092545 + }, + { + "x": 7.676733458548059, + "y": 7.463125667757038, + "heading": 3.2022028135705005, + "angularVelocity": -0.3469032927225913, + "velocityX": 1.7547006221423278, + "velocityY": 0.0952122934934494, + "timestamp": 1.761323784247172 + }, + { + "x": 7.791009935801521, + "y": 7.466876839352261, + "heading": 3.177509999915845, + "angularVelocity": -0.29440872336224383, + "velocityX": 1.3625013434701525, + "velocityY": 0.04472465778536462, + "timestamp": 1.8451963454017992 + }, + { + "x": 7.872391460113929, + "y": 7.466397510828961, + "heading": 3.1572405778603585, + "angularVelocity": -0.24166928702841778, + "velocityX": 0.9702997403688861, + "velocityY": -0.005714962279684649, + "timestamp": 1.9290689065564264 + }, + { + "x": 7.920877880694152, + "y": 7.461690816856239, + "heading": 3.1414104927277844, + "angularVelocity": -0.1887397369849021, + "velocityX": 0.5780963394075183, + "velocityY": -0.05611720815399605, + "timestamp": 2.0129414677110535 }, { "x": 7.936469078063965, "y": 7.452759265899658, "heading": 3.1300325241315123, - "angularVelocity": -1.3108321526994196, - "velocityX": 0.3122914448573039, - "velocityY": -0.13619534121251922, - "timestamp": 3.4309101199460663 - }, - { - "x": 7.897313989149588, - "y": 7.432003540468781, - "heading": 3.2231598958654137, - "angularVelocity": 0.9773255115620281, - "velocityX": -0.41091321048106677, - "velocityY": -0.21782102937809103, - "timestamp": 3.5261980978328387 - }, - { - "x": 7.764258804459074, - "y": 7.410354349811778, - "heading": 3.302887541742508, - "angularVelocity": 0.8367020441112922, - "velocityX": -1.3963480770719, - "velocityY": -0.2271975031683705, - "timestamp": 3.621486075719611 - }, - { - "x": 7.611104944022113, - "y": 7.387923020288099, - "heading": 3.3028890943761, - "angularVelocity": 0.000016294118377845236, - "velocityX": -1.607273696395563, - "velocityY": -0.23540566212575834, - "timestamp": 3.7167740536063834 - }, - { - "x": 7.457951083368431, - "y": 7.365491692124674, - "heading": 3.3028906469716444, - "angularVelocity": 0.000016293719092580313, - "velocityX": -1.6072736986022782, - "velocityY": -0.23540564768491373, - "timestamp": 3.8120620314931557 - }, - { - "x": 7.3047972227130815, - "y": 7.3430603639785605, - "heading": 3.3028921995394867, - "angularVelocity": 0.0000162934283709874, - "velocityX": -1.6072736986590501, - "velocityY": -0.23540564774414025, - "timestamp": 3.907350009379928 - }, - { - "x": 7.15164336205228, - "y": 7.320629035813856, - "heading": 3.3028937520796284, - "angularVelocity": 0.000016293137669547086, - "velocityX": -1.6072736987158178, - "velocityY": -0.23540564780336143, - "timestamp": 4.0026379872667 - }, - { - "x": 6.998489501387815, - "y": 7.298197707651154, - "heading": 3.3028953045920724, - "angularVelocity": 0.00001629284698917995, - "velocityX": -1.6072736987725829, - "velocityY": -0.23540564786256962, - "timestamp": 4.097925965153473 - }, - { - "x": 6.845335640713116, - "y": 7.275766379466654, - "heading": 3.3028968570768193, - "angularVelocity": 0.000016292556328916256, - "velocityX": -1.607273698829345, - "velocityY": -0.23540564792176438, - "timestamp": 4.193213943040245 - }, - { - "x": 6.692181780035948, - "y": 7.253335051289657, - "heading": 3.3028984095338725, - "angularVelocity": 0.0000162922656907247, - "velocityX": -1.6072736988861036, - "velocityY": -0.2354056479809454, - "timestamp": 4.288501920927017 - }, - { - "x": 6.539027919353228, - "y": 7.230903723107964, - "heading": 3.302899961963233, - "angularVelocity": 0.00001629197507403925, - "velocityX": -1.6072736989428598, - "velocityY": -0.23540564804011319, - "timestamp": 4.38378989881379 - }, - { - "x": 6.385874058664987, - "y": 7.2084723949147005, - "heading": 3.3029015143649034, - "angularVelocity": 0.000016291684475842535, - "velocityX": -1.607273698999613, - "velocityY": -0.2354056480992679, - "timestamp": 4.479077876700562 - }, - { - "x": 6.232720197971391, - "y": 7.186041066717774, - "heading": 3.3029030667388852, - "angularVelocity": 0.000016291393901831136, - "velocityX": -1.6072736990563632, - "velocityY": -0.2354056481584089, - "timestamp": 4.5743658545873345 - }, - { - "x": 6.079566337272002, - "y": 7.163609738524357, - "heading": 3.302904619085181, - "angularVelocity": 0.000016291103348699848, - "velocityX": -1.6072736991131096, - "velocityY": -0.2354056482175359, - "timestamp": 4.669653832474107 - }, - { - "x": 5.9264124765671236, - "y": 7.141178410308786, - "heading": 3.3029061714037917, - "angularVelocity": 0.000016290812813611415, - "velocityX": -1.6072736991698542, - "velocityY": -0.23540564827665061, - "timestamp": 4.764941810360879 - }, - { - "x": 5.773258615858539, - "y": 7.118747082103052, - "heading": 3.302907723694721, - "angularVelocity": 0.000016290522303354113, - "velocityX": -1.6072736992265952, - "velocityY": -0.23540564833575084, - "timestamp": 4.8602297882476515 - }, - { - "x": 5.620104755142444, - "y": 7.096315753876903, - "heading": 3.30290927595797, - "angularVelocity": 0.000016290231812342035, - "velocityX": -1.6072736992833332, - "velocityY": -0.2354056483948381, - "timestamp": 4.955517766134424 - }, - { - "x": 5.466950894422645, - "y": 7.07388442565226, - "heading": 3.30291082819354, - "angularVelocity": 0.000016289941343623743, - "velocityX": -1.6072736993400683, - "velocityY": -0.23540564845391188, - "timestamp": 5.050805744021196 - }, - { - "x": 5.313797033694609, - "y": 7.0514530974310325, - "heading": 3.3029123804014344, - "angularVelocity": 0.000016289650893731917, - "velocityX": -1.607273699396801, - "velocityY": -0.23540564851297285, - "timestamp": 5.1460937219079685 - }, - { - "x": 5.160643172963514, - "y": 7.02902176918985, - "heading": 3.3029139325816548, - "angularVelocity": 0.000016289360468842725, - "velocityX": -1.6072736994535295, - "velocityY": -0.23540564857201896, - "timestamp": 5.241381699794741 - }, - { - "x": 5.007489312227579, - "y": 7.006590440945802, - "heading": 3.3029154847342026, - "angularVelocity": 0.000016289070060669773, - "velocityX": -1.6072736995102561, - "velocityY": -0.23540564863105312, - "timestamp": 5.336669677681513 - }, - { - "x": 4.854335451483483, - "y": 6.984159112697374, - "heading": 3.30291703685908, - "angularVelocity": 0.00001628877967680068, - "velocityX": -1.6072736995669794, - "velocityY": -0.23540564869007324, - "timestamp": 5.431957655568286 - }, - { - "x": 4.701181590737123, - "y": 6.961727784450269, - "heading": 3.3029185889562895, - "angularVelocity": 0.00001628848931160111, - "velocityX": -1.6072736996236994, - "velocityY": -0.23540564874908013, - "timestamp": 5.527245633455058 - }, - { - "x": 4.548027729985049, - "y": 6.939296456188679, - "heading": 3.3029201410258326, - "angularVelocity": 0.000016288198966670516, - "velocityX": -1.6072736996804171, - "velocityY": -0.23540564880807416, - "timestamp": 5.62253361134183 - }, - { - "x": 4.394873869225821, - "y": 6.916865127932724, - "heading": 3.3029216930677117, - "angularVelocity": 0.000016287908648086418, - "velocityX": -1.607273699737131, - "velocityY": -0.2354056488670535, - "timestamp": 5.717821589228603 - }, - { - "x": 4.2417200084613995, - "y": 6.8944337996586516, - "heading": 3.302923245081929, - "angularVelocity": 0.000016287618349487135, - "velocityX": -1.607273699793842, - "velocityY": -0.23540564892601998, - "timestamp": 5.813109567115375 - }, - { - "x": 4.088566147690897, - "y": 6.872002471380619, - "heading": 3.3029247970684854, - "angularVelocity": 0.00001628732806480796, - "velocityX": -1.607273699850551, - "velocityY": -0.23540564898497424, - "timestamp": 5.908397545002147 - }, - { - "x": 3.935412286918391, - "y": 6.849571143103449, - "heading": 3.3029263490273832, - "angularVelocity": 0.00001628703780483265, - "velocityX": -1.6072736999072568, - "velocityY": -0.2354056490439145, - "timestamp": 6.00368552288892 - }, - { - "x": 3.7822584261353844, - "y": 6.827139814806336, - "heading": 3.302927900958626, - "angularVelocity": 0.000016286747572595648, - "velocityX": -1.6072736999639583, - "velocityY": -0.2354056491028402, - "timestamp": 6.098973500775692 - }, - { - "x": 3.629104565353085, - "y": 6.804708486525145, - "heading": 3.3029294528622137, - "angularVelocity": 0.000016286457354449532, - "velocityX": -1.607273700020658, - "velocityY": -0.2354056491617539, - "timestamp": 6.194261478662464 - }, - { - "x": 3.475950704559541, - "y": 6.782277158221499, - "heading": 3.3029310047381495, - "angularVelocity": 0.000016286167156294597, - "velocityX": -1.607273700077355, - "velocityY": -0.2354056492206542, - "timestamp": 6.289549456549237 - }, - { - "x": 3.3227968437635895, - "y": 6.759845829932118, - "heading": 3.3029325565864354, - "angularVelocity": 0.000016285876982629207, - "velocityX": -1.6072737001340485, - "velocityY": -0.23540564927954047, - "timestamp": 6.384837434436009 - }, - { - "x": 3.16964298296173, - "y": 6.737414501611197, - "heading": 3.3029341084070736, - "angularVelocity": 0.00001628558683240146, - "velocityX": -1.6072737001907338, - "velocityY": -0.23540564933844627, - "timestamp": 6.480125412322781 - }, - { - "x": 3.0164891229666777, - "y": 6.714983167772551, - "heading": 3.302935660207411, - "angularVelocity": 0.00001628537379077082, - "velocityX": -1.607273691741332, - "velocityY": -0.23540570734761523, - "timestamp": 6.575413390209554 - }, - { - "x": 2.8768170840789846, - "y": 6.685183755439867, - "heading": 3.351795711849992, - "angularVelocity": 0.512761974030336, - "velocityX": -1.4657886753698728, - "velocityY": -0.3127300316833431, - "timestamp": 6.670701368096327 + "angularVelocity": -0.13565781752265163, + "velocityX": 0.18589151392514228, + "velocityY": -0.1064895459686019, + "timestamp": 2.0968140288656807 + }, + { + "x": 7.915740116584839, + "y": 7.438450021195838, + "heading": 3.1229758381188732, + "angularVelocity": -0.07896960965622021, + "velocityX": -0.23197262761210297, + "velocityY": -0.16013118150815783, + "timestamp": 2.1861735437899514 + }, + { + "x": 7.857668877454443, + "y": 7.419352965299199, + "heading": 3.1208942356371594, + "angularVelocity": -0.023294693167012128, + "velocityX": -0.6498607247321063, + "velocityY": -0.21371038006220794, + "timestamp": 2.275533058714222 + }, + { + "x": 7.762252743153032, + "y": 7.395475025916455, + "heading": 3.123675562058841, + "angularVelocity": 0.031125128913673642, + "velocityX": -1.0677781138614328, + "velocityY": -0.2672120523816525, + "timestamp": 2.364892573638493 + }, + { + "x": 7.629488438537512, + "y": 7.366825038470432, + "heading": 3.1311770789195053, + "angularVelocity": 0.08394760051038563, + "velocityX": -1.4857321542986437, + "velocityY": -0.3206148496923158, + "timestamp": 2.4542520885627637 + }, + { + "x": 7.459371747842553, + "y": 7.333414662601681, + "heading": 3.14321079663535, + "angularVelocity": 0.13466632765456588, + "velocityX": -1.9037333723121508, + "velocityY": -0.3738871668793641, + "timestamp": 2.5436116034870344 + }, + { + "x": 7.251897047158697, + "y": 7.295259995220328, + "heading": 3.159517771914932, + "angularVelocity": 0.1824872851357962, + "velocityX": -2.3217975260909065, + "velocityY": -0.4269793475679505, + "timestamp": 2.632971118411305 + }, + { + "x": 7.007056479155703, + "y": 7.252384696832851, + "heading": 3.179718570361924, + "angularVelocity": 0.2260620871108287, + "velocityX": -2.739949609288821, + "velocityY": -0.47980674944141977, + "timestamp": 2.722330633335576 + }, + { + "x": 6.724838378878884, + "y": 7.204826948209543, + "heading": 3.2032038202698816, + "angularVelocity": 0.2628175626049468, + "velocityX": -3.1582322320794676, + "velocityY": -0.5322068798561834, + "timestamp": 2.811690148259847 + }, + { + "x": 6.405224102249246, + "y": 7.152658687501974, + "heading": 3.2288345481257252, + "angularVelocity": 0.2868270701510058, + "velocityX": -3.5767234960988827, + "velocityY": -0.5838019683945127, + "timestamp": 2.9010496631841183 + }, + { + "x": 6.048184900918235, + "y": 7.096065538236557, + "heading": 3.253716466109861, + "angularVelocity": 0.2784473260091271, + "velocityX": -3.995536475702559, + "velocityY": -0.6333197904373026, + "timestamp": 2.9904091781083895 + }, + { + "x": 5.654508056038536, + "y": 7.03656993570583, + "heading": 3.253716532797699, + "angularVelocity": 7.462869273514143e-7, + "velocityX": -4.4055391886731465, + "velocityY": -0.6658004195876279, + "timestamp": 3.0797686930326607 + }, + { + "x": 5.259674739202379, + "y": 6.9853073355549045, + "heading": 3.253716547435891, + "angularVelocity": 1.6381235089876507e-7, + "velocityX": -4.418480977328111, + "velocityY": -0.573666947435528, + "timestamp": 3.169128207956932 + }, + { + "x": 4.864841395212643, + "y": 6.934044944547169, + "heading": 3.253716562074652, + "angularVelocity": 1.63818712302411e-7, + "velocityX": -4.41848128119701, + "velocityY": -0.5736646069663508, + "timestamp": 3.258487722881203 + }, + { + "x": 4.491651487875698, + "y": 6.885589548517627, + "heading": 3.312737130762719, + "angularVelocity": 0.6604844345684403, + "velocityX": -4.17627499044966, + "velocityY": -0.5422522276514719, + "timestamp": 3.347847237805474 + }, + { + "x": 4.155780465580293, + "y": 6.841979518441731, + "heading": 3.3658484725068094, + "angularVelocity": 0.5943557525922112, + "velocityX": -3.7586486741791827, + "velocityY": -0.48802894815235986, + "timestamp": 3.4372067527297454 + }, + { + "x": 3.857228363312986, + "y": 6.803214924393239, + "heading": 3.413052217911606, + "angularVelocity": 0.528245318305502, + "velocityX": -3.3410219663828666, + "velocityY": -0.43380488447528814, + "timestamp": 3.5265662676540166 + }, + { + "x": 3.5959952091477, + "y": 6.769295821195016, + "heading": 3.454350393436854, + "angularVelocity": 0.46215756162336336, + "velocityX": -2.9233949444183165, + "velocityY": -0.37958020728927433, + "timestamp": 3.615925782578288 + }, + { + "x": 3.3720810297941446, + "y": 6.740222250549493, + "heading": 3.4897450705258297, + "angularVelocity": 0.3960929859452755, + "velocityX": -2.5057676235520625, + "velocityY": -0.32535506342174403, + "timestamp": 3.705285297502559 + }, + { + "x": 3.1854858494686082, + "y": 6.715994242916669, + "heading": 3.519238149861463, + "angularVelocity": 0.33004968033484894, + "velocityX": -2.0881400316873893, + "velocityY": -0.2711295786839986, + "timestamp": 3.79464481242683 + }, + { + "x": 3.036209688317166, + "y": 6.696611819522511, + "heading": 3.542831216825285, + "angularVelocity": 0.2640241163329568, + "velocityX": -1.6705122143730164, + "velocityY": -0.21690385641174914, + "timestamp": 3.8840043273511013 + }, + { + "x": 2.924252561165524, + "y": 6.682074994259449, + "heading": 3.560525448236687, + "angularVelocity": 0.19801172182276905, + "velocityX": -1.2528842311478772, + "velocityY": -0.1626779786728065, + "timestamp": 3.9733638422753725 + }, + { + "x": 2.849614476676597, + "y": 6.6723837752661295, + "heading": 3.5723215560021737, + "angularVelocity": 0.13200729408035738, + "velocityX": -0.8352561509782144, + "velocityY": -0.10845200985627608, + "timestamp": 4.062723357199644 }, { "x": 2.812295436859131, "y": 6.667538166046143, "heading": 3.5782197562990956, - "angularVelocity": 2.3762078855126445, - "velocityX": -0.6771226407787638, - "velocityY": -0.18518169637911341, - "timestamp": 6.7659893459831 + "angularVelocity": 0.06600528552466699, + "velocityX": -0.41762804832918915, + "velocityY": -0.05422600183195843, + "timestamp": 4.1520828721239145 }, { "x": 2.812295436859131, "y": 6.667538166046143, "heading": 3.5782197562990956, - "angularVelocity": -9.273365059762218e-23, - "velocityX": 5.842590739873529e-23, - "velocityY": -3.6959137012591573e-22, - "timestamp": 6.861277323869873 + "angularVelocity": -6.3889947065303214e-27, + "velocityX": 2.638834492143492e-26, + "velocityY": 4.3134967976763594e-26, + "timestamp": 4.2414423870481865 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/FrontWing3Contested5.traj b/src/main/deploy/choreo/FrontWing3Contested5.traj index 73648914..64ea1355 100644 --- a/src/main/deploy/choreo/FrontWing3Contested5.traj +++ b/src/main/deploy/choreo/FrontWing3Contested5.traj @@ -4,829 +4,595 @@ "x": 1.2263859510421753, "y": 5.594110488891602, "heading": 3.141, - "angularVelocity": -9.716627282364513e-28, - "velocityX": -2.1006296109192272e-27, - "velocityY": 1.6816897032610772e-27, + "angularVelocity": 1.2684261528823466e-28, + "velocityX": -8.90285455606977e-27, + "velocityY": -9.123583685606214e-27, "timestamp": 0 }, { - "x": 1.2629372172985613, - "y": 5.611868166522511, - "heading": 3.3611684637424437, - "angularVelocity": 2.6151040269567423, - "velocityX": 0.4341464801664794, - "velocityY": 0.21092109874697887, - "timestamp": 0.08419109200740245 - }, - { - "x": 1.3467908643065862, - "y": 5.672396849103953, - "heading": 3.4705725495976436, - "angularVelocity": 1.2994734151397, - "velocityX": 0.9959919156370148, - "velocityY": 0.7189440252909496, - "timestamp": 0.1683821840148049 - }, - { - "x": 1.4464012359618026, - "y": 5.766107214252463, - "heading": 3.4705728088664487, - "angularVelocity": 0.0000030795277620559296, - "velocityX": 1.1831462127425316, - "velocityY": 1.1130674625323809, - "timestamp": 0.25257327602220736 - }, - { - "x": 1.5460115925515412, - "y": 5.859817595415609, - "heading": 3.4705730681338594, - "angularVelocity": 0.0000030795111997424536, - "velocityX": 1.1831460337986872, - "velocityY": 1.1130676527500878, - "timestamp": 0.3367643680296098 - }, - { - "x": 1.6456219491412756, - "y": 5.9535279765787505, - "heading": 3.4705733274013144, - "angularVelocity": 0.000003079511724050569, - "velocityX": 1.183146033798638, - "velocityY": 1.1130676527500365, - "timestamp": 0.42095546003701223 - }, - { - "x": 1.7452323057310184, - "y": 6.0472383577418745, - "heading": 3.4705735866688134, - "angularVelocity": 0.000003079512247611481, - "velocityX": 1.1831460337987365, - "velocityY": 1.113067652749828, - "timestamp": 0.5051465520444147 - }, - { - "x": 1.8448426623207694, - "y": 6.1409487389049815, - "heading": 3.4705738459363564, - "angularVelocity": 0.0000030795127711126187, - "velocityX": 1.1831460337988353, - "velocityY": 1.1130676527496197, - "timestamp": 0.5893376440518172 - }, - { - "x": 1.9444530189105287, - "y": 6.234659120068071, - "heading": 3.470574105203944, - "angularVelocity": 0.000003079513294842049, - "velocityX": 1.183146033798934, - "velocityY": 1.1130676527494117, - "timestamp": 0.6735287360592197 - }, - { - "x": 2.0440633755002966, - "y": 6.328369501231142, - "heading": 3.470574364471575, - "angularVelocity": 0.0000030795138187518754, - "velocityX": 1.1831460337990323, - "velocityY": 1.1130676527492032, - "timestamp": 0.7577198280666222 - }, - { - "x": 2.1436737320900723, - "y": 6.422079882394196, - "heading": 3.4705746237392505, - "angularVelocity": 0.0000030795143424110884, - "velocityX": 1.1831460337991306, - "velocityY": 1.113067652748995, - "timestamp": 0.8419109200740247 - }, - { - "x": 2.2432840886798564, - "y": 6.515790263557233, - "heading": 3.47057488300697, - "angularVelocity": 0.0000030795148666763967, - "velocityX": 1.1831460337992294, - "velocityY": 1.1130676527487866, - "timestamp": 0.9261020120814272 - }, - { - "x": 2.342894445269649, - "y": 6.609500644720252, - "heading": 3.4705751422747335, - "angularVelocity": 0.0000030795153898968674, - "velocityX": 1.1831460337993278, - "velocityY": 1.1130676527485786, - "timestamp": 1.0102931040888297 - }, - { - "x": 2.4425048018594557, - "y": 6.703211025883247, - "heading": 3.470575401542541, - "angularVelocity": 0.000003079515913944498, - "velocityX": 1.1831460337994937, - "velocityY": 1.1130676527482986, - "timestamp": 1.0944841960962322 - }, - { - "x": 2.5421151652954292, - "y": 6.796921399768766, - "heading": 3.4705756608109883, - "angularVelocity": 0.0000030795235106850793, - "velocityX": 1.183146115116498, - "velocityY": 1.1130675663083123, - "timestamp": 1.1786752881036346 - }, - { - "x": 2.632005364209705, - "y": 6.868564037074326, - "heading": 3.5437695190039196, - "angularVelocity": 0.8693777031244078, - "velocityX": 1.0676925167614182, - "velocityY": 0.8509527029208858, - "timestamp": 1.2628663801110371 + "x": 1.2489662072831478, + "y": 5.614521121433257, + "heading": 3.1501023791424476, + "angularVelocity": 0.11304779998591061, + "velocityX": 0.2804374824661162, + "velocityY": 0.2534916497154969, + "timestamp": 0.08051796800629471 + }, + { + "x": 1.2941269040941792, + "y": 5.65534214931359, + "heading": 3.1683083464792072, + "angularVelocity": 0.22611061589800957, + "velocityX": 0.5608772542235648, + "velocityY": 0.506980353467706, + "timestamp": 0.16103593601258942 + }, + { + "x": 1.3618682728307967, + "y": 5.716573194164052, + "heading": 3.1956213802625175, + "angularVelocity": 0.33921663027033977, + "velocityX": 0.8413198993213754, + "velocityY": 0.7604643580383921, + "timestamp": 0.24155390401888413 + }, + { + "x": 1.4521905498783176, + "y": 5.798213731151769, + "heading": 3.2320485280043223, + "angularVelocity": 0.4524101718383757, + "velocityX": 1.121765480227465, + "velocityY": 1.0139418444009154, + "timestamp": 0.32207187202517884 + }, + { + "x": 1.5650939157362251, + "y": 5.900263093874502, + "heading": 3.2776019617285486, + "angularVelocity": 0.5657548849303424, + "velocityX": 1.4022133028627977, + "velocityY": 1.2674110543221202, + "timestamp": 0.4025898400314736 + }, + { + "x": 1.700578414867027, + "y": 6.022720499588671, + "heading": 3.3323005252490514, + "angularVelocity": 0.6793336304292585, + "velocityX": 1.6826616777041643, + "velocityY": 1.5208705428904563, + "timestamp": 0.4831078080377683 + }, + { + "x": 1.8586438590589127, + "y": 6.165585103627761, + "heading": 3.3961709105895657, + "angularVelocity": 0.7932438798694066, + "velocityX": 1.9631077150323568, + "velocityY": 1.7743195410485293, + "timestamp": 0.563625776044063 + }, + { + "x": 2.039289707486661, + "y": 6.328856070494732, + "heading": 3.469248537641403, + "angularVelocity": 0.9075940297713951, + "velocityX": 2.2435470355340517, + "velocityY": 2.027758162677519, + "timestamp": 0.6441437440503578 + }, + { + "x": 2.1973352722504775, + "y": 6.471697816178928, + "heading": 3.5342798000329774, + "angularVelocity": 0.8076614947174351, + "velocityX": 1.9628608207234048, + "velocityY": 1.774035649695346, + "timestamp": 0.7246617120566525 + }, + { + "x": 2.3328015579338945, + "y": 6.594133674846713, + "heading": 3.590056917522341, + "angularVelocity": 0.6927288265024656, + "velocityX": 1.682435474189146, + "velocityY": 1.520602937448862, + "timestamp": 0.8051796800629473 + }, + { + "x": 2.4456892468776004, + "y": 6.6961637450440366, + "heading": 3.6365583693082697, + "angularVelocity": 0.5775288787999897, + "velocityX": 1.4020186020452068, + "velocityY": 1.2671714491024832, + "timestamp": 0.885697648069242 + }, + { + "x": 2.5359988834189062, + "y": 6.777787981324914, + "heading": 3.67376995651372, + "angularVelocity": 0.4621525868926695, + "velocityX": 1.121608490346475, + "velocityY": 1.013739396335684, + "timestamp": 0.9662156160755367 + }, + { + "x": 2.603730869861698, + "y": 6.839006281819378, + "heading": 3.701682776944854, + "angularVelocity": 0.3466657334044004, + "velocityX": 0.8412033750963059, + "velocityY": 0.7603060783858558, + "timestamp": 1.0467335840818315 + }, + { + "x": 2.648885462775646, + "y": 6.879818541544654, + "heading": 3.720292007909602, + "angularVelocity": 0.23111898406692608, + "velocityX": 0.5608014463357825, + "velocityY": 0.5068714565932112, + "timestamp": 1.127251552088126 }, { "x": 2.6714627742767334, "y": 6.900224685668945, "heading": 3.729596046054495, - "angularVelocity": 2.207199391525135, - "velocityX": 0.46866490416300804, - "velocityY": 0.3760569894002088, - "timestamp": 1.3470574721184396 + "angularVelocity": 0.11555232173973205, + "velocityX": 0.28040091000958045, + "velocityY": 0.25343590542047173, + "timestamp": 1.2077695200944207 }, { "x": 2.6714627742767334, "y": 6.900224685668945, "heading": 3.729596046054495, - "angularVelocity": -1.0211590332128658e-23, - "velocityX": -1.1480030002254912e-23, - "velocityY": -4.344519998520169e-23, - "timestamp": 1.4312485641258421 - }, - { - "x": 2.720827723501713, - "y": 6.91216585910853, - "heading": 3.486294732820543, - "angularVelocity": -2.69475141570759, - "velocityX": 0.5467552354822697, - "velocityY": 0.13225779017707837, - "timestamp": 1.5215356725454756 - }, - { - "x": 2.8392095989605575, - "y": 6.942185818091124, - "heading": 3.389947764111763, - "angularVelocity": -1.0671176693463396, - "velocityX": 1.311171412297561, - "velocityY": 0.33249441150564496, - "timestamp": 1.6118227809651091 - }, - { - "x": 2.985101373007063, - "y": 6.9572215101019514, - "heading": 3.3899472630700718, - "angularVelocity": -0.000005549426712336868, - "velocityX": 1.615864951266793, - "velocityY": 0.16653199192676998, - "timestamp": 1.7021098893847426 - }, - { - "x": 3.1309931487591207, - "y": 6.972257185567622, - "heading": 3.3899467620302377, - "angularVelocity": -0.000005549406141075235, - "velocityX": 1.6158649701561478, - "velocityY": 0.16653180870033996, - "timestamp": 1.7923969978043761 - }, - { - "x": 3.276884924511046, - "y": 6.987292861034595, - "heading": 3.389946260989866, - "angularVelocity": -0.000005549412101088006, - "velocityX": 1.615864970155008, - "velocityY": 0.16653180870111933, - "timestamp": 1.8826841062240096 - }, - { - "x": 3.4227767002625384, - "y": 7.002328536502587, - "heading": 3.3899457599489553, - "angularVelocity": -0.000005549418061933105, - "velocityX": 1.6158649701538554, - "velocityY": 0.16653180870202178, - "timestamp": 1.9729712146436431 - }, - { - "x": 3.5686684760143703, - "y": 7.017364211969376, - "heading": 3.389945258907507, - "angularVelocity": -0.000005549424021960657, - "velocityX": 1.6158649701527033, - "velocityY": 0.16653180870292417, - "timestamp": 2.0632583230632764 - }, - { - "x": 3.7145602517660743, - "y": 7.032399887436731, - "heading": 3.3899447578655204, - "angularVelocity": -0.000005549429982841055, - "velocityX": 1.6158649701515504, - "velocityY": 0.16653180870382667, - "timestamp": 2.1535454314829097 - }, - { - "x": 3.8604520275173986, - "y": 7.04743556290717, - "heading": 3.3899442568229956, - "angularVelocity": -0.000005549435944010941, - "velocityX": 1.615864970150398, - "velocityY": 0.1665318087047291, - "timestamp": 2.243832539902543 - }, - { - "x": 4.00634380326868, - "y": 7.062471238372739, - "heading": 3.389943755779933, - "angularVelocity": -0.000005549441903913974, - "velocityX": 1.6158649701492453, - "velocityY": 0.1665318087056315, - "timestamp": 2.334119648322176 - }, - { - "x": 4.1522355790200765, - "y": 7.077506913841867, - "heading": 3.389943254736332, - "angularVelocity": -0.0000055494478640805644, - "velocityX": 1.615864970148093, - "velocityY": 0.16653180870653383, - "timestamp": 2.4244067567418095 - }, - { - "x": 4.298127354771182, - "y": 7.0925425893096525, - "heading": 3.389942753692193, - "angularVelocity": -0.000005549453823846852, - "velocityX": 1.6158649701469403, - "velocityY": 0.16653180870743617, - "timestamp": 2.5146938651614428 - }, - { - "x": 4.444019130522188, - "y": 7.10757826477934, - "heading": 3.389942252647516, - "angularVelocity": -0.000005549459785648991, - "velocityX": 1.6158649701457874, - "velocityY": 0.16653180870833867, - "timestamp": 2.604980973581076 - }, - { - "x": 4.5899109062731025, - "y": 7.122613940245511, - "heading": 3.3899417516023003, - "angularVelocity": -0.000005549465746228024, - "velocityX": 1.6158649701446353, - "velocityY": 0.1665318087092411, - "timestamp": 2.6952680820007093 - }, - { - "x": 4.735802682023898, - "y": 7.137649615713241, - "heading": 3.389941250556547, - "angularVelocity": -0.00000554947170730299, - "velocityX": 1.6158649701434824, - "velocityY": 0.16653180871014361, - "timestamp": 2.7855551904203426 - }, - { - "x": 4.881694457774774, - "y": 7.152685291182225, - "heading": 3.389940749510255, - "angularVelocity": -0.000005549477667769927, - "velocityX": 1.61586497014233, - "velocityY": 0.1665318087110459, - "timestamp": 2.875842298839976 - }, - { - "x": 5.027586233525358, - "y": 7.167720966650761, - "heading": 3.3899402484634247, - "angularVelocity": -0.000005549483629157811, - "velocityX": 1.6158649701411771, - "velocityY": 0.16653180871194842, - "timestamp": 2.966129407259609 - }, - { - "x": 5.173478009275799, - "y": 7.182756642119004, - "heading": 3.3899397474160566, - "angularVelocity": -0.000005549489589198961, - "velocityX": 1.6158649701400245, - "velocityY": 0.1665318087128507, - "timestamp": 3.0564165156792424 - }, - { - "x": 5.319369785026301, - "y": 7.197792317586818, - "heading": 3.38993924636815, - "angularVelocity": -0.0000055494955497599425, - "velocityX": 1.615864970138872, - "velocityY": 0.16653180871375314, - "timestamp": 3.1467036240988757 - }, - { - "x": 5.4652615607764785, - "y": 7.212827993058087, - "heading": 3.3899387453197054, - "angularVelocity": -0.000005549501510730972, - "velocityX": 1.6158649701377192, - "velocityY": 0.16653180871465564, - "timestamp": 3.236990732518509 - }, - { - "x": 5.611153336526806, - "y": 7.227863668525338, - "heading": 3.3899382442707227, - "angularVelocity": -0.000005549507472025305, - "velocityX": 1.6158649701365668, - "velocityY": 0.16653180871555806, - "timestamp": 3.3272778409381423 - }, - { - "x": 5.757045112276785, - "y": 7.242899343993592, - "heading": 3.3899377432212017, - "angularVelocity": -0.000005549513433122579, - "velocityX": 1.6158649701354142, - "velocityY": 0.16653180871646053, - "timestamp": 3.4175649493577755 - }, - { - "x": 5.902936888026919, - "y": 7.257935019462863, - "heading": 3.3899372421711425, - "angularVelocity": -0.000005549519393768537, - "velocityX": 1.6158649701342616, - "velocityY": 0.1665318087173629, - "timestamp": 3.507852057777409 - }, - { - "x": 6.04882866377676, - "y": 7.272970694932001, - "heading": 3.389936741120545, - "angularVelocity": -0.000005549525355144424, - "velocityX": 1.6158649701331087, - "velocityY": 0.16653180871826534, - "timestamp": 3.598139166197042 - }, - { - "x": 6.194720439526491, - "y": 7.288006370401304, - "heading": 3.3899362400694093, - "angularVelocity": -0.000005549531315809206, - "velocityX": 1.615864970131956, - "velocityY": 0.16653180871916778, - "timestamp": 3.6884262746166754 - }, - { - "x": 6.34061221527629, - "y": 7.30304204587104, - "heading": 3.3899357390177354, - "angularVelocity": -0.000005549537277178968, - "velocityX": 1.6158649701308037, - "velocityY": 0.1665318087200703, - "timestamp": 3.7787133830363087 - }, - { - "x": 6.486503991025816, - "y": 7.318077721337987, - "heading": 3.389935237965523, - "angularVelocity": -0.0000055495432383876534, - "velocityX": 1.6158649701296508, - "velocityY": 0.16653180872097262, - "timestamp": 3.869000491455942 - }, - { - "x": 6.632395766775363, - "y": 7.333113396808981, - "heading": 3.389934736912773, - "angularVelocity": -0.000005549549199340779, - "velocityX": 1.615864970128498, - "velocityY": 0.16653180872187498, - "timestamp": 3.959287599875575 - }, - { - "x": 6.778287542524652, - "y": 7.3481490722785985, - "heading": 3.389934235859484, - "angularVelocity": -0.00000554955516023689, - "velocityX": 1.6158649701273455, - "velocityY": 0.16653180872277748, - "timestamp": 4.049574708295209 - }, - { - "x": 6.924179318273876, - "y": 7.363184747747038, - "heading": 3.3899337348056577, - "angularVelocity": -0.000005549561121255593, - "velocityX": 1.6158649701261931, - "velocityY": 0.16653180872368004, - "timestamp": 4.139861816714842 - }, - { - "x": 7.070071094023151, - "y": 7.378220423218671, - "heading": 3.389933233751293, - "angularVelocity": -0.0000055495670825865805, - "velocityX": 1.6158649701250403, - "velocityY": 0.1665318087245824, - "timestamp": 4.2301489251344755 - }, - { - "x": 7.215962869772269, - "y": 7.393256098685687, - "heading": 3.38993273269639, - "angularVelocity": -0.000005549573043968301, - "velocityX": 1.6158649701238874, - "velocityY": 0.16653180872548476, - "timestamp": 4.320436033554109 - }, - { - "x": 7.361854645521179, - "y": 7.4082917741551615, - "heading": 3.389932231640949, - "angularVelocity": -0.00000554957900552128, - "velocityX": 1.6158649701227348, - "velocityY": 0.16653180872638726, - "timestamp": 4.410723141973742 - }, - { - "x": 7.507746421269845, - "y": 7.423327449627381, - "heading": 3.3899317305849697, - "angularVelocity": -0.0000055495849662374065, - "velocityX": 1.615864970121582, - "velocityY": 0.16653180872728968, - "timestamp": 4.501010250393375 - }, - { - "x": 7.653638197018768, - "y": 7.4383631250955204, - "heading": 3.389931229528452, - "angularVelocity": -0.000005549590927964199, - "velocityX": 1.615864970120429, - "velocityY": 0.16653180872819684, - "timestamp": 4.591297358813009 - }, - { - "x": 7.79952997270157, - "y": 7.45339880119461, - "heading": 3.389930728467707, - "angularVelocity": -0.000005549637747689064, - "velocityX": 1.6158649693899627, - "velocityY": 0.16653181569922934, - "timestamp": 4.681584467232642 - }, - { - "x": 7.908273186523536, - "y": 7.465055949439882, - "heading": 3.2483837688222263, - "angularVelocity": -1.567742750023664, - "velocityX": 1.2044157324947478, - "velocityY": 0.1291119900382222, - "timestamp": 4.771871575652275 + "angularVelocity": 7.708394912303633e-28, + "velocityX": 2.52399716725713e-26, + "velocityY": 2.1878261081032532e-26, + "timestamp": 1.2882874881007154 + }, + { + "x": 2.7043659064830394, + "y": 6.904431852524175, + "heading": 3.7255178406599083, + "angularVelocity": -0.04862383285360791, + "velocityX": 0.39229912325731986, + "velocityY": 0.05016142105727286, + "timestamp": 1.3721600492553427 + }, + { + "x": 2.770172988802701, + "y": 6.912846664761324, + "heading": 3.7174222347433226, + "angularVelocity": -0.09652269830726562, + "velocityX": 0.7846079982980328, + "velocityY": 0.10032854751670894, + "timestamp": 1.4560326104099701 + }, + { + "x": 2.868885003911175, + "y": 6.925469706226451, + "heading": 3.7053835964173025, + "angularVelocity": -0.1435348838796717, + "velocityX": 1.1769285896312263, + "velocityY": 0.15050263508533468, + "timestamp": 1.5399051715645975 + }, + { + "x": 3.000503155089929, + "y": 6.942301704976706, + "heading": 3.689495079575308, + "angularVelocity": -0.1894364095154122, + "velocityX": 1.5692635275093452, + "velocityY": 0.2006854031704579, + "timestamp": 1.6237777327192249 + }, + { + "x": 3.1650289492172274, + "y": 6.963343594326816, + "heading": 3.669876864825881, + "angularVelocity": -0.23390503973354093, + "velocityX": 1.9616164316716036, + "velocityY": 0.250879299027448, + "timestamp": 1.7076502938738523 + }, + { + "x": 3.362464324784135, + "y": 6.988596614292936, + "heading": 3.64668996545627, + "angularVelocity": -0.2764539326140709, + "velocityX": 2.3539924481728387, + "velocityY": 0.301087979411566, + "timestamp": 1.7915228550284796 + }, + { + "x": 3.592811857209642, + "y": 7.0180624930033995, + "heading": 3.620161246344741, + "angularVelocity": -0.316297949488161, + "velocityX": 2.7463991710094446, + "velocityY": 0.35131726401129654, + "timestamp": 1.875395416183107 + }, + { + "x": 3.8560750973941316, + "y": 7.05174380499803, + "heading": 3.5906339439313264, + "angularVelocity": -0.3520496096330931, + "velocityX": 3.1388482307001198, + "velocityY": 0.40157724446419313, + "timestamp": 1.9592679773377344 + }, + { + "x": 4.152259097249652, + "y": 7.089644787764795, + "heading": 3.558687346907366, + "angularVelocity": -0.38089449736802783, + "velocityX": 3.5313575235824133, + "velocityY": 0.4518877478522514, + "timestamp": 2.0431405384923615 + }, + { + "x": 4.481370515300424, + "y": 7.131773671322726, + "heading": 3.52550603083455, + "angularVelocity": -0.39561586788368536, + "velocityX": 3.9239462050529617, + "velocityY": 0.5022963765260808, + "timestamp": 2.1270130996469887 + }, + { + "x": 4.843397664915269, + "y": 7.178152540283051, + "heading": 3.4948677816779656, + "angularVelocity": -0.3652952614634004, + "velocityX": 4.316395548568172, + "velocityY": 0.5529683167158834, + "timestamp": 2.210885660801616 + }, + { + "x": 5.214084962853593, + "y": 7.225506645206567, + "heading": 3.4948677687882737, + "angularVelocity": -1.5368186922063484e-7, + "velocityX": 4.419649201541919, + "velocityY": 0.5645959092177173, + "timestamp": 2.294758221956243 + }, + { + "x": 5.584772271414454, + "y": 7.2728606669772775, + "heading": 3.4948677558987455, + "angularVelocity": -1.5367991215657251e-7, + "velocityX": 4.419649328192821, + "velocityY": 0.5645949177992666, + "timestamp": 2.37863078311087 + }, + { + "x": 5.955780374663183, + "y": 7.317631980849406, + "heading": 3.4948677392182756, + "angularVelocity": -1.9887875297845992e-7, + "velocityX": 4.423474115268035, + "velocityY": 0.5338016778763695, + "timestamp": 2.4625033442654973 + }, + { + "x": 6.300300548190503, + "y": 7.351378145453142, + "heading": 3.441189014071279, + "angularVelocity": -0.6400034100310172, + "velocityX": 4.107662491576513, + "velocityY": 0.40235047242115085, + "timestamp": 2.5463759054201245 + }, + { + "x": 6.611937161645573, + "y": 7.380701076408195, + "heading": 3.3909317361493, + "angularVelocity": -0.5992100065875497, + "velocityX": 3.7155967239457195, + "velocityY": 0.3496129193073305, + "timestamp": 2.6302484665747516 + }, + { + "x": 6.890683200327295, + "y": 7.405709842435669, + "heading": 3.344663622374181, + "angularVelocity": -0.551647799210751, + "velocityX": 3.323447321083073, + "velocityY": 0.29817577623948227, + "timestamp": 2.714121027729379 + }, + { + "x": 7.136536630210955, + "y": 7.426441407681926, + "heading": 3.302577269433113, + "angularVelocity": -0.5017892903434469, + "velocityX": 2.931273666848028, + "velocityY": 0.2471793511591556, + "timestamp": 2.797993588884006 + }, + { + "x": 7.349496491178017, + "y": 7.442914379961346, + "heading": 3.264769514381459, + "angularVelocity": -0.45077620775108873, + "velocityX": 2.539088565263299, + "velocityY": 0.196404784266102, + "timestamp": 2.881866150038633 + }, + { + "x": 7.529562223309363, + "y": 7.455139968848337, + "heading": 3.2312984812041172, + "angularVelocity": -0.39907012158165334, + "velocityX": 2.1468967878467162, + "velocityY": 0.14576386745185102, + "timestamp": 2.9657387111932603 + }, + { + "x": 7.676733458548059, + "y": 7.463125667757038, + "heading": 3.2022028135705005, + "angularVelocity": -0.3469032927225913, + "velocityX": 1.7547006221423278, + "velocityY": 0.0952122934934494, + "timestamp": 3.0496112723478874 + }, + { + "x": 7.791009935801521, + "y": 7.466876839352261, + "heading": 3.177509999915845, + "angularVelocity": -0.29440872336224383, + "velocityX": 1.3625013434701525, + "velocityY": 0.04472465778536462, + "timestamp": 3.1334838335025146 + }, + { + "x": 7.872391460113929, + "y": 7.466397510828961, + "heading": 3.1572405778603585, + "angularVelocity": -0.24166928702841778, + "velocityX": 0.9702997403688861, + "velocityY": -0.005714962279684649, + "timestamp": 3.2173563946571417 + }, + { + "x": 7.920877880694152, + "y": 7.461690816856239, + "heading": 3.1414104927277844, + "angularVelocity": -0.1887397369849021, + "velocityX": 0.5780963394075183, + "velocityY": -0.05611720815399605, + "timestamp": 3.301228955811769 }, { "x": 7.936469078063965, "y": 7.452759265899658, "heading": 3.1300325241315123, - "angularVelocity": -1.3108321526994196, - "velocityX": 0.3122914448573039, - "velocityY": -0.13619534121251922, - "timestamp": 4.862158684071908 - }, - { - "x": 7.897313989149588, - "y": 7.432003540468781, - "heading": 3.2231598958654137, - "angularVelocity": 0.9773255115620281, - "velocityX": -0.41091321048106677, - "velocityY": -0.21782102937809103, - "timestamp": 4.957446661958681 - }, - { - "x": 7.764258804459074, - "y": 7.410354349811778, - "heading": 3.302887541742508, - "angularVelocity": 0.8367020441112922, - "velocityX": -1.3963480770719, - "velocityY": -0.2271975031683705, - "timestamp": 5.052734639845453 - }, - { - "x": 7.611104944022113, - "y": 7.387923020288099, - "heading": 3.3028890943761, - "angularVelocity": 0.000016294118377845236, - "velocityX": -1.607273696395563, - "velocityY": -0.23540566212575834, - "timestamp": 5.1480226177322255 - }, - { - "x": 7.457951083368431, - "y": 7.365491692124674, - "heading": 3.3028906469716444, - "angularVelocity": 0.000016293719092580313, - "velocityX": -1.6072736986022782, - "velocityY": -0.23540564768491373, - "timestamp": 5.243310595618998 - }, - { - "x": 7.3047972227130815, - "y": 7.3430603639785605, - "heading": 3.3028921995394867, - "angularVelocity": 0.0000162934283709874, - "velocityX": -1.6072736986590501, - "velocityY": -0.23540564774414025, - "timestamp": 5.33859857350577 - }, - { - "x": 7.15164336205228, - "y": 7.320629035813856, - "heading": 3.3028937520796284, - "angularVelocity": 0.000016293137669547086, - "velocityX": -1.6072736987158178, - "velocityY": -0.23540564780336143, - "timestamp": 5.4338865513925425 - }, - { - "x": 6.998489501387815, - "y": 7.298197707651154, - "heading": 3.3028953045920724, - "angularVelocity": 0.00001629284698917995, - "velocityX": -1.6072736987725829, - "velocityY": -0.23540564786256962, - "timestamp": 5.529174529279315 - }, - { - "x": 6.845335640713116, - "y": 7.275766379466654, - "heading": 3.3028968570768193, - "angularVelocity": 0.000016292556328916256, - "velocityX": -1.607273698829345, - "velocityY": -0.23540564792176438, - "timestamp": 5.624462507166087 - }, - { - "x": 6.692181780035948, - "y": 7.253335051289657, - "heading": 3.3028984095338725, - "angularVelocity": 0.0000162922656907247, - "velocityX": -1.6072736988861036, - "velocityY": -0.2354056479809454, - "timestamp": 5.7197504850528595 - }, - { - "x": 6.539027919353228, - "y": 7.230903723107964, - "heading": 3.302899961963233, - "angularVelocity": 0.00001629197507403925, - "velocityX": -1.6072736989428598, - "velocityY": -0.23540564804011319, - "timestamp": 5.815038462939632 - }, - { - "x": 6.385874058664987, - "y": 7.2084723949147005, - "heading": 3.3029015143649034, - "angularVelocity": 0.000016291684475842535, - "velocityX": -1.607273698999613, - "velocityY": -0.2354056480992679, - "timestamp": 5.910326440826404 - }, - { - "x": 6.232720197971391, - "y": 7.186041066717774, - "heading": 3.3029030667388852, - "angularVelocity": 0.000016291393901831136, - "velocityX": -1.6072736990563632, - "velocityY": -0.2354056481584089, - "timestamp": 6.005614418713177 - }, - { - "x": 6.079566337272002, - "y": 7.163609738524357, - "heading": 3.302904619085181, - "angularVelocity": 0.000016291103348699848, - "velocityX": -1.6072736991131096, - "velocityY": -0.2354056482175359, - "timestamp": 6.100902396599949 - }, - { - "x": 5.9264124765671236, - "y": 7.141178410308786, - "heading": 3.3029061714037917, - "angularVelocity": 0.000016290812813611415, - "velocityX": -1.6072736991698542, - "velocityY": -0.23540564827665061, - "timestamp": 6.196190374486721 - }, - { - "x": 5.773258615858539, - "y": 7.118747082103052, - "heading": 3.302907723694721, - "angularVelocity": 0.000016290522303354113, - "velocityX": -1.6072736992265952, - "velocityY": -0.23540564833575084, - "timestamp": 6.291478352373494 - }, - { - "x": 5.620104755142444, - "y": 7.096315753876903, - "heading": 3.30290927595797, - "angularVelocity": 0.000016290231812342035, - "velocityX": -1.6072736992833332, - "velocityY": -0.2354056483948381, - "timestamp": 6.386766330260266 - }, - { - "x": 5.466950894422645, - "y": 7.07388442565226, - "heading": 3.30291082819354, - "angularVelocity": 0.000016289941343623743, - "velocityX": -1.6072736993400683, - "velocityY": -0.23540564845391188, - "timestamp": 6.482054308147038 - }, - { - "x": 5.313797033694609, - "y": 7.0514530974310325, - "heading": 3.3029123804014344, - "angularVelocity": 0.000016289650893731917, - "velocityX": -1.607273699396801, - "velocityY": -0.23540564851297285, - "timestamp": 6.577342286033811 - }, - { - "x": 5.160643172963514, - "y": 7.02902176918985, - "heading": 3.3029139325816548, - "angularVelocity": 0.000016289360468842725, - "velocityX": -1.6072736994535295, - "velocityY": -0.23540564857201896, - "timestamp": 6.672630263920583 - }, - { - "x": 5.007489312227579, - "y": 7.006590440945802, - "heading": 3.3029154847342026, - "angularVelocity": 0.000016289070060669773, - "velocityX": -1.6072736995102561, - "velocityY": -0.23540564863105312, - "timestamp": 6.767918241807355 - }, - { - "x": 4.854335451483483, - "y": 6.984159112697374, - "heading": 3.30291703685908, - "angularVelocity": 0.00001628877967680068, - "velocityX": -1.6072736995669794, - "velocityY": -0.23540564869007324, - "timestamp": 6.863206219694128 - }, - { - "x": 4.701181590737123, - "y": 6.961727784450269, - "heading": 3.3029185889562895, - "angularVelocity": 0.00001628848931160111, - "velocityX": -1.6072736996236994, - "velocityY": -0.23540564874908013, - "timestamp": 6.9584941975809 - }, - { - "x": 4.548027729985049, - "y": 6.939296456188679, - "heading": 3.3029201410258326, - "angularVelocity": 0.000016288198966670516, - "velocityX": -1.6072736996804171, - "velocityY": -0.23540564880807416, - "timestamp": 7.053782175467672 - }, - { - "x": 4.394873869225821, - "y": 6.916865127932724, - "heading": 3.3029216930677117, - "angularVelocity": 0.000016287908648086418, - "velocityX": -1.607273699737131, - "velocityY": -0.2354056488670535, - "timestamp": 7.149070153354445 - }, - { - "x": 4.2417200084613995, - "y": 6.8944337996586516, - "heading": 3.302923245081929, - "angularVelocity": 0.000016287618349487135, - "velocityX": -1.607273699793842, - "velocityY": -0.23540564892601998, - "timestamp": 7.244358131241217 - }, - { - "x": 4.088566147690897, - "y": 6.872002471380619, - "heading": 3.3029247970684854, - "angularVelocity": 0.00001628732806480796, - "velocityX": -1.607273699850551, - "velocityY": -0.23540564898497424, - "timestamp": 7.339646109127989 - }, - { - "x": 3.935412286918391, - "y": 6.849571143103449, - "heading": 3.3029263490273832, - "angularVelocity": 0.00001628703780483265, - "velocityX": -1.6072736999072568, - "velocityY": -0.2354056490439145, - "timestamp": 7.434934087014762 - }, - { - "x": 3.7822584261353844, - "y": 6.827139814806336, - "heading": 3.302927900958626, - "angularVelocity": 0.000016286747572595648, - "velocityX": -1.6072736999639583, - "velocityY": -0.2354056491028402, - "timestamp": 7.530222064901534 - }, - { - "x": 3.629104565353085, - "y": 6.804708486525145, - "heading": 3.3029294528622137, - "angularVelocity": 0.000016286457354449532, - "velocityX": -1.607273700020658, - "velocityY": -0.2354056491617539, - "timestamp": 7.6255100427883065 - }, - { - "x": 3.475950704559541, - "y": 6.782277158221499, - "heading": 3.3029310047381495, - "angularVelocity": 0.000016286167156294597, - "velocityX": -1.607273700077355, - "velocityY": -0.2354056492206542, - "timestamp": 7.720798020675079 - }, - { - "x": 3.3227968437635895, - "y": 6.759845829932118, - "heading": 3.3029325565864354, - "angularVelocity": 0.000016285876982629207, - "velocityX": -1.6072737001340485, - "velocityY": -0.23540564927954047, - "timestamp": 7.816085998561851 - }, - { - "x": 3.16964298296173, - "y": 6.737414501611197, - "heading": 3.3029341084070736, - "angularVelocity": 0.00001628558683240146, - "velocityX": -1.6072737001907338, - "velocityY": -0.23540564933844627, - "timestamp": 7.9113739764486235 - }, - { - "x": 3.0164891229666777, - "y": 6.714983167772551, - "heading": 3.302935660207411, - "angularVelocity": 0.00001628537379077082, - "velocityX": -1.607273691741332, - "velocityY": -0.23540570734761523, - "timestamp": 8.006661954335396 - }, - { - "x": 2.8768170840789846, - "y": 6.685183755439867, - "heading": 3.351795711849992, - "angularVelocity": 0.512761974030336, - "velocityX": -1.4657886753698728, - "velocityY": -0.3127300316833431, - "timestamp": 8.101949932222169 + "angularVelocity": -0.13565781752265163, + "velocityX": 0.18589151392514228, + "velocityY": -0.1064895459686019, + "timestamp": 3.385101516966396 + }, + { + "x": 7.915740116584839, + "y": 7.438450021195838, + "heading": 3.1229758381188732, + "angularVelocity": -0.07896960965622021, + "velocityX": -0.23197262761210297, + "velocityY": -0.16013118150815783, + "timestamp": 3.474461031890667 + }, + { + "x": 7.857668877454443, + "y": 7.419352965299199, + "heading": 3.1208942356371594, + "angularVelocity": -0.023294693167012128, + "velocityX": -0.6498607247321063, + "velocityY": -0.21371038006220794, + "timestamp": 3.5638205468149375 + }, + { + "x": 7.762252743153032, + "y": 7.395475025916455, + "heading": 3.123675562058841, + "angularVelocity": 0.031125128913673642, + "velocityX": -1.0677781138614328, + "velocityY": -0.2672120523816525, + "timestamp": 3.6531800617392083 + }, + { + "x": 7.629488438537512, + "y": 7.366825038470432, + "heading": 3.1311770789195053, + "angularVelocity": 0.08394760051038563, + "velocityX": -1.4857321542986437, + "velocityY": -0.3206148496923158, + "timestamp": 3.742539576663479 + }, + { + "x": 7.459371747842553, + "y": 7.333414662601681, + "heading": 3.14321079663535, + "angularVelocity": 0.13466632765456588, + "velocityX": -1.9037333723121508, + "velocityY": -0.3738871668793641, + "timestamp": 3.8318990915877498 + }, + { + "x": 7.251897047158697, + "y": 7.295259995220328, + "heading": 3.159517771914932, + "angularVelocity": 0.1824872851357962, + "velocityX": -2.3217975260909065, + "velocityY": -0.4269793475679505, + "timestamp": 3.9212586065120205 + }, + { + "x": 7.007056479155703, + "y": 7.252384696832851, + "heading": 3.179718570361924, + "angularVelocity": 0.2260620871108287, + "velocityX": -2.739949609288821, + "velocityY": -0.47980674944141977, + "timestamp": 4.010618121436291 + }, + { + "x": 6.724838378878884, + "y": 7.204826948209543, + "heading": 3.2032038202698816, + "angularVelocity": 0.2628175626049468, + "velocityX": -3.1582322320794676, + "velocityY": -0.5322068798561834, + "timestamp": 4.0999776363605624 + }, + { + "x": 6.405224102249246, + "y": 7.152658687501974, + "heading": 3.2288345481257252, + "angularVelocity": 0.2868270701510058, + "velocityX": -3.5767234960988827, + "velocityY": -0.5838019683945127, + "timestamp": 4.189337151284834 + }, + { + "x": 6.048184900918235, + "y": 7.096065538236557, + "heading": 3.253716466109861, + "angularVelocity": 0.2784473260091271, + "velocityX": -3.995536475702559, + "velocityY": -0.6333197904373026, + "timestamp": 4.278696666209105 + }, + { + "x": 5.654508056038536, + "y": 7.03656993570583, + "heading": 3.253716532797699, + "angularVelocity": 7.462869273514143e-7, + "velocityX": -4.4055391886731465, + "velocityY": -0.6658004195876279, + "timestamp": 4.368056181133376 + }, + { + "x": 5.259674739202379, + "y": 6.9853073355549045, + "heading": 3.253716547435891, + "angularVelocity": 1.6381235089876507e-7, + "velocityX": -4.418480977328111, + "velocityY": -0.573666947435528, + "timestamp": 4.457415696057647 + }, + { + "x": 4.864841395212643, + "y": 6.934044944547169, + "heading": 3.253716562074652, + "angularVelocity": 1.63818712302411e-7, + "velocityX": -4.41848128119701, + "velocityY": -0.5736646069663508, + "timestamp": 4.546775210981918 + }, + { + "x": 4.491651487875698, + "y": 6.885589548517627, + "heading": 3.312737130762719, + "angularVelocity": 0.6604844345684403, + "velocityX": -4.17627499044966, + "velocityY": -0.5422522276514719, + "timestamp": 4.63613472590619 + }, + { + "x": 4.155780465580293, + "y": 6.841979518441731, + "heading": 3.3658484725068094, + "angularVelocity": 0.5943557525922112, + "velocityX": -3.7586486741791827, + "velocityY": -0.48802894815235986, + "timestamp": 4.725494240830461 + }, + { + "x": 3.857228363312986, + "y": 6.803214924393239, + "heading": 3.413052217911606, + "angularVelocity": 0.528245318305502, + "velocityX": -3.3410219663828666, + "velocityY": -0.43380488447528814, + "timestamp": 4.814853755754732 + }, + { + "x": 3.5959952091477, + "y": 6.769295821195016, + "heading": 3.454350393436854, + "angularVelocity": 0.46215756162336336, + "velocityX": -2.9233949444183165, + "velocityY": -0.37958020728927433, + "timestamp": 4.904213270679003 + }, + { + "x": 3.3720810297941446, + "y": 6.740222250549493, + "heading": 3.4897450705258297, + "angularVelocity": 0.3960929859452755, + "velocityX": -2.5057676235520625, + "velocityY": -0.32535506342174403, + "timestamp": 4.993572785603274 + }, + { + "x": 3.1854858494686082, + "y": 6.715994242916669, + "heading": 3.519238149861463, + "angularVelocity": 0.33004968033484894, + "velocityX": -2.0881400316873893, + "velocityY": -0.2711295786839986, + "timestamp": 5.0829323005275455 + }, + { + "x": 3.036209688317166, + "y": 6.696611819522511, + "heading": 3.542831216825285, + "angularVelocity": 0.2640241163329568, + "velocityX": -1.6705122143730164, + "velocityY": -0.21690385641174914, + "timestamp": 5.172291815451817 + }, + { + "x": 2.924252561165524, + "y": 6.682074994259449, + "heading": 3.560525448236687, + "angularVelocity": 0.19801172182276905, + "velocityX": -1.2528842311478772, + "velocityY": -0.1626779786728065, + "timestamp": 5.261651330376088 + }, + { + "x": 2.849614476676597, + "y": 6.6723837752661295, + "heading": 3.5723215560021737, + "angularVelocity": 0.13200729408035738, + "velocityX": -0.8352561509782144, + "velocityY": -0.10845200985627608, + "timestamp": 5.351010845300359 }, { "x": 2.812295436859131, "y": 6.667538166046143, "heading": 3.5782197562990956, - "angularVelocity": 2.3762078855126445, - "velocityX": -0.6771226407787638, - "velocityY": -0.18518169637911341, - "timestamp": 8.197237910108942 + "angularVelocity": 0.06600528552466699, + "velocityX": -0.41762804832918915, + "velocityY": -0.05422600183195843, + "timestamp": 5.44037036022463 }, { "x": 2.812295436859131, "y": 6.667538166046143, "heading": 3.5782197562990956, - "angularVelocity": -9.273365059762218e-23, - "velocityX": 5.842590739873529e-23, - "velocityY": -3.6959137012591573e-22, - "timestamp": 8.292525887995716 + "angularVelocity": -6.3889947065303214e-27, + "velocityX": 2.638834492143492e-26, + "velocityY": 4.3134967976763594e-26, + "timestamp": 5.5297298751489015 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceContested1.1.traj b/src/main/deploy/choreo/SourceContested1.1.traj index 46b0f61d..24511fe2 100644 --- a/src/main/deploy/choreo/SourceContested1.1.traj +++ b/src/main/deploy/choreo/SourceContested1.1.traj @@ -4,523 +4,388 @@ "x": 0.8210453391075134, "y": 4.576303482055664, "heading": 2.111215568027718, - "angularVelocity": -1.8256752618245288e-22, - "velocityX": -7.65916010942737e-22, - "velocityY": 5.834863940365417e-22, + "angularVelocity": 1.0347096801356489e-22, + "velocityX": -4.48084535985214e-23, + "velocityY": -5.937888990335369e-23, "timestamp": 0 }, { - "x": 0.8644346902480902, - "y": 4.538605095268569, - "heading": 2.3610664104627634, - "angularVelocity": 2.6730073050757213, - "velocityX": 0.4641971643197831, - "velocityY": -0.4033128817553312, - "timestamp": 0.09347181429718082 - }, - { - "x": 0.9651901704673621, - "y": 4.45361330197089, - "heading": 2.440857637900237, - "angularVelocity": 0.8536394424076145, - "velocityX": 1.0779236604837226, - "velocityY": -0.9092772397405119, - "timestamp": 0.18694362859436164 - }, - { - "x": 1.0866380874820318, - "y": 4.362483747012655, - "heading": 2.440865267791502, - "angularVelocity": 0.00008162772192147527, - "velocityX": 1.299299879090211, - "velocityY": -0.9749415440733835, - "timestamp": 0.28041544289154247 - }, - { - "x": 1.2080860091215304, - "y": 4.271354198213206, - "heading": 2.4408728977229917, - "angularVelocity": 0.00008162815226084984, - "velocityX": 1.2992999285685376, - "velocityY": -0.9749414781841598, - "timestamp": 0.3738872571887233 - }, - { - "x": 1.3295339322232647, - "y": 4.1802246514640125, - "heading": 2.440880527950737, - "angularVelocity": 0.0000816313217297415, - "velocityX": 1.299299944212136, - "velocityY": -0.9749414562496848, - "timestamp": 0.4673590714859041 - }, - { - "x": 1.4509818569281794, - "y": 4.089095106952003, - "heading": 2.440888158472548, - "angularVelocity": 0.00008163446776368671, - "velocityX": 1.2992999613636238, - "velocityY": -0.9749414323153683, - "timestamp": 0.5608308857830849 - }, - { - "x": 1.5724297833961638, - "y": 3.997965564889279, - "heading": 2.440895789286055, - "angularVelocity": 0.00008163758844700525, - "velocityX": 1.2992999802256675, - "velocityY": -0.9749414061119055, - "timestamp": 0.6543027000802657 - }, - { - "x": 1.6938777118095218, - "y": 3.906836025517733, - "heading": 2.4409034203886897, - "angularVelocity": 0.00008164068165665073, - "velocityX": 1.299300001038079, - "velocityY": -0.9749413773205733, - "timestamp": 0.7477745143774466 - }, - { - "x": 1.8153256423773063, - "y": 3.8157064891148127, - "heading": 2.4409110517776624, - "angularVelocity": 0.00008164374501723712, - "velocityX": 1.299300024087019, - "velocityY": -0.9749413455609843, - "timestamp": 0.8412463286746275 - }, - { - "x": 1.9367735753408017, - "y": 3.7245769560008215, - "heading": 2.440918683449934, - "angularVelocity": 0.00008164677586345695, - "velocityX": 1.2993000497173246, - "velocityY": -0.9749413103746718, - "timestamp": 0.9347181429718083 - }, - { - "x": 2.0582215109806032, - "y": 3.6334474265483334, - "heading": 2.4409263154021836, - "angularVelocity": 0.0000816497711865655, - "velocityX": 1.2993000783495519, - "velocityY": -0.9749412712023965, - "timestamp": 1.0281899572689892 - }, - { - "x": 2.1796694496259765, - "y": 3.542317901194656, - "heading": 2.440933947630772, - "angularVelocity": 0.00008165272757387169, - "velocityX": 1.2993001105043924, - "velocityY": -0.9749412273516357, - "timestamp": 1.12166177156617 - }, - { - "x": 2.3011173916676397, - "y": 3.451188380458844, - "heading": 2.440941580131694, - "angularVelocity": 0.00008165564110490276, - "velocityX": 1.2993001468393033, - "velocityY": -0.9749411779478068, - "timestamp": 1.215133585863351 - }, - { - "x": 2.422565337575994, - "y": 3.3600588649659713, - "heading": 2.440949212900517, - "angularVelocity": 0.00008165850722838425, - "velocityX": 1.2993001882067592, - "velocityY": -0.97494112185668, - "timestamp": 1.3086054001605318 - }, - { - "x": 2.5440132879286748, - "y": 3.268929355483818, - "heading": 2.44095684593231, - "angularVelocity": 0.00008166132058271907, - "velocityX": 1.2993002357539973, - "velocityY": -0.9749410575515263, - "timestamp": 1.4020772144577127 - }, - { - "x": 2.6654612434555944, - "y": 3.1777998529828566, - "heading": 2.4409644792215364, - "angularVelocity": 0.00008166407471316297, - "velocityX": 1.2993002911101328, - "velocityY": -0.9749409828638569, - "timestamp": 1.4955490287548936 - }, - { - "x": 2.7869092051206428, - "y": 3.0866703587450828, - "heading": 2.4409721127619197, - "angularVelocity": 0.00008166676169345355, - "velocityX": 1.2993003567783705, - "velocityY": -0.9749408944608698, - "timestamp": 1.5890208430520745 - }, - { - "x": 2.908357174291307, - "y": 2.995540874590004, - "heading": 2.440979746546289, - "angularVelocity": 0.00008166937195400168, - "velocityX": 1.2993004370765409, - "velocityY": -0.9749407865920395, - "timestamp": 1.6824926573492553 - }, - { - "x": 3.0298051531585033, - "y": 2.9044114034341826, - "heading": 2.440987380566619, - "angularVelocity": 0.00008167189636088749, - "velocityX": 1.299300540814034, - "velocityY": -0.9749406475206273, - "timestamp": 1.7759644716464362 - }, - { - "x": 3.1512531460668667, - "y": 2.813281951064894, - "heading": 2.4409950148158184, - "angularVelocity": 0.00008167434490013359, - "velocityX": 1.2993006910322324, - "velocityY": -0.9749404465345578, - "timestamp": 1.869436285943617 - }, - { - "x": 3.272701165942649, - "y": 2.722152534713991, - "heading": 2.4410026493051027, - "angularVelocity": 0.00008167691342827456, - "velocityX": 1.2993009795407924, - "velocityY": -0.9749400611950203, - "timestamp": 1.962908100240798 - }, - { - "x": 3.3941493332630563, - "y": 2.6310233151653177, - "heading": 2.441010284564204, - "angularVelocity": 0.00008168514924485609, - "velocityX": 1.2993025569641734, - "velocityY": -0.9749379557236404, - "timestamp": 2.056379914537979 - }, - { - "x": 3.506643194922811, - "y": 2.543133701847389, - "heading": 2.478368219245545, - "angularVelocity": 0.39967058478790624, - "velocityX": 1.2035057038916106, - "velocityY": -0.9402793128471436, - "timestamp": 2.1498517288351597 - }, - { - "x": 3.621033917156409, - "y": 2.462292474945906, - "heading": 2.526376588848021, - "angularVelocity": 0.513613327862023, - "velocityX": 1.2237991002283153, - "velocityY": -0.8648727695009685, - "timestamp": 2.2433235431323406 - }, - { - "x": 3.7386117914711243, - "y": 2.3880497601699076, - "heading": 2.5783804352338895, - "angularVelocity": 0.5563585854932618, - "velocityX": 1.2578965669896234, - "velocityY": -0.7942791667652267, - "timestamp": 2.3367953574295215 - }, - { - "x": 3.8565091125978217, - "y": 2.322003920666466, - "heading": 2.644821825276836, - "angularVelocity": 0.7108173789342068, - "velocityX": 1.2613141406655402, - "velocityY": -0.7065856162100131, - "timestamp": 2.4302671717267024 - }, - { - "x": 3.9764532017194134, - "y": 2.262287896924173, - "heading": 2.7144845491695992, - "angularVelocity": 0.7452805363473517, - "velocityX": 1.2832113083869967, - "velocityY": -0.6388666379410853, - "timestamp": 2.5237389860238832 - }, - { - "x": 4.100272385460963, - "y": 2.211007382543893, - "heading": 2.7840189996546836, - "angularVelocity": 0.7439082145555573, - "velocityX": 1.3246686680102626, - "velocityY": -0.5486200815278979, - "timestamp": 2.617210800321064 - }, - { - "x": 4.2303737393158425, - "y": 2.1552421719955963, - "heading": 2.8233254460416966, - "angularVelocity": 0.42051656622437983, - "velocityX": 1.3918779134985075, - "velocityY": -0.5965992098002822, - "timestamp": 2.710682614618245 - }, - { - "x": 4.368649457253469, - "y": 2.0958799166815227, - "heading": 2.828594846354344, - "angularVelocity": 0.056374216679845754, - "velocityX": 1.479330630065645, - "velocityY": -0.6350818774667083, - "timestamp": 2.804154428915426 - }, - { - "x": 4.510357504555393, - "y": 2.041351647410644, - "heading": 2.8285983828965033, - "angularVelocity": 0.00003783538584099394, - "velocityX": 1.5160511044685778, - "velocityY": -0.5833659021265345, - "timestamp": 2.8976262432126068 - }, - { - "x": 4.652065594721857, - "y": 1.986823489488553, - "heading": 2.8286019193610827, - "angularVelocity": 0.00003783455586217891, - "velocityX": 1.516051563051105, - "velocityY": -0.5833647108713029, - "timestamp": 2.9910980575097876 - }, - { - "x": 4.793773697805799, - "y": 1.9322953651089438, - "heading": 2.8286054557754516, - "angularVelocity": 0.000037834018690956495, - "velocityX": 1.5160517012476136, - "velocityY": -0.5833643520200054, - "timestamp": 3.0845698718069685 - }, - { - "x": 4.935481808422067, - "y": 1.8777672602773423, - "heading": 2.8286089921399973, - "angularVelocity": 0.0000378334856573034, - "velocityX": 1.5160517818315487, - "velocityY": -0.5833641428873607, - "timestamp": 3.1780416861041494 - }, - { - "x": 5.0771899244715755, - "y": 1.8232391695375483, - "heading": 2.82861252845304, - "angularVelocity": 0.00003783293466201255, - "velocityX": 1.516051839958576, - "velocityY": -0.5833639921273963, - "timestamp": 3.2715135004013303 - }, - { - "x": 5.218898044817724, - "y": 1.76871108993463, - "heading": 2.8286160647128784, - "angularVelocity": 0.0000378323654513879, - "velocityX": 1.5160518859258105, - "velocityY": -0.5833638729805077, - "timestamp": 3.364985314698511 - }, - { - "x": 5.360606168722747, - "y": 1.7141830195502608, - "heading": 2.828619600918001, - "angularVelocity": 0.00003783178008651746, - "velocityX": 1.5160519240001233, - "velocityY": -0.5833637743566714, - "timestamp": 3.458457128995692 - }, - { - "x": 5.5023142956547595, - "y": 1.6596549570012744, - "heading": 2.828623137067082, - "angularVelocity": 0.0000378311805264838, - "velocityX": 1.516051956384105, - "velocityY": -0.5833636905305171, - "timestamp": 3.551928943292873 - }, - { - "x": 5.644022425205511, - "y": 1.605126901225908, - "heading": 2.828626673158945, - "angularVelocity": 0.00003783056838692872, - "velocityX": 1.5160519844004554, - "velocityY": -0.5833636180635361, - "timestamp": 3.6454007575900538 - }, - { - "x": 5.785730557049162, - "y": 1.5505988513766498, - "heading": 2.828630209192539, - "angularVelocity": 0.000037829944994143904, - "velocityX": 1.516052008930833, - "velocityY": -0.5833635546635878, - "timestamp": 3.7388725718872347 - }, - { - "x": 5.927438690918852, - "y": 1.4960708067593358, - "heading": 2.8286337451669143, - "angularVelocity": 0.00003782931145939862, - "velocityX": 1.5160520306062335, - "velocityY": -0.5833634986900924, - "timestamp": 3.8323443861844155 - }, - { - "x": 6.069146826592036, - "y": 1.4415427667950262, - "heading": 2.828637281081213, - "angularVelocity": 0.00003782866873129857, - "velocityX": 1.5160520499007515, - "velocityY": -0.583363448910333, - "timestamp": 3.9258162004815964 - }, - { - "x": 6.210854963880611, - "y": 1.387014730994346, - "heading": 2.828640816934652, - "angularVelocity": 0.000037828017628644405, - "velocityX": 1.516052067182881, - "velocityY": -0.5833634043661104, - "timestamp": 4.019288014778777 - }, - { - "x": 6.35256310262391, - "y": 1.332486698939259, - "heading": 2.8286443527265157, - "angularVelocity": 0.00003782735887192692, - "velocityX": 1.5160520827461144, - "velocityY": -0.5833633642942074, - "timestamp": 4.112759829075958 - }, - { - "x": 6.494271242683516, - "y": 1.2779586702695989, - "heading": 2.828647888456148, - "angularVelocity": 0.00003782669309301873, - "velocityX": 1.516052096828514, - "velocityY": -0.583363328075519, - "timestamp": 4.206231643373139 - }, - { - "x": 6.635979383939319, - "y": 1.223430644672807, - "heading": 2.828651424122945, - "angularVelocity": 0.0000378260208545108, - "velocityX": 1.5160521096259227, - "velocityY": -0.5833632952007055, - "timestamp": 4.2997034576703195 - }, - { - "x": 6.7776875262864404, - "y": 1.1689026218759404, - "heading": 2.8286549597263497, - "angularVelocity": 0.0000378253426576497, - "velocityX": 1.516052121301291, - "velocityY": -0.5833632652459484, - "timestamp": 4.3931752719675 - }, - { - "x": 6.9193956696327925, - "y": 1.114374601639329, - "heading": 2.8286584952658473, - "angularVelocity": 0.00003782465895599354, - "velocityX": 1.516052131991471, - "velocityY": -0.5833632378552868, - "timestamp": 4.486647086264681 - }, - { - "x": 7.061103813897117, - "y": 1.0598465837514708, - "heading": 2.828662030740962, - "angularVelocity": 0.000037823970152435987, - "velocityX": 1.5160521418123194, - "velocityY": -0.5833632127273561, - "timestamp": 4.580118900561862 - }, - { - "x": 7.202811959007387, - "y": 1.0053185680248766, - "heading": 2.8286655661512494, - "angularVelocity": 0.00003782327661332265, - "velocityX": 1.5160521508625917, - "velocityY": -0.5833631896052619, - "timestamp": 4.673590714859043 - }, - { - "x": 7.344520104899491, - "y": 0.9507905542926514, - "heading": 2.828669101496299, - "angularVelocity": 0.000037822578668805064, - "velocityX": 1.5160521592269784, - "velocityY": -0.5833631682686824, - "timestamp": 4.767062529156224 - }, - { - "x": 7.486228251516145, - "y": 0.8962625424056586, - "heading": 2.828672636775727, - "angularVelocity": 0.000037821876620878625, - "velocityX": 1.5160521669785085, - "velocityY": -0.5833631485276237, - "timestamp": 4.860534343453405 - }, - { - "x": 7.627936398805981, - "y": 0.8417345322301597, - "heading": 2.828676171989175, - "angularVelocity": 0.00003782117074336596, - "velocityX": 1.516052174180496, - "velocityY": -0.5833631302173573, - "timestamp": 4.954006157750586 - }, - { - "x": 7.769644550650757, - "y": 0.7872065338976291, - "heading": 2.8286797071997065, - "angularVelocity": 0.000037821139539143325, - "velocityX": 1.5160522229111162, - "velocityY": -0.5833630035164005, - "timestamp": 5.0474779720477665 - }, - { - "x": 7.89860842703643, - "y": 0.754016059353999, - "heading": 2.902617724341904, - "angularVelocity": 0.7910193858773472, - "velocityX": 1.3797087106456418, - "velocityY": -0.3550853783377481, - "timestamp": 5.140949786344947 - }, - { - "x": 7.956855773925781, - "y": 0.7431064248085022, + "x": 0.8376034098013856, + "y": 4.562699117747838, + "heading": 2.123652893108774, + "angularVelocity": 0.1828236177507875, + "velocityX": 0.24339690146379658, + "velocityY": -0.19997861949792262, + "timestamp": 0.06802909402006102 + }, + { + "x": 0.8707358038741012, + "y": 4.535482743319388, + "heading": 2.1482712139566598, + "angularVelocity": 0.3618792988868299, + "velocityX": 0.4870327107832017, + "velocityY": -0.4000696293327653, + "timestamp": 0.13605818804012204 + }, + { + "x": 0.9204609539749747, + "y": 4.4946461788288365, + "heading": 2.1847789288455908, + "angularVelocity": 0.5366485533111098, + "velocityX": 0.7309394725469969, + "velocityY": -0.600280880978807, + "timestamp": 0.20408728206018306 + }, + { + "x": 0.9868005236891458, + "y": 4.440179990916992, + "heading": 2.232822407442878, + "angularVelocity": 0.70621958574253, + "velocityX": 0.9751646801970973, + "velocityY": -0.8006307991663675, + "timestamp": 0.2721163760802441 + }, + { + "x": 1.0697804545770624, + "y": 4.372072363754176, + "heading": 2.291950205070784, + "angularVelocity": 0.8691545651110574, + "velocityX": 1.2197712182297578, + "velocityY": -1.0011544052421333, + "timestamp": 0.3401454701003051 + }, + { + "x": 1.1694318368712635, + "y": 4.290307500716387, + "heading": 2.3615667397256703, + "angularVelocity": 1.0233347313776844, + "velocityX": 1.4648347700296431, + "velocityY": -1.2019102152628651, + "timestamp": 0.4081745641203661 + }, + { + "x": 1.2857914547624776, + "y": 4.194863719874867, + "heading": 2.4408735330108198, + "angularVelocity": 1.165777590125818, + "velocityX": 1.7104390344651783, + "velocityY": -1.4029847408135987, + "timestamp": 0.47620365814042714 + }, + { + "x": 1.4189020134002508, + "y": 4.085711763901469, + "heading": 2.5287912003062516, + "angularVelocity": 1.292353934178601, + "velocityX": 1.956671047221653, + "velocityY": -1.6044893371828546, + "timestamp": 0.5442327521604882 + }, + { + "x": 1.5688122322603035, + "y": 3.9628143203631563, + "heading": 2.6238365711205818, + "angularVelocity": 1.3971282755331538, + "velocityX": 2.2036192164465107, + "velocityY": -1.8065424111347401, + "timestamp": 0.6122618461805491 + }, + { + "x": 1.7355761304311574, + "y": 3.8261286744353766, + "heading": 2.7238718533453037, + "angularVelocity": 1.4704779427934513, + "velocityX": 2.451361444291423, + "velocityY": -2.0092233756262123, + "timestamp": 0.6802909402006101 + }, + { + "x": 1.9192427382156467, + "y": 3.6756195148686888, + "heading": 2.8254629684407537, + "angularVelocity": 1.4933480529006078, + "velocityX": 2.6998243976368146, + "velocityY": -2.2124234011157657, + "timestamp": 0.7483200342206711 + }, + { + "x": 2.1197678234399393, + "y": 3.511336132708068, + "heading": 2.921883740999518, + "angularVelocity": 1.417346121503996, + "velocityX": 2.94763715602563, + "velocityY": -2.41489886830149, + "timestamp": 0.816349128240732 + }, + { + "x": 2.336325957396502, + "y": 3.3346123423953884, + "heading": 2.9945552520993064, + "angularVelocity": 1.0682416420003875, + "velocityX": 3.183316448293462, + "velocityY": -2.5977678059414684, + "timestamp": 0.884378222260793 + }, + { + "x": 2.5671944876290005, + "y": 3.1506814395963123, + "heading": 3.018943844161631, + "angularVelocity": 0.3585023792192819, + "velocityX": 3.3936734504272232, + "velocityY": -2.7037094268054926, + "timestamp": 0.952407316280854 + }, + { + "x": 2.801664380200398, + "y": 2.9585918594844425, + "heading": 3.0189438734994596, + "angularVelocity": 4.312541447677092e-7, + "velocityX": 3.446612011358773, + "velocityY": -2.8236386634110495, + "timestamp": 1.020436410300915 + }, + { + "x": 3.0361342064803303, + "y": 2.766502198455338, + "heading": 3.018943902837169, + "angularVelocity": 4.3125239350876155e-7, + "velocityX": 3.446611036901202, + "velocityY": -2.8236398528614792, + "timestamp": 1.0884655043209759 + }, + { + "x": 3.2706040874248963, + "y": 2.5744126041513797, + "heading": 3.018943932174878, + "angularVelocity": 4.312523803489459e-7, + "velocityX": 3.446611840449086, + "velocityY": -2.8236388720289733, + "timestamp": 1.1564945983410369 + }, + { + "x": 3.50614012111681, + "y": 2.38363180288057, + "heading": 3.0189439616606526, + "angularVelocity": 4.334288912332302e-7, + "velocityX": 3.4622838520009824, + "velocityY": -2.804400147009905, + "timestamp": 1.2245236923610978 + }, + { + "x": 3.746237311752337, + "y": 2.198624029599204, + "heading": 3.0189439927171957, + "angularVelocity": 4.565185470115094e-7, + "velocityX": 3.5293310030664933, + "velocityY": -2.719538984699832, + "timestamp": 1.2925527863811588 + }, + { + "x": 3.986334650538585, + "y": 2.013616448583285, + "heading": 3.0189440237737535, + "angularVelocity": 4.565187647674116e-7, + "velocityX": 3.529333180821812, + "velocityY": -2.719536158475997, + "timestamp": 1.3605818804012197 + }, + { + "x": 4.226432008113782, + "y": 1.8286088919511099, + "heading": 3.0189440548303175, + "angularVelocity": 4.5651885597350814e-7, + "velocityX": 3.529333457011711, + "velocityY": -2.7195358000448815, + "timestamp": 1.4286109744212807 + }, + { + "x": 4.466529379665209, + "y": 1.643601353456878, + "heading": 3.0189440858868863, + "angularVelocity": 4.565189266869802e-7, + "velocityX": 3.5293336624566067, + "velocityY": -2.7195355334244926, + "timestamp": 1.4966400684413417 + }, + { + "x": 4.706629767865023, + "y": 1.4585977299467376, + "heading": 3.0189441169435973, + "angularVelocity": 4.5652101213496753e-7, + "velocityX": 3.5293780059603734, + "velocityY": -2.7194779847514217, + "timestamp": 1.5646691624614026 + }, + { + "x": 4.958832142878476, + "y": 1.2904653444393646, + "heading": 3.0189441488124733, + "angularVelocity": 4.684595064516387e-7, + "velocityX": 3.707272287634489, + "velocityY": -2.4714776512795065, + "timestamp": 1.6326982564814636 + }, + { + "x": 5.22248738866272, + "y": 1.140931240984889, + "heading": 3.0189441823956185, + "angularVelocity": 4.936585621819922e-7, + "velocityX": 3.875624827614127, + "velocityY": -2.1980904730317214, + "timestamp": 1.7007273505015246 + }, + { + "x": 5.4962265319262364, + "y": 1.010772045584542, + "heading": 3.0189442291355726, + "angularVelocity": 6.870583117392439e-7, + "velocityX": 4.0238540172649335, + "velocityY": -1.9132872085870314, + "timestamp": 1.7687564445215855 + }, + { + "x": 5.777018253708926, + "y": 0.9011877519054788, + "heading": 3.0248033325009245, + "angularVelocity": 0.08612643531051656, + "velocityX": 4.127524051693051, + "velocityY": -1.6108445255312074, + "timestamp": 1.8367855385416465 + }, + { + "x": 6.05633409210439, + "y": 0.8114216921379119, + "heading": 3.058880871989868, + "angularVelocity": 0.5009259638074025, + "velocityX": 4.105829166460713, + "velocityY": -1.3195245513793832, + "timestamp": 1.9048146325617075 + }, + { + "x": 6.32467753606711, + "y": 0.7405027635497223, + "heading": 3.0964917239018948, + "angularVelocity": 0.5528642186670262, + "velocityX": 3.9445394331370727, + "velocityY": -1.0424793922329247, + "timestamp": 1.9728437265817684 + }, + { + "x": 6.579380074717612, + "y": 0.6865702377068368, + "heading": 3.129527002941156, + "angularVelocity": 0.48560515930904025, + "velocityX": 3.744023675743692, + "velocityY": -0.7927861839080411, + "timestamp": 2.0408728206018294 + }, + { + "x": 6.819417953491211, + "y": 0.648584246635437, + "heading": 3.1544674706525395, + "angularVelocity": 0.3666147266936806, + "velocityX": 3.528459142831067, + "velocityY": -0.5583786116598611, + "timestamp": 2.1089019146218906 + }, + { + "x": 7.056356249212082, + "y": 0.6255394234040195, + "heading": 3.169663027692689, + "angularVelocity": 0.21114304614497145, + "velocityX": 3.2922697979889413, + "velocityY": -0.3202090033354899, + "timestamp": 2.180869982325699 + }, + { + "x": 7.2727644426179525, + "y": 0.6154361557285976, + "heading": 3.176323379743751, + "angularVelocity": 0.09254593410057531, + "velocityX": 3.0070029710471062, + "velocityY": -0.1403854236715493, + "timestamp": 2.2528380500295073 + }, + { + "x": 7.466589513658325, + "y": 0.6144567823314835, + "heading": 3.176929612086991, + "angularVelocity": 0.008423629570475733, + "velocityX": 2.6932093249756126, + "velocityY": -0.013608443694012455, + "timestamp": 2.3248061177333157 + }, + { + "x": 7.6367535402426725, + "y": 0.6194648455903583, + "heading": 3.1735679082028514, + "angularVelocity": -0.04671104826623231, + "velocityX": 2.3644378960495933, + "velocityY": 0.06958729640325968, + "timestamp": 2.396774185437124 + }, + { + "x": 7.78273930823007, + "y": 0.6279831761643265, + "heading": 3.1678878851398564, + "angularVelocity": -0.07892421242114901, + "velocityX": 2.0284797500499288, + "velocityY": 0.11836264117894969, + "timestamp": 2.4687422531409324 + }, + { + "x": 7.904333300767225, + "y": 0.6380692684622451, + "heading": 3.1611775939336626, + "angularVelocity": -0.09323984122806457, + "velocityX": 1.689554776398703, + "velocityY": 0.1401467709183037, + "timestamp": 2.540710320844741 + }, + { + "x": 8.001482657352598, + "y": 0.6481882157692571, + "heading": 3.1544521899594713, + "angularVelocity": -0.09344983391620998, + "velocityX": 1.3498953033615035, + "velocityY": 0.14060329295844945, + "timestamp": 2.612678388548549 + }, + { + "x": 8.074218176420764, + "y": 0.6571108749361716, + "heading": 3.148524448427661, + "angularVelocity": -0.08236627327840042, + "velocityX": 1.0106637761557824, + "velocityY": 0.12398080776097292, + "timestamp": 2.6846464562523575 + }, + { + "x": 8.122612952729549, + "y": 0.6638382057961907, + "heading": 3.144055791313293, + "angularVelocity": -0.06209222029914048, + "velocityX": 0.6724479043672269, + "velocityY": 0.09347660809382861, + "timestamp": 2.756614523956166 + }, + { + "x": 8.146759986877441, + "y": 0.667546272277832, "heading": 3.141592653589793, - "angularVelocity": 2.5566523025657903, - "velocityX": 0.6231541275550934, - "velocityY": -0.1167157675019664, - "timestamp": 5.234421600642128 + "angularVelocity": -0.034225425276624886, + "velocityX": 0.3355242806750317, + "velocityY": 0.05152377436201764, + "timestamp": 2.8285825916599743 }, { - "x": 7.956855773925781, - "y": 0.7431064248085022, + "x": 8.146759986877441, + "y": 0.667546272277832, "heading": 3.141592653589793, - "angularVelocity": -3.8315823290727504e-21, - "velocityX": 3.876602036733102e-21, - "velocityY": -3.3558838471561212e-21, - "timestamp": 5.327893414939309 + "angularVelocity": 6.15835594457233e-23, + "velocityX": 1.29593094240872e-22, + "velocityY": -4.905397235503692e-22, + "timestamp": 2.9005506593637826 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceContested1.2.traj b/src/main/deploy/choreo/SourceContested1.2.traj index 295ce698..6b2f076d 100644 --- a/src/main/deploy/choreo/SourceContested1.2.traj +++ b/src/main/deploy/choreo/SourceContested1.2.traj @@ -1,337 +1,283 @@ { "samples": [ { - "x": 7.956855773925781, - "y": 0.7431064248085022, + "x": 8.146759986877441, + "y": 0.667546272277832, "heading": 3.141592653589793, - "angularVelocity": -3.8315823290727504e-21, - "velocityX": 3.876602036733102e-21, - "velocityY": -3.3558838471561212e-21, + "angularVelocity": 6.15835594457233e-23, + "velocityX": 1.29593094240872e-22, + "velocityY": -4.905397235503692e-22, "timestamp": 0 }, { - "x": 7.9018904258286256, - "y": 0.7580071883715975, - "heading": 2.929746437641678, - "angularVelocity": -2.3660710929042184, - "velocityX": -0.6138977779803758, - "velocityY": 0.166423864457769, - "timestamp": 0.08953501717823986 - }, - { - "x": 7.77927283258943, - "y": 0.7936488654478715, - "heading": 2.8587142949291526, - "angularVelocity": -0.7933448269867456, - "velocityX": -1.3694931559023147, - "velocityY": 0.39807528048295576, - "timestamp": 0.17907003435647972 - }, - { - "x": 7.643021297144732, - "y": 0.8351292518640833, - "heading": 2.8460714923728077, - "angularVelocity": -0.14120511677768, - "velocityX": -1.5217681275858608, - "velocityY": 0.4632867421428616, - "timestamp": 0.2686050515347196 - }, - { - "x": 7.50584253248777, - "y": 0.8779834119507215, - "heading": 2.8388066609223235, - "angularVelocity": -0.08113955499692087, - "velocityX": -1.532124178676125, - "velocityY": 0.4786301654617149, - "timestamp": 0.35814006871295945 - }, - { - "x": 7.368255234864718, - "y": 0.9216599007543135, - "heading": 2.8342113751430746, - "angularVelocity": -0.051323894539503874, - "velocityX": -1.536687007600088, - "velocityY": 0.487814602376678, - "timestamp": 0.4476750858911993 - }, - { - "x": 7.230614869935536, - "y": 0.9659066250673984, - "heading": 2.8305583662971365, - "angularVelocity": -0.040799778243928046, - "velocityX": -1.5372797064993953, - "velocityY": 0.4941834570155044, - "timestamp": 0.5372101030694392 - }, - { - "x": 7.093252862402945, - "y": 1.010681152263652, - "heading": 2.826475143484932, - "angularVelocity": -0.045604758237506364, - "velocityX": -1.5341707843663217, - "velocityY": 0.5000783895212758, - "timestamp": 0.626745120247679 - }, - { - "x": 6.956119170941034, - "y": 1.0557271520364861, - "heading": 2.821845381251112, - "angularVelocity": -0.051708955665952475, - "velocityX": -1.5316207645207218, - "velocityY": 0.5031104163766442, - "timestamp": 0.7162801374259189 - }, - { - "x": 6.818992484234107, - "y": 1.100769539311748, - "heading": 2.817204104711705, - "angularVelocity": -0.0518375568093923, - "velocityX": -1.5315425297115504, - "velocityY": 0.5030700690613045, - "timestamp": 0.8058151546041588 - }, - { - "x": 6.681872127416794, - "y": 1.1458071900530102, - "heading": 2.8125526096190336, - "angularVelocity": -0.05195168593547435, - "velocityX": -1.5314718323484902, - "velocityY": 0.5030171675915902, - "timestamp": 0.8953501717823986 - }, - { - "x": 6.544758118625867, - "y": 1.1908399522969457, - "heading": 2.8078906701771453, - "angularVelocity": -0.052068336934676634, - "velocityX": -1.5314009324192217, - "velocityY": 0.5029625688716561, - "timestamp": 0.9848851889606385 - }, - { - "x": 6.407650518790602, - "y": 1.2358677646327403, - "heading": 2.8032180092193197, - "angularVelocity": -0.05218808355755985, - "velocityX": -1.5313293519821691, - "velocityY": 0.5029072842657369, - "timestamp": 1.0744202061388783 - }, - { - "x": 6.270549391355044, - "y": 1.280890584680893, - "heading": 2.7985343668363263, - "angularVelocity": -0.05231073305843581, - "velocityX": -1.531257062950314, - "velocityY": 0.5028515263310278, - "timestamp": 1.1639552233171182 - }, - { - "x": 6.1334547976207645, - "y": 1.3259083749515967, - "heading": 2.793839500454244, - "angularVelocity": -0.05243609182244443, - "velocityX": -1.5311840892526187, - "velocityY": 0.5027953496796241, - "timestamp": 1.253490240495358 - }, - { - "x": 5.996366796772198, - "y": 1.3709210989414258, - "heading": 2.7891331795005336, - "angularVelocity": -0.05256402580837642, - "velocityX": -1.5311104545349274, - "velocityY": 0.5027387653282189, - "timestamp": 1.343025257673598 - }, - { - "x": 5.8592854465592445, - "y": 1.4159287198651518, - "heading": 2.784415180924841, - "angularVelocity": -0.052694451002343774, - "velocityX": -1.5310361748193169, - "velocityY": 0.5026817701294257, - "timestamp": 1.4325602748518378 - }, - { - "x": 5.722210803960778, - "y": 1.4609312002186288, - "heading": 2.7796852859248706, - "angularVelocity": -0.05282731995856272, - "velocityX": -1.530961258717216, - "velocityY": 0.5026243560537855, - "timestamp": 1.5220952920300777 - }, - { - "x": 5.585142925732609, - "y": 1.5059285016384842, - "heading": 2.7749432775568774, - "angularVelocity": -0.052962611919236485, - "velocityX": -1.5308857087200383, - "velocityY": 0.5025665135047443, - "timestamp": 1.6116303092083175 - }, - { - "x": 5.448081868853608, - "y": 1.5509205848763696, - "heading": 2.7701889389309033, - "angularVelocity": -0.05310032628362197, - "velocityX": -1.5308095223363887, - "velocityY": 0.502508232598188, - "timestamp": 1.7011653263865574 - }, - { - "x": 5.311027690899557, - "y": 1.5959074098195543, - "heading": 2.7654220517793227, - "angularVelocity": -0.05324047843863795, - "velocityX": -1.5307326928995175, - "velocityY": 0.502449503679975, - "timestamp": 1.7907003435647972 - }, - { - "x": 5.173980450370318, - "y": 1.6408889355303347, - "heading": 2.7606423952580355, - "angularVelocity": -0.05338309716043176, - "velocityX": -1.5306552100885358, - "velocityY": 0.5023903175361529, - "timestamp": 1.880235360743037 - }, - { - "x": 5.0369402069873335, - "y": 1.6858651202909567, - "heading": 2.7558497448937236, - "angularVelocity": -0.05352822298309391, - "velocityX": -1.5305770602598503, - "velocityY": 0.5023306654544649, - "timestamp": 1.969770377921277 - }, - { - "x": 4.899907021964721, - "y": 1.7308359216420997, - "heading": 2.751043871646217, - "angularVelocity": -0.05367590691292534, - "velocityX": -1.5304982267420264, - "velocityY": 0.5022705391525029, - "timestamp": 2.059305395099517 - }, - { - "x": 4.762880958231051, - "y": 1.7758012963934744, - "heading": 2.7462245411420234, - "angularVelocity": -0.05382620851683086, - "velocityX": -1.5304186903866774, - "velocityY": 0.5022099304662124, - "timestamp": 2.1488404122777567 - }, - { - "x": 4.625862080529766, - "y": 1.8207612005630536, - "heading": 2.7413915132887583, - "angularVelocity": -0.053979191668033985, - "velocityX": -1.5303384309238324, - "velocityY": 0.5021488305528153, - "timestamp": 2.2383754294559965 - }, - { - "x": 4.488850455262, - "y": 1.8657155891757824, - "heading": 2.736544542679806, - "angularVelocity": -0.054134915720223636, - "velocityX": -1.5302574298390257, - "velocityY": 0.5020872283213718, - "timestamp": 2.3279104466342364 - }, - { - "x": 4.351846149940031, - "y": 1.910664415870897, - "heading": 2.7316833801907157, - "angularVelocity": -0.05429342219718387, - "velocityX": -1.5301756747221047, - "velocityY": 0.5020251082951581, - "timestamp": 2.4174454638124763 - }, - { - "x": 4.214849232355917, - "y": 1.955607632403858, - "heading": 2.7268077754532425, - "angularVelocity": -0.05445472499064026, - "velocityX": -1.530093162448295, - "velocityY": 0.5019624494346333, - "timestamp": 2.506980480990716 - }, - { - "x": 4.077859769965423, - "y": 2.0005451883147503, - "heading": 2.721917478680163, - "angularVelocity": -0.05461881761126215, - "velocityX": -1.5300098967735323, - "velocityY": 0.5018992269966714, - "timestamp": 2.596515498168956 - }, - { - "x": 3.940877826047202, - "y": 2.045477029542233, - "heading": 2.717012252912238, - "angularVelocity": -0.05478555678567875, - "velocityX": -1.5299259243512187, - "velocityY": 0.5018354007576298, - "timestamp": 2.686050515347196 - }, - { - "x": 3.8039034641003533, - "y": 2.0904031002081847, - "heading": 2.712091859898031, - "angularVelocity": -0.05495495694618896, - "velocityX": -1.5298412427192656, - "velocityY": 0.5017709504262021, - "timestamp": 2.775585532525435 - }, - { - "x": 3.66693674900291, - "y": 2.135323343068443, - "heading": 2.707156056414113, - "angularVelocity": -0.05512707362408464, - "velocityX": -1.5297558364765886, - "velocityY": 0.501705860745354, - "timestamp": 2.865120549703674 - }, - { - "x": 3.5299779535424887, - "y": 2.1802377886608806, - "heading": 2.7022039564715556, - "angularVelocity": -0.05530908574796876, - "velocityX": -1.5296673835196157, - "velocityY": 0.5016411121363245, - "timestamp": 2.9546555668819128 - }, - { - "x": 3.426502516345769, - "y": 2.233179068985383, - "heading": 2.594355857349509, - "angularVelocity": -1.2045354155386023, - "velocityX": -1.1556979655315056, - "velocityY": 0.5912913404496402, - "timestamp": 3.0441905840601517 - }, - { - "x": 3.387338399887085, - "y": 2.2590601444244385, - "heading": 2.3477629859714533, - "angularVelocity": -2.7541500426269785, - "velocityX": -0.43741675260662705, - "velocityY": 0.28906093118330495, - "timestamp": 3.1337256012383907 - }, - { - "x": 3.387338399887085, - "y": 2.2590601444244385, - "heading": 2.3477629859714533, - "angularVelocity": -2.400686558954734e-26, - "velocityX": 2.2132770883951947e-26, - "velocityY": 2.6443990744659133e-26, - "timestamp": 3.2232606184166297 + "x": 8.116302316915732, + "y": 0.6788007824238615, + "heading": 3.1342355794344465, + "angularVelocity": -0.08856487332455099, + "velocityX": -0.36665114758415307, + "velocityY": 0.13548242743870734, + "timestamp": 0.0830698885368073 + }, + { + "x": 8.055384628279041, + "y": 0.7013126982960538, + "heading": 3.119642832006715, + "angularVelocity": -0.17566831597788424, + "velocityX": -0.7333305691110794, + "velocityY": 0.2709997120342536, + "timestamp": 0.1661397770736146 + }, + { + "x": 7.964004124464221, + "y": 0.7350855445347456, + "heading": 3.09796265201497, + "angularVelocity": -0.26098722862904256, + "velocityX": -1.1000436551004955, + "velocityY": 0.40655942644882115, + "timestamp": 0.2492096656104219 + }, + { + "x": 7.842157398019484, + "y": 0.7801237085031743, + "heading": 3.0693814277169014, + "angularVelocity": -0.34406238892934254, + "velocityX": -1.4667977601865743, + "velocityY": 0.542171956189308, + "timestamp": 0.3322795541472292 + }, + { + "x": 7.689840193239616, + "y": 0.8364327959906936, + "heading": 3.0341415239312566, + "angularVelocity": -0.424219947881965, + "velocityX": -1.8336030956918719, + "velocityY": 0.6778519687379819, + "timestamp": 0.4153494426840365 + }, + { + "x": 7.50704705480538, + "y": 0.904020204266213, + "heading": 2.992571084782186, + "angularVelocity": -0.5004272893739545, + "velocityX": -2.2004741026376022, + "velocityY": 0.8136210300266904, + "timestamp": 0.4984193312208438 + }, + { + "x": 7.293770807150263, + "y": 0.982896119722086, + "heading": 2.9451379766381653, + "angularVelocity": -0.5710024291534157, + "velocityX": -2.567431489457423, + "velocityY": 0.9495127156806547, + "timestamp": 0.5814892197576511 + }, + { + "x": 7.050001837904705, + "y": 1.073075442966285, + "heading": 2.892559152885557, + "angularVelocity": -0.6329468436605835, + "velocityX": -2.934504590523767, + "velocityY": 1.0855837756931817, + "timestamp": 0.6645591082944584 + }, + { + "x": 6.775727633239132, + "y": 1.1745820471884034, + "heading": 2.8360637721730617, + "angularVelocity": -0.6800945770796716, + "velocityX": -3.301728333775782, + "velocityY": 1.2219422225075236, + "timestamp": 0.7476289968312657 + }, + { + "x": 6.470938505156052, + "y": 1.2874600105843308, + "heading": 2.7782378900044695, + "angularVelocity": -0.6961112285948248, + "velocityX": -3.6690687016876415, + "velocityY": 1.358831285128218, + "timestamp": 0.830698885368073 + }, + { + "x": 6.135781167752857, + "y": 1.4117970937816189, + "heading": 2.7282295068076383, + "angularVelocity": -0.6020037353808817, + "velocityX": -4.034642917026322, + "velocityY": 1.4967768151295402, + "timestamp": 0.9137687739048803 + }, + { + "x": 5.78853790859181, + "y": 1.5399120528524686, + "heading": 2.728229496122602, + "angularVelocity": -1.2862706865047314e-7, + "velocityX": -4.180133924306256, + "velocityY": 1.542255097815422, + "timestamp": 0.9968386624416876 + }, + { + "x": 5.441294649641329, + "y": 1.6680270124940895, + "heading": 2.728229485437636, + "angularVelocity": -1.286262206803139e-7, + "velocityX": -4.180133921771438, + "velocityY": 1.5422551046863975, + "timestamp": 1.079908550978495 + }, + { + "x": 5.094051397734534, + "y": 1.7961419912269363, + "heading": 2.7282294747526707, + "angularVelocity": -1.286262181377533e-7, + "velocityX": -4.180133836979181, + "velocityY": 1.5422553345076508, + "timestamp": 1.1629784395153018 + }, + { + "x": 4.746808156293299, + "y": 1.9242569983256745, + "heading": 2.7282294640677045, + "angularVelocity": -1.2862622783165573e-7, + "velocityX": -4.180133710994155, + "velocityY": 1.5422556759778505, + "timestamp": 1.2460483280521086 + }, + { + "x": 4.399565062124353, + "y": 2.0523724045908915, + "heading": 2.7282294533827316, + "angularVelocity": -1.286263045468436e-7, + "velocityX": -4.180131938122049, + "velocityY": 1.5422604811664191, + "timestamp": 1.3291182165889155 + }, + { + "x": 4.059443344314116, + "y": 2.1983468564274284, + "heading": 2.728229437713722, + "angularVelocity": -1.8862441726861322e-7, + "velocityX": -4.094404408157266, + "velocityY": 1.7572486782843997, + "timestamp": 1.4121881051257223 + }, + { + "x": 3.7193219629342615, + "y": 2.3443220921470425, + "heading": 2.7282294220445418, + "angularVelocity": -1.8862647326726582e-7, + "velocityX": -4.094400358189376, + "velocityY": 1.7572581147130661, + "timestamp": 1.4952579936625292 + }, + { + "x": 3.395818553566254, + "y": 2.4935219406512408, + "heading": 2.6724500576638754, + "angularVelocity": -0.6714751321226504, + "velocityX": -3.89435227452686, + "velocityY": 1.7960761851520912, + "timestamp": 1.578327882199336 + }, + { + "x": 3.101026535812032, + "y": 2.6290188837333135, + "heading": 2.59592195667292, + "angularVelocity": -0.9212495928298584, + "velocityX": -3.548722924114715, + "velocityY": 1.631119837388922, + "timestamp": 1.661397770736143 + }, + { + "x": 2.835772833375676, + "y": 2.750798938045291, + "heading": 2.519084794831671, + "angularVelocity": -0.9249700847642709, + "velocityX": -3.1931390195476896, + "velocityY": 1.4659951572947918, + "timestamp": 1.7444676592729498 + }, + { + "x": 2.6000574792867495, + "y": 2.8589475580822414, + "heading": 2.4467191944954414, + "angularVelocity": -0.8711411753509879, + "velocityX": -2.8375547173689126, + "velocityY": 1.301899183228487, + "timestamp": 1.8275375478097566 + }, + { + "x": 2.393855398523055, + "y": 2.953514187395407, + "heading": 2.3809962926356145, + "angularVelocity": -0.7911759967115644, + "velocityX": -2.4822722697205672, + "velocityY": 1.1383984134186498, + "timestamp": 1.9106074363465635 + }, + { + "x": 2.2171452899215467, + "y": 3.0345296096879184, + "heading": 2.3231458932555134, + "angularVelocity": -0.6964063681687525, + "velocityX": -2.1272462466734887, + "velocityY": 0.9752682195620702, + "timestamp": 1.9936773248833703 + }, + { + "x": 2.0699108508844217, + "y": 3.102014617448754, + "heading": 2.273952675680828, + "angularVelocity": -0.592190725678996, + "velocityX": -1.7724164752175795, + "velocityY": 0.8123883268596626, + "timestamp": 2.076747213420177 + }, + { + "x": 1.95213960106252, + "y": 3.155984197182567, + "heading": 2.2339585252158036, + "angularVelocity": -0.4814518373562503, + "velocityX": -1.4177369429082447, + "velocityY": 0.6496888425449069, + "timestamp": 2.159817101956984 + }, + { + "x": 1.8638217687898557, + "y": 3.1964496924625165, + "heading": 2.203560612114051, + "angularVelocity": -0.36593179113613356, + "velocityX": -1.0631750424647786, + "velocityY": 0.48712591280317163, + "timestamp": 2.242886990493791 + }, + { + "x": 1.8049494737197318, + "y": 3.223419994217748, + "heading": 2.1830643911586045, + "angularVelocity": -0.24673466302250652, + "velocityX": -0.7087080060789848, + "velocityY": 0.3246700125675676, + "timestamp": 2.325956879030598 + }, + { + "x": 1.775516152381897, + "y": 3.236902236938477, + "heading": 2.1727145819710088, + "angularVelocity": -0.12459158631240587, + "velocityX": -0.35431998111798746, + "velocityY": 0.16229999772727494, + "timestamp": 2.4090267675674046 + }, + { + "x": 1.775516152381897, + "y": 3.236902236938477, + "heading": 2.1727145819710088, + "angularVelocity": 3.884064768338255e-24, + "velocityX": -1.446989921221479e-23, + "velocityY": -2.0280809780864772e-23, + "timestamp": 2.4920966561042115 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceContested1.traj b/src/main/deploy/choreo/SourceContested1.traj index eea3ac49..65f8b846 100644 --- a/src/main/deploy/choreo/SourceContested1.traj +++ b/src/main/deploy/choreo/SourceContested1.traj @@ -4,847 +4,658 @@ "x": 0.8210453391075134, "y": 4.576303482055664, "heading": 2.111215568027718, - "angularVelocity": -1.8256752618245288e-22, - "velocityX": -7.65916010942737e-22, - "velocityY": 5.834863940365417e-22, + "angularVelocity": 1.0347096801356489e-22, + "velocityX": -4.48084535985214e-23, + "velocityY": -5.937888990335369e-23, "timestamp": 0 }, { - "x": 0.8644346902480902, - "y": 4.538605095268569, - "heading": 2.3610664104627634, - "angularVelocity": 2.6730073050757213, - "velocityX": 0.4641971643197831, - "velocityY": -0.4033128817553312, - "timestamp": 0.09347181429718082 - }, - { - "x": 0.9651901704673621, - "y": 4.45361330197089, - "heading": 2.440857637900237, - "angularVelocity": 0.8536394424076145, - "velocityX": 1.0779236604837226, - "velocityY": -0.9092772397405119, - "timestamp": 0.18694362859436164 - }, - { - "x": 1.0866380874820318, - "y": 4.362483747012655, - "heading": 2.440865267791502, - "angularVelocity": 0.00008162772192147527, - "velocityX": 1.299299879090211, - "velocityY": -0.9749415440733835, - "timestamp": 0.28041544289154247 - }, - { - "x": 1.2080860091215304, - "y": 4.271354198213206, - "heading": 2.4408728977229917, - "angularVelocity": 0.00008162815226084984, - "velocityX": 1.2992999285685376, - "velocityY": -0.9749414781841598, - "timestamp": 0.3738872571887233 - }, - { - "x": 1.3295339322232647, - "y": 4.1802246514640125, - "heading": 2.440880527950737, - "angularVelocity": 0.0000816313217297415, - "velocityX": 1.299299944212136, - "velocityY": -0.9749414562496848, - "timestamp": 0.4673590714859041 - }, - { - "x": 1.4509818569281794, - "y": 4.089095106952003, - "heading": 2.440888158472548, - "angularVelocity": 0.00008163446776368671, - "velocityX": 1.2992999613636238, - "velocityY": -0.9749414323153683, - "timestamp": 0.5608308857830849 - }, - { - "x": 1.5724297833961638, - "y": 3.997965564889279, - "heading": 2.440895789286055, - "angularVelocity": 0.00008163758844700525, - "velocityX": 1.2992999802256675, - "velocityY": -0.9749414061119055, - "timestamp": 0.6543027000802657 - }, - { - "x": 1.6938777118095218, - "y": 3.906836025517733, - "heading": 2.4409034203886897, - "angularVelocity": 0.00008164068165665073, - "velocityX": 1.299300001038079, - "velocityY": -0.9749413773205733, - "timestamp": 0.7477745143774466 - }, - { - "x": 1.8153256423773063, - "y": 3.8157064891148127, - "heading": 2.4409110517776624, - "angularVelocity": 0.00008164374501723712, - "velocityX": 1.299300024087019, - "velocityY": -0.9749413455609843, - "timestamp": 0.8412463286746275 - }, - { - "x": 1.9367735753408017, - "y": 3.7245769560008215, - "heading": 2.440918683449934, - "angularVelocity": 0.00008164677586345695, - "velocityX": 1.2993000497173246, - "velocityY": -0.9749413103746718, - "timestamp": 0.9347181429718083 - }, - { - "x": 2.0582215109806032, - "y": 3.6334474265483334, - "heading": 2.4409263154021836, - "angularVelocity": 0.0000816497711865655, - "velocityX": 1.2993000783495519, - "velocityY": -0.9749412712023965, - "timestamp": 1.0281899572689892 - }, - { - "x": 2.1796694496259765, - "y": 3.542317901194656, - "heading": 2.440933947630772, - "angularVelocity": 0.00008165272757387169, - "velocityX": 1.2993001105043924, - "velocityY": -0.9749412273516357, - "timestamp": 1.12166177156617 - }, - { - "x": 2.3011173916676397, - "y": 3.451188380458844, - "heading": 2.440941580131694, - "angularVelocity": 0.00008165564110490276, - "velocityX": 1.2993001468393033, - "velocityY": -0.9749411779478068, - "timestamp": 1.215133585863351 - }, - { - "x": 2.422565337575994, - "y": 3.3600588649659713, - "heading": 2.440949212900517, - "angularVelocity": 0.00008165850722838425, - "velocityX": 1.2993001882067592, - "velocityY": -0.97494112185668, - "timestamp": 1.3086054001605318 - }, - { - "x": 2.5440132879286748, - "y": 3.268929355483818, - "heading": 2.44095684593231, - "angularVelocity": 0.00008166132058271907, - "velocityX": 1.2993002357539973, - "velocityY": -0.9749410575515263, - "timestamp": 1.4020772144577127 - }, - { - "x": 2.6654612434555944, - "y": 3.1777998529828566, - "heading": 2.4409644792215364, - "angularVelocity": 0.00008166407471316297, - "velocityX": 1.2993002911101328, - "velocityY": -0.9749409828638569, - "timestamp": 1.4955490287548936 - }, - { - "x": 2.7869092051206428, - "y": 3.0866703587450828, - "heading": 2.4409721127619197, - "angularVelocity": 0.00008166676169345355, - "velocityX": 1.2993003567783705, - "velocityY": -0.9749408944608698, - "timestamp": 1.5890208430520745 - }, - { - "x": 2.908357174291307, - "y": 2.995540874590004, - "heading": 2.440979746546289, - "angularVelocity": 0.00008166937195400168, - "velocityX": 1.2993004370765409, - "velocityY": -0.9749407865920395, - "timestamp": 1.6824926573492553 - }, - { - "x": 3.0298051531585033, - "y": 2.9044114034341826, - "heading": 2.440987380566619, - "angularVelocity": 0.00008167189636088749, - "velocityX": 1.299300540814034, - "velocityY": -0.9749406475206273, - "timestamp": 1.7759644716464362 - }, - { - "x": 3.1512531460668667, - "y": 2.813281951064894, - "heading": 2.4409950148158184, - "angularVelocity": 0.00008167434490013359, - "velocityX": 1.2993006910322324, - "velocityY": -0.9749404465345578, - "timestamp": 1.869436285943617 - }, - { - "x": 3.272701165942649, - "y": 2.722152534713991, - "heading": 2.4410026493051027, - "angularVelocity": 0.00008167691342827456, - "velocityX": 1.2993009795407924, - "velocityY": -0.9749400611950203, - "timestamp": 1.962908100240798 - }, - { - "x": 3.3941493332630563, - "y": 2.6310233151653177, - "heading": 2.441010284564204, - "angularVelocity": 0.00008168514924485609, - "velocityX": 1.2993025569641734, - "velocityY": -0.9749379557236404, - "timestamp": 2.056379914537979 - }, - { - "x": 3.506643194922811, - "y": 2.543133701847389, - "heading": 2.478368219245545, - "angularVelocity": 0.39967058478790624, - "velocityX": 1.2035057038916106, - "velocityY": -0.9402793128471436, - "timestamp": 2.1498517288351597 - }, - { - "x": 3.621033917156409, - "y": 2.462292474945906, - "heading": 2.526376588848021, - "angularVelocity": 0.513613327862023, - "velocityX": 1.2237991002283153, - "velocityY": -0.8648727695009685, - "timestamp": 2.2433235431323406 - }, - { - "x": 3.7386117914711243, - "y": 2.3880497601699076, - "heading": 2.5783804352338895, - "angularVelocity": 0.5563585854932618, - "velocityX": 1.2578965669896234, - "velocityY": -0.7942791667652267, - "timestamp": 2.3367953574295215 - }, - { - "x": 3.8565091125978217, - "y": 2.322003920666466, - "heading": 2.644821825276836, - "angularVelocity": 0.7108173789342068, - "velocityX": 1.2613141406655402, - "velocityY": -0.7065856162100131, - "timestamp": 2.4302671717267024 - }, - { - "x": 3.9764532017194134, - "y": 2.262287896924173, - "heading": 2.7144845491695992, - "angularVelocity": 0.7452805363473517, - "velocityX": 1.2832113083869967, - "velocityY": -0.6388666379410853, - "timestamp": 2.5237389860238832 - }, - { - "x": 4.100272385460963, - "y": 2.211007382543893, - "heading": 2.7840189996546836, - "angularVelocity": 0.7439082145555573, - "velocityX": 1.3246686680102626, - "velocityY": -0.5486200815278979, - "timestamp": 2.617210800321064 - }, - { - "x": 4.2303737393158425, - "y": 2.1552421719955963, - "heading": 2.8233254460416966, - "angularVelocity": 0.42051656622437983, - "velocityX": 1.3918779134985075, - "velocityY": -0.5965992098002822, - "timestamp": 2.710682614618245 - }, - { - "x": 4.368649457253469, - "y": 2.0958799166815227, - "heading": 2.828594846354344, - "angularVelocity": 0.056374216679845754, - "velocityX": 1.479330630065645, - "velocityY": -0.6350818774667083, - "timestamp": 2.804154428915426 - }, - { - "x": 4.510357504555393, - "y": 2.041351647410644, - "heading": 2.8285983828965033, - "angularVelocity": 0.00003783538584099394, - "velocityX": 1.5160511044685778, - "velocityY": -0.5833659021265345, - "timestamp": 2.8976262432126068 - }, - { - "x": 4.652065594721857, - "y": 1.986823489488553, - "heading": 2.8286019193610827, - "angularVelocity": 0.00003783455586217891, - "velocityX": 1.516051563051105, - "velocityY": -0.5833647108713029, - "timestamp": 2.9910980575097876 - }, - { - "x": 4.793773697805799, - "y": 1.9322953651089438, - "heading": 2.8286054557754516, - "angularVelocity": 0.000037834018690956495, - "velocityX": 1.5160517012476136, - "velocityY": -0.5833643520200054, - "timestamp": 3.0845698718069685 - }, - { - "x": 4.935481808422067, - "y": 1.8777672602773423, - "heading": 2.8286089921399973, - "angularVelocity": 0.0000378334856573034, - "velocityX": 1.5160517818315487, - "velocityY": -0.5833641428873607, - "timestamp": 3.1780416861041494 - }, - { - "x": 5.0771899244715755, - "y": 1.8232391695375483, - "heading": 2.82861252845304, - "angularVelocity": 0.00003783293466201255, - "velocityX": 1.516051839958576, - "velocityY": -0.5833639921273963, - "timestamp": 3.2715135004013303 - }, - { - "x": 5.218898044817724, - "y": 1.76871108993463, - "heading": 2.8286160647128784, - "angularVelocity": 0.0000378323654513879, - "velocityX": 1.5160518859258105, - "velocityY": -0.5833638729805077, - "timestamp": 3.364985314698511 - }, - { - "x": 5.360606168722747, - "y": 1.7141830195502608, - "heading": 2.828619600918001, - "angularVelocity": 0.00003783178008651746, - "velocityX": 1.5160519240001233, - "velocityY": -0.5833637743566714, - "timestamp": 3.458457128995692 - }, - { - "x": 5.5023142956547595, - "y": 1.6596549570012744, - "heading": 2.828623137067082, - "angularVelocity": 0.0000378311805264838, - "velocityX": 1.516051956384105, - "velocityY": -0.5833636905305171, - "timestamp": 3.551928943292873 - }, - { - "x": 5.644022425205511, - "y": 1.605126901225908, - "heading": 2.828626673158945, - "angularVelocity": 0.00003783056838692872, - "velocityX": 1.5160519844004554, - "velocityY": -0.5833636180635361, - "timestamp": 3.6454007575900538 - }, - { - "x": 5.785730557049162, - "y": 1.5505988513766498, - "heading": 2.828630209192539, - "angularVelocity": 0.000037829944994143904, - "velocityX": 1.516052008930833, - "velocityY": -0.5833635546635878, - "timestamp": 3.7388725718872347 - }, - { - "x": 5.927438690918852, - "y": 1.4960708067593358, - "heading": 2.8286337451669143, - "angularVelocity": 0.00003782931145939862, - "velocityX": 1.5160520306062335, - "velocityY": -0.5833634986900924, - "timestamp": 3.8323443861844155 - }, - { - "x": 6.069146826592036, - "y": 1.4415427667950262, - "heading": 2.828637281081213, - "angularVelocity": 0.00003782866873129857, - "velocityX": 1.5160520499007515, - "velocityY": -0.583363448910333, - "timestamp": 3.9258162004815964 - }, - { - "x": 6.210854963880611, - "y": 1.387014730994346, - "heading": 2.828640816934652, - "angularVelocity": 0.000037828017628644405, - "velocityX": 1.516052067182881, - "velocityY": -0.5833634043661104, - "timestamp": 4.019288014778777 - }, - { - "x": 6.35256310262391, - "y": 1.332486698939259, - "heading": 2.8286443527265157, - "angularVelocity": 0.00003782735887192692, - "velocityX": 1.5160520827461144, - "velocityY": -0.5833633642942074, - "timestamp": 4.112759829075958 - }, - { - "x": 6.494271242683516, - "y": 1.2779586702695989, - "heading": 2.828647888456148, - "angularVelocity": 0.00003782669309301873, - "velocityX": 1.516052096828514, - "velocityY": -0.583363328075519, - "timestamp": 4.206231643373139 - }, - { - "x": 6.635979383939319, - "y": 1.223430644672807, - "heading": 2.828651424122945, - "angularVelocity": 0.0000378260208545108, - "velocityX": 1.5160521096259227, - "velocityY": -0.5833632952007055, - "timestamp": 4.2997034576703195 - }, - { - "x": 6.7776875262864404, - "y": 1.1689026218759404, - "heading": 2.8286549597263497, - "angularVelocity": 0.0000378253426576497, - "velocityX": 1.516052121301291, - "velocityY": -0.5833632652459484, - "timestamp": 4.3931752719675 - }, - { - "x": 6.9193956696327925, - "y": 1.114374601639329, - "heading": 2.8286584952658473, - "angularVelocity": 0.00003782465895599354, - "velocityX": 1.516052131991471, - "velocityY": -0.5833632378552868, - "timestamp": 4.486647086264681 - }, - { - "x": 7.061103813897117, - "y": 1.0598465837514708, - "heading": 2.828662030740962, - "angularVelocity": 0.000037823970152435987, - "velocityX": 1.5160521418123194, - "velocityY": -0.5833632127273561, - "timestamp": 4.580118900561862 - }, - { - "x": 7.202811959007387, - "y": 1.0053185680248766, - "heading": 2.8286655661512494, - "angularVelocity": 0.00003782327661332265, - "velocityX": 1.5160521508625917, - "velocityY": -0.5833631896052619, - "timestamp": 4.673590714859043 - }, - { - "x": 7.344520104899491, - "y": 0.9507905542926514, - "heading": 2.828669101496299, - "angularVelocity": 0.000037822578668805064, - "velocityX": 1.5160521592269784, - "velocityY": -0.5833631682686824, - "timestamp": 4.767062529156224 - }, - { - "x": 7.486228251516145, - "y": 0.8962625424056586, - "heading": 2.828672636775727, - "angularVelocity": 0.000037821876620878625, - "velocityX": 1.5160521669785085, - "velocityY": -0.5833631485276237, - "timestamp": 4.860534343453405 - }, - { - "x": 7.627936398805981, - "y": 0.8417345322301597, - "heading": 2.828676171989175, - "angularVelocity": 0.00003782117074336596, - "velocityX": 1.516052174180496, - "velocityY": -0.5833631302173573, - "timestamp": 4.954006157750586 - }, - { - "x": 7.769644550650757, - "y": 0.7872065338976291, - "heading": 2.8286797071997065, - "angularVelocity": 0.000037821139539143325, - "velocityX": 1.5160522229111162, - "velocityY": -0.5833630035164005, - "timestamp": 5.0474779720477665 - }, - { - "x": 7.89860842703643, - "y": 0.754016059353999, - "heading": 2.902617724341904, - "angularVelocity": 0.7910193858773472, - "velocityX": 1.3797087106456418, - "velocityY": -0.3550853783377481, - "timestamp": 5.140949786344947 - }, - { - "x": 7.956855773925781, - "y": 0.7431064248085022, + "x": 0.8376034098013856, + "y": 4.562699117747838, + "heading": 2.123652893108774, + "angularVelocity": 0.1828236177507875, + "velocityX": 0.24339690146379658, + "velocityY": -0.19997861949792262, + "timestamp": 0.06802909402006102 + }, + { + "x": 0.8707358038741012, + "y": 4.535482743319388, + "heading": 2.1482712139566598, + "angularVelocity": 0.3618792988868299, + "velocityX": 0.4870327107832017, + "velocityY": -0.4000696293327653, + "timestamp": 0.13605818804012204 + }, + { + "x": 0.9204609539749747, + "y": 4.4946461788288365, + "heading": 2.1847789288455908, + "angularVelocity": 0.5366485533111098, + "velocityX": 0.7309394725469969, + "velocityY": -0.600280880978807, + "timestamp": 0.20408728206018306 + }, + { + "x": 0.9868005236891458, + "y": 4.440179990916992, + "heading": 2.232822407442878, + "angularVelocity": 0.70621958574253, + "velocityX": 0.9751646801970973, + "velocityY": -0.8006307991663675, + "timestamp": 0.2721163760802441 + }, + { + "x": 1.0697804545770624, + "y": 4.372072363754176, + "heading": 2.291950205070784, + "angularVelocity": 0.8691545651110574, + "velocityX": 1.2197712182297578, + "velocityY": -1.0011544052421333, + "timestamp": 0.3401454701003051 + }, + { + "x": 1.1694318368712635, + "y": 4.290307500716387, + "heading": 2.3615667397256703, + "angularVelocity": 1.0233347313776844, + "velocityX": 1.4648347700296431, + "velocityY": -1.2019102152628651, + "timestamp": 0.4081745641203661 + }, + { + "x": 1.2857914547624776, + "y": 4.194863719874867, + "heading": 2.4408735330108198, + "angularVelocity": 1.165777590125818, + "velocityX": 1.7104390344651783, + "velocityY": -1.4029847408135987, + "timestamp": 0.47620365814042714 + }, + { + "x": 1.4189020134002508, + "y": 4.085711763901469, + "heading": 2.5287912003062516, + "angularVelocity": 1.292353934178601, + "velocityX": 1.956671047221653, + "velocityY": -1.6044893371828546, + "timestamp": 0.5442327521604882 + }, + { + "x": 1.5688122322603035, + "y": 3.9628143203631563, + "heading": 2.6238365711205818, + "angularVelocity": 1.3971282755331538, + "velocityX": 2.2036192164465107, + "velocityY": -1.8065424111347401, + "timestamp": 0.6122618461805491 + }, + { + "x": 1.7355761304311574, + "y": 3.8261286744353766, + "heading": 2.7238718533453037, + "angularVelocity": 1.4704779427934513, + "velocityX": 2.451361444291423, + "velocityY": -2.0092233756262123, + "timestamp": 0.6802909402006101 + }, + { + "x": 1.9192427382156467, + "y": 3.6756195148686888, + "heading": 2.8254629684407537, + "angularVelocity": 1.4933480529006078, + "velocityX": 2.6998243976368146, + "velocityY": -2.2124234011157657, + "timestamp": 0.7483200342206711 + }, + { + "x": 2.1197678234399393, + "y": 3.511336132708068, + "heading": 2.921883740999518, + "angularVelocity": 1.417346121503996, + "velocityX": 2.94763715602563, + "velocityY": -2.41489886830149, + "timestamp": 0.816349128240732 + }, + { + "x": 2.336325957396502, + "y": 3.3346123423953884, + "heading": 2.9945552520993064, + "angularVelocity": 1.0682416420003875, + "velocityX": 3.183316448293462, + "velocityY": -2.5977678059414684, + "timestamp": 0.884378222260793 + }, + { + "x": 2.5671944876290005, + "y": 3.1506814395963123, + "heading": 3.018943844161631, + "angularVelocity": 0.3585023792192819, + "velocityX": 3.3936734504272232, + "velocityY": -2.7037094268054926, + "timestamp": 0.952407316280854 + }, + { + "x": 2.801664380200398, + "y": 2.9585918594844425, + "heading": 3.0189438734994596, + "angularVelocity": 4.312541447677092e-7, + "velocityX": 3.446612011358773, + "velocityY": -2.8236386634110495, + "timestamp": 1.020436410300915 + }, + { + "x": 3.0361342064803303, + "y": 2.766502198455338, + "heading": 3.018943902837169, + "angularVelocity": 4.3125239350876155e-7, + "velocityX": 3.446611036901202, + "velocityY": -2.8236398528614792, + "timestamp": 1.0884655043209759 + }, + { + "x": 3.2706040874248963, + "y": 2.5744126041513797, + "heading": 3.018943932174878, + "angularVelocity": 4.312523803489459e-7, + "velocityX": 3.446611840449086, + "velocityY": -2.8236388720289733, + "timestamp": 1.1564945983410369 + }, + { + "x": 3.50614012111681, + "y": 2.38363180288057, + "heading": 3.0189439616606526, + "angularVelocity": 4.334288912332302e-7, + "velocityX": 3.4622838520009824, + "velocityY": -2.804400147009905, + "timestamp": 1.2245236923610978 + }, + { + "x": 3.746237311752337, + "y": 2.198624029599204, + "heading": 3.0189439927171957, + "angularVelocity": 4.565185470115094e-7, + "velocityX": 3.5293310030664933, + "velocityY": -2.719538984699832, + "timestamp": 1.2925527863811588 + }, + { + "x": 3.986334650538585, + "y": 2.013616448583285, + "heading": 3.0189440237737535, + "angularVelocity": 4.565187647674116e-7, + "velocityX": 3.529333180821812, + "velocityY": -2.719536158475997, + "timestamp": 1.3605818804012197 + }, + { + "x": 4.226432008113782, + "y": 1.8286088919511099, + "heading": 3.0189440548303175, + "angularVelocity": 4.5651885597350814e-7, + "velocityX": 3.529333457011711, + "velocityY": -2.7195358000448815, + "timestamp": 1.4286109744212807 + }, + { + "x": 4.466529379665209, + "y": 1.643601353456878, + "heading": 3.0189440858868863, + "angularVelocity": 4.565189266869802e-7, + "velocityX": 3.5293336624566067, + "velocityY": -2.7195355334244926, + "timestamp": 1.4966400684413417 + }, + { + "x": 4.706629767865023, + "y": 1.4585977299467376, + "heading": 3.0189441169435973, + "angularVelocity": 4.5652101213496753e-7, + "velocityX": 3.5293780059603734, + "velocityY": -2.7194779847514217, + "timestamp": 1.5646691624614026 + }, + { + "x": 4.958832142878476, + "y": 1.2904653444393646, + "heading": 3.0189441488124733, + "angularVelocity": 4.684595064516387e-7, + "velocityX": 3.707272287634489, + "velocityY": -2.4714776512795065, + "timestamp": 1.6326982564814636 + }, + { + "x": 5.22248738866272, + "y": 1.140931240984889, + "heading": 3.0189441823956185, + "angularVelocity": 4.936585621819922e-7, + "velocityX": 3.875624827614127, + "velocityY": -2.1980904730317214, + "timestamp": 1.7007273505015246 + }, + { + "x": 5.4962265319262364, + "y": 1.010772045584542, + "heading": 3.0189442291355726, + "angularVelocity": 6.870583117392439e-7, + "velocityX": 4.0238540172649335, + "velocityY": -1.9132872085870314, + "timestamp": 1.7687564445215855 + }, + { + "x": 5.777018253708926, + "y": 0.9011877519054788, + "heading": 3.0248033325009245, + "angularVelocity": 0.08612643531051656, + "velocityX": 4.127524051693051, + "velocityY": -1.6108445255312074, + "timestamp": 1.8367855385416465 + }, + { + "x": 6.05633409210439, + "y": 0.8114216921379119, + "heading": 3.058880871989868, + "angularVelocity": 0.5009259638074025, + "velocityX": 4.105829166460713, + "velocityY": -1.3195245513793832, + "timestamp": 1.9048146325617075 + }, + { + "x": 6.32467753606711, + "y": 0.7405027635497223, + "heading": 3.0964917239018948, + "angularVelocity": 0.5528642186670262, + "velocityX": 3.9445394331370727, + "velocityY": -1.0424793922329247, + "timestamp": 1.9728437265817684 + }, + { + "x": 6.579380074717612, + "y": 0.6865702377068368, + "heading": 3.129527002941156, + "angularVelocity": 0.48560515930904025, + "velocityX": 3.744023675743692, + "velocityY": -0.7927861839080411, + "timestamp": 2.0408728206018294 + }, + { + "x": 6.819417953491211, + "y": 0.648584246635437, + "heading": 3.1544674706525395, + "angularVelocity": 0.3666147266936806, + "velocityX": 3.528459142831067, + "velocityY": -0.5583786116598611, + "timestamp": 2.1089019146218906 + }, + { + "x": 7.056356249212082, + "y": 0.6255394234040195, + "heading": 3.169663027692689, + "angularVelocity": 0.21114304614497145, + "velocityX": 3.2922697979889413, + "velocityY": -0.3202090033354899, + "timestamp": 2.180869982325699 + }, + { + "x": 7.2727644426179525, + "y": 0.6154361557285976, + "heading": 3.176323379743751, + "angularVelocity": 0.09254593410057531, + "velocityX": 3.0070029710471062, + "velocityY": -0.1403854236715493, + "timestamp": 2.2528380500295073 + }, + { + "x": 7.466589513658325, + "y": 0.6144567823314835, + "heading": 3.176929612086991, + "angularVelocity": 0.008423629570475733, + "velocityX": 2.6932093249756126, + "velocityY": -0.013608443694012455, + "timestamp": 2.3248061177333157 + }, + { + "x": 7.6367535402426725, + "y": 0.6194648455903583, + "heading": 3.1735679082028514, + "angularVelocity": -0.04671104826623231, + "velocityX": 2.3644378960495933, + "velocityY": 0.06958729640325968, + "timestamp": 2.396774185437124 + }, + { + "x": 7.78273930823007, + "y": 0.6279831761643265, + "heading": 3.1678878851398564, + "angularVelocity": -0.07892421242114901, + "velocityX": 2.0284797500499288, + "velocityY": 0.11836264117894969, + "timestamp": 2.4687422531409324 + }, + { + "x": 7.904333300767225, + "y": 0.6380692684622451, + "heading": 3.1611775939336626, + "angularVelocity": -0.09323984122806457, + "velocityX": 1.689554776398703, + "velocityY": 0.1401467709183037, + "timestamp": 2.540710320844741 + }, + { + "x": 8.001482657352598, + "y": 0.6481882157692571, + "heading": 3.1544521899594713, + "angularVelocity": -0.09344983391620998, + "velocityX": 1.3498953033615035, + "velocityY": 0.14060329295844945, + "timestamp": 2.612678388548549 + }, + { + "x": 8.074218176420764, + "y": 0.6571108749361716, + "heading": 3.148524448427661, + "angularVelocity": -0.08236627327840042, + "velocityX": 1.0106637761557824, + "velocityY": 0.12398080776097292, + "timestamp": 2.6846464562523575 + }, + { + "x": 8.122612952729549, + "y": 0.6638382057961907, + "heading": 3.144055791313293, + "angularVelocity": -0.06209222029914048, + "velocityX": 0.6724479043672269, + "velocityY": 0.09347660809382861, + "timestamp": 2.756614523956166 + }, + { + "x": 8.146759986877441, + "y": 0.667546272277832, "heading": 3.141592653589793, - "angularVelocity": 2.5566523025657903, - "velocityX": 0.6231541275550934, - "velocityY": -0.1167157675019664, - "timestamp": 5.234421600642128 + "angularVelocity": -0.034225425276624886, + "velocityX": 0.3355242806750317, + "velocityY": 0.05152377436201764, + "timestamp": 2.8285825916599743 }, { - "x": 7.956855773925781, - "y": 0.7431064248085022, + "x": 8.146759986877441, + "y": 0.667546272277832, "heading": 3.141592653589793, - "angularVelocity": -3.8315823290727504e-21, - "velocityX": 3.876602036733102e-21, - "velocityY": -3.3558838471561212e-21, - "timestamp": 5.327893414939309 - }, - { - "x": 7.9018904258286256, - "y": 0.7580071883715975, - "heading": 2.929746437641678, - "angularVelocity": -2.3660710929042184, - "velocityX": -0.6138977779803758, - "velocityY": 0.166423864457769, - "timestamp": 5.417428432117549 - }, - { - "x": 7.77927283258943, - "y": 0.7936488654478715, - "heading": 2.8587142949291526, - "angularVelocity": -0.7933448269867456, - "velocityX": -1.3694931559023147, - "velocityY": 0.39807528048295576, - "timestamp": 5.506963449295789 - }, - { - "x": 7.643021297144732, - "y": 0.8351292518640833, - "heading": 2.8460714923728077, - "angularVelocity": -0.14120511677768, - "velocityX": -1.5217681275858608, - "velocityY": 0.4632867421428616, - "timestamp": 5.596498466474029 - }, - { - "x": 7.50584253248777, - "y": 0.8779834119507215, - "heading": 2.8388066609223235, - "angularVelocity": -0.08113955499692087, - "velocityX": -1.532124178676125, - "velocityY": 0.4786301654617149, - "timestamp": 5.686033483652269 - }, - { - "x": 7.368255234864718, - "y": 0.9216599007543135, - "heading": 2.8342113751430746, - "angularVelocity": -0.051323894539503874, - "velocityX": -1.536687007600088, - "velocityY": 0.487814602376678, - "timestamp": 5.775568500830508 - }, - { - "x": 7.230614869935536, - "y": 0.9659066250673984, - "heading": 2.8305583662971365, - "angularVelocity": -0.040799778243928046, - "velocityX": -1.5372797064993953, - "velocityY": 0.4941834570155044, - "timestamp": 5.865103518008748 - }, - { - "x": 7.093252862402945, - "y": 1.010681152263652, - "heading": 2.826475143484932, - "angularVelocity": -0.045604758237506364, - "velocityX": -1.5341707843663217, - "velocityY": 0.5000783895212758, - "timestamp": 5.954638535186988 - }, - { - "x": 6.956119170941034, - "y": 1.0557271520364861, - "heading": 2.821845381251112, - "angularVelocity": -0.051708955665952475, - "velocityX": -1.5316207645207218, - "velocityY": 0.5031104163766442, - "timestamp": 6.044173552365228 - }, - { - "x": 6.818992484234107, - "y": 1.100769539311748, - "heading": 2.817204104711705, - "angularVelocity": -0.0518375568093923, - "velocityX": -1.5315425297115504, - "velocityY": 0.5030700690613045, - "timestamp": 6.133708569543468 - }, - { - "x": 6.681872127416794, - "y": 1.1458071900530102, - "heading": 2.8125526096190336, - "angularVelocity": -0.05195168593547435, - "velocityX": -1.5314718323484902, - "velocityY": 0.5030171675915902, - "timestamp": 6.223243586721708 - }, - { - "x": 6.544758118625867, - "y": 1.1908399522969457, - "heading": 2.8078906701771453, - "angularVelocity": -0.052068336934676634, - "velocityX": -1.5314009324192217, - "velocityY": 0.5029625688716561, - "timestamp": 6.312778603899948 - }, - { - "x": 6.407650518790602, - "y": 1.2358677646327403, - "heading": 2.8032180092193197, - "angularVelocity": -0.05218808355755985, - "velocityX": -1.5313293519821691, - "velocityY": 0.5029072842657369, - "timestamp": 6.4023136210781875 - }, - { - "x": 6.270549391355044, - "y": 1.280890584680893, - "heading": 2.7985343668363263, - "angularVelocity": -0.05231073305843581, - "velocityX": -1.531257062950314, - "velocityY": 0.5028515263310278, - "timestamp": 6.491848638256427 - }, - { - "x": 6.1334547976207645, - "y": 1.3259083749515967, - "heading": 2.793839500454244, - "angularVelocity": -0.05243609182244443, - "velocityX": -1.5311840892526187, - "velocityY": 0.5027953496796241, - "timestamp": 6.581383655434667 - }, - { - "x": 5.996366796772198, - "y": 1.3709210989414258, - "heading": 2.7891331795005336, - "angularVelocity": -0.05256402580837642, - "velocityX": -1.5311104545349274, - "velocityY": 0.5027387653282189, - "timestamp": 6.670918672612907 - }, - { - "x": 5.8592854465592445, - "y": 1.4159287198651518, - "heading": 2.784415180924841, - "angularVelocity": -0.052694451002343774, - "velocityX": -1.5310361748193169, - "velocityY": 0.5026817701294257, - "timestamp": 6.760453689791147 - }, - { - "x": 5.722210803960778, - "y": 1.4609312002186288, - "heading": 2.7796852859248706, - "angularVelocity": -0.05282731995856272, - "velocityX": -1.530961258717216, - "velocityY": 0.5026243560537855, - "timestamp": 6.849988706969387 - }, - { - "x": 5.585142925732609, - "y": 1.5059285016384842, - "heading": 2.7749432775568774, - "angularVelocity": -0.052962611919236485, - "velocityX": -1.5308857087200383, - "velocityY": 0.5025665135047443, - "timestamp": 6.939523724147627 - }, - { - "x": 5.448081868853608, - "y": 1.5509205848763696, - "heading": 2.7701889389309033, - "angularVelocity": -0.05310032628362197, - "velocityX": -1.5308095223363887, - "velocityY": 0.502508232598188, - "timestamp": 7.0290587413258665 - }, - { - "x": 5.311027690899557, - "y": 1.5959074098195543, - "heading": 2.7654220517793227, - "angularVelocity": -0.05324047843863795, - "velocityX": -1.5307326928995175, - "velocityY": 0.502449503679975, - "timestamp": 7.118593758504106 - }, - { - "x": 5.173980450370318, - "y": 1.6408889355303347, - "heading": 2.7606423952580355, - "angularVelocity": -0.05338309716043176, - "velocityX": -1.5306552100885358, - "velocityY": 0.5023903175361529, - "timestamp": 7.208128775682346 - }, - { - "x": 5.0369402069873335, - "y": 1.6858651202909567, - "heading": 2.7558497448937236, - "angularVelocity": -0.05352822298309391, - "velocityX": -1.5305770602598503, - "velocityY": 0.5023306654544649, - "timestamp": 7.297663792860586 - }, - { - "x": 4.899907021964721, - "y": 1.7308359216420997, - "heading": 2.751043871646217, - "angularVelocity": -0.05367590691292534, - "velocityX": -1.5304982267420264, - "velocityY": 0.5022705391525029, - "timestamp": 7.387198810038826 - }, - { - "x": 4.762880958231051, - "y": 1.7758012963934744, - "heading": 2.7462245411420234, - "angularVelocity": -0.05382620851683086, - "velocityX": -1.5304186903866774, - "velocityY": 0.5022099304662124, - "timestamp": 7.476733827217066 - }, - { - "x": 4.625862080529766, - "y": 1.8207612005630536, - "heading": 2.7413915132887583, - "angularVelocity": -0.053979191668033985, - "velocityX": -1.5303384309238324, - "velocityY": 0.5021488305528153, - "timestamp": 7.566268844395306 - }, - { - "x": 4.488850455262, - "y": 1.8657155891757824, - "heading": 2.736544542679806, - "angularVelocity": -0.054134915720223636, - "velocityX": -1.5302574298390257, - "velocityY": 0.5020872283213718, - "timestamp": 7.6558038615735455 - }, - { - "x": 4.351846149940031, - "y": 1.910664415870897, - "heading": 2.7316833801907157, - "angularVelocity": -0.05429342219718387, - "velocityX": -1.5301756747221047, - "velocityY": 0.5020251082951581, - "timestamp": 7.745338878751785 - }, - { - "x": 4.214849232355917, - "y": 1.955607632403858, - "heading": 2.7268077754532425, - "angularVelocity": -0.05445472499064026, - "velocityX": -1.530093162448295, - "velocityY": 0.5019624494346333, - "timestamp": 7.834873895930025 - }, - { - "x": 4.077859769965423, - "y": 2.0005451883147503, - "heading": 2.721917478680163, - "angularVelocity": -0.05461881761126215, - "velocityX": -1.5300098967735323, - "velocityY": 0.5018992269966714, - "timestamp": 7.924408913108265 - }, - { - "x": 3.940877826047202, - "y": 2.045477029542233, - "heading": 2.717012252912238, - "angularVelocity": -0.05478555678567875, - "velocityX": -1.5299259243512187, - "velocityY": 0.5018354007576298, - "timestamp": 8.013943930286505 - }, - { - "x": 3.8039034641003533, - "y": 2.0904031002081847, - "heading": 2.712091859898031, - "angularVelocity": -0.05495495694618896, - "velocityX": -1.5298412427192656, - "velocityY": 0.5017709504262021, - "timestamp": 8.103478947464744 - }, - { - "x": 3.66693674900291, - "y": 2.135323343068443, - "heading": 2.707156056414113, - "angularVelocity": -0.05512707362408464, - "velocityX": -1.5297558364765886, - "velocityY": 0.501705860745354, - "timestamp": 8.193013964642983 - }, - { - "x": 3.5299779535424887, - "y": 2.1802377886608806, - "heading": 2.7022039564715556, - "angularVelocity": -0.05530908574796876, - "velocityX": -1.5296673835196157, - "velocityY": 0.5016411121363245, - "timestamp": 8.282548981821222 - }, - { - "x": 3.426502516345769, - "y": 2.233179068985383, - "heading": 2.594355857349509, - "angularVelocity": -1.2045354155386023, - "velocityX": -1.1556979655315056, - "velocityY": 0.5912913404496402, - "timestamp": 8.37208399899946 - }, - { - "x": 3.387338399887085, - "y": 2.2590601444244385, - "heading": 2.3477629859714533, - "angularVelocity": -2.7541500426269785, - "velocityX": -0.43741675260662705, - "velocityY": 0.28906093118330495, - "timestamp": 8.4616190161777 - }, - { - "x": 3.387338399887085, - "y": 2.2590601444244385, - "heading": 2.3477629859714533, - "angularVelocity": -2.400686558954734e-26, - "velocityX": 2.2132770883951947e-26, - "velocityY": 2.6443990744659133e-26, - "timestamp": 8.551154033355939 + "angularVelocity": 6.15835594457233e-23, + "velocityX": 1.29593094240872e-22, + "velocityY": -4.905397235503692e-22, + "timestamp": 2.9005506593637826 + }, + { + "x": 8.116302316915732, + "y": 0.6788007824238615, + "heading": 3.1342355794344465, + "angularVelocity": -0.08856487332455099, + "velocityX": -0.36665114758415307, + "velocityY": 0.13548242743870734, + "timestamp": 2.98362054790059 + }, + { + "x": 8.055384628279041, + "y": 0.7013126982960538, + "heading": 3.119642832006715, + "angularVelocity": -0.17566831597788424, + "velocityX": -0.7333305691110794, + "velocityY": 0.2709997120342536, + "timestamp": 3.0666904364373972 + }, + { + "x": 7.964004124464221, + "y": 0.7350855445347456, + "heading": 3.09796265201497, + "angularVelocity": -0.26098722862904256, + "velocityX": -1.1000436551004955, + "velocityY": 0.40655942644882115, + "timestamp": 3.1497603249742046 + }, + { + "x": 7.842157398019484, + "y": 0.7801237085031743, + "heading": 3.0693814277169014, + "angularVelocity": -0.34406238892934254, + "velocityX": -1.4667977601865743, + "velocityY": 0.542171956189308, + "timestamp": 3.232830213511012 + }, + { + "x": 7.689840193239616, + "y": 0.8364327959906936, + "heading": 3.0341415239312566, + "angularVelocity": -0.424219947881965, + "velocityX": -1.8336030956918719, + "velocityY": 0.6778519687379819, + "timestamp": 3.315900102047819 + }, + { + "x": 7.50704705480538, + "y": 0.904020204266213, + "heading": 2.992571084782186, + "angularVelocity": -0.5004272893739545, + "velocityX": -2.2004741026376022, + "velocityY": 0.8136210300266904, + "timestamp": 3.3989699905846265 + }, + { + "x": 7.293770807150263, + "y": 0.982896119722086, + "heading": 2.9451379766381653, + "angularVelocity": -0.5710024291534157, + "velocityX": -2.567431489457423, + "velocityY": 0.9495127156806547, + "timestamp": 3.4820398791214338 + }, + { + "x": 7.050001837904705, + "y": 1.073075442966285, + "heading": 2.892559152885557, + "angularVelocity": -0.6329468436605835, + "velocityX": -2.934504590523767, + "velocityY": 1.0855837756931817, + "timestamp": 3.565109767658241 + }, + { + "x": 6.775727633239132, + "y": 1.1745820471884034, + "heading": 2.8360637721730617, + "angularVelocity": -0.6800945770796716, + "velocityX": -3.301728333775782, + "velocityY": 1.2219422225075236, + "timestamp": 3.6481796561950484 + }, + { + "x": 6.470938505156052, + "y": 1.2874600105843308, + "heading": 2.7782378900044695, + "angularVelocity": -0.6961112285948248, + "velocityX": -3.6690687016876415, + "velocityY": 1.358831285128218, + "timestamp": 3.7312495447318557 + }, + { + "x": 6.135781167752857, + "y": 1.4117970937816189, + "heading": 2.7282295068076383, + "angularVelocity": -0.6020037353808817, + "velocityX": -4.034642917026322, + "velocityY": 1.4967768151295402, + "timestamp": 3.814319433268663 + }, + { + "x": 5.78853790859181, + "y": 1.5399120528524686, + "heading": 2.728229496122602, + "angularVelocity": -1.2862706865047314e-7, + "velocityX": -4.180133924306256, + "velocityY": 1.542255097815422, + "timestamp": 3.8973893218054703 + }, + { + "x": 5.441294649641329, + "y": 1.6680270124940895, + "heading": 2.728229485437636, + "angularVelocity": -1.286262206803139e-7, + "velocityX": -4.180133921771438, + "velocityY": 1.5422551046863975, + "timestamp": 3.9804592103422776 + }, + { + "x": 5.094051397734534, + "y": 1.7961419912269363, + "heading": 2.7282294747526707, + "angularVelocity": -1.286262181377533e-7, + "velocityX": -4.180133836979181, + "velocityY": 1.5422553345076508, + "timestamp": 4.063529098879084 + }, + { + "x": 4.746808156293299, + "y": 1.9242569983256745, + "heading": 2.7282294640677045, + "angularVelocity": -1.2862622783165573e-7, + "velocityX": -4.180133710994155, + "velocityY": 1.5422556759778505, + "timestamp": 4.146598987415891 + }, + { + "x": 4.399565062124353, + "y": 2.0523724045908915, + "heading": 2.7282294533827316, + "angularVelocity": -1.286263045468436e-7, + "velocityX": -4.180131938122049, + "velocityY": 1.5422604811664191, + "timestamp": 4.229668875952698 + }, + { + "x": 4.059443344314116, + "y": 2.1983468564274284, + "heading": 2.728229437713722, + "angularVelocity": -1.8862441726861322e-7, + "velocityX": -4.094404408157266, + "velocityY": 1.7572486782843997, + "timestamp": 4.312738764489505 + }, + { + "x": 3.7193219629342615, + "y": 2.3443220921470425, + "heading": 2.7282294220445418, + "angularVelocity": -1.8862647326726582e-7, + "velocityX": -4.094400358189376, + "velocityY": 1.7572581147130661, + "timestamp": 4.395808653026312 + }, + { + "x": 3.395818553566254, + "y": 2.4935219406512408, + "heading": 2.6724500576638754, + "angularVelocity": -0.6714751321226504, + "velocityX": -3.89435227452686, + "velocityY": 1.7960761851520912, + "timestamp": 4.478878541563119 + }, + { + "x": 3.101026535812032, + "y": 2.6290188837333135, + "heading": 2.59592195667292, + "angularVelocity": -0.9212495928298584, + "velocityX": -3.548722924114715, + "velocityY": 1.631119837388922, + "timestamp": 4.561948430099926 + }, + { + "x": 2.835772833375676, + "y": 2.750798938045291, + "heading": 2.519084794831671, + "angularVelocity": -0.9249700847642709, + "velocityX": -3.1931390195476896, + "velocityY": 1.4659951572947918, + "timestamp": 4.645018318636732 + }, + { + "x": 2.6000574792867495, + "y": 2.8589475580822414, + "heading": 2.4467191944954414, + "angularVelocity": -0.8711411753509879, + "velocityX": -2.8375547173689126, + "velocityY": 1.301899183228487, + "timestamp": 4.728088207173539 + }, + { + "x": 2.393855398523055, + "y": 2.953514187395407, + "heading": 2.3809962926356145, + "angularVelocity": -0.7911759967115644, + "velocityX": -2.4822722697205672, + "velocityY": 1.1383984134186498, + "timestamp": 4.811158095710346 + }, + { + "x": 2.2171452899215467, + "y": 3.0345296096879184, + "heading": 2.3231458932555134, + "angularVelocity": -0.6964063681687525, + "velocityX": -2.1272462466734887, + "velocityY": 0.9752682195620702, + "timestamp": 4.894227984247153 + }, + { + "x": 2.0699108508844217, + "y": 3.102014617448754, + "heading": 2.273952675680828, + "angularVelocity": -0.592190725678996, + "velocityX": -1.7724164752175795, + "velocityY": 0.8123883268596626, + "timestamp": 4.97729787278396 + }, + { + "x": 1.95213960106252, + "y": 3.155984197182567, + "heading": 2.2339585252158036, + "angularVelocity": -0.4814518373562503, + "velocityX": -1.4177369429082447, + "velocityY": 0.6496888425449069, + "timestamp": 5.060367761320767 + }, + { + "x": 1.8638217687898557, + "y": 3.1964496924625165, + "heading": 2.203560612114051, + "angularVelocity": -0.36593179113613356, + "velocityX": -1.0631750424647786, + "velocityY": 0.48712591280317163, + "timestamp": 5.143437649857574 + }, + { + "x": 1.8049494737197318, + "y": 3.223419994217748, + "heading": 2.1830643911586045, + "angularVelocity": -0.24673466302250652, + "velocityX": -0.7087080060789848, + "velocityY": 0.3246700125675676, + "timestamp": 5.22650753839438 + }, + { + "x": 1.775516152381897, + "y": 3.236902236938477, + "heading": 2.1727145819710088, + "angularVelocity": -0.12459158631240587, + "velocityX": -0.35431998111798746, + "velocityY": 0.16229999772727494, + "timestamp": 5.309577426931187 + }, + { + "x": 1.775516152381897, + "y": 3.236902236938477, + "heading": 2.1727145819710088, + "angularVelocity": 3.884064768338255e-24, + "velocityX": -1.446989921221479e-23, + "velocityY": -2.0280809780864772e-23, + "timestamp": 5.392647315467994 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceWing1.1.traj b/src/main/deploy/choreo/SourceWing1.1.traj index 0cfce415..ef59a618 100644 --- a/src/main/deploy/choreo/SourceWing1.1.traj +++ b/src/main/deploy/choreo/SourceWing1.1.traj @@ -4,181 +4,181 @@ "x": 0.7885606288909912, "y": 4.587131977081299, "heading": 2.068139227089701, - "angularVelocity": -4.000873155660283e-28, - "velocityX": -1.1169568989245428e-27, - "velocityY": -1.0444692137856757e-26, + "angularVelocity": 7.405153834607566e-23, + "velocityX": -9.165802676126164e-23, + "velocityY": 1.4488206597403162e-23, "timestamp": 0 }, { - "x": 0.8077628028564999, - "y": 4.577523858678819, - "heading": 2.29346899836082, - "angularVelocity": 2.9086898452542567, - "velocityX": 0.24787300899124248, - "velocityY": -0.12402727021663285, - "timestamp": 0.07746778902493198 - }, - { - "x": 0.8565084293998517, - "y": 4.542709239371979, - "heading": 2.521391906348417, - "angularVelocity": 2.942163586393862, - "velocityX": 0.6292373534458789, - "velocityY": -0.44940767956648386, - "timestamp": 0.15493557804986396 - }, - { - "x": 0.9320047259554965, - "y": 4.504567603535149, - "heading": 2.6737831770723717, - "angularVelocity": 1.96715657748938, - "velocityX": 0.9745508101612039, - "velocityY": -0.492354775022096, - "timestamp": 0.23240336707479595 - }, - { - "x": 1.0259716426537533, - "y": 4.468035910816095, - "heading": 2.7708019811352167, - "angularVelocity": 1.252376055700012, - "velocityX": 1.2129804900978245, - "velocityY": -0.4715726778686935, - "timestamp": 0.30987115609972793 - }, - { - "x": 1.1315666128977582, - "y": 4.434097705174351, - "heading": 2.8306190294366744, - "angularVelocity": 0.7721538081099244, - "velocityX": 1.3630822768159876, - "velocityY": -0.43809441406442845, - "timestamp": 0.38733894512465994 - }, - { - "x": 1.2441769248956835, - "y": 4.4023522435792275, - "heading": 2.866815836119521, - "angularVelocity": 0.46724977101382253, - "velocityX": 1.4536404538624839, - "velocityY": -0.4097891781177288, - "timestamp": 0.46480673414959195 - }, - { - "x": 1.360949666856534, - "y": 4.372150081143396, - "heading": 2.8884951991985233, - "angularVelocity": 0.2798500299527736, - "velocityX": 1.5073715595945123, - "velocityY": -0.3898673605633771, - "timestamp": 0.542274523174524 - }, - { - "x": 1.483714050289093, - "y": 4.344503608621978, - "heading": 2.888502422435885, - "angularVelocity": 0.00009324181640315531, - "velocityX": 1.584715208446818, - "velocityY": -0.356877004873872, - "timestamp": 0.619742312199456 - }, - { - "x": 1.6064784345242633, - "y": 4.316857140064094, - "heading": 2.88850964608229, - "angularVelocity": 0.00009324709657913404, - "velocityX": 1.5847152188073954, - "velocityY": -0.3568769537102325, - "timestamp": 0.697210101224388 - }, - { - "x": 1.729242818618949, - "y": 4.289210671426577, - "heading": 2.8885168702684654, - "angularVelocity": 0.00009325406426226353, - "velocityX": 1.5847152169939396, - "velocityY": -0.3568769547381692, - "timestamp": 0.77467789024932 - }, - { - "x": 1.8520072025730983, - "y": 4.261564202709402, - "heading": 2.888524094994609, - "angularVelocity": 0.00009326103448260538, - "velocityX": 1.5847152151798087, - "velocityY": -0.3568769557664531, - "timestamp": 0.852145679274252 - }, - { - "x": 1.9747715863866588, - "y": 4.233917733912545, - "heading": 2.8885313202609173, - "angularVelocity": 0.00009326800724451237, - "velocityX": 1.5847152133650135, - "velocityY": -0.35687695679502923, - "timestamp": 0.929613468299184 - }, - { - "x": 2.09753597005958, - "y": 4.206271265035981, - "heading": 2.8885385460675863, - "angularVelocity": 0.00009327498254687209, - "velocityX": 1.584715211549548, - "velocityY": -0.35687695782393064, - "timestamp": 1.007081257324116 - }, - { - "x": 2.2203003530348537, - "y": 4.178624793690813, - "heading": 2.888545772490986, - "angularVelocity": 0.00009328294366610178, - "velocityX": 1.5847152025439089, - "velocityY": -0.3568769896901373, - "timestamp": 1.084549046349048 - }, - { - "x": 2.339535767958966, - "y": 4.149427051618625, - "heading": 2.9014383733628053, - "angularVelocity": 0.16642531088204846, - "velocityX": 1.5391611975105928, - "velocityY": -0.37690170895145225, - "timestamp": 1.16201683537398 - }, - { - "x": 2.4602156964640742, - "y": 4.1208550801313235, - "heading": 2.9091068110983676, - "angularVelocity": 0.0989887259218709, - "velocityX": 1.5578078324433486, - "velocityY": -0.36882389244523134, - "timestamp": 1.239484624398912 - }, - { - "x": 2.563006485689252, - "y": 4.105677497528831, - "heading": 2.9949971538769358, - "angularVelocity": 1.1087233011248803, - "velocityX": 1.3268842511059193, - "velocityY": -0.1959212053619926, - "timestamp": 1.316952413423844 + "x": 0.8092723820430497, + "y": 4.581756141058556, + "heading": 2.0914655240689815, + "angularVelocity": 0.3351714697575674, + "velocityX": 0.2976035481069952, + "velocityY": -0.0772444448648893, + "timestamp": 0.06959511499040384 + }, + { + "x": 0.8507515012715718, + "y": 4.570851691637496, + "heading": 2.1374460572115312, + "angularVelocity": 0.6606862155323626, + "velocityX": 0.59600618857002, + "velocityY": -0.15668412104160437, + "timestamp": 0.13919022998080768 + }, + { + "x": 0.9130744655083555, + "y": 4.554273162378978, + "heading": 2.20529724746531, + "angularVelocity": 0.9749418513517004, + "velocityX": 0.8955077413892276, + "velocityY": -0.23821397896682367, + "timestamp": 0.20878534497121154 + }, + { + "x": 0.9963957154773515, + "y": 4.531931817223244, + "heading": 2.2937212565875593, + "angularVelocity": 1.2705490771075152, + "velocityX": 1.1972284258814936, + "velocityY": -0.32101886977002797, + "timestamp": 0.27838045996161537 + }, + { + "x": 1.1010580704953445, + "y": 4.503787887410387, + "heading": 2.3994943696463134, + "angularVelocity": 1.5198353084600695, + "velocityX": 1.503875021004259, + "velocityY": -0.40439519090868825, + "timestamp": 0.3479755749520192 + }, + { + "x": 1.227655866348761, + "y": 4.469686827398336, + "heading": 2.513230323560048, + "angularVelocity": 1.634251971987077, + "velocityX": 1.8190615227932214, + "velocityY": -0.48999214983341605, + "timestamp": 0.417570689942423 + }, + { + "x": 1.3716687344293004, + "y": 4.429078416516423, + "heading": 2.5833875612817048, + "angularVelocity": 1.0080770429265136, + "velocityX": 2.0692956409425562, + "velocityY": -0.5834951330637533, + "timestamp": 0.48716580493282685 + }, + { + "x": 1.5310835689434033, + "y": 4.382853965770288, + "heading": 2.5991130952622044, + "angularVelocity": 0.22595743943619906, + "velocityX": 2.290603795052096, + "velocityY": -0.6641910247940317, + "timestamp": 0.5567609199232307 + }, + { + "x": 1.69623233687307, + "y": 4.340284063453759, + "heading": 2.5991131342417417, + "angularVelocity": 5.600901392666899e-7, + "velocityX": 2.372993678542496, + "velocityY": -0.6116794594333097, + "timestamp": 0.6263560349136346 + }, + { + "x": 1.8613811081667575, + "y": 4.297714174187834, + "heading": 2.5991131732212707, + "angularVelocity": 5.600900148939747e-7, + "velocityX": 2.372993726879466, + "velocityY": -0.6116792719114585, + "timestamp": 0.6959511499040384 + }, + { + "x": 2.0217130268983476, + "y": 4.252932737260293, + "heading": 2.613201494408751, + "angularVelocity": 0.20243261598781087, + "velocityX": 2.3037812173123697, + "velocityY": -0.6434566123457787, + "timestamp": 0.7655462648944422 + }, + { + "x": 2.16688524944999, + "y": 4.21338989219855, + "heading": 2.683886289405167, + "angularVelocity": 1.015657420871611, + "velocityX": 2.0859542019819104, + "velocityY": -0.5681842046987992, + "timestamp": 0.835141379884846 + }, + { + "x": 2.294050574677846, + "y": 4.179894439044184, + "heading": 2.796544364902464, + "angularVelocity": 1.6187641260860197, + "velocityX": 1.8272162528273361, + "velocityY": -0.48129029112149246, + "timestamp": 0.9047364948752499 + }, + { + "x": 2.3992728564214008, + "y": 4.152626972084835, + "heading": 2.9035177926378313, + "angularVelocity": 1.5370824195077049, + "velocityX": 1.5119205099102537, + "velocityY": -0.3918014499021688, + "timestamp": 0.9743316098656537 + }, + { + "x": 2.4829722545309574, + "y": 4.131221801674751, + "heading": 2.995152785659621, + "angularVelocity": 1.316687141538951, + "velocityX": 1.2026619701841783, + "velocityY": -0.3075671390590503, + "timestamp": 1.0439267248560575 + }, + { + "x": 2.5454789720874493, + "y": 4.115415801961452, + "heading": 3.0669003824978076, + "angularVelocity": 1.0309286341157697, + "velocityX": 0.8981480606090146, + "velocityY": -0.2271136374367465, + "timestamp": 1.1135218398464615 + }, + { + "x": 2.5870063195907287, + "y": 4.1050110004584175, + "heading": 3.1162558708577963, + "angularVelocity": 0.7091803550693766, + "velocityX": 0.5966991721906145, + "velocityY": -0.14950476774798632, + "timestamp": 1.1831169548368654 }, { "x": 2.6077051162719727, "y": 4.099861145019531, "heading": 3.141592653589793, - "angularVelocity": 1.8923413402914784, - "velocityX": 0.5769963380306965, - "velocityY": -0.07508091533924856, - "timestamp": 1.394420202448776 + "angularVelocity": 0.36405978689007856, + "velocityX": 0.29741737885042957, + "velocityY": -0.07399736949348479, + "timestamp": 1.2527120698272693 }, { "x": 2.6077051162719727, "y": 4.099861145019531, "heading": 3.141592653589793, - "angularVelocity": 9.529530648984287e-27, - "velocityX": -6.64992161175884e-27, - "velocityY": -1.7807748804280264e-26, - "timestamp": 1.471887991473708 + "angularVelocity": -7.013511745248326e-23, + "velocityX": -2.566963461380566e-23, + "velocityY": 1.8780462981611305e-24, + "timestamp": 1.3223071848176733 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceWing1.2.traj b/src/main/deploy/choreo/SourceWing1.2.traj index 6a290671..6d4a871b 100644 --- a/src/main/deploy/choreo/SourceWing1.2.traj +++ b/src/main/deploy/choreo/SourceWing1.2.traj @@ -4,82 +4,91 @@ "x": 2.6077051162719727, "y": 4.099861145019531, "heading": 3.141592653589793, - "angularVelocity": 9.529530648984287e-27, - "velocityX": -6.64992161175884e-27, - "velocityY": -1.7807748804280264e-26, + "angularVelocity": -7.013511745248326e-23, + "velocityX": -2.566963461380566e-23, + "velocityY": 1.8780462981611305e-24, "timestamp": 0 }, { - "x": 2.582723056141482, - "y": 4.106470038143139, - "heading": 3.0254564505751103, - "angularVelocity": -1.8328957625774476, - "velocityX": -0.39427423116151483, - "velocityY": 0.10430349785119646, - "timestamp": 0.06336214278294272 + "x": 2.58482285126615, + "y": 4.108903526368604, + "heading": 3.1172534062149904, + "angularVelocity": -0.328671725346054, + "velocityX": -0.3089969629494254, + "velocityY": 0.12210628510695666, + "timestamp": 0.07405336540335616 }, { - "x": 2.5176865762372205, - "y": 4.122909182686089, - "heading": 2.894011234282021, - "angularVelocity": -2.0745071192332674, - "velocityX": -1.0264248816056334, - "velocityY": 0.25944742114017705, - "timestamp": 0.12672428556588544 + "x": 2.5390877283653834, + "y": 4.12699559592087, + "heading": 3.068221389953176, + "angularVelocity": -0.6621173257251091, + "velocityX": -0.6175968188839946, + "velocityY": 0.2443112403294962, + "timestamp": 0.14810673080671233 }, { - "x": 2.4297948421318796, - "y": 4.1567674114258075, - "heading": 2.8608182151959642, - "angularVelocity": -0.5238620038429632, - "velocityX": -1.3871332351626442, - "velocityY": 0.5343605385269978, - "timestamp": 0.19008642834882816 + "x": 2.470551207754016, + "y": 4.154160087682156, + "heading": 2.993960294612306, + "angularVelocity": -1.0028051383807046, + "velocityX": -0.9255017680569747, + "velocityY": 0.3668231904562775, + "timestamp": 0.2221600962100685 }, { - "x": 2.336899878456132, - "y": 4.201089714349744, - "heading": 2.8608175146596406, - "angularVelocity": -0.00001105607059469762, - "velocityX": -1.4660956778872583, - "velocityY": 0.6995076393765645, - "timestamp": 0.2534485711317709 + "x": 2.379281332174413, + "y": 4.190453494467717, + "heading": 2.893905888815326, + "angularVelocity": -1.3511122047188575, + "velocityX": -1.232487883331929, + "velocityY": 0.49009800686138194, + "timestamp": 0.29621346161342466 }, { - "x": 2.2457294091191753, - "y": 4.241992503114134, - "heading": 2.8495777745026225, - "angularVelocity": -0.17738888969588018, - "velocityX": -1.4388792003022337, - "velocityY": 0.6455398597315226, - "timestamp": 0.3168107139147136 + "x": 2.2887269490195696, + "y": 4.227798636551948, + "heading": 2.7901909299452776, + "angularVelocity": -1.4005434905642753, + "velocityX": -1.22282603446329, + "velocityY": 0.5043004039157333, + "timestamp": 0.3702668270167808 }, { - "x": 2.180218037779224, - "y": 4.270414402297769, - "heading": 2.7322527229675986, - "angularVelocity": -1.8516585200872955, - "velocityX": -1.033919758117567, - "velocityY": 0.44856278426375645, - "timestamp": 0.3801728566976563 + "x": 2.220820804164644, + "y": 4.255857671323432, + "heading": 2.7125381341765133, + "angularVelocity": -1.0486058985409061, + "velocityX": -0.9169893155435849, + "velocityY": 0.37890289818224415, + "timestamp": 0.444320192420137 + }, + { + "x": 2.1755524378616315, + "y": 4.274580208282235, + "heading": 2.660825121415167, + "angularVelocity": -0.6983208997953653, + "velocityX": -0.6112938427099196, + "velocityY": 0.25282493046506116, + "timestamp": 0.5183735578234931 }, { "x": 2.152919054031372, "y": 4.283941268920898, "heading": 2.634960813022188, - "angularVelocity": -1.5354895789856624, - "velocityX": -0.4308406021142514, - "velocityY": 0.21348499323117995, - "timestamp": 0.44353499948059905 + "angularVelocity": -0.34926580651804395, + "velocityX": -0.3056361275004237, + "velocityY": 0.12640965859786993, + "timestamp": 0.5924269232268493 }, { "x": 2.152919054031372, "y": 4.283941268920898, "heading": 2.634960813022188, - "angularVelocity": 1.4290307836372668e-27, - "velocityX": -1.821728341071586e-27, - "velocityY": 7.158150670810205e-27, - "timestamp": 0.5068971422635418 + "angularVelocity": -3.288439423134819e-23, + "velocityX": 3.096618824782888e-23, + "velocityY": -2.3851261489110276e-23, + "timestamp": 0.6664802886302055 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceWing1.traj b/src/main/deploy/choreo/SourceWing1.traj index 776fd957..9ddf700d 100644 --- a/src/main/deploy/choreo/SourceWing1.traj +++ b/src/main/deploy/choreo/SourceWing1.traj @@ -4,253 +4,262 @@ "x": 0.7885606288909912, "y": 4.587131977081299, "heading": 2.068139227089701, - "angularVelocity": -4.000873155660283e-28, - "velocityX": -1.1169568989245428e-27, - "velocityY": -1.0444692137856757e-26, + "angularVelocity": 7.405153834607566e-23, + "velocityX": -9.165802676126164e-23, + "velocityY": 1.4488206597403162e-23, "timestamp": 0 }, { - "x": 0.8077628028564999, - "y": 4.577523858678819, - "heading": 2.29346899836082, - "angularVelocity": 2.9086898452542567, - "velocityX": 0.24787300899124248, - "velocityY": -0.12402727021663285, - "timestamp": 0.07746778902493198 + "x": 0.8092723820430497, + "y": 4.581756141058556, + "heading": 2.0914655240689815, + "angularVelocity": 0.3351714697575674, + "velocityX": 0.2976035481069952, + "velocityY": -0.0772444448648893, + "timestamp": 0.06959511499040384 }, { - "x": 0.8565084293998517, - "y": 4.542709239371979, - "heading": 2.521391906348417, - "angularVelocity": 2.942163586393862, - "velocityX": 0.6292373534458789, - "velocityY": -0.44940767956648386, - "timestamp": 0.15493557804986396 + "x": 0.8507515012715718, + "y": 4.570851691637496, + "heading": 2.1374460572115312, + "angularVelocity": 0.6606862155323626, + "velocityX": 0.59600618857002, + "velocityY": -0.15668412104160437, + "timestamp": 0.13919022998080768 }, { - "x": 0.9320047259554965, - "y": 4.504567603535149, - "heading": 2.6737831770723717, - "angularVelocity": 1.96715657748938, - "velocityX": 0.9745508101612039, - "velocityY": -0.492354775022096, - "timestamp": 0.23240336707479595 + "x": 0.9130744655083555, + "y": 4.554273162378978, + "heading": 2.20529724746531, + "angularVelocity": 0.9749418513517004, + "velocityX": 0.8955077413892276, + "velocityY": -0.23821397896682367, + "timestamp": 0.20878534497121154 }, { - "x": 1.0259716426537533, - "y": 4.468035910816095, - "heading": 2.7708019811352167, - "angularVelocity": 1.252376055700012, - "velocityX": 1.2129804900978245, - "velocityY": -0.4715726778686935, - "timestamp": 0.30987115609972793 + "x": 0.9963957154773515, + "y": 4.531931817223244, + "heading": 2.2937212565875593, + "angularVelocity": 1.2705490771075152, + "velocityX": 1.1972284258814936, + "velocityY": -0.32101886977002797, + "timestamp": 0.27838045996161537 }, { - "x": 1.1315666128977582, - "y": 4.434097705174351, - "heading": 2.8306190294366744, - "angularVelocity": 0.7721538081099244, - "velocityX": 1.3630822768159876, - "velocityY": -0.43809441406442845, - "timestamp": 0.38733894512465994 + "x": 1.1010580704953445, + "y": 4.503787887410387, + "heading": 2.3994943696463134, + "angularVelocity": 1.5198353084600695, + "velocityX": 1.503875021004259, + "velocityY": -0.40439519090868825, + "timestamp": 0.3479755749520192 }, { - "x": 1.2441769248956835, - "y": 4.4023522435792275, - "heading": 2.866815836119521, - "angularVelocity": 0.46724977101382253, - "velocityX": 1.4536404538624839, - "velocityY": -0.4097891781177288, - "timestamp": 0.46480673414959195 + "x": 1.227655866348761, + "y": 4.469686827398336, + "heading": 2.513230323560048, + "angularVelocity": 1.634251971987077, + "velocityX": 1.8190615227932214, + "velocityY": -0.48999214983341605, + "timestamp": 0.417570689942423 }, { - "x": 1.360949666856534, - "y": 4.372150081143396, - "heading": 2.8884951991985233, - "angularVelocity": 0.2798500299527736, - "velocityX": 1.5073715595945123, - "velocityY": -0.3898673605633771, - "timestamp": 0.542274523174524 + "x": 1.3716687344293004, + "y": 4.429078416516423, + "heading": 2.5833875612817048, + "angularVelocity": 1.0080770429265136, + "velocityX": 2.0692956409425562, + "velocityY": -0.5834951330637533, + "timestamp": 0.48716580493282685 }, { - "x": 1.483714050289093, - "y": 4.344503608621978, - "heading": 2.888502422435885, - "angularVelocity": 0.00009324181640315531, - "velocityX": 1.584715208446818, - "velocityY": -0.356877004873872, - "timestamp": 0.619742312199456 + "x": 1.5310835689434033, + "y": 4.382853965770288, + "heading": 2.5991130952622044, + "angularVelocity": 0.22595743943619906, + "velocityX": 2.290603795052096, + "velocityY": -0.6641910247940317, + "timestamp": 0.5567609199232307 }, { - "x": 1.6064784345242633, - "y": 4.316857140064094, - "heading": 2.88850964608229, - "angularVelocity": 0.00009324709657913404, - "velocityX": 1.5847152188073954, - "velocityY": -0.3568769537102325, - "timestamp": 0.697210101224388 + "x": 1.69623233687307, + "y": 4.340284063453759, + "heading": 2.5991131342417417, + "angularVelocity": 5.600901392666899e-7, + "velocityX": 2.372993678542496, + "velocityY": -0.6116794594333097, + "timestamp": 0.6263560349136346 }, { - "x": 1.729242818618949, - "y": 4.289210671426577, - "heading": 2.8885168702684654, - "angularVelocity": 0.00009325406426226353, - "velocityX": 1.5847152169939396, - "velocityY": -0.3568769547381692, - "timestamp": 0.77467789024932 + "x": 1.8613811081667575, + "y": 4.297714174187834, + "heading": 2.5991131732212707, + "angularVelocity": 5.600900148939747e-7, + "velocityX": 2.372993726879466, + "velocityY": -0.6116792719114585, + "timestamp": 0.6959511499040384 }, { - "x": 1.8520072025730983, - "y": 4.261564202709402, - "heading": 2.888524094994609, - "angularVelocity": 0.00009326103448260538, - "velocityX": 1.5847152151798087, - "velocityY": -0.3568769557664531, - "timestamp": 0.852145679274252 + "x": 2.0217130268983476, + "y": 4.252932737260293, + "heading": 2.613201494408751, + "angularVelocity": 0.20243261598781087, + "velocityX": 2.3037812173123697, + "velocityY": -0.6434566123457787, + "timestamp": 0.7655462648944422 }, { - "x": 1.9747715863866588, - "y": 4.233917733912545, - "heading": 2.8885313202609173, - "angularVelocity": 0.00009326800724451237, - "velocityX": 1.5847152133650135, - "velocityY": -0.35687695679502923, - "timestamp": 0.929613468299184 + "x": 2.16688524944999, + "y": 4.21338989219855, + "heading": 2.683886289405167, + "angularVelocity": 1.015657420871611, + "velocityX": 2.0859542019819104, + "velocityY": -0.5681842046987992, + "timestamp": 0.835141379884846 }, { - "x": 2.09753597005958, - "y": 4.206271265035981, - "heading": 2.8885385460675863, - "angularVelocity": 0.00009327498254687209, - "velocityX": 1.584715211549548, - "velocityY": -0.35687695782393064, - "timestamp": 1.007081257324116 + "x": 2.294050574677846, + "y": 4.179894439044184, + "heading": 2.796544364902464, + "angularVelocity": 1.6187641260860197, + "velocityX": 1.8272162528273361, + "velocityY": -0.48129029112149246, + "timestamp": 0.9047364948752499 }, { - "x": 2.2203003530348537, - "y": 4.178624793690813, - "heading": 2.888545772490986, - "angularVelocity": 0.00009328294366610178, - "velocityX": 1.5847152025439089, - "velocityY": -0.3568769896901373, - "timestamp": 1.084549046349048 + "x": 2.3992728564214008, + "y": 4.152626972084835, + "heading": 2.9035177926378313, + "angularVelocity": 1.5370824195077049, + "velocityX": 1.5119205099102537, + "velocityY": -0.3918014499021688, + "timestamp": 0.9743316098656537 }, { - "x": 2.339535767958966, - "y": 4.149427051618625, - "heading": 2.9014383733628053, - "angularVelocity": 0.16642531088204846, - "velocityX": 1.5391611975105928, - "velocityY": -0.37690170895145225, - "timestamp": 1.16201683537398 + "x": 2.4829722545309574, + "y": 4.131221801674751, + "heading": 2.995152785659621, + "angularVelocity": 1.316687141538951, + "velocityX": 1.2026619701841783, + "velocityY": -0.3075671390590503, + "timestamp": 1.0439267248560575 }, { - "x": 2.4602156964640742, - "y": 4.1208550801313235, - "heading": 2.9091068110983676, - "angularVelocity": 0.0989887259218709, - "velocityX": 1.5578078324433486, - "velocityY": -0.36882389244523134, - "timestamp": 1.239484624398912 + "x": 2.5454789720874493, + "y": 4.115415801961452, + "heading": 3.0669003824978076, + "angularVelocity": 1.0309286341157697, + "velocityX": 0.8981480606090146, + "velocityY": -0.2271136374367465, + "timestamp": 1.1135218398464615 }, { - "x": 2.563006485689252, - "y": 4.105677497528831, - "heading": 2.9949971538769358, - "angularVelocity": 1.1087233011248803, - "velocityX": 1.3268842511059193, - "velocityY": -0.1959212053619926, - "timestamp": 1.316952413423844 + "x": 2.5870063195907287, + "y": 4.1050110004584175, + "heading": 3.1162558708577963, + "angularVelocity": 0.7091803550693766, + "velocityX": 0.5966991721906145, + "velocityY": -0.14950476774798632, + "timestamp": 1.1831169548368654 }, { "x": 2.6077051162719727, "y": 4.099861145019531, "heading": 3.141592653589793, - "angularVelocity": 1.8923413402914784, - "velocityX": 0.5769963380306965, - "velocityY": -0.07508091533924856, - "timestamp": 1.394420202448776 + "angularVelocity": 0.36405978689007856, + "velocityX": 0.29741737885042957, + "velocityY": -0.07399736949348479, + "timestamp": 1.2527120698272693 }, { "x": 2.6077051162719727, "y": 4.099861145019531, "heading": 3.141592653589793, - "angularVelocity": 9.529530648984287e-27, - "velocityX": -6.64992161175884e-27, - "velocityY": -1.7807748804280264e-26, - "timestamp": 1.471887991473708 + "angularVelocity": -7.013511745248326e-23, + "velocityX": -2.566963461380566e-23, + "velocityY": 1.8780462981611305e-24, + "timestamp": 1.3223071848176733 }, { - "x": 2.582723056141482, - "y": 4.106470038143139, - "heading": 3.0254564505751103, - "angularVelocity": -1.8328957625774476, - "velocityX": -0.39427423116151483, - "velocityY": 0.10430349785119646, - "timestamp": 1.5352501342566507 + "x": 2.58482285126615, + "y": 4.108903526368604, + "heading": 3.1172534062149904, + "angularVelocity": -0.328671725346054, + "velocityX": -0.3089969629494254, + "velocityY": 0.12210628510695666, + "timestamp": 1.3963605502210295 }, { - "x": 2.5176865762372205, - "y": 4.122909182686089, - "heading": 2.894011234282021, - "angularVelocity": -2.0745071192332674, - "velocityX": -1.0264248816056334, - "velocityY": 0.25944742114017705, - "timestamp": 1.5986122770395934 + "x": 2.5390877283653834, + "y": 4.12699559592087, + "heading": 3.068221389953176, + "angularVelocity": -0.6621173257251091, + "velocityX": -0.6175968188839946, + "velocityY": 0.2443112403294962, + "timestamp": 1.4704139156243856 }, { - "x": 2.4297948421318796, - "y": 4.1567674114258075, - "heading": 2.8608182151959642, - "angularVelocity": -0.5238620038429632, - "velocityX": -1.3871332351626442, - "velocityY": 0.5343605385269978, - "timestamp": 1.6619744198225361 + "x": 2.470551207754016, + "y": 4.154160087682156, + "heading": 2.993960294612306, + "angularVelocity": -1.0028051383807046, + "velocityX": -0.9255017680569747, + "velocityY": 0.3668231904562775, + "timestamp": 1.5444672810277418 }, { - "x": 2.336899878456132, - "y": 4.201089714349744, - "heading": 2.8608175146596406, - "angularVelocity": -0.00001105607059469762, - "velocityX": -1.4660956778872583, - "velocityY": 0.6995076393765645, - "timestamp": 1.7253365626054789 + "x": 2.379281332174413, + "y": 4.190453494467717, + "heading": 2.893905888815326, + "angularVelocity": -1.3511122047188575, + "velocityX": -1.232487883331929, + "velocityY": 0.49009800686138194, + "timestamp": 1.618520646431098 }, { - "x": 2.2457294091191753, - "y": 4.241992503114134, - "heading": 2.8495777745026225, - "angularVelocity": -0.17738888969588018, - "velocityX": -1.4388792003022337, - "velocityY": 0.6455398597315226, - "timestamp": 1.7886987053884216 + "x": 2.2887269490195696, + "y": 4.227798636551948, + "heading": 2.7901909299452776, + "angularVelocity": -1.4005434905642753, + "velocityX": -1.22282603446329, + "velocityY": 0.5043004039157333, + "timestamp": 1.692574011834454 }, { - "x": 2.180218037779224, - "y": 4.270414402297769, - "heading": 2.7322527229675986, - "angularVelocity": -1.8516585200872955, - "velocityX": -1.033919758117567, - "velocityY": 0.44856278426375645, - "timestamp": 1.8520608481713643 + "x": 2.220820804164644, + "y": 4.255857671323432, + "heading": 2.7125381341765133, + "angularVelocity": -1.0486058985409061, + "velocityX": -0.9169893155435849, + "velocityY": 0.37890289818224415, + "timestamp": 1.7666273772378103 + }, + { + "x": 2.1755524378616315, + "y": 4.274580208282235, + "heading": 2.660825121415167, + "angularVelocity": -0.6983208997953653, + "velocityX": -0.6112938427099196, + "velocityY": 0.25282493046506116, + "timestamp": 1.8406807426411664 }, { "x": 2.152919054031372, "y": 4.283941268920898, "heading": 2.634960813022188, - "angularVelocity": -1.5354895789856624, - "velocityX": -0.4308406021142514, - "velocityY": 0.21348499323117995, - "timestamp": 1.915422990954307 + "angularVelocity": -0.34926580651804395, + "velocityX": -0.3056361275004237, + "velocityY": 0.12640965859786993, + "timestamp": 1.9147341080445226 }, { "x": 2.152919054031372, "y": 4.283941268920898, "heading": 2.634960813022188, - "angularVelocity": 1.4290307836372668e-27, - "velocityX": -1.821728341071586e-27, - "velocityY": 7.158150670810205e-27, - "timestamp": 1.9787851337372497 + "angularVelocity": -3.288439423134819e-23, + "velocityX": 3.096618824782888e-23, + "velocityY": -2.3851261489110276e-23, + "timestamp": 1.9887874734478788 } ], "eventMarkers": [] diff --git a/src/main/deploy/choreo/SourceWing1Contested1.1.traj b/src/main/deploy/choreo/SourceWing1Contested1.1.traj new file mode 100644 index 00000000..09a7ba96 --- /dev/null +++ b/src/main/deploy/choreo/SourceWing1Contested1.1.traj @@ -0,0 +1,176 @@ +{ + "samples": [ + { + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "angularVelocity": -6.880993397933692e-22, + "velocityX": 2.429300243510835e-20, + "velocityY": -7.839141866791818e-21, + "timestamp": 0 + }, + { + "x": 0.8426777300221479, + "y": 4.5704094309279295, + "heading": 2.1231094632130714, + "angularVelocity": 0.17115431232368958, + "velocityX": 0.3112922161505791, + "velocityY": -0.08481597086968504, + "timestamp": 0.06949223203245897 + }, + { + "x": 0.8859417314267117, + "y": 4.558618393899322, + "heading": 2.1468991418417867, + "angularVelocity": 0.34233579686436316, + "velocityX": 0.6225732019135002, + "velocityY": -0.16967417341121008, + "timestamp": 0.13898446406491793 + }, + { + "x": 0.9508356832892374, + "y": 4.540926256854508, + "heading": 2.1825982004444193, + "angularVelocity": 0.5137129368064951, + "velocityX": 0.9338302996544205, + "velocityY": -0.2545915784738378, + "timestamp": 0.2084766960973769 + }, + { + "x": 1.0373567008976705, + "y": 4.517328145491284, + "heading": 2.230241303381707, + "angularVelocity": 0.6855889002821759, + "velocityX": 1.2450458861073865, + "velocityY": -0.3395791252208121, + "timestamp": 0.27796892812983587 + }, + { + "x": 1.145500350674682, + "y": 4.487819157938406, + "heading": 2.2898948322449537, + "angularVelocity": 0.858420101334255, + "velocityX": 1.5561976729499636, + "velocityY": -0.42463721037329055, + "timestamp": 0.34746116016229484 + }, + { + "x": 1.2752605130590804, + "y": 4.452395483406523, + "heading": 2.361665844547121, + "angularVelocity": 1.0327918704445085, + "velocityX": 1.8672613987098476, + "velocityY": -0.5097501331564366, + "timestamp": 0.4169533921947538 + }, + { + "x": 1.4266296582889453, + "y": 4.411055876279096, + "heading": 2.445704537096077, + "angularVelocity": 1.2093249862762028, + "velocityX": 2.178216770460937, + "velocityY": -0.5948809804830839, + "timestamp": 0.48644562422721277 + }, + { + "x": 1.5995997714382222, + "y": 4.363803254246465, + "heading": 2.542194010250006, + "angularVelocity": 1.3884929341290912, + "velocityX": 2.489056806644011, + "velocityY": -0.6799698419610362, + "timestamp": 0.5559378562596717 + }, + { + "x": 1.7941640365139602, + "y": 4.310645922199146, + "heading": 2.651322016971142, + "angularVelocity": 1.5703626654294764, + "velocityX": 2.799798760023414, + "velocityY": -0.7649391952540748, + "timestamp": 0.6254300882921306 + }, + { + "x": 1.9669767586764846, + "y": 4.263729609127996, + "heading": 2.753018473554398, + "angularVelocity": 1.463421933774627, + "velocityX": 2.486791934986428, + "velocityY": -0.6751303231882987, + "timestamp": 0.6949223203245896 + }, + { + "x": 2.1181975297514413, + "y": 4.222696517463293, + "heading": 2.841907068392746, + "angularVelocity": 1.2791155534740832, + "velocityX": 2.176081651893456, + "velocityY": -0.5904701930647012, + "timestamp": 0.7644145523570485 + }, + { + "x": 2.247823798372861, + "y": 4.187537365169347, + "heading": 2.917988709739883, + "angularVelocity": 1.0948222430328662, + "velocityX": 1.8653346543952263, + "velocityY": -0.5059436323404242, + "timestamp": 0.8339067843895074 + }, + { + "x": 2.3558519321956344, + "y": 4.158245220739795, + "heading": 2.981299294871451, + "angularVelocity": 0.9110454978909971, + "velocityX": 1.5545353870964267, + "velocityY": -0.4215168166690949, + "timestamp": 0.9033990164219663 + }, + { + "x": 2.4422782681617843, + "y": 4.134815338806806, + "heading": 3.0318877577047387, + "angularVelocity": 0.7279729165938769, + "velocityX": 1.2436834080358974, + "velocityY": -0.3371582873039003, + "timestamp": 0.9728912484544252 + }, + { + "x": 2.5070997855530313, + "y": 4.117244720393447, + "heading": 3.0698000752586414, + "angularVelocity": 0.545561949085099, + "velocityX": 0.9327879605445623, + "velocityY": -0.25284291350943383, + "timestamp": 1.0423834804868841 + }, + { + "x": 2.5503144871302985, + "y": 4.105531623902024, + "heading": 3.0950688818766996, + "angularVelocity": 0.36362059296434324, + "velocityX": 0.6218637725880274, + "velocityY": -0.16855260147567344, + "timestamp": 1.111875712519343 + }, + { + "x": 2.5719215869903564, + "y": 4.099675178527832, + "heading": 3.10770755459014, + "angularVelocity": 0.18187173362825426, + "velocityX": 0.3109282754073192, + "velocityY": -0.0842748204066407, + "timestamp": 1.181367944551802 + }, + { + "x": 2.5719215869903564, + "y": 4.099675178527832, + "heading": 3.10770755459014, + "angularVelocity": 6.1463734412376215e-22, + "velocityX": -2.4282322719240246e-20, + "velocityY": 7.607758723620182e-21, + "timestamp": 1.2508601765842609 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceWing1Contested1.2.traj b/src/main/deploy/choreo/SourceWing1Contested1.2.traj new file mode 100644 index 00000000..d473904e --- /dev/null +++ b/src/main/deploy/choreo/SourceWing1Contested1.2.traj @@ -0,0 +1,131 @@ +{ + "samples": [ + { + "x": 2.5719215869903564, + "y": 4.099675178527832, + "heading": 3.10770755459014, + "angularVelocity": 6.1463734412376215e-22, + "velocityX": -2.4282322719240246e-20, + "velocityY": 7.607758723620182e-21, + "timestamp": 0 + }, + { + "x": 2.5521511502862917, + "y": 4.1016130613202035, + "heading": 3.0885052062119422, + "angularVelocity": -0.28892914084268495, + "velocityX": -0.29747691159878115, + "velocityY": 0.029158454956973664, + "timestamp": 0.06646040728945746 + }, + { + "x": 2.5126144541789404, + "y": 4.10551220988183, + "heading": 3.050075070321908, + "angularVelocity": -0.578241052942358, + "velocityX": -0.5948909692225571, + "velocityY": 0.05866874310060908, + "timestamp": 0.13292081457891491 + }, + { + "x": 2.453314067800273, + "y": 4.111408158656473, + "heading": 2.9924347557750335, + "angularVelocity": -0.8672880124828585, + "velocityX": -0.892266370267546, + "velocityY": 0.08871370211386269, + "timestamp": 0.19938122186837237 + }, + { + "x": 2.3742451801100097, + "y": 4.119346701366484, + "heading": 2.915722057438157, + "angularVelocity": -1.1542616343406789, + "velocityX": -1.189714160882167, + "velocityY": 0.11944769876953625, + "timestamp": 0.26584162915782983 + }, + { + "x": 2.2753885511312752, + "y": 4.129377042526428, + "heading": 2.820281114601736, + "angularVelocity": -1.4360571463359206, + "velocityX": -1.4874514468167541, + "velocityY": 0.15092205373129233, + "timestamp": 0.3323020364472873 + }, + { + "x": 2.1567048547929897, + "y": 4.141537544068227, + "heading": 2.70671634696271, + "angularVelocity": -1.7087582256968925, + "velocityX": -1.7857804545400682, + "velocityY": 0.18297362351147275, + "timestamp": 0.39876244373674474 + }, + { + "x": 2.0370126884468625, + "y": 4.154030980294255, + "heading": 2.6076428490845873, + "angularVelocity": -1.490714576072696, + "velocityX": -1.8009544513446583, + "velocityY": 0.1879831426794298, + "timestamp": 0.4652228510262022 + }, + { + "x": 1.9372285222359857, + "y": 4.164397106788352, + "heading": 2.52557974085617, + "angularVelocity": -1.2347668570703279, + "velocityX": -1.501407684371891, + "velocityY": 0.15597446535271473, + "timestamp": 0.5316832583156597 + }, + { + "x": 1.8573807268012847, + "y": 4.172657809666075, + "heading": 2.4601710748158503, + "angularVelocity": -0.9841749201963685, + "velocityX": -1.2014340370641543, + "velocityY": 0.12429509861029223, + "timestamp": 0.5981436656051171 + }, + { + "x": 1.7974869423285986, + "y": 4.178835602778205, + "heading": 2.4112055231539617, + "angularVelocity": -0.7367627382815587, + "velocityX": -0.9011949657760652, + "velocityY": 0.09295448770309593, + "timestamp": 0.6646040728945746 + }, + { + "x": 1.757556356355509, + "y": 4.182947964217865, + "heading": 2.378578032753243, + "angularVelocity": -0.4909312435991425, + "velocityX": -0.6008176537224419, + "velocityY": 0.06187686183969354, + "timestamp": 0.731064480184032 + }, + { + "x": 1.7375919818878174, + "y": 4.185004234313965, + "heading": 2.362255080817056, + "angularVelocity": -0.24560415143252423, + "velocityX": -0.30039500631917604, + "velocityY": 0.030939775724573536, + "timestamp": 0.7975248874734895 + }, + { + "x": 1.7375919818878174, + "y": 4.185004234313965, + "heading": 2.362255080817056, + "angularVelocity": -3.596507047850015e-24, + "velocityX": -6.256878628501107e-23, + "velocityY": -6.331189922772691e-23, + "timestamp": 0.8639852947629469 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceWing1Contested1.3.traj b/src/main/deploy/choreo/SourceWing1Contested1.3.traj new file mode 100644 index 00000000..aba88e59 --- /dev/null +++ b/src/main/deploy/choreo/SourceWing1Contested1.3.traj @@ -0,0 +1,374 @@ +{ + "samples": [ + { + "x": 1.7375919818878174, + "y": 4.185004234313965, + "heading": 2.362255080817056, + "angularVelocity": -3.596507047850015e-24, + "velocityX": -6.256878628501107e-23, + "velocityY": -6.331189922772691e-23, + "timestamp": 0 + }, + { + "x": 1.7500448547508285, + "y": 4.169918910269556, + "heading": 2.3726443658683603, + "angularVelocity": 0.1601015115213246, + "velocityX": 0.19190192186522692, + "velocityY": -0.23246866067994823, + "timestamp": 0.06489186112350032 + }, + { + "x": 1.7749616511915172, + "y": 4.139736833623858, + "heading": 2.393148152590508, + "angularVelocity": 0.31596854161919546, + "velocityX": 0.3839741380397122, + "velocityY": -0.4651134383132558, + "timestamp": 0.12978372224700063 + }, + { + "x": 1.8123551092604897, + "y": 4.094444892066208, + "heading": 2.4234406505462944, + "angularVelocity": 0.4668150586424898, + "velocityX": 0.5762426507972481, + "velocityY": -0.6979602799718191, + "timestamp": 0.19467558337050095 + }, + { + "x": 1.862239603437118, + "y": 4.0340277852116175, + "heading": 2.4631338121351902, + "angularVelocity": 0.6116816639509415, + "velocityX": 0.7687326779191858, + "velocityY": -0.9310429044345997, + "timestamp": 0.25956744449400126 + }, + { + "x": 1.9246311116449513, + "y": 3.9584675364071735, + "heading": 2.5117637367500465, + "angularVelocity": 0.7493994435188912, + "velocityX": 0.9614689288860394, + "velocityY": -1.164402553667557, + "timestamp": 0.3244593056175016 + }, + { + "x": 1.9995472742646765, + "y": 3.8677431003581373, + "heading": 2.5687734572106, + "angularVelocity": 0.878534217905294, + "velocityX": 1.1544770225829577, + "velocityY": -1.3980865162176925, + "timestamp": 0.3893511667410019 + }, + { + "x": 2.08700767845586, + "y": 3.7618301407634718, + "heading": 2.633488115256641, + "angularVelocity": 0.9972692557372997, + "velocityX": 1.347786959365091, + "velocityY": -1.6321455073247935, + "timestamp": 0.4542430278645022 + }, + { + "x": 2.18703451414934, + "y": 3.640701026603113, + "heading": 2.7050730144651673, + "angularVelocity": 1.1031414104811743, + "velocityX": 1.5414388485963126, + "velocityY": -1.8666302994427872, + "timestamp": 0.5191348889880025 + }, + { + "x": 2.2996535810248653, + "y": 3.5043251322158286, + "heading": 2.7824542959291714, + "angularVelocity": 1.1924651277412772, + "velocityX": 1.7354883174207654, + "velocityY": -2.101587040749814, + "timestamp": 0.5840267501115028 + }, + { + "x": 2.4248949182977166, + "y": 3.3526700317076736, + "heading": 2.864159859448204, + "angularVelocity": 1.2591034084156272, + "velocityX": 1.9300006981537456, + "velocityY": -2.3370434732875047, + "timestamp": 0.6489186112350032 + }, + { + "x": 2.5627898036818046, + "y": 3.185706945978596, + "heading": 2.947984812199732, + "angularVelocity": 1.2917637327737392, + "velocityX": 2.1249950763725343, + "velocityY": -2.572943399039195, + "timestamp": 0.7138104723585035 + }, + { + "x": 2.7133500598265714, + "y": 3.0034380376889436, + "heading": 3.0302259153482303, + "angularVelocity": 1.2673562096174134, + "velocityX": 2.3201716446107956, + "velocityY": -2.808809997647686, + "timestamp": 0.7787023334820038 + }, + { + "x": 2.8764454665102543, + "y": 2.8060484554493246, + "heading": 3.1036395778244232, + "angularVelocity": 1.1313231151819507, + "velocityX": 2.5133414862810737, + "velocityY": -3.041823409317128, + "timestamp": 0.8435941946055041 + }, + { + "x": 3.049983564231236, + "y": 2.59451668989809, + "heading": 3.1503578251858677, + "angularVelocity": 0.7199400133174118, + "velocityX": 2.674265997560302, + "velocityY": -3.2597580326545725, + "timestamp": 0.9084860557290044 + }, + { + "x": 3.229588483804468, + "y": 2.3696781969148084, + "heading": 3.1544671382222993, + "angularVelocity": 0.06332555370250754, + "velocityX": 2.7677572574380895, + "velocityY": -3.4648180694860256, + "timestamp": 0.9733779168525047 + }, + { + "x": 3.413445860556505, + "y": 2.1465356197492285, + "heading": 3.154467162773942, + "angularVelocity": 3.783470257874771e-7, + "velocityX": 2.833288698595432, + "velocityY": -3.4386835776046256, + "timestamp": 1.038269777976005 + }, + { + "x": 3.6000973110406074, + "y": 1.9257248846021084, + "heading": 3.1544671873317807, + "angularVelocity": 3.7844251267652267e-7, + "velocityX": 2.876346081812528, + "velocityY": -3.4027493020562343, + "timestamp": 1.1031616390995054 + }, + { + "x": 3.8014757154623195, + "y": 1.718257097054393, + "heading": 3.154467212149594, + "angularVelocity": 3.8244877762487134e-7, + "velocityX": 3.1032921684655466, + "velocityY": -3.1971311032807224, + "timestamp": 1.1680535002230057 + }, + { + "x": 4.016630577734113, + "y": 1.5251132914284782, + "heading": 3.154467237801092, + "angularVelocity": 3.952960817114755e-7, + "velocityX": 3.315590869898414, + "velocityY": -2.976394917358426, + "timestamp": 1.232945361346506 + }, + { + "x": 4.2445454275226515, + "y": 1.3472061098038577, + "heading": 3.1544672649477823, + "angularVelocity": 4.1833736664202863e-7, + "velocityX": 3.512225506289282, + "velocityY": -2.7415946860582885, + "timestamp": 1.2978372224700063 + }, + { + "x": 4.484143440597044, + "y": 1.1853761509334682, + "heading": 3.1544672944204697, + "angularVelocity": 4.541815744966032e-7, + "velocityX": 3.6922660088049732, + "velocityY": -2.4938406152722283, + "timestamp": 1.3627290835935066 + }, + { + "x": 4.734292576211658, + "y": 1.040388039013423, + "heading": 3.1544673273474686, + "angularVelocity": 5.074133830222093e-7, + "velocityX": 3.8548614769814935, + "velocityY": -2.23430349214531, + "timestamp": 1.427620944717007 + }, + { + "x": 4.993810935799209, + "y": 0.9129268192433763, + "heading": 3.154467365380173, + "angularVelocity": 5.860936051826097e-7, + "velocityX": 3.9992435891712756, + "velocityY": -1.9642096491494785, + "timestamp": 1.4925128058405073 + }, + { + "x": 5.261472350224649, + "y": 0.8035947237152907, + "heading": 3.1544674111329933, + "angularVelocity": 7.050625321396259e-7, + "velocityX": 4.12473012472298, + "velocityY": -1.6848352572290692, + "timestamp": 1.5574046669640076 + }, + { + "x": 5.536012172698975, + "y": 0.7129083275794983, + "heading": 3.1544674706525395, + "angularVelocity": 9.172112660822983e-7, + "velocityX": 4.230728133252795, + "velocityY": -1.3975003115290687, + "timestamp": 1.622296528087508 + }, + { + "x": 5.851986653314552, + "y": 0.6350614800439672, + "heading": 3.1539640111947476, + "angularVelocity": -0.006887282897011849, + "velocityX": 4.322504230591005, + "velocityY": -1.0649383050015728, + "timestamp": 1.6953963942205195 + }, + { + "x": 6.163084948302816, + "y": 0.5819578289574815, + "heading": 3.153020697079212, + "angularVelocity": -0.012904457496809069, + "velocityX": 4.25579842269744, + "velocityY": -0.7264534655899257, + "timestamp": 1.768496260353531 + }, + { + "x": 6.458568718598011, + "y": 0.5486582997656804, + "heading": 3.1517988719890786, + "angularVelocity": -0.01671446412651432, + "velocityX": 4.042193042563675, + "velocityY": -0.4555347492868147, + "timestamp": 1.8415961264865426 + }, + { + "x": 6.732865281543566, + "y": 0.5290381776313112, + "heading": 3.1504499662020073, + "angularVelocity": -0.018452917336628596, + "velocityX": 3.752353833951604, + "velocityY": -0.26840161510923477, + "timestamp": 1.9146959926195541 + }, + { + "x": 6.98366753282154, + "y": 0.5185855599477185, + "heading": 3.149074247230133, + "angularVelocity": -0.018819719442045503, + "velocityX": 3.430953632960416, + "velocityY": -0.14299092784347003, + "timestamp": 1.9877958587525657 + }, + { + "x": 7.210009572737756, + "y": 0.5142749171101598, + "heading": 3.147735302871607, + "angularVelocity": -0.018316645834748266, + "velocityX": 3.096340005662489, + "velocityY": -0.058969230254336036, + "timestamp": 2.0608957248855773 + }, + { + "x": 7.411462955935819, + "y": 0.5140361320742359, + "heading": 3.1464751819616255, + "angularVelocity": -0.017238347710359158, + "velocityX": 2.755865282044427, + "velocityY": -0.003266559141017675, + "timestamp": 2.133995591018589 + }, + { + "x": 7.587830777283194, + "y": 0.5163933493371166, + "heading": 3.145323181311359, + "angularVelocity": -0.015759271681428454, + "velocityX": 2.4126969128296243, + "velocityY": 0.03224653323703015, + "timestamp": 2.2070954571516004 + }, + { + "x": 7.739023807932044, + "y": 0.5202518916978398, + "heading": 3.144300674526786, + "angularVelocity": -0.013987806526385212, + "velocityX": 2.068307900506172, + "velocityY": 0.052784533882760894, + "timestamp": 2.280195323284612 + }, + { + "x": 7.865006007354459, + "y": 0.5247719050263068, + "heading": 3.1434238563856503, + "angularVelocity": -0.011994798178430731, + "velocityX": 1.7234258568022467, + "velocityY": 0.0618334009017538, + "timestamp": 2.3532951894176235 + }, + { + "x": 7.96576870810601, + "y": 0.5292907028384434, + "heading": 3.1427053813238377, + "angularVelocity": -0.009828678215427785, + "velocityX": 1.378425243189977, + "velocityY": 0.06181677274092789, + "timestamp": 2.426395055550635 + }, + { + "x": 8.041317623102847, + "y": 0.5332730840639932, + "heading": 3.1421553895932863, + "angularVelocity": -0.007523840461631992, + "velocityX": 1.0335027817885274, + "velocityY": 0.05447863910316233, + "timestamp": 2.4994949216836466 + }, + { + "x": 8.091665990307739, + "y": 0.5362783280705644, + "heading": 3.1417821778172224, + "angularVelocity": -0.005105505602228686, + "velocityX": 0.688761414600654, + "velocityY": 0.04111148440549697, + "timestamp": 2.572594787816658 + }, + { + "x": 8.116830825805664, + "y": 0.5379375219345093, + "heading": 3.141592653589793, + "angularVelocity": -0.0025926754377961075, + "velocityX": 0.3442528260193612, + "velocityY": 0.02269763204389355, + "timestamp": 2.6456946539496697 + }, + { + "x": 8.116830825805664, + "y": 0.5379375219345093, + "heading": 3.141592653589793, + "angularVelocity": 2.6782007527142383e-24, + "velocityX": -3.91857307116395e-24, + "velocityY": 2.7111362097714367e-23, + "timestamp": 2.7187945200826813 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceWing1Contested1.4.traj b/src/main/deploy/choreo/SourceWing1Contested1.4.traj new file mode 100644 index 00000000..dd1cc02b --- /dev/null +++ b/src/main/deploy/choreo/SourceWing1Contested1.4.traj @@ -0,0 +1,293 @@ +{ + "samples": [ + { + "x": 8.116830825805664, + "y": 0.5379375219345093, + "heading": 3.141592653589793, + "angularVelocity": 2.6782007527142383e-24, + "velocityX": -3.91857307116395e-24, + "velocityY": 2.7111362097714367e-23, + "timestamp": 0 + }, + { + "x": 8.089255596234777, + "y": 0.54942402960786, + "heading": 3.1321392994576667, + "angularVelocity": -0.11847903719259653, + "velocityX": -0.34560078933473104, + "velocityY": 0.14396058275432985, + "timestamp": 0.07978925518072266 + }, + { + "x": 8.034097606978237, + "y": 0.5724008755052853, + "heading": 3.113461638242577, + "angularVelocity": -0.23408742408717345, + "velocityX": -0.6912959537171532, + "velocityY": 0.2879691738616022, + "timestamp": 0.15957851036144532 + }, + { + "x": 7.9513480563517875, + "y": 0.606872555226039, + "heading": 3.085834950822301, + "angularVelocity": -0.346245711376821, + "velocityX": -1.0371014297479375, + "velocityY": 0.43203410838559014, + "timestamp": 0.239367765542168 + }, + { + "x": 7.840996433281568, + "y": 0.652844444117696, + "heading": 3.0496002995849607, + "angularVelocity": -0.4541294583496149, + "velocityX": -1.3830386412340105, + "velocityY": 0.5761664122259446, + "timestamp": 0.31915702072289065 + }, + { + "x": 7.703029917572737, + "y": 0.7103230512929849, + "heading": 3.005193906243408, + "angularVelocity": -0.5565460316802363, + "velocityX": -1.7291365284252544, + "velocityY": 0.720380294879303, + "timestamp": 0.3989462759036133 + }, + { + "x": 7.537432570924173, + "y": 0.7793163153547259, + "heading": 2.9531931917128307, + "angularVelocity": -0.65172577952753, + "velocityX": -2.075434170597116, + "velocityY": 0.8646936721676576, + "timestamp": 0.478735531084336 + }, + { + "x": 7.344184305821202, + "y": 0.8598339479102136, + "heading": 2.8943934690100765, + "angularVelocity": -0.7369378567273606, + "velocityX": -2.421983569908799, + "velocityY": 1.0091287651841792, + "timestamp": 0.5585247862650586 + }, + { + "x": 7.123259821783499, + "y": 0.9518877987646011, + "heading": 2.8299481315961454, + "angularVelocity": -0.8076944353968832, + "velocityX": -2.7688500605414945, + "velocityY": 1.1537123719965283, + "timestamp": 0.6383140414457813 + }, + { + "x": 6.874628809836458, + "y": 1.055491871977118, + "heading": 2.761663241881283, + "angularVelocity": -0.8558156052490734, + "velocityX": -3.1160964140333447, + "velocityY": 1.2984714919051967, + "timestamp": 0.718103296626504 + }, + { + "x": 6.598266120224734, + "y": 1.1706587451437058, + "heading": 2.692764023482451, + "angularVelocity": -0.8635149963835985, + "velocityX": -3.4636579698076297, + "velocityY": 1.4433882470231743, + "timestamp": 0.7978925518072266 + }, + { + "x": 6.294260106065497, + "y": 1.2973571312742034, + "heading": 2.630823871265101, + "angularVelocity": -0.7762969096159158, + "velocityX": -3.810112194563846, + "velocityY": 1.5879128817975097, + "timestamp": 0.8776818069879493 + }, + { + "x": 5.9677610677766175, + "y": 1.434232859949075, + "heading": 2.6252073225176806, + "angularVelocity": -0.07039229448500041, + "velocityX": -4.092017622540287, + "velocityY": 1.715465677237474, + "timestamp": 0.957471062168672 + }, + { + "x": 5.6396041489574, + "y": 1.5709726586832762, + "heading": 2.6252073065947137, + "angularVelocity": -1.9956279736518076e-7, + "velocityX": -4.1127958655077475, + "velocityY": 1.7137620651362904, + "timestamp": 1.0372603173493946 + }, + { + "x": 5.311447232039342, + "y": 1.7077124619800061, + "heading": 2.6252072906717543, + "angularVelocity": -1.995627014046446e-7, + "velocityX": -4.1127958416804935, + "velocityY": 1.7137621223185358, + "timestamp": 1.1170495725301173 + }, + { + "x": 4.9832903234128985, + "y": 1.8444522851754865, + "heading": 2.6252072747487953, + "angularVelocity": -1.9956269204035307e-7, + "velocityX": -4.112795737761537, + "velocityY": 1.713762371709888, + "timestamp": 1.19683882771084 + }, + { + "x": 4.655133427137336, + "y": 1.9811921380113924, + "heading": 2.625207258825836, + "angularVelocity": -1.9956270204696126e-7, + "velocityX": -4.112795582967754, + "velocityY": 1.7137627431938112, + "timestamp": 1.2766280828915626 + }, + { + "x": 4.326976566232365, + "y": 2.117932075731836, + "heading": 2.625207242902869, + "angularVelocity": -1.9956279397787226e-7, + "velocityX": -4.112795139667575, + "velocityY": 1.7137638070530719, + "timestamp": 1.3564173380722853 + }, + { + "x": 3.999438899364335, + "y": 2.256148618450317, + "heading": 2.6252072265396365, + "angularVelocity": -2.050806574466794e-7, + "velocityX": -4.105034770987135, + "velocityY": 1.7322701209006253, + "timestamp": 1.436206593253008 + }, + { + "x": 3.6745952182551624, + "y": 2.394736220372859, + "heading": 2.616353473366314, + "angularVelocity": -0.11096422887102513, + "velocityX": -4.071271004766277, + "velocityY": 1.7369206117871623, + "timestamp": 1.5159958484337306 + }, + { + "x": 3.371360246276191, + "y": 2.5227681355611673, + "heading": 2.54789899304901, + "angularVelocity": -0.8579410869578161, + "velocityX": -3.8004487106960148, + "velocityY": 1.6046260226181663, + "timestamp": 1.5957851036144532 + }, + { + "x": 3.09571714689087, + "y": 2.6391416829563425, + "heading": 2.4725529095377854, + "angularVelocity": -0.9443136590329878, + "velocityX": -3.454639334093173, + "velocityY": 1.4585115142582736, + "timestamp": 1.675574358795176 + }, + { + "x": 2.8477516687682445, + "y": 2.7438274798328672, + "heading": 2.3982762432251263, + "angularVelocity": -0.9309106363309555, + "velocityX": -3.1077552680619074, + "velocityY": 1.3120287517336962, + "timestamp": 1.7553636139758986 + }, + { + "x": 2.6274310870254833, + "y": 2.83684141662163, + "heading": 2.3284405360388263, + "angularVelocity": -0.875252025202168, + "velocityX": -2.7612813435059107, + "velocityY": 1.1657451442313427, + "timestamp": 1.8351528691566212 + }, + { + "x": 2.4347195920645524, + "y": 2.918199144350896, + "heading": 2.2649041443604916, + "angularVelocity": -0.7963026040840354, + "velocityX": -2.4152562212097375, + "velocityY": 1.019657691314338, + "timestamp": 1.914942124337344 + }, + { + "x": 2.269588113206454, + "y": 2.9879129508006694, + "heading": 2.208834388541783, + "angularVelocity": -0.7027231384941546, + "velocityX": -2.0695954421942657, + "velocityY": 0.8737242413389122, + "timestamp": 1.9947313795180666 + }, + { + "x": 2.1320138837828355, + "y": 3.0459924554790536, + "heading": 2.1610282462744177, + "angularVelocity": -0.5991551388603021, + "velocityX": -1.7242199981941622, + "velocityY": 0.7279113528110278, + "timestamp": 2.0745206346987892 + }, + { + "x": 2.0219789365980305, + "y": 3.092445388285202, + "heading": 2.1220638838563577, + "angularVelocity": -0.4883409718489752, + "velocityX": -1.379069737342154, + "velocityY": 0.5821953432317734, + "timestamp": 2.154309889879512 + }, + { + "x": 1.93946881514082, + "y": 3.1272780949907744, + "heading": 2.09238087210519, + "angularVelocity": -0.37201765681275867, + "velocityX": -1.0341006601744267, + "velocityY": 0.4365588653093307, + "timestamp": 2.2340991450602345 + }, + { + "x": 1.884471623102071, + "y": 3.1504958321346646, + "heading": 2.0723259960019775, + "angularVelocity": -0.25134808011164267, + "velocityX": -0.6892806796376343, + "velocityY": 0.29098826767216296, + "timestamp": 2.313888400240957 + }, + { + "x": 1.8569773435592651, + "y": 3.1621029376983643, + "heading": 2.0621810870916404, + "angularVelocity": -0.12714630419044576, + "velocityX": -0.344586241349558, + "velocityY": 0.14547203802578762, + "timestamp": 2.39367765542168 + }, + { + "x": 1.8569773435592651, + "y": 3.1621029376983643, + "heading": 2.0621810870916404, + "angularVelocity": 1.0052435240302537e-24, + "velocityX": -4.289779840395474e-24, + "velocityY": -8.499784651893661e-24, + "timestamp": 2.4734669106024025 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/SourceWing1Contested1.traj b/src/main/deploy/choreo/SourceWing1Contested1.traj new file mode 100644 index 00000000..2eefaed5 --- /dev/null +++ b/src/main/deploy/choreo/SourceWing1Contested1.traj @@ -0,0 +1,932 @@ +{ + "samples": [ + { + "x": 0.8210453391075134, + "y": 4.576303482055664, + "heading": 2.111215568027718, + "angularVelocity": -6.880993397933692e-22, + "velocityX": 2.429300243510835e-20, + "velocityY": -7.839141866791818e-21, + "timestamp": 0 + }, + { + "x": 0.8426777300221479, + "y": 4.5704094309279295, + "heading": 2.1231094632130714, + "angularVelocity": 0.17115431232368958, + "velocityX": 0.3112922161505791, + "velocityY": -0.08481597086968504, + "timestamp": 0.06949223203245897 + }, + { + "x": 0.8859417314267117, + "y": 4.558618393899322, + "heading": 2.1468991418417867, + "angularVelocity": 0.34233579686436316, + "velocityX": 0.6225732019135002, + "velocityY": -0.16967417341121008, + "timestamp": 0.13898446406491793 + }, + { + "x": 0.9508356832892374, + "y": 4.540926256854508, + "heading": 2.1825982004444193, + "angularVelocity": 0.5137129368064951, + "velocityX": 0.9338302996544205, + "velocityY": -0.2545915784738378, + "timestamp": 0.2084766960973769 + }, + { + "x": 1.0373567008976705, + "y": 4.517328145491284, + "heading": 2.230241303381707, + "angularVelocity": 0.6855889002821759, + "velocityX": 1.2450458861073865, + "velocityY": -0.3395791252208121, + "timestamp": 0.27796892812983587 + }, + { + "x": 1.145500350674682, + "y": 4.487819157938406, + "heading": 2.2898948322449537, + "angularVelocity": 0.858420101334255, + "velocityX": 1.5561976729499636, + "velocityY": -0.42463721037329055, + "timestamp": 0.34746116016229484 + }, + { + "x": 1.2752605130590804, + "y": 4.452395483406523, + "heading": 2.361665844547121, + "angularVelocity": 1.0327918704445085, + "velocityX": 1.8672613987098476, + "velocityY": -0.5097501331564366, + "timestamp": 0.4169533921947538 + }, + { + "x": 1.4266296582889453, + "y": 4.411055876279096, + "heading": 2.445704537096077, + "angularVelocity": 1.2093249862762028, + "velocityX": 2.178216770460937, + "velocityY": -0.5948809804830839, + "timestamp": 0.48644562422721277 + }, + { + "x": 1.5995997714382222, + "y": 4.363803254246465, + "heading": 2.542194010250006, + "angularVelocity": 1.3884929341290912, + "velocityX": 2.489056806644011, + "velocityY": -0.6799698419610362, + "timestamp": 0.5559378562596717 + }, + { + "x": 1.7941640365139602, + "y": 4.310645922199146, + "heading": 2.651322016971142, + "angularVelocity": 1.5703626654294764, + "velocityX": 2.799798760023414, + "velocityY": -0.7649391952540748, + "timestamp": 0.6254300882921306 + }, + { + "x": 1.9669767586764846, + "y": 4.263729609127996, + "heading": 2.753018473554398, + "angularVelocity": 1.463421933774627, + "velocityX": 2.486791934986428, + "velocityY": -0.6751303231882987, + "timestamp": 0.6949223203245896 + }, + { + "x": 2.1181975297514413, + "y": 4.222696517463293, + "heading": 2.841907068392746, + "angularVelocity": 1.2791155534740832, + "velocityX": 2.176081651893456, + "velocityY": -0.5904701930647012, + "timestamp": 0.7644145523570485 + }, + { + "x": 2.247823798372861, + "y": 4.187537365169347, + "heading": 2.917988709739883, + "angularVelocity": 1.0948222430328662, + "velocityX": 1.8653346543952263, + "velocityY": -0.5059436323404242, + "timestamp": 0.8339067843895074 + }, + { + "x": 2.3558519321956344, + "y": 4.158245220739795, + "heading": 2.981299294871451, + "angularVelocity": 0.9110454978909971, + "velocityX": 1.5545353870964267, + "velocityY": -0.4215168166690949, + "timestamp": 0.9033990164219663 + }, + { + "x": 2.4422782681617843, + "y": 4.134815338806806, + "heading": 3.0318877577047387, + "angularVelocity": 0.7279729165938769, + "velocityX": 1.2436834080358974, + "velocityY": -0.3371582873039003, + "timestamp": 0.9728912484544252 + }, + { + "x": 2.5070997855530313, + "y": 4.117244720393447, + "heading": 3.0698000752586414, + "angularVelocity": 0.545561949085099, + "velocityX": 0.9327879605445623, + "velocityY": -0.25284291350943383, + "timestamp": 1.0423834804868841 + }, + { + "x": 2.5503144871302985, + "y": 4.105531623902024, + "heading": 3.0950688818766996, + "angularVelocity": 0.36362059296434324, + "velocityX": 0.6218637725880274, + "velocityY": -0.16855260147567344, + "timestamp": 1.111875712519343 + }, + { + "x": 2.5719215869903564, + "y": 4.099675178527832, + "heading": 3.10770755459014, + "angularVelocity": 0.18187173362825426, + "velocityX": 0.3109282754073192, + "velocityY": -0.0842748204066407, + "timestamp": 1.181367944551802 + }, + { + "x": 2.5719215869903564, + "y": 4.099675178527832, + "heading": 3.10770755459014, + "angularVelocity": 6.1463734412376215e-22, + "velocityX": -2.4282322719240246e-20, + "velocityY": 7.607758723620182e-21, + "timestamp": 1.2508601765842609 + }, + { + "x": 2.5521511502862917, + "y": 4.1016130613202035, + "heading": 3.0885052062119422, + "angularVelocity": -0.28892914084268495, + "velocityX": -0.29747691159878115, + "velocityY": 0.029158454956973664, + "timestamp": 1.3173205838737183 + }, + { + "x": 2.5126144541789404, + "y": 4.10551220988183, + "heading": 3.050075070321908, + "angularVelocity": -0.578241052942358, + "velocityX": -0.5948909692225571, + "velocityY": 0.05866874310060908, + "timestamp": 1.3837809911631758 + }, + { + "x": 2.453314067800273, + "y": 4.111408158656473, + "heading": 2.9924347557750335, + "angularVelocity": -0.8672880124828585, + "velocityX": -0.892266370267546, + "velocityY": 0.08871370211386269, + "timestamp": 1.4502413984526332 + }, + { + "x": 2.3742451801100097, + "y": 4.119346701366484, + "heading": 2.915722057438157, + "angularVelocity": -1.1542616343406789, + "velocityX": -1.189714160882167, + "velocityY": 0.11944769876953625, + "timestamp": 1.5167018057420907 + }, + { + "x": 2.2753885511312752, + "y": 4.129377042526428, + "heading": 2.820281114601736, + "angularVelocity": -1.4360571463359206, + "velocityX": -1.4874514468167541, + "velocityY": 0.15092205373129233, + "timestamp": 1.5831622130315481 + }, + { + "x": 2.1567048547929897, + "y": 4.141537544068227, + "heading": 2.70671634696271, + "angularVelocity": -1.7087582256968925, + "velocityX": -1.7857804545400682, + "velocityY": 0.18297362351147275, + "timestamp": 1.6496226203210056 + }, + { + "x": 2.0370126884468625, + "y": 4.154030980294255, + "heading": 2.6076428490845873, + "angularVelocity": -1.490714576072696, + "velocityX": -1.8009544513446583, + "velocityY": 0.1879831426794298, + "timestamp": 1.716083027610463 + }, + { + "x": 1.9372285222359857, + "y": 4.164397106788352, + "heading": 2.52557974085617, + "angularVelocity": -1.2347668570703279, + "velocityX": -1.501407684371891, + "velocityY": 0.15597446535271473, + "timestamp": 1.7825434348999205 + }, + { + "x": 1.8573807268012847, + "y": 4.172657809666075, + "heading": 2.4601710748158503, + "angularVelocity": -0.9841749201963685, + "velocityX": -1.2014340370641543, + "velocityY": 0.12429509861029223, + "timestamp": 1.849003842189378 + }, + { + "x": 1.7974869423285986, + "y": 4.178835602778205, + "heading": 2.4112055231539617, + "angularVelocity": -0.7367627382815587, + "velocityX": -0.9011949657760652, + "velocityY": 0.09295448770309593, + "timestamp": 1.9154642494788354 + }, + { + "x": 1.757556356355509, + "y": 4.182947964217865, + "heading": 2.378578032753243, + "angularVelocity": -0.4909312435991425, + "velocityX": -0.6008176537224419, + "velocityY": 0.06187686183969354, + "timestamp": 1.9819246567682929 + }, + { + "x": 1.7375919818878174, + "y": 4.185004234313965, + "heading": 2.362255080817056, + "angularVelocity": -0.24560415143252423, + "velocityX": -0.30039500631917604, + "velocityY": 0.030939775724573536, + "timestamp": 2.0483850640577503 + }, + { + "x": 1.7375919818878174, + "y": 4.185004234313965, + "heading": 2.362255080817056, + "angularVelocity": -3.596507047850015e-24, + "velocityX": -6.256878628501107e-23, + "velocityY": -6.331189922772691e-23, + "timestamp": 2.114845471347208 + }, + { + "x": 1.7500448547508285, + "y": 4.169918910269556, + "heading": 2.3726443658683603, + "angularVelocity": 0.1601015115213246, + "velocityX": 0.19190192186522692, + "velocityY": -0.23246866067994823, + "timestamp": 2.179737332470708 + }, + { + "x": 1.7749616511915172, + "y": 4.139736833623858, + "heading": 2.393148152590508, + "angularVelocity": 0.31596854161919546, + "velocityX": 0.3839741380397122, + "velocityY": -0.4651134383132558, + "timestamp": 2.2446291935942084 + }, + { + "x": 1.8123551092604897, + "y": 4.094444892066208, + "heading": 2.4234406505462944, + "angularVelocity": 0.4668150586424898, + "velocityX": 0.5762426507972481, + "velocityY": -0.6979602799718191, + "timestamp": 2.3095210547177087 + }, + { + "x": 1.862239603437118, + "y": 4.0340277852116175, + "heading": 2.4631338121351902, + "angularVelocity": 0.6116816639509415, + "velocityX": 0.7687326779191858, + "velocityY": -0.9310429044345997, + "timestamp": 2.374412915841209 + }, + { + "x": 1.9246311116449513, + "y": 3.9584675364071735, + "heading": 2.5117637367500465, + "angularVelocity": 0.7493994435188912, + "velocityX": 0.9614689288860394, + "velocityY": -1.164402553667557, + "timestamp": 2.4393047769647094 + }, + { + "x": 1.9995472742646765, + "y": 3.8677431003581373, + "heading": 2.5687734572106, + "angularVelocity": 0.878534217905294, + "velocityX": 1.1544770225829577, + "velocityY": -1.3980865162176925, + "timestamp": 2.5041966380882097 + }, + { + "x": 2.08700767845586, + "y": 3.7618301407634718, + "heading": 2.633488115256641, + "angularVelocity": 0.9972692557372997, + "velocityX": 1.347786959365091, + "velocityY": -1.6321455073247935, + "timestamp": 2.56908849921171 + }, + { + "x": 2.18703451414934, + "y": 3.640701026603113, + "heading": 2.7050730144651673, + "angularVelocity": 1.1031414104811743, + "velocityX": 1.5414388485963126, + "velocityY": -1.8666302994427872, + "timestamp": 2.6339803603352103 + }, + { + "x": 2.2996535810248653, + "y": 3.5043251322158286, + "heading": 2.7824542959291714, + "angularVelocity": 1.1924651277412772, + "velocityX": 1.7354883174207654, + "velocityY": -2.101587040749814, + "timestamp": 2.6988722214587106 + }, + { + "x": 2.4248949182977166, + "y": 3.3526700317076736, + "heading": 2.864159859448204, + "angularVelocity": 1.2591034084156272, + "velocityX": 1.9300006981537456, + "velocityY": -2.3370434732875047, + "timestamp": 2.763764082582211 + }, + { + "x": 2.5627898036818046, + "y": 3.185706945978596, + "heading": 2.947984812199732, + "angularVelocity": 1.2917637327737392, + "velocityX": 2.1249950763725343, + "velocityY": -2.572943399039195, + "timestamp": 2.8286559437057113 + }, + { + "x": 2.7133500598265714, + "y": 3.0034380376889436, + "heading": 3.0302259153482303, + "angularVelocity": 1.2673562096174134, + "velocityX": 2.3201716446107956, + "velocityY": -2.808809997647686, + "timestamp": 2.8935478048292116 + }, + { + "x": 2.8764454665102543, + "y": 2.8060484554493246, + "heading": 3.1036395778244232, + "angularVelocity": 1.1313231151819507, + "velocityX": 2.5133414862810737, + "velocityY": -3.041823409317128, + "timestamp": 2.958439665952712 + }, + { + "x": 3.049983564231236, + "y": 2.59451668989809, + "heading": 3.1503578251858677, + "angularVelocity": 0.7199400133174118, + "velocityX": 2.674265997560302, + "velocityY": -3.2597580326545725, + "timestamp": 3.023331527076212 + }, + { + "x": 3.229588483804468, + "y": 2.3696781969148084, + "heading": 3.1544671382222993, + "angularVelocity": 0.06332555370250754, + "velocityX": 2.7677572574380895, + "velocityY": -3.4648180694860256, + "timestamp": 3.0882233881997125 + }, + { + "x": 3.413445860556505, + "y": 2.1465356197492285, + "heading": 3.154467162773942, + "angularVelocity": 3.783470257874771e-7, + "velocityX": 2.833288698595432, + "velocityY": -3.4386835776046256, + "timestamp": 3.153115249323213 + }, + { + "x": 3.6000973110406074, + "y": 1.9257248846021084, + "heading": 3.1544671873317807, + "angularVelocity": 3.7844251267652267e-7, + "velocityX": 2.876346081812528, + "velocityY": -3.4027493020562343, + "timestamp": 3.218007110446713 + }, + { + "x": 3.8014757154623195, + "y": 1.718257097054393, + "heading": 3.154467212149594, + "angularVelocity": 3.8244877762487134e-7, + "velocityX": 3.1032921684655466, + "velocityY": -3.1971311032807224, + "timestamp": 3.2828989715702135 + }, + { + "x": 4.016630577734113, + "y": 1.5251132914284782, + "heading": 3.154467237801092, + "angularVelocity": 3.952960817114755e-7, + "velocityX": 3.315590869898414, + "velocityY": -2.976394917358426, + "timestamp": 3.347790832693714 + }, + { + "x": 4.2445454275226515, + "y": 1.3472061098038577, + "heading": 3.1544672649477823, + "angularVelocity": 4.1833736664202863e-7, + "velocityX": 3.512225506289282, + "velocityY": -2.7415946860582885, + "timestamp": 3.412682693817214 + }, + { + "x": 4.484143440597044, + "y": 1.1853761509334682, + "heading": 3.1544672944204697, + "angularVelocity": 4.541815744966032e-7, + "velocityX": 3.6922660088049732, + "velocityY": -2.4938406152722283, + "timestamp": 3.4775745549407144 + }, + { + "x": 4.734292576211658, + "y": 1.040388039013423, + "heading": 3.1544673273474686, + "angularVelocity": 5.074133830222093e-7, + "velocityX": 3.8548614769814935, + "velocityY": -2.23430349214531, + "timestamp": 3.5424664160642148 + }, + { + "x": 4.993810935799209, + "y": 0.9129268192433763, + "heading": 3.154467365380173, + "angularVelocity": 5.860936051826097e-7, + "velocityX": 3.9992435891712756, + "velocityY": -1.9642096491494785, + "timestamp": 3.607358277187715 + }, + { + "x": 5.261472350224649, + "y": 0.8035947237152907, + "heading": 3.1544674111329933, + "angularVelocity": 7.050625321396259e-7, + "velocityX": 4.12473012472298, + "velocityY": -1.6848352572290692, + "timestamp": 3.6722501383112154 + }, + { + "x": 5.536012172698975, + "y": 0.7129083275794983, + "heading": 3.1544674706525395, + "angularVelocity": 9.172112660822983e-7, + "velocityX": 4.230728133252795, + "velocityY": -1.3975003115290687, + "timestamp": 3.7371419994347157 + }, + { + "x": 5.851986653314552, + "y": 0.6350614800439672, + "heading": 3.1539640111947476, + "angularVelocity": -0.006887282897011849, + "velocityX": 4.322504230591005, + "velocityY": -1.0649383050015728, + "timestamp": 3.8102418655677273 + }, + { + "x": 6.163084948302816, + "y": 0.5819578289574815, + "heading": 3.153020697079212, + "angularVelocity": -0.012904457496809069, + "velocityX": 4.25579842269744, + "velocityY": -0.7264534655899257, + "timestamp": 3.883341731700739 + }, + { + "x": 6.458568718598011, + "y": 0.5486582997656804, + "heading": 3.1517988719890786, + "angularVelocity": -0.01671446412651432, + "velocityX": 4.042193042563675, + "velocityY": -0.4555347492868147, + "timestamp": 3.9564415978337504 + }, + { + "x": 6.732865281543566, + "y": 0.5290381776313112, + "heading": 3.1504499662020073, + "angularVelocity": -0.018452917336628596, + "velocityX": 3.752353833951604, + "velocityY": -0.26840161510923477, + "timestamp": 4.029541463966762 + }, + { + "x": 6.98366753282154, + "y": 0.5185855599477185, + "heading": 3.149074247230133, + "angularVelocity": -0.018819719442045503, + "velocityX": 3.430953632960416, + "velocityY": -0.14299092784347003, + "timestamp": 4.1026413300997735 + }, + { + "x": 7.210009572737756, + "y": 0.5142749171101598, + "heading": 3.147735302871607, + "angularVelocity": -0.018316645834748266, + "velocityX": 3.096340005662489, + "velocityY": -0.058969230254336036, + "timestamp": 4.175741196232785 + }, + { + "x": 7.411462955935819, + "y": 0.5140361320742359, + "heading": 3.1464751819616255, + "angularVelocity": -0.017238347710359158, + "velocityX": 2.755865282044427, + "velocityY": -0.003266559141017675, + "timestamp": 4.248841062365797 + }, + { + "x": 7.587830777283194, + "y": 0.5163933493371166, + "heading": 3.145323181311359, + "angularVelocity": -0.015759271681428454, + "velocityX": 2.4126969128296243, + "velocityY": 0.03224653323703015, + "timestamp": 4.321940928498808 + }, + { + "x": 7.739023807932044, + "y": 0.5202518916978398, + "heading": 3.144300674526786, + "angularVelocity": -0.013987806526385212, + "velocityX": 2.068307900506172, + "velocityY": 0.052784533882760894, + "timestamp": 4.39504079463182 + }, + { + "x": 7.865006007354459, + "y": 0.5247719050263068, + "heading": 3.1434238563856503, + "angularVelocity": -0.011994798178430731, + "velocityX": 1.7234258568022467, + "velocityY": 0.0618334009017538, + "timestamp": 4.468140660764831 + }, + { + "x": 7.96576870810601, + "y": 0.5292907028384434, + "heading": 3.1427053813238377, + "angularVelocity": -0.009828678215427785, + "velocityX": 1.378425243189977, + "velocityY": 0.06181677274092789, + "timestamp": 4.541240526897843 + }, + { + "x": 8.041317623102847, + "y": 0.5332730840639932, + "heading": 3.1421553895932863, + "angularVelocity": -0.007523840461631992, + "velocityX": 1.0335027817885274, + "velocityY": 0.05447863910316233, + "timestamp": 4.614340393030854 + }, + { + "x": 8.091665990307739, + "y": 0.5362783280705644, + "heading": 3.1417821778172224, + "angularVelocity": -0.005105505602228686, + "velocityX": 0.688761414600654, + "velocityY": 0.04111148440549697, + "timestamp": 4.687440259163866 + }, + { + "x": 8.116830825805664, + "y": 0.5379375219345093, + "heading": 3.141592653589793, + "angularVelocity": -0.0025926754377961075, + "velocityX": 0.3442528260193612, + "velocityY": 0.02269763204389355, + "timestamp": 4.7605401252968775 + }, + { + "x": 8.116830825805664, + "y": 0.5379375219345093, + "heading": 3.141592653589793, + "angularVelocity": 2.6782007527142383e-24, + "velocityX": -3.91857307116395e-24, + "velocityY": 2.7111362097714367e-23, + "timestamp": 4.833639991429889 + }, + { + "x": 8.089255596234777, + "y": 0.54942402960786, + "heading": 3.1321392994576667, + "angularVelocity": -0.11847903719259653, + "velocityX": -0.34560078933473104, + "velocityY": 0.14396058275432985, + "timestamp": 4.913429246610612 + }, + { + "x": 8.034097606978237, + "y": 0.5724008755052853, + "heading": 3.113461638242577, + "angularVelocity": -0.23408742408717345, + "velocityX": -0.6912959537171532, + "velocityY": 0.2879691738616022, + "timestamp": 4.993218501791334 + }, + { + "x": 7.9513480563517875, + "y": 0.606872555226039, + "heading": 3.085834950822301, + "angularVelocity": -0.346245711376821, + "velocityX": -1.0371014297479375, + "velocityY": 0.43203410838559014, + "timestamp": 5.073007756972057 + }, + { + "x": 7.840996433281568, + "y": 0.652844444117696, + "heading": 3.0496002995849607, + "angularVelocity": -0.4541294583496149, + "velocityX": -1.3830386412340105, + "velocityY": 0.5761664122259446, + "timestamp": 5.15279701215278 + }, + { + "x": 7.703029917572737, + "y": 0.7103230512929849, + "heading": 3.005193906243408, + "angularVelocity": -0.5565460316802363, + "velocityX": -1.7291365284252544, + "velocityY": 0.720380294879303, + "timestamp": 5.232586267333502 + }, + { + "x": 7.537432570924173, + "y": 0.7793163153547259, + "heading": 2.9531931917128307, + "angularVelocity": -0.65172577952753, + "velocityX": -2.075434170597116, + "velocityY": 0.8646936721676576, + "timestamp": 5.312375522514225 + }, + { + "x": 7.344184305821202, + "y": 0.8598339479102136, + "heading": 2.8943934690100765, + "angularVelocity": -0.7369378567273606, + "velocityX": -2.421983569908799, + "velocityY": 1.0091287651841792, + "timestamp": 5.392164777694948 + }, + { + "x": 7.123259821783499, + "y": 0.9518877987646011, + "heading": 2.8299481315961454, + "angularVelocity": -0.8076944353968832, + "velocityX": -2.7688500605414945, + "velocityY": 1.1537123719965283, + "timestamp": 5.47195403287567 + }, + { + "x": 6.874628809836458, + "y": 1.055491871977118, + "heading": 2.761663241881283, + "angularVelocity": -0.8558156052490734, + "velocityX": -3.1160964140333447, + "velocityY": 1.2984714919051967, + "timestamp": 5.551743288056393 + }, + { + "x": 6.598266120224734, + "y": 1.1706587451437058, + "heading": 2.692764023482451, + "angularVelocity": -0.8635149963835985, + "velocityX": -3.4636579698076297, + "velocityY": 1.4433882470231743, + "timestamp": 5.631532543237116 + }, + { + "x": 6.294260106065497, + "y": 1.2973571312742034, + "heading": 2.630823871265101, + "angularVelocity": -0.7762969096159158, + "velocityX": -3.810112194563846, + "velocityY": 1.5879128817975097, + "timestamp": 5.711321798417838 + }, + { + "x": 5.9677610677766175, + "y": 1.434232859949075, + "heading": 2.6252073225176806, + "angularVelocity": -0.07039229448500041, + "velocityX": -4.092017622540287, + "velocityY": 1.715465677237474, + "timestamp": 5.791111053598561 + }, + { + "x": 5.6396041489574, + "y": 1.5709726586832762, + "heading": 2.6252073065947137, + "angularVelocity": -1.9956279736518076e-7, + "velocityX": -4.1127958655077475, + "velocityY": 1.7137620651362904, + "timestamp": 5.870900308779284 + }, + { + "x": 5.311447232039342, + "y": 1.7077124619800061, + "heading": 2.6252072906717543, + "angularVelocity": -1.995627014046446e-7, + "velocityX": -4.1127958416804935, + "velocityY": 1.7137621223185358, + "timestamp": 5.950689563960006 + }, + { + "x": 4.9832903234128985, + "y": 1.8444522851754865, + "heading": 2.6252072747487953, + "angularVelocity": -1.9956269204035307e-7, + "velocityX": -4.112795737761537, + "velocityY": 1.713762371709888, + "timestamp": 6.030478819140729 + }, + { + "x": 4.655133427137336, + "y": 1.9811921380113924, + "heading": 2.625207258825836, + "angularVelocity": -1.9956270204696126e-7, + "velocityX": -4.112795582967754, + "velocityY": 1.7137627431938112, + "timestamp": 6.110268074321452 + }, + { + "x": 4.326976566232365, + "y": 2.117932075731836, + "heading": 2.625207242902869, + "angularVelocity": -1.9956279397787226e-7, + "velocityX": -4.112795139667575, + "velocityY": 1.7137638070530719, + "timestamp": 6.190057329502174 + }, + { + "x": 3.999438899364335, + "y": 2.256148618450317, + "heading": 2.6252072265396365, + "angularVelocity": -2.050806574466794e-7, + "velocityX": -4.105034770987135, + "velocityY": 1.7322701209006253, + "timestamp": 6.269846584682897 + }, + { + "x": 3.6745952182551624, + "y": 2.394736220372859, + "heading": 2.616353473366314, + "angularVelocity": -0.11096422887102513, + "velocityX": -4.071271004766277, + "velocityY": 1.7369206117871623, + "timestamp": 6.34963583986362 + }, + { + "x": 3.371360246276191, + "y": 2.5227681355611673, + "heading": 2.54789899304901, + "angularVelocity": -0.8579410869578161, + "velocityX": -3.8004487106960148, + "velocityY": 1.6046260226181663, + "timestamp": 6.429425095044342 + }, + { + "x": 3.09571714689087, + "y": 2.6391416829563425, + "heading": 2.4725529095377854, + "angularVelocity": -0.9443136590329878, + "velocityX": -3.454639334093173, + "velocityY": 1.4585115142582736, + "timestamp": 6.509214350225065 + }, + { + "x": 2.8477516687682445, + "y": 2.7438274798328672, + "heading": 2.3982762432251263, + "angularVelocity": -0.9309106363309555, + "velocityX": -3.1077552680619074, + "velocityY": 1.3120287517336962, + "timestamp": 6.589003605405788 + }, + { + "x": 2.6274310870254833, + "y": 2.83684141662163, + "heading": 2.3284405360388263, + "angularVelocity": -0.875252025202168, + "velocityX": -2.7612813435059107, + "velocityY": 1.1657451442313427, + "timestamp": 6.66879286058651 + }, + { + "x": 2.4347195920645524, + "y": 2.918199144350896, + "heading": 2.2649041443604916, + "angularVelocity": -0.7963026040840354, + "velocityX": -2.4152562212097375, + "velocityY": 1.019657691314338, + "timestamp": 6.748582115767233 + }, + { + "x": 2.269588113206454, + "y": 2.9879129508006694, + "heading": 2.208834388541783, + "angularVelocity": -0.7027231384941546, + "velocityX": -2.0695954421942657, + "velocityY": 0.8737242413389122, + "timestamp": 6.828371370947956 + }, + { + "x": 2.1320138837828355, + "y": 3.0459924554790536, + "heading": 2.1610282462744177, + "angularVelocity": -0.5991551388603021, + "velocityX": -1.7242199981941622, + "velocityY": 0.7279113528110278, + "timestamp": 6.908160626128678 + }, + { + "x": 2.0219789365980305, + "y": 3.092445388285202, + "heading": 2.1220638838563577, + "angularVelocity": -0.4883409718489752, + "velocityX": -1.379069737342154, + "velocityY": 0.5821953432317734, + "timestamp": 6.987949881309401 + }, + { + "x": 1.93946881514082, + "y": 3.1272780949907744, + "heading": 2.09238087210519, + "angularVelocity": -0.37201765681275867, + "velocityX": -1.0341006601744267, + "velocityY": 0.4365588653093307, + "timestamp": 7.067739136490124 + }, + { + "x": 1.884471623102071, + "y": 3.1504958321346646, + "heading": 2.0723259960019775, + "angularVelocity": -0.25134808011164267, + "velocityX": -0.6892806796376343, + "velocityY": 0.29098826767216296, + "timestamp": 7.147528391670846 + }, + { + "x": 1.8569773435592651, + "y": 3.1621029376983643, + "heading": 2.0621810870916404, + "angularVelocity": -0.12714630419044576, + "velocityX": -0.344586241349558, + "velocityY": 0.14547203802578762, + "timestamp": 7.227317646851569 + }, + { + "x": 1.8569773435592651, + "y": 3.1621029376983643, + "heading": 2.0621810870916404, + "angularVelocity": 1.0052435240302537e-24, + "velocityX": -4.289779840395474e-24, + "velocityY": -8.499784651893661e-24, + "timestamp": 7.307106902032292 + } + ], + "eventMarkers": [] +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/AmpWing3.auto b/src/main/deploy/pathplanner/autos/AmpWing3.auto new file mode 100644 index 00000000..09757452 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/AmpWing3.auto @@ -0,0 +1,50 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8817893862724304, + "y": 6.609423637390137 + }, + "rotation": -121.26372732872956 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "AmpWing3Contested5.1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/AmpWing3Contested5.auto b/src/main/deploy/pathplanner/autos/AmpWing3Contested5.auto new file mode 100644 index 00000000..611f181d --- /dev/null +++ b/src/main/deploy/pathplanner/autos/AmpWing3Contested5.auto @@ -0,0 +1,139 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8817893862724304, + "y": 6.609423637390137 + }, + "rotation": -121.26372732872956 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "AmpWing3Contested5.1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.25 + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "AmpWing3Contested5.2" + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "AmpWing3Contested5.3" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/ChoreoTest2.auto b/src/main/deploy/pathplanner/autos/ChoreoTest2.auto deleted file mode 100644 index 4f6033f3..00000000 --- a/src/main/deploy/pathplanner/autos/ChoreoTest2.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.3137660622596754, - "y": 6.865104675292969 - }, - "rotation": 179.6548494375836 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Test2.1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 2.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Test2.2" - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto b/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto index 1fd0c9df..ca62f8d2 100644 --- a/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto +++ b/src/main/deploy/pathplanner/autos/FrontWing3Contested5.auto @@ -66,9 +66,22 @@ } }, { - "type": "named", + "type": "sequential", "data": { - "name": "Intake" + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 2.0 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto deleted file mode 100644 index 6ce2c5d3..00000000 --- a/src/main/deploy/pathplanner/autos/New Auto.auto +++ /dev/null @@ -1,37 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.4869295358657837, - "y": 5.564249038696289 - }, - "rotation": 179.96604345059157 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Test1.1" - } - }, - { - "type": "wait", - "data": { - "waitTime": 5.0 - } - }, - { - "type": "path", - "data": { - "pathName": "Test1.2" - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/PathPlannerTest.auto b/src/main/deploy/pathplanner/autos/PathPlannerTest.auto deleted file mode 100644 index e2736b56..00000000 --- a/src/main/deploy/pathplanner/autos/PathPlannerTest.auto +++ /dev/null @@ -1,19 +0,0 @@ -{ - "version": 1.0, - "startingPose": null, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "OnePieceAmpStart" - } - } - ] - } - }, - "folder": null, - "choreoAuto": false -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceContested1.auto b/src/main/deploy/pathplanner/autos/SourceContested1.auto new file mode 100644 index 00000000..50dc199b --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SourceContested1.auto @@ -0,0 +1,82 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.8210453391075134, + "y": 4.576303482055664 + }, + "rotation": 120.96374169030298 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceContested1.1" + } + }, + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + } + ] + } + }, + { + "type": "sequential", + "data": { + "commands": [ + { + "type": "wait", + "data": { + "waitTime": 0.5 + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SourceContested1.2" + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceWing1.auto b/src/main/deploy/pathplanner/autos/SourceWing1.auto new file mode 100644 index 00000000..221fad14 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SourceWing1.auto @@ -0,0 +1,56 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.7885606288909912, + "y": 4.587131977081299 + }, + "rotation": 118.495649157688 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceWing1.1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "path", + "data": { + "pathName": "SourceWing1.2" + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/SourceWing1Contested1.auto b/src/main/deploy/pathplanner/autos/SourceWing1Contested1.auto new file mode 100644 index 00000000..10deb348 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/SourceWing1Contested1.auto @@ -0,0 +1,126 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 0.6472748517990112, + "y": 4.497877597808838 + }, + "rotation": 123.17848329106131 + }, + "command": { + "type": "sequential", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceWing1Contested1.1" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceWing1Contested1.2" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceWing1Contested1.3" + } + }, + { + "type": "named", + "data": { + "name": "Intake" + } + } + ] + } + }, + { + "type": "parallel", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SourceWing1Contested1.4" + } + }, + { + "type": "race", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 1.5 + } + } + ] + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "AimAndShoot" + } + } + ] + } + }, + "folder": null, + "choreoAuto": true +} \ 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 921f88a9..16b788aa 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -115,7 +115,7 @@ private DriveConstants() { ); public static final HolonomicPathFollowerConfig HOLONOMIC_CONFIG = new HolonomicPathFollowerConfig( - new PIDConstants(4.0, 0.0), new PIDConstants(4.0, 0.0), + new PIDConstants(5.0, 0.4), new PIDConstants(5.0, 0.8), DriveConstants.MAX_LINEAR_SPEED * 0.5, DriveConstants.DRIVE_BASE_RADIUS, new ReplanningConfig()); public static final ModuleConstants FL_MOD_CONSTANTS = new ModuleConstants( @@ -230,6 +230,8 @@ private ArmConstants() {} } public static class ArmSetpoints { + public static final ArmPose PASS_SETPOINT = new ArmPose("ArmPoses/Pass Setpoint", false, 45, 55); + private ArmSetpoints() { throw new IllegalStateException("Static classes should not be constructed"); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 87189773..ac5b3eab 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -163,15 +163,25 @@ public RobotContainer() { private void configureButtonBindings() { Trigger intakeTrigger = controller.y().and(controller.x().negate()) .and(controller.a().negate()) // make sure we don't amp - .and(controller.b().negate()); + .and(controller.b().negate()) + .and(controller.leftTrigger().negate()); Trigger spinUpTrigger = controller.x().and(controller.y().negate()) .and(controller.a().negate()) // make sure we don't amp .and(controller.b().negate()); + Trigger passSpinUpTrigger = controller.leftTrigger() + .and(spinUpTrigger.negate()) + .and(controller.y().negate()); + + Trigger passTrigger = controller.leftTrigger() + .and(spinUpTrigger.negate()) + .and(controller.y()); + Trigger shootTrigger = controller.x().and(controller.y()) .and(controller.a().negate()) // make sure we don't amp - .and(controller.b().negate()); + .and(controller.b().negate()) + .and(controller.leftTrigger().negate()); Trigger ampLineupTrigger = controller.b().and(controller.a().negate()) .debounce(0.1, Debouncer.DebounceType.kBoth); @@ -224,9 +234,23 @@ private void configureButtonBindings() { m_shooter.runShooterVelocity(true, m_leftRPM.get(), m_rightRPM.get()) .alongWith(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.MANUAL_WRIST))); + passSpinUpTrigger.whileTrue( + m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.PASS) + .alongWith(m_shooter.runShooterVelocity(false, 4500, 4500))); + + passTrigger.whileTrue( + m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.PASS) + .alongWith(m_shooter.runShooterVelocity(true, 3500, 3500))); + + passTrigger.whileTrue(new AimbotCommand( + m_armSubsystem, + m_driveSubsystem, + m_shooter, + controller.getHID(), + true, + true)); + controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); - controller.rightTrigger().whileTrue(m_shooter.runShooterVelocity(false, m_leftRPM.get(), m_rightRPM.get())); - controller.leftTrigger().whileTrue(m_shooter.runShooterVelocity(true, m_leftRPM.get(), m_rightRPM.get())); controller.pov(0).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE)); // 96.240234375 diff --git a/src/main/java/frc/robot/commands/AimbotCommand.java b/src/main/java/frc/robot/commands/AimbotCommand.java index 63bd826f..b89159a7 100644 --- a/src/main/java/frc/robot/commands/AimbotCommand.java +++ b/src/main/java/frc/robot/commands/AimbotCommand.java @@ -37,14 +37,24 @@ public class AimbotCommand extends Command { private final PidProperty m_fastProperty; private boolean m_runKicker; + private boolean m_pass; private static final double TOLERENCE_DEGREES = 10.0; public AimbotCommand(ArmSubsystem armSubsystem, DriveSubsystem driveSubsystem, ShooterSubsystem shooterSubsystem, - XboxController driverController, + XboxController xboxController, boolean runKicker) { + this (armSubsystem, driveSubsystem, shooterSubsystem, xboxController, runKicker, false); + } + + public AimbotCommand(ArmSubsystem armSubsystem, + DriveSubsystem driveSubsystem, + ShooterSubsystem shooterSubsystem, + XboxController driverController, + boolean runKicker, + boolean pass) { this.m_armSubsystem = armSubsystem; this.m_driveSubsystem = driveSubsystem; this.m_shooterSubsystem = shooterSubsystem; @@ -56,6 +66,9 @@ public AimbotCommand(ArmSubsystem armSubsystem, m_smallController.enableContinuousInput(-180, 180); m_fastController.enableContinuousInput(-180, 180); + m_smallController.setTolerance(5.0); + m_fastController.setTolerance(10.0); + m_smallProperty = new WpiPidPropertyBuilder("Drive/Aimbot Small", false, m_smallController) .addP(1.0) .addI(0.0) @@ -68,6 +81,7 @@ public AimbotCommand(ArmSubsystem armSubsystem, .build(); m_runKicker = runKicker; + m_pass = pass; // each subsystem used by the command must be passed into the // addRequirements() method (which takes a vararg of Subsystem) @@ -89,29 +103,45 @@ public void execute() { FieldRelativeAccel fieldRelativeAccel = m_driveSubsystem.getFieldRelativeAcceleration(); // TODO make an actual equation for shot time based on distance - double distance = AimbotUtils.getDistanceFromSpeaker(m_driveSubsystem.getVisionPose()); - double shotTime = 1.05; + double shotTime = 0.5; - Translation2d target = FieldConstants.CENTER_SPEAKER.toTranslation2d(); - Translation2d movingTarget = new Translation2d(); + Translation3d target = FieldConstants.CENTER_SPEAKER; + Translation3d movingTarget = new Translation3d(); // loop over movement calcs to better adjust for acceleration if (true) { - for (int i = 0; i < 5; i++) { + for (int i = 0; i < 1; i++) { double virtualGoalX = target.getX() - + shotTime * (fieldRelativeSpeed.vx + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + - shotTime * ( + MathUtil.applyDeadband(fieldRelativeSpeed.vx, 0.15) + + MathUtil.applyDeadband( + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue(), 0.1)); + double virtualGoalY = target.getY() - + shotTime * (fieldRelativeSpeed.vy + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue()); + - shotTime * ( + MathUtil.applyDeadband(fieldRelativeSpeed.vy, 0.15) + + MathUtil.applyDeadband( + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue(), 0.1)); - movingTarget = new Translation2d(virtualGoalX, virtualGoalY); + movingTarget = new Translation3d(virtualGoalX, virtualGoalY, 0.0); } } else { movingTarget = target; } + Logger.recordOutput("Aimbot/Target",target); + Logger.recordOutput("Aimbot/Moving Target", movingTarget); + + Logger.recordOutput("Aimbot/Field Relative Velocity", + new ChassisSpeeds( + fieldRelativeSpeed.vx, + fieldRelativeSpeed.vy, + fieldRelativeSpeed.omega + )); + // get our desired rotation and error from it double desiredRotationDegs = - AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose()) + AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose(), movingTarget) .getDegrees(); double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees()); @@ -126,15 +156,21 @@ public void execute() { // add a feedforward component to compensate for horizontal movement Translation2d linearFieldVelocity = new Translation2d(fieldRelativeSpeed.vx, fieldRelativeSpeed.vy); Translation2d tangentalVelocity = linearFieldVelocity - .rotateBy(Rotation2d.fromDegrees(-desiredRotationDegs).unaryMinus()); + .rotateBy(Rotation2d.fromDegrees(desiredRotationDegs).unaryMinus()); double tangentalComponent = tangentalVelocity.getX(); double x = -DriveCommands.setSensitivity(-m_driverController.getLeftY(), 0.25); double y = -DriveCommands.setSensitivity(-m_driverController.getLeftX(), 0.25); + Translation2d linearSpeedVector = new Translation2d(x, y); + double omegaFF = linearSpeedVector.getY() * Constants.DriveConstants.MAX_ANGULAR_SPEED * 0.5; + x = MathUtil.applyDeadband(x, 0.1); y = MathUtil.applyDeadband(y, 0.1); + x = MathUtil.clamp(x, -0.25, 0.25); + y = MathUtil.clamp(y, -0.25, 0.25); + Rotation2d heading; // if red change heading goal @@ -145,17 +181,17 @@ public void execute() { heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); } - Logger.recordOutput("Aimbot/Tangental Velocity", tangentalComponent); + Logger.recordOutput("Aimbot/Linear Speed", omegaFF); // Convert to field relative speeds & send command m_driveSubsystem.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( x * Constants.DriveConstants.MAX_LINEAR_SPEED, y * Constants.DriveConstants.MAX_LINEAR_SPEED, - omega + tangentalComponent, + omega,// - (omegaFF * 0.25), heading )); - m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM); + m_armSubsystem.setDesiredState(m_pass ? ArmSubsystem.ArmState.PASS : ArmSubsystem.ArmState.AUTO_AIM); m_shooterSubsystem.runShooterVelocity(m_runKicker).execute(); // set shooter speeds and rumble controller diff --git a/src/main/java/frc/robot/commands/auto/AutoFactory.java b/src/main/java/frc/robot/commands/auto/AutoFactory.java index 69d25819..3db1ff7c 100644 --- a/src/main/java/frc/robot/commands/auto/AutoFactory.java +++ b/src/main/java/frc/robot/commands/auto/AutoFactory.java @@ -13,7 +13,12 @@ public enum AutoModes { FRONT_WING_1("FrontWing1"), FRONT_WING_2("FrontWing2"), FRONT_WING_3("FrontWing3"), - FRONT_WING_3_CONTESTED_5("FrontWing3Contested5"); + FRONT_WING_3_CONTESTED_5("FrontWing3Contested5"), + AMP_WING_3("AmpWing3"), + AMP_WING_3_CONTESTED_5("AmpWing3Contested5"), + SOURCE_WING_1("SourceWing1"), + SOURCE_CONTESTED_1("SourceContested1"), + SOURCE_WING_1_CONTESTED_1("SourceWing1Contested1"); public final String m_modeName; diff --git a/src/main/java/frc/robot/commands/auto/IntakeControlCommand.java b/src/main/java/frc/robot/commands/auto/IntakeControlCommand.java index 3deaf298..f914974d 100644 --- a/src/main/java/frc/robot/commands/auto/IntakeControlCommand.java +++ b/src/main/java/frc/robot/commands/auto/IntakeControlCommand.java @@ -37,31 +37,21 @@ public void execute() { m_shooterSubsystem.setShooterPowerLeft(-0.1); m_shooterSubsystem.setShooterPowerRight(-0.1); m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.INTAKE); - } else if (m_shooterSubsystem.hasPiece() && !m_timer.hasElapsed(0.0001)) { - // stop running intake - m_shooterSubsystem.setIntakePower(0.0); - m_shooterSubsystem.setKickerPower(0.0); - m_shooterSubsystem.setShooterPowerLeft(0.0); - m_shooterSubsystem.setShooterPowerRight(0.0); - - // piece detected, mark as we have a piece and start moving up - m_timer.start(); - m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); - } else if (!m_timer.hasElapsed(0.075) && m_armSubsystem.bothAtSetpoint()) { - // arm is up, haven't run kickers back yet - m_shooterSubsystem.setKickerPower(-0.2); } else { m_shooterSubsystem.setIntakePower(0.0); m_shooterSubsystem.setKickerPower(0.0); m_shooterSubsystem.setShooterPowerLeft(0.0); m_shooterSubsystem.setShooterPowerRight(0.0); } + + if (m_shooterSubsystem.hasPiece()) { + m_timer.restart(); + } } @Override public boolean isFinished() { - // We won't return true because it's being ran by a .whileTrue() method - return false; + return m_shooterSubsystem.hasPiece() && m_timer.hasElapsed(0.25); } @Override diff --git a/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java b/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java index 319d7053..710240c2 100644 --- a/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java +++ b/src/main/java/frc/robot/commands/auto/ShooterAutoCommand.java @@ -1,6 +1,7 @@ package frc.robot.commands.auto; import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj.Timer; @@ -10,6 +11,7 @@ import frc.robot.subsystems.shooter.ShooterSubsystem; import lib.utils.AimbotUtils; import lib.utils.FieldConstants; +import org.littletonrobotics.junction.Logger; public class ShooterAutoCommand extends Command { @@ -44,12 +46,16 @@ public void initialize() { @Override public void execute() { // Calculate output to align to speaker - double omega = m_driveSubsystem.alignToAngle(AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose())); + Rotation2d desiredAngle = AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose()); + double omega = m_driveSubsystem.alignToAngle(desiredAngle); m_driveSubsystem.runVelocity(new ChassisSpeeds(0.0, 0.0, omega)); + double error = Math.abs(desiredAngle.getDegrees() - m_driveSubsystem.getRotation().getDegrees()); + Logger.recordOutput("Shooter/Auto Error", error); + // only actually shoot if we're aligned close enough to speaker and flywheels are at speed m_shooterSubsystem.runShooterVelocity(m_shooterSubsystem.atSpeed() - && omega < 1).execute(); + && error < 10.0 && m_armSubsystem.wristAtSetpoint()).execute(); m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.AUTO_AIM); // check to see if the state of having a note has changed, mark if it has @@ -67,7 +73,7 @@ public boolean isFinished() { // too long check may change as system gets tuned return (!m_shooterSubsystem.hasPiece() && m_hasChanged - && m_timer.hasElapsed(0.5)) || m_timer.hasElapsed(5.0); + && m_timer.hasElapsed(0.75)) || m_timer.hasElapsed(5.0); } @Override @@ -75,4 +81,5 @@ public void end(boolean interrupted) { m_shooterSubsystem.setShooterPowerFactory(0.0, 0.0, 0.0).execute(); m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.STOW); } + } diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 9757d7ef..2a316977 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -31,6 +31,7 @@ public enum ArmState { TRANSITION_AMP, SOURCE, TRANSITION_SOURCE, + PASS, DISABLED, MANUAL_WRIST } @@ -176,6 +177,10 @@ public void handleState() { m_desiredArmPoseDegs = ArmSetpoints.AMP_SETPOINT.armAngle(); m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristAngle(); } + case PASS -> { + m_desiredWristPoseDegs = 50.0; + m_desiredArmPoseDegs = 0.0; + } case MANUAL_WRIST -> { m_desiredWristPoseDegs = ArmSetpoints.STATIC_SHOOTER.wristAngle(); diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 704d66e5..74f223a5 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -250,9 +250,9 @@ public void periodic() { } // make sure we're not moving too fast before trying to update vision poses - if ((kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond <= DriveConstants.MAX_LINEAR_SPEED / 2.0) - && (kinematics.toChassisSpeeds(getModuleStates()).vyMetersPerSecond <= DriveConstants.MAX_LINEAR_SPEED / 2.0) - && (kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond <= DriveConstants.MAX_ANGULAR_SPEED / 2.0) + if ((kinematics.toChassisSpeeds(getModuleStates()).vxMetersPerSecond <= DriveConstants.MAX_LINEAR_SPEED) + && (kinematics.toChassisSpeeds(getModuleStates()).vyMetersPerSecond <= DriveConstants.MAX_LINEAR_SPEED) + && (kinematics.toChassisSpeeds(getModuleStates()).omegaRadiansPerSecond <= DriveConstants.MAX_ANGULAR_SPEED) /* || DriverStation.isTeleop() */) { for (VisionSubsystem camera : m_cameras) { camera.updateInputs(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java index f77374a9..86addac0 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOKraken.java @@ -166,8 +166,8 @@ public void updateInputs(ShooterIOInputsAutoLogged inputs) { m_indexerTemperatureSignal ); - inputs.tlVelocityRPM = m_leftVelSignal.getValueAsDouble() * 60.0; - inputs.trVelocityRPM = m_rightVelSignal.getValueAsDouble() * 60.0; + inputs.tlVelocityRPM = m_leftVelSignal.refresh().getValueAsDouble() * 60.0; + inputs.trVelocityRPM = m_rightVelSignal.refresh().getValueAsDouble() * 60.0; inputs.tlAppliedVolts = m_leftVoltOutSignal.getValueAsDouble(); inputs.trAppliedVolts = m_rightVoltOutSignal.getValueAsDouble(); diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java index a747de73..5e90f074 100644 --- a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java @@ -124,7 +124,7 @@ public Command intakeCommand(double intakePower, double kickerPower, double time setKickerPower(kickerPower); timer.restart(); } else if (!timer.hasElapsed(timeout)) { - setKickerPower(-0.1); + setKickerPower(-0.13); setIntakePower(0.0); } else { setShooterPowerRight(0.0); @@ -170,7 +170,7 @@ public void setupLogging() { } public boolean atSpeed() { - return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 100 - && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 100; + return Math.abs(m_leftSpeedSetpoint - m_inputs.tlVelocityRPM) < 75 + && Math.abs(m_rightSpeedSetpoint - m_inputs.trVelocityRPM) < 75; } } diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java index faad2dd4..e9550245 100644 --- a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java +++ b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java @@ -136,5 +136,9 @@ public int[] getTargetIDs () { return results; } + + public boolean apriltagConnected() { + return m_camera.isConnected() && m_camera.getPipelineIndex() == 0; + } } diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 04afe75c..607f7fb3 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -17,7 +17,7 @@ public class AimbotUtils { private static final LoggedDashboardNumber m_offsetNudge = new LoggedDashboardNumber("Wrist Angle Nudge", 0.01); - private static final double Y_TARGET = 0.35; + private static final double Y_TARGET = 0.2; static { // angle measurements, meters -> degrees @@ -47,16 +47,18 @@ public class AimbotUtils { /** Linear interpolation tables for aiming */ public static double getWristAngle(double distance) { -// return m_angleLerpTable.get(distance); -// return 52.409 - ((0.1224 + m_offsetNudge.get()) * distance); -// return 48.903-(0.09001 * distance); -// if (distance <= 100.0) { -// return 50.218 - (0.1108 * distance); -// } else if (100.0 < distance && distance <= 150) { - return 49.319 + (1.427 * Y_TARGET) + (-0.10599 * distance); -// } else { -// return 50.951 - (0.117 * distance); -// } + double angle = 49.319 + (1.427 * Y_TARGET) + (-0.10599 * distance); + if (100.0 >= distance && distance > 60.0) { + return angle; + } else if (150.0 >= distance && distance > 100.0) { + return angle - 1.75; + } else if (175.0 >= distance && distance > 150.0) { + return angle - 3.5; + } else if (distance > 175.0) { + return angle; + } else { + return 55.0; + } } public static double getLeftSpeed(double distance) { From f424b0da90e5a6315ecc519bdf941e3bab903dc8 Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sat, 16 Mar 2024 20:15:27 -0400 Subject: [PATCH 29/30] tuned some values and added manual override if lose cameras --- src/main/java/frc/robot/RobotContainer.java | 13 +- .../frc/robot/commands/AimbotCommand.java | 185 ++++++++++-------- .../frc/robot/commands/DriveCommands.java | 2 +- .../robot/subsystems/arm/ArmSubsystem.java | 6 +- .../subsystems/drive/DriveSubsystem.java | 9 +- src/main/java/lib/utils/AimbotUtils.java | 7 +- 6 files changed, 122 insertions(+), 100 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ac5b3eab..d6719a3a 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -236,23 +236,18 @@ private void configureButtonBindings() { passSpinUpTrigger.whileTrue( m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.PASS) - .alongWith(m_shooter.runShooterVelocity(false, 4500, 4500))); + .alongWith(m_shooter.runShooterVelocity(false, 3500, 3500))); passTrigger.whileTrue( m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.PASS) .alongWith(m_shooter.runShooterVelocity(true, 3500, 3500))); - passTrigger.whileTrue(new AimbotCommand( - m_armSubsystem, - m_driveSubsystem, - m_shooter, - controller.getHID(), - true, - true)); - controller.pov(180).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.AMP)); controller.pov(0).whileTrue(m_armSubsystem.setDesiredStateFactory(ArmSubsystem.ArmState.ANTI_DEFENSE)); + controller.pov(90).whileTrue(m_shooter.intakeCommand(-0.75, -0.75, 0.0)) + .whileFalse(m_shooter.intakeCommand(0.0, 0.0, 0.0)); + // 96.240234375 // 60.029296875 // 2250 diff --git a/src/main/java/frc/robot/commands/AimbotCommand.java b/src/main/java/frc/robot/commands/AimbotCommand.java index b89159a7..0f0ff8cd 100644 --- a/src/main/java/frc/robot/commands/AimbotCommand.java +++ b/src/main/java/frc/robot/commands/AimbotCommand.java @@ -95,110 +95,125 @@ public void initialize() { @Override public void execute() { - m_smallProperty.updateIfChanged(); - m_fastProperty.updateIfChanged(); - - // get the robots velocity and acceleration - FieldRelativeSpeed fieldRelativeSpeed = m_driveSubsystem.getFieldRelativeVelocity(); - FieldRelativeAccel fieldRelativeAccel = m_driveSubsystem.getFieldRelativeAcceleration(); - - // TODO make an actual equation for shot time based on distance - double shotTime = 0.5; - - Translation3d target = FieldConstants.CENTER_SPEAKER; - Translation3d movingTarget = new Translation3d(); - - // loop over movement calcs to better adjust for acceleration - if (true) { - for (int i = 0; i < 1; i++) { - double virtualGoalX = target.getX() - - shotTime * ( - MathUtil.applyDeadband(fieldRelativeSpeed.vx, 0.15) - + MathUtil.applyDeadband( - fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue(), 0.1)); - - double virtualGoalY = target.getY() - - shotTime * ( - MathUtil.applyDeadband(fieldRelativeSpeed.vy, 0.15) - + MathUtil.applyDeadband( - fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue(), 0.1)); - - movingTarget = new Translation3d(virtualGoalX, virtualGoalY, 0.0); - } - } else { - movingTarget = target; - } - - Logger.recordOutput("Aimbot/Target",target); - Logger.recordOutput("Aimbot/Moving Target", movingTarget); - - Logger.recordOutput("Aimbot/Field Relative Velocity", - new ChassisSpeeds( - fieldRelativeSpeed.vx, - fieldRelativeSpeed.vy, - fieldRelativeSpeed.omega - )); - - // get our desired rotation and error from it - double desiredRotationDegs = - AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose(), movingTarget) - .getDegrees(); - double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees()); - - // if we're far from our setpoint, move faster - double omega;// = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs);; - if (error > 5.0) { - omega = m_fastController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); - } else { - omega = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); - } - - // add a feedforward component to compensate for horizontal movement - Translation2d linearFieldVelocity = new Translation2d(fieldRelativeSpeed.vx, fieldRelativeSpeed.vy); - Translation2d tangentalVelocity = linearFieldVelocity - .rotateBy(Rotation2d.fromDegrees(desiredRotationDegs).unaryMinus()); - double tangentalComponent = tangentalVelocity.getX(); - double x = -DriveCommands.setSensitivity(-m_driverController.getLeftY(), 0.25); double y = -DriveCommands.setSensitivity(-m_driverController.getLeftX(), 0.25); - Translation2d linearSpeedVector = new Translation2d(x, y); - double omegaFF = linearSpeedVector.getY() * Constants.DriveConstants.MAX_ANGULAR_SPEED * 0.5; - x = MathUtil.applyDeadband(x, 0.1); y = MathUtil.applyDeadband(y, 0.1); - x = MathUtil.clamp(x, -0.25, 0.25); - y = MathUtil.clamp(y, -0.25, 0.25); + double o = -DriveCommands.setSensitivity(-m_driverController.getRightX(), 0.15); + o = MathUtil.applyDeadband(0, 0.1); Rotation2d heading; // if red change heading goal if (DriverStation.getAlliance().isPresent() - && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { + && DriverStation.getAlliance().get() == DriverStation.Alliance.Red) { heading = m_driveSubsystem.getRotation(); } else { heading = m_driveSubsystem.getRotation().plus(Rotation2d.fromDegrees(180)); } - Logger.recordOutput("Aimbot/Linear Speed", omegaFF); - - // Convert to field relative speeds & send command - m_driveSubsystem.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( - x * Constants.DriveConstants.MAX_LINEAR_SPEED, - y * Constants.DriveConstants.MAX_LINEAR_SPEED, - omega,// - (omegaFF * 0.25), - heading - )); + if (m_driveSubsystem.hasAprilTagCams()) { + m_smallProperty.updateIfChanged(); + m_fastProperty.updateIfChanged(); + + // get the robots velocity and acceleration + FieldRelativeSpeed fieldRelativeSpeed = m_driveSubsystem.getFieldRelativeVelocity(); + FieldRelativeAccel fieldRelativeAccel = m_driveSubsystem.getFieldRelativeAcceleration(); + + // TODO make an actual equation for shot time based on distance + double shotTime = 0.5; + + Translation3d target = FieldConstants.CENTER_SPEAKER; + Translation3d movingTarget = new Translation3d(); + + // loop over movement calcs to better adjust for acceleration + if (true) { + for (int i = 0; i < 1; i++) { + double virtualGoalX = target.getX() + - shotTime * ( + MathUtil.applyDeadband(fieldRelativeSpeed.vx, 0.15) + + MathUtil.applyDeadband( + fieldRelativeAccel.ax * ShooterConstants.ACCEL_COMP_FACTOR.getValue(), 0.1)); + + double virtualGoalY = target.getY() + - shotTime * ( + MathUtil.applyDeadband(fieldRelativeSpeed.vy, 0.15) + + MathUtil.applyDeadband( + fieldRelativeAccel.ay * ShooterConstants.ACCEL_COMP_FACTOR.getValue(), 0.1)); + + movingTarget = new Translation3d(virtualGoalX, virtualGoalY, 0.0); + } + } else { + movingTarget = target; + } - m_armSubsystem.setDesiredState(m_pass ? ArmSubsystem.ArmState.PASS : ArmSubsystem.ArmState.AUTO_AIM); - m_shooterSubsystem.runShooterVelocity(m_runKicker).execute(); + Logger.recordOutput("Aimbot/Target", target); + Logger.recordOutput("Aimbot/Moving Target", movingTarget); + + Logger.recordOutput("Aimbot/Field Relative Velocity", + new ChassisSpeeds( + fieldRelativeSpeed.vx, + fieldRelativeSpeed.vy, + fieldRelativeSpeed.omega + )); + + // get our desired rotation and error from it + double desiredRotationDegs = + AimbotUtils.getDrivebaseAimingAngle(m_driveSubsystem.getVisionPose(), movingTarget) + .getDegrees(); + double error = Math.abs(desiredRotationDegs - m_driveSubsystem.getRotation().getDegrees()); + + x = MathUtil.clamp(x, -0.25, 0.25); + y = MathUtil.clamp(y, -0.25, 0.25); + + // if we're far from our setpoint, move faster + double omega;// = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs);; + if (error > 5.0) { + omega = m_fastController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); + } else { + omega = m_smallController.calculate(m_driveSubsystem.getRotation().getDegrees(), desiredRotationDegs); + } - // set shooter speeds and rumble controller - if (m_shooterSubsystem.atSpeed() && error < 15.0) { - m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 1.0); + // add a feedforward component to compensate for horizontal movement + Translation2d linearFieldVelocity = new Translation2d(fieldRelativeSpeed.vx, fieldRelativeSpeed.vy); + Translation2d tangentalVelocity = linearFieldVelocity + .rotateBy(Rotation2d.fromDegrees(desiredRotationDegs).unaryMinus()); + double tangentalComponent = tangentalVelocity.getX(); + + Translation2d linearSpeedVector = new Translation2d(x, y); + double omegaFF = linearSpeedVector.getY() * Constants.DriveConstants.MAX_ANGULAR_SPEED * 0.5; + + Logger.recordOutput("Aimbot/Linear Speed", omegaFF); + + // Convert to field relative speeds & send command + m_driveSubsystem.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( + x * Constants.DriveConstants.MAX_LINEAR_SPEED, + y * Constants.DriveConstants.MAX_LINEAR_SPEED, + omega,// - (omegaFF * 0.25), + heading + )); + + m_armSubsystem.setDesiredState(m_pass ? ArmSubsystem.ArmState.PASS : ArmSubsystem.ArmState.AUTO_AIM); + m_shooterSubsystem.runShooterVelocity(m_runKicker).execute(); + + // set shooter speeds and rumble controller + if (m_shooterSubsystem.atSpeed() && error < 15.0) { + m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 1.0); + } else { + m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0); + } } else { - m_driverController.setRumble(GenericHID.RumbleType.kBothRumble, 0.0); + m_driveSubsystem.runVelocity(ChassisSpeeds.fromFieldRelativeSpeeds( + x, + y, + o, + heading + )); + + m_armSubsystem.setDesiredState(ArmSubsystem.ArmState.BACKUP_SHOT); + m_shooterSubsystem.runShooterVelocity(m_runKicker); } } diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 1d569f3e..5fb85f38 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -43,7 +43,7 @@ public static Command joystickDrive( () -> { double xInput = setSensitivity(xSupplier.getAsDouble(), 0.25); double yInput = setSensitivity(ySupplier.getAsDouble(), 0.25); - double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.0) * 0.85; + double omegaInput = setSensitivity(omegaSupplier.getAsDouble(), 0.0) * 0.75; // Apply deadband double linearMagnitude = diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 2a316977..456a99bc 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -33,7 +33,7 @@ public enum ArmState { TRANSITION_SOURCE, PASS, DISABLED, - MANUAL_WRIST + BACKUP_SHOT, MANUAL_WRIST } private final ArmIO m_io; @@ -178,6 +178,10 @@ public void handleState() { m_desiredWristPoseDegs = ArmSetpoints.AMP_SETPOINT.wristAngle(); } case PASS -> { + m_desiredWristPoseDegs = 45.0; + m_desiredArmPoseDegs = 0.0; + } + case BACKUP_SHOT -> { m_desiredWristPoseDegs = 50.0; m_desiredArmPoseDegs = 0.0; } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 74f223a5..3c6d7084 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -45,6 +45,7 @@ import frc.robot.subsystems.drive.module.PhoenixOdometryThread; import frc.robot.subsystems.vision.VisionSubsystem; +import java.util.Arrays; import java.util.List; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; @@ -343,6 +344,11 @@ public double getCharacterizationVelocity() { return driveVelocityAverage / 4.0; } + /** Returns whether there are connected april tag cameras */ + public boolean hasAprilTagCams() { + return Arrays.stream(m_cameras).anyMatch((VisionSubsystem::apriltagConnected)); + } + /** Returns the module states (turn angles and drive velocities) for all the modules. */ @AutoLogOutput(key = "SwerveStates/Measured") private SwerveModuleState[] getModuleStates() { @@ -400,7 +406,8 @@ public double getMaxAngularSpeedRadPerSec() { public Command pathfollowFactory(Pose2d pose) { return AutoBuilder.pathfindToPoseFlipped( - pose, DriveConstants.DEFAULT_CONSTRAINTS).withInterruptBehavior(Command.InterruptionBehavior.kCancelSelf); + pose, DriveConstants.DEFAULT_CONSTRAINTS).withInterruptBehavior(Command.InterruptionBehavior.kCancelSelf) + .unless(() -> !hasAprilTagCams()); } /** Returns an array of module translations. */ diff --git a/src/main/java/lib/utils/AimbotUtils.java b/src/main/java/lib/utils/AimbotUtils.java index 607f7fb3..839e084e 100644 --- a/src/main/java/lib/utils/AimbotUtils.java +++ b/src/main/java/lib/utils/AimbotUtils.java @@ -48,12 +48,13 @@ public class AimbotUtils { /** Linear interpolation tables for aiming */ public static double getWristAngle(double distance) { double angle = 49.319 + (1.427 * Y_TARGET) + (-0.10599 * distance); - if (100.0 >= distance && distance > 60.0) { + if (100.0 >= distance && distance > 55.0) { return angle; } else if (150.0 >= distance && distance > 100.0) { return angle - 1.75; - } else if (175.0 >= distance && distance > 150.0) { - return angle - 3.5; + } else if ((175.0 >= distance && distance > 150.0) + || (200.0 >= distance && distance > 175.0)) { + return angle - 3.0; } else if (distance > 175.0) { return angle; } else { From 282c1c9e08acf8923231f83ac95b507360ad1d2b Mon Sep 17 00:00:00 2001 From: Logan Hollowood <98189655+GearBoxFox@users.noreply.github.com> Date: Sun, 17 Mar 2024 12:05:41 -0400 Subject: [PATCH 30/30] removed whitespace --- src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java index 456a99bc..8f697b74 100644 --- a/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/frc/robot/subsystems/arm/ArmSubsystem.java @@ -48,7 +48,6 @@ public enum ArmState { private ArmState m_desiredState = ArmState.STOW; private ArmState m_currentState = ArmState.DISABLED; - private final DataLogUtil.DataLogTable logUtil = DataLogUtil.getTable("Arm"); private final ArmVisualizer m_setpointVisualizer; @@ -277,4 +276,3 @@ public Command setWristPowerFactory(double power) { () -> m_io.setWristVoltage(0.0)); } } -