From 91ccb8c448d2a0891c35558e7f771015b3c3fb63 Mon Sep 17 00:00:00 2001 From: AT727 Date: Thu, 7 Mar 2024 20:29:03 -0500 Subject: [PATCH 1/5] lowered shiftBackFromEntranceRPM, lowered intake setpoint, added intakeNote w/o beambreak --- .../java/org/robolancers321/Constants.java | 4 +-- .../org/robolancers321/RobotContainer.java | 6 ++++- .../commands/IntakeNoteManual.java | 27 +++++++++++++++++++ .../robolancers321/subsystems/LED/LED.java | 8 +++--- .../subsystems/drivetrain/Drivetrain.java | 3 ++- .../subsystems/intake/Retractor.java | 1 + .../subsystems/launcher/Pivot.java | 1 + 7 files changed, 42 insertions(+), 8 deletions(-) create mode 100644 src/main/java/org/robolancers321/commands/IntakeNoteManual.java diff --git a/src/main/java/org/robolancers321/Constants.java b/src/main/java/org/robolancers321/Constants.java index 2a3dbdb..06794a4 100644 --- a/src/main/java/org/robolancers321/Constants.java +++ b/src/main/java/org/robolancers321/Constants.java @@ -119,7 +119,7 @@ public enum RetractorSetpoint { kRetracted(155), kMating(154), kClearFromLauncher(70), - kIntake(-12), + kIntake(-15), kOuttake(20.0); public final double angle; @@ -186,7 +186,7 @@ public static final class IndexerConstants { public static final double kHandoffRPM = 3000; public static final double kShiftBackFromExitRPM = -300; - public static final double kShiftBackFromEntranceRPM = -150; + public static final double kShiftBackFromEntranceRPM = -75; //150 public static final double kOuttakeRPM = 3000; } diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 3547e24..3e8acd6 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -14,6 +14,7 @@ import org.robolancers321.commands.AutoPickupNote; import org.robolancers321.commands.EmergencyCancel; import org.robolancers321.commands.IntakeNote; +import org.robolancers321.commands.IntakeNoteManual; import org.robolancers321.commands.OuttakeNote; import org.robolancers321.commands.ScoreAmp; import org.robolancers321.commands.ScoreSpeakerFixedAuto; @@ -138,7 +139,7 @@ private void configureDriverController() { .whileFalse(new InstantCommand(() -> this.drivetrain.slowMode = false)); new Trigger(() -> this.driverController.getRightTriggerAxis() > 0.8) - .whileTrue(new IntakeNote()); + .whileTrue(new IntakeNoteManual()); new Trigger(() -> this.driverController.getRightTriggerAxis() > 0.8) .onFalse(this.retractor.moveToRetracted()); @@ -189,6 +190,9 @@ private void configureManipulatorController() { .outtake() .raceWith(this.sucker.out()) .alongWith(new InstantCommand(() -> {}, this.flywheel))); + + new Trigger(() -> this.manipulatorController.getLeftY() < -0.8).onTrue(this.pivot.aimAtAmp()); + new Trigger(() -> this.manipulatorController.getLeftY() > 0.8).onTrue(this.pivot.moveToRetracted()); } private void configureAuto() { diff --git a/src/main/java/org/robolancers321/commands/IntakeNoteManual.java b/src/main/java/org/robolancers321/commands/IntakeNoteManual.java new file mode 100644 index 0000000..4ebf9be --- /dev/null +++ b/src/main/java/org/robolancers321/commands/IntakeNoteManual.java @@ -0,0 +1,27 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands; + +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Pivot; + +public class IntakeNoteManual extends SequentialCommandGroup { + private Retractor retractor; + private Sucker sucker; + + public IntakeNoteManual() { + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + + this.addCommands( + new ParallelCommandGroup( + this.retractor.moveToIntake(), + this.sucker.in() + ) + ); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/LED/LED.java b/src/main/java/org/robolancers321/subsystems/LED/LED.java index efbefd0..eccfda9 100644 --- a/src/main/java/org/robolancers321/subsystems/LED/LED.java +++ b/src/main/java/org/robolancers321/subsystems/LED/LED.java @@ -30,7 +30,7 @@ public class LED extends VirtualSubsystem { // TODO: move this into constants public static final int kLEDPWMPort = 9; - public static final int kLEDStripLength = 25; + public static final int kLEDStripLength = 14; private static final double kStrobeDuration = 0.2; private static final double kBreathDuration = 1.0; @@ -369,10 +369,10 @@ private static void fire( private static void setPixelHeatColor(AddressableLEDBuffer buffer, int pixel, int temperature) { if (temperature > 170) { // hottest: white buffer.setRGB(pixel, 255, 255, temperature); - } else if (temperature > 85) { // middle: orange & yellow - buffer.setRGB(pixel, 255, (int) (temperature * 1.3), 0); + } else if (temperature > 60) { // middle: orange & yellow + buffer.setRGB(pixel, 255, (int) (temperature), 0); } else { // lowest: red - buffer.setRGB(pixel, temperature * 3, 0, 0); + buffer.setRGB(pixel, temperature * 2, 0, 0); } } diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 4eff20e..1de052a 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -408,7 +408,8 @@ public Command stop() { return runOnce(() -> this.drive(0.0, 0.0, 0.0, false)); } - public Command teleopDrive(XboxController controller, boolean fieldCentric) { + public Command + teleopDrive(XboxController controller, boolean fieldCentric) { return run(() -> { double multiplier = controller.getRightBumper() ? 0.4 : 1.0; diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 5319b08..33c6014 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -145,6 +145,7 @@ private void doSendables() { SmartDashboard.putNumber("retractor position (deg)", this.getPositionDeg()); SmartDashboard.putNumber("retractor velocity (deg)", this.m_controller.getGoal().position); SmartDashboard.putNumber("retractor mp goal (deg)", this.getVelocityDeg()); + SmartDashboard.putNumber("retractor position actual", this.encoder.getPosition()); } @Override diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 7141079..0cd5a16 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -15,6 +15,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import java.util.function.DoubleSupplier; From 80dea52d81c8094394d07a6da4a4962b8163efe3 Mon Sep 17 00:00:00 2001 From: AT727 Date: Fri, 8 Mar 2024 08:34:04 -0500 Subject: [PATCH 2/5] tuned new pivot constraints --- src/main/java/org/robolancers321/Constants.java | 8 ++++---- src/main/java/org/robolancers321/RobotContainer.java | 2 ++ .../org/robolancers321/subsystems/intake/Retractor.java | 1 - .../org/robolancers321/subsystems/launcher/Flywheel.java | 3 ++- .../org/robolancers321/subsystems/launcher/Pivot.java | 8 +++----- 5 files changed, 11 insertions(+), 11 deletions(-) diff --git a/src/main/java/org/robolancers321/Constants.java b/src/main/java/org/robolancers321/Constants.java index 06794a4..6ccba63 100644 --- a/src/main/java/org/robolancers321/Constants.java +++ b/src/main/java/org/robolancers321/Constants.java @@ -151,7 +151,7 @@ public static final class FlywheelConstants { public static final int kMotorPort = 17; public static final boolean kInvertMotor = false; - public static final int kCurrentLimit = 75; + public static final int kCurrentLimit = 60; public static final double kRampUpRate = 10000000; // effectively infinite ramp up, keeping this for the infrastructure @@ -207,11 +207,11 @@ public static final class PivotConstants { public static final double kD = 0.0; // 0.02; public static final double kS = 0.0; - public static final double kG = 0.0; + public static final double kG = 0.023; public static final double kV = 0.0; // 0.35 - public static final double kMaxVelocityDeg = 150; - public static final double kMaxAccelerationDeg = 1500; + public static final double kMaxVelocityDeg = 160; + public static final double kMaxAccelerationDeg = 2000; public static TrapezoidProfile.Constraints kProfileConstraints = new Constraints(kMaxVelocityDeg, kMaxAccelerationDeg); diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 3e8acd6..0104e81 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -73,6 +73,8 @@ public RobotContainer() { private void configureLEDs() { // default, meteor red LED.registerSignal(1, () -> true, LED.meteorRain(0.02, LED.kDefaultMeteorColors)); + // LED.registerSignal(1, () -> true, LED.wave(Section.FULL, new Color(28, 169, 201), new Color(49,140,231))); + // sees note, blink orange LED.registerSignal( diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 33c6014..8cbb8f7 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -143,7 +143,6 @@ protected void useOutput(double output, TrapezoidProfile.State setpoint) { private void doSendables() { SmartDashboard.putBoolean("retractor at goal", this.atGoal()); SmartDashboard.putNumber("retractor position (deg)", this.getPositionDeg()); - SmartDashboard.putNumber("retractor velocity (deg)", this.m_controller.getGoal().position); SmartDashboard.putNumber("retractor mp goal (deg)", this.getVelocityDeg()); SmartDashboard.putNumber("retractor position actual", this.encoder.getPosition()); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java index 2c9d2ac..f812f22 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Flywheel.java @@ -91,9 +91,10 @@ public boolean isRevved() { private void doSendables() { SmartDashboard.putNumber("flywheel rpm", this.getRPM()); SmartDashboard.putNumber("flywheel voltage", this.motor.getBusVoltage()); - SmartDashboard.putNumber("flywheel current", this.motor.getOutputCurrent()); + SmartDashboard.putNumber("flywheel current (amps)", this.motor.getOutputCurrent()); SmartDashboard.putBoolean("flywheel isRevved", this.isRevved()); + SmartDashboard.putNumber("flywheel mp goal (rpm)", this.goalRPM); } diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java index 0cd5a16..c365a9a 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Pivot.java @@ -72,8 +72,8 @@ private void configureMotor() { this.motor.setSmartCurrentLimit(PivotConstants.kCurrentLimit); this.motor.enableVoltageCompensation(12); - this.motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 65535); // analog sensor - this.motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, 65535); // alternate encoder + this.motor.setPeriodicFramePeriod(PeriodicFrame.kStatus3, 10000); // analog sensor + this.motor.setPeriodicFramePeriod(PeriodicFrame.kStatus4, 10000); // alternate encoder this.motor.setPeriodicFramePeriod(PeriodicFrame.kStatus5, 20); // abs encoder position this.motor.setPeriodicFramePeriod(PeriodicFrame.kStatus6, 20); // abs encoder velocity @@ -133,7 +133,7 @@ private void setGoal(double position) { protected void useOutput(TrapezoidProfile.State setpoint) { double feedforwardOutput = this.feedforwardController.calculate( - setpoint.position * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0); + (setpoint.position - 40) * Math.PI / 180.0, setpoint.velocity * Math.PI / 180.0); SmartDashboard.putNumber("pivot position setpoint mp (deg)", setpoint.position); SmartDashboard.putNumber("pivot velocity setpoint mp (deg)", setpoint.velocity); @@ -154,8 +154,6 @@ protected void useOutput(TrapezoidProfile.State setpoint) { public void doSendables() { SmartDashboard.putBoolean("pivot at goal", this.atGoal()); SmartDashboard.putNumber("pivot position (deg)", this.getPositionDeg()); - SmartDashboard.putNumber("pivot mp setpoint pos (deg)", this.previousReference.position); - SmartDashboard.putNumber("pivot mp setpoint vel (deg)", this.previousReference.velocity); SmartDashboard.putNumber("pivot velocity (deg)", this.getVelocityDeg()); SmartDashboard.putNumber("pivot relative encoder position", this.relativeEncoder.getPosition()); } From 96c1689248ec8e58a9f370ebd2ee2eaca9089d07 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 8 Mar 2024 10:23:54 -0500 Subject: [PATCH 3/5] pure pursuit for auto pickup and new flywheel ff --- .../java/org/robolancers321/Constants.java | 12 ++++----- .../org/robolancers321/RobotContainer.java | 11 ++++---- .../commands/AutoPickupNote.java | 13 ++++----- .../subsystems/drivetrain/Drivetrain.java | 27 ++++++++++++++----- .../subsystems/launcher/Indexer.java | 3 ++- 5 files changed, 42 insertions(+), 24 deletions(-) diff --git a/src/main/java/org/robolancers321/Constants.java b/src/main/java/org/robolancers321/Constants.java index 6ccba63..2e2d4d5 100644 --- a/src/main/java/org/robolancers321/Constants.java +++ b/src/main/java/org/robolancers321/Constants.java @@ -65,7 +65,7 @@ public static final class DrivetrainConstants { public static final double kHeadingI = 0.0; public static final double kHeadingD = 0.01; // 0.01? - public static final double kHeadingTolerance = 3.0; + public static final double kHeadingTolerance = 1.5; } public static final class SwerveModuleConstants { @@ -156,12 +156,12 @@ public static final class FlywheelConstants { public static final double kRampUpRate = 10000000; // effectively infinite ramp up, keeping this for the infrastructure - public static final double kFF = 0.00016; - public static final double kToleranceRPM = 120.0; + public static final double kFF = 0.0001502; + public static final double kToleranceRPM = 60.0; public enum FlywheelSetpoint { kAcceptHandoff(300), - kShiftBackward(-1000), + kShiftBackward(-2000), kAmp(1500.0), kSpeaker(2500.0); @@ -185,8 +185,8 @@ public static final class IndexerConstants { public static final double kFF = 0.000153; public static final double kHandoffRPM = 3000; - public static final double kShiftBackFromExitRPM = -300; - public static final double kShiftBackFromEntranceRPM = -75; //150 + public static final double kShiftBackFromExitRPM = -200; + public static final double kShiftBackFromEntranceRPM = -50; //150 public static final double kOuttakeRPM = 3000; } diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 0104e81..a93228e 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -63,11 +63,13 @@ public RobotContainer() { this.led = new LED(); this.ledSim = new AddressableLEDSim(led.ledStrip); + this.flywheel.setDefaultCommand(this.flywheel.tuneController()); + this.configureLEDs(); - this.configureDefaultCommands(); - this.configureDriverController(); - this.configureManipulatorController(); - this.configureAuto(); + // this.configureDefaultCommands(); + // this.configureDriverController(); + // this.configureManipulatorController(); + // this.configureAuto(); } private void configureLEDs() { @@ -75,7 +77,6 @@ private void configureLEDs() { LED.registerSignal(1, () -> true, LED.meteorRain(0.02, LED.kDefaultMeteorColors)); // LED.registerSignal(1, () -> true, LED.wave(Section.FULL, new Color(28, 169, 201), new Color(49,140,231))); - // sees note, blink orange LED.registerSignal( 2, this.drivetrain::seesNote, LED.strobe(Section.FULL, new Color(180, 30, 0))); diff --git a/src/main/java/org/robolancers321/commands/AutoPickupNote.java b/src/main/java/org/robolancers321/commands/AutoPickupNote.java index 3dd095b..63b4da0 100644 --- a/src/main/java/org/robolancers321/commands/AutoPickupNote.java +++ b/src/main/java/org/robolancers321/commands/AutoPickupNote.java @@ -28,13 +28,14 @@ public AutoPickupNote() { new WaitCommand(0.2), new ConditionalCommand( new SequentialCommandGroup( - this.drivetrain.turnToNote(), + // this.drivetrain.turnToNote(), + // this.drivetrain + // .driveCommand(0.0, 2.0, 0.0, false) + // .until(() -> !this.drivetrain.seesNote()) + // .withTimeout(1.5), + this.drivetrain.driveIntoNote(), this.drivetrain - .driveCommand(0.0, 2.0, 0.0, false) - .until(() -> !this.drivetrain.seesNote()) - .withTimeout(1.5), - this.drivetrain - .driveCommand(0.0, 2.0, 0.0, false) + .driveCommand(0.0, 2.5, 0.0, false) .until(this.sucker::noteDetected) .withTimeout(1.0)), new InstantCommand(), diff --git a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java index 1de052a..fe282bb 100644 --- a/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/org/robolancers321/subsystems/drivetrain/Drivetrain.java @@ -18,6 +18,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.kinematics.SwerveModulePosition; import edu.wpi.first.math.kinematics.SwerveModuleState; +import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.SPI; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.smartdashboard.Field2d; @@ -34,6 +35,7 @@ import org.photonvision.targeting.PhotonPipelineResult; import org.photonvision.targeting.PhotonTrackedTarget; import org.robolancers321.Constants.DrivetrainConstants; +import org.robolancers321.subsystems.intake.Sucker; import org.robolancers321.util.MyAlliance; public class Drivetrain extends SubsystemBase { @@ -304,8 +306,8 @@ public double getDistanceToSpeaker() { } public boolean seesNote() { - return this.noteCamera.getLatestResult().hasTargets() - && Math.abs(this.noteCamera.getLatestResult().getBestTarget().getYaw()) < 15.0; + return this.noteCamera.getLatestResult().hasTargets(); + // && Math.abs(this.noteCamera.getLatestResult().getBestTarget().getYaw()) < 15.0; } private double getNoteAngle() { @@ -315,9 +317,9 @@ private double getNoteAngle() { PhotonTrackedTarget bestTarget = latestResult.getBestTarget(); - if (Math.abs(bestTarget.getYaw()) > 15.0) return 0.0; + // if (Math.abs(bestTarget.getYaw()) > 15.0) return 0.0; - return -0.6 * bestTarget.getYaw(); + return -0.5 * bestTarget.getYaw(); } // private Translation2d getRelativeNoteLocation() { @@ -329,8 +331,8 @@ private double getNoteAngle() { // // TODO: plus or minus mount pitch? // double dz = - // kNoteCameraMountHeight - // / Math.tan((-bestTarget.getPitch() + kNoteCameraMountPitch) * Math.PI / 180.0); + // Units.inchesToMeters(10) + // / Math.tan((-bestTarget.getPitch() + 24) * Math.PI / 180.0); // double dx = dz * Math.tan(bestTarget.getYaw() * Math.PI / 180.0); // return new Translation2d(dx, dz); @@ -383,6 +385,11 @@ private void doSendables() { SmartDashboard.putBoolean("sees note", this.seesNote()); SmartDashboard.putNumber("angle to note", this.getNoteAngle()); + + // Translation2d notePose = this.getRelativeNoteLocation(); + + // SmartDashboard.putNumber("note pose x", notePose.getX()); + // SmartDashboard.putNumber("note pose y", notePose.getY()); } @Override @@ -474,6 +481,14 @@ public Command turnToAngle(double angle) { .until(this.headingController::atSetpoint)); } + public Command driveIntoNote(){ + return run(() -> { + double headingControllerOutput = -this.headingController.calculate(this.getNoteAngle(), 0.0); + + this.drive(0.0, 1.5, headingControllerOutput, false); + }).until(() -> !this.seesNote()).withTimeout(1.5); + } + private Command turnToAngle(DoubleSupplier angleSupplier) { return runOnce(() -> this.headingController.setSetpoint(angleSupplier.getAsDouble())) .andThen( diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index 2c520ff..5373b97 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -9,6 +9,7 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import org.robolancers321.Constants.IndexerConstants; @@ -153,7 +154,7 @@ public Command acceptHandoff() { () -> { this.goalRPM = IndexerConstants.kHandoffRPM; }) - .alongWith(new WaitUntilCommand(this::exitBeamBroken).withTimeout(2.0)); + .alongWith((new WaitUntilCommand(this::exitBeamBroken).andThen(new WaitCommand(0.2))).withTimeout(2.0)); } public Command outtake() { From ff2d61638c4c89f6bc09ff48f671c4e245bbcf68 Mon Sep 17 00:00:00 2001 From: mattperls-code Date: Fri, 8 Mar 2024 10:24:37 -0500 Subject: [PATCH 4/5] added default commands and buttons back --- src/main/java/org/robolancers321/RobotContainer.java | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index a93228e..09e5e01 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -63,13 +63,11 @@ public RobotContainer() { this.led = new LED(); this.ledSim = new AddressableLEDSim(led.ledStrip); - this.flywheel.setDefaultCommand(this.flywheel.tuneController()); - this.configureLEDs(); - // this.configureDefaultCommands(); - // this.configureDriverController(); - // this.configureManipulatorController(); - // this.configureAuto(); + this.configureDefaultCommands(); + this.configureDriverController(); + this.configureManipulatorController(); + this.configureAuto(); } private void configureLEDs() { From 09710fbb97e9620066e84d0f9bfb4d6352d51b6e Mon Sep 17 00:00:00 2001 From: AT727 Date: Fri, 8 Mar 2024 13:51:14 -0500 Subject: [PATCH 5/5] autos and teleop work, w team lessgooo --- Autos.chor | 10529 ++++++++++++++ NoTeamBottom.chor | 11549 ---------------- src/main/deploy/choreo/3NB-1stN.1.traj | 139 - src/main/deploy/choreo/3NB-1stN.2.traj | 328 - src/main/deploy/choreo/3NB-1stN.3.traj | 328 - src/main/deploy/choreo/3NB-1stN.traj | 769 - src/main/deploy/choreo/3NB-2ndN.1.traj | 157 - src/main/deploy/choreo/3NB-2ndN.2.traj | 346 - src/main/deploy/choreo/3NB-2ndN.3.traj | 247 - src/main/deploy/choreo/3NB-2ndN.traj | 724 - src/main/deploy/choreo/3NB-Close.1.traj | 157 + src/main/deploy/choreo/3NB-Close.2.traj | 346 + src/main/deploy/choreo/3NB-Close.3.traj | 265 + src/main/deploy/choreo/3NB-Close.traj | 742 + src/main/deploy/choreo/3NB-Skip.1.traj | 373 + src/main/deploy/choreo/3NB-Skip.2.traj | 229 + src/main/deploy/choreo/3NB-Skip.3.traj | 94 + src/main/deploy/choreo/3NB-Skip.traj | 670 + src/main/deploy/choreo/3NBClose.1.traj | 112 - src/main/deploy/choreo/3NBClose.2.traj | 292 - src/main/deploy/choreo/3NBClose.3.traj | 283 - src/main/deploy/choreo/3NBClose.traj | 796 -- src/main/deploy/choreo/3NBMid1st.1.traj | 292 - src/main/deploy/choreo/3NBMid1st.2.traj | 283 - src/main/deploy/choreo/3NBMid1st.3.traj | 292 - src/main/deploy/choreo/3NBMid1st.4.traj | 112 - src/main/deploy/choreo/3NBMid1st.5.traj | 76 - src/main/deploy/choreo/3NBMid1st.traj | 1336 -- .../choreo/{3NM.1.traj => 3NM-Close.1.traj} | 0 .../choreo/{3NM.2.traj => 3NM-Close.2.traj} | 0 .../choreo/{3NM.3.traj => 3NM-Close.3.traj} | 0 .../choreo/{3NM.traj => 3NM-Close.traj} | 0 src/main/deploy/choreo/3NT-2ndN.1.traj | 157 - src/main/deploy/choreo/3NT-2ndN.2.traj | 274 - src/main/deploy/choreo/3NT-2ndN.3.traj | 265 - src/main/deploy/choreo/3NT-2ndN.traj | 670 - src/main/deploy/choreo/3NT-Close.1.traj | 157 + src/main/deploy/choreo/3NT-Close.2.traj | 193 + src/main/deploy/choreo/3NT-Close.3.traj | 175 + src/main/deploy/choreo/3NT-Close.traj | 499 + src/main/deploy/choreo/3NTMid1st.1.traj | 355 - src/main/deploy/choreo/3NTMid1st.2.traj | 229 - src/main/deploy/choreo/3NTMid1st.3.traj | 238 - src/main/deploy/choreo/3NTMid1st.4.traj | 274 - src/main/deploy/choreo/3NTMid1st.5.traj | 94 - src/main/deploy/choreo/3NTMid1st.6.traj | 94 - src/main/deploy/choreo/3NTMid1st.traj | 1219 -- src/main/deploy/choreo/4NB-Skip.1.traj | 382 + src/main/deploy/choreo/4NB-Skip.2.traj | 292 + src/main/deploy/choreo/4NB-Skip.3.traj | 292 + src/main/deploy/choreo/4NB-Skip.4.traj | 310 + src/main/deploy/choreo/4NB-Skip.5.traj | 85 + src/main/deploy/choreo/4NB-Skip.traj | 1309 ++ src/main/deploy/choreo/4NB-Sweep.1.traj | 166 + src/main/deploy/choreo/4NB-Sweep.2.traj | 166 + src/main/deploy/choreo/4NB-Sweep.3.traj | 184 + src/main/deploy/choreo/4NB-Sweep.traj | 490 + .../deploy/choreo/4NB-SweepStraight.1.traj | 328 + .../deploy/choreo/4NB-SweepStraight.2.traj | 112 + .../deploy/choreo/4NB-SweepStraight.3.traj | 103 + src/main/deploy/choreo/4NB-SweepStraight.traj | 517 + src/main/deploy/choreo/4NM-Sweep.1.traj | 175 + src/main/deploy/choreo/4NM-Sweep.2.traj | 175 + src/main/deploy/choreo/4NM-Sweep.3.traj | 175 + src/main/deploy/choreo/4NM-Sweep.traj | 499 + src/main/deploy/choreo/4NM-SweepFender.1.traj | 175 + src/main/deploy/choreo/4NM-SweepFender.2.traj | 112 + src/main/deploy/choreo/4NM-SweepFender.3.traj | 103 + src/main/deploy/choreo/4NM-SweepFender.4.traj | 103 + src/main/deploy/choreo/4NM-SweepFender.5.traj | 175 + src/main/deploy/choreo/4NM-SweepFender.6.traj | 121 + src/main/deploy/choreo/4NM-SweepFender.traj | 724 + .../choreo/4NM-SweepFenderStraight.1.traj | 139 + .../choreo/4NM-SweepFenderStraight.2.traj | 139 + .../choreo/4NM-SweepFenderStraight.3.traj | 103 + .../choreo/4NM-SweepFenderStraight.4.traj | 103 + .../choreo/4NM-SweepFenderStraight.5.traj | 139 + .../choreo/4NM-SweepFenderStraight.6.traj | 139 + .../choreo/4NM-SweepFenderStraight.traj | 697 + src/main/deploy/choreo/4NT-Close.1.traj | 157 + src/main/deploy/choreo/4NT-Close.2.traj | 193 + src/main/deploy/choreo/4NT-Close.3.traj | 175 + src/main/deploy/choreo/4NT-Close.4.traj | 238 + src/main/deploy/choreo/4NT-Close.5.traj | 238 + src/main/deploy/choreo/4NT-Close.traj | 949 ++ src/main/deploy/choreo/4NT-Skip.1.traj | 180 - src/main/deploy/choreo/4NT-Skip.2.traj | 326 +- src/main/deploy/choreo/4NT-Skip.3.traj | 297 +- src/main/deploy/choreo/4NT-Skip.4.traj | 353 +- src/main/deploy/choreo/4NT-Skip.5.traj | 211 + src/main/deploy/choreo/4NT-Sweep.1.traj | 139 + src/main/deploy/choreo/4NT-Sweep.2.traj | 157 + src/main/deploy/choreo/4NT-Sweep.3.traj | 175 + src/main/deploy/choreo/4NT-Sweep.traj | 445 + src/main/deploy/choreo/4NT.1.traj | 139 - src/main/deploy/choreo/4NT.2.traj | 229 - src/main/deploy/choreo/4NT.3.traj | 184 - src/main/deploy/choreo/4NT.4.traj | 265 - src/main/deploy/choreo/4NT.5.traj | 265 - src/main/deploy/choreo/4NT.traj | 1030 -- src/main/deploy/choreo/New Path.1.traj | 103 + .../choreo/NoTeamBottom-Straight.1.traj | 184 - .../choreo/NoTeamBottom-Straight.2.traj | 202 - .../choreo/NoTeamBottom-Straight.3.traj | 175 - .../deploy/choreo/NoTeamBottom-Straight.traj | 535 - src/main/deploy/choreo/NoTeamBottom.1.traj | 175 - src/main/deploy/choreo/NoTeamBottom.2.traj | 184 - src/main/deploy/choreo/NoTeamBottom.3.traj | 202 - src/main/deploy/choreo/NoTeamBottom.traj | 535 - .../deploy/choreo/NoTeamBottomStraight.1.traj | 346 - .../deploy/choreo/NoTeamBottomStraight.2.traj | 112 - .../deploy/choreo/NoTeamBottomStraight.3.traj | 112 - .../deploy/choreo/NoTeamBottomStraight.traj | 544 - src/main/deploy/choreo/NoTeamMiddle.1.traj | 112 - src/main/deploy/choreo/NoTeamMiddle.2.traj | 166 - src/main/deploy/choreo/NoTeamMiddle.3.traj | 166 - src/main/deploy/choreo/NoTeamMiddle.traj | 418 - src/main/deploy/choreo/NoTeamTop.1.traj | 94 - src/main/deploy/choreo/NoTeamTop.2.traj | 94 - src/main/deploy/choreo/NoTeamTop.3.traj | 85 - src/main/deploy/choreo/NoTeamTop.4.traj | 76 - src/main/deploy/choreo/NoTeamTop.5.traj | 94 - src/main/deploy/choreo/NoTeamTop.traj | 526 - .../deploy/choreo/SpeakerLineSweep.1.traj | 112 - .../deploy/choreo/SpeakerLineSweep.2.traj | 58 - .../deploy/choreo/SpeakerLineSweep.3.traj | 166 - .../deploy/choreo/SpeakerLineSweep.4.traj | 166 - src/main/deploy/choreo/SpeakerLineSweep.traj | 463 - src/main/deploy/choreo/straight.1.traj | 463 - .../deploy/pathplanner/autos/3NB-Sweep.auto | 203 - src/main/deploy/pathplanner/autos/3NB.auto | 133 - src/main/deploy/pathplanner/autos/3NM.auto | 139 - src/main/deploy/pathplanner/autos/4NT.auto | 184 - .../deploy/pathplanner/autos/New Auto.auto | 18 + .../pathplanner/autos/NoScoring_3NBSweep.auto | 25 - .../java/org/robolancers321/Constants.java | 16 +- .../org/robolancers321/RobotContainer.java | 71 +- .../commands/AutoPickupNote.java | 5 +- .../robolancers321/commands/IntakeNote.java | 1 + .../robolancers321/commands/PathAndAutoPickup | 32 + .../commands/PathAndIntake.java | 33 + .../commands/PathAndRetract.java | 26 + .../robolancers321/commands/PathAndShoot.java | 9 +- .../org/robolancers321/commands/ScoreAmp.java | 6 +- .../commands/ScoreSpeakerFixedAuto.java | 6 +- .../commands/ScoreSpeakerFixedTeleop.java | 2 +- .../commands/ScoreSpeakerFromDistance.java | 3 +- .../commands/autonomous/Auto3NBClose.java | 44 + .../commands/autonomous/Auto3NBSweep.java | 25 - .../autonomous/Auto3NBSweepStraight.java | 25 - .../commands/autonomous/Auto3NMClose.java | 44 + .../commands/autonomous/Auto3NTClose.java | 44 + .../commands/autonomous/Auto4NBSkip.java | 47 + .../commands/autonomous/Auto4NBSweep.java | 40 + .../autonomous/Auto4NBSweepStraight.java | 40 + .../commands/autonomous/Auto4NMSweep.java | 40 + .../autonomous/Auto4NMSweepFender.java | 50 + .../Auto4NMSweepFenderStraight.java | 50 + .../commands/autonomous/Auto4NTClose.java | 47 + .../commands/autonomous/Auto4NTSweep.java | 40 + .../subsystems/intake/Retractor.java | 3 +- .../subsystems/launcher/Indexer.java | 2 +- 162 files changed, 27504 insertions(+), 32453 deletions(-) create mode 100644 Autos.chor delete mode 100644 NoTeamBottom.chor delete mode 100644 src/main/deploy/choreo/3NB-1stN.1.traj delete mode 100644 src/main/deploy/choreo/3NB-1stN.2.traj delete mode 100644 src/main/deploy/choreo/3NB-1stN.3.traj delete mode 100644 src/main/deploy/choreo/3NB-1stN.traj delete mode 100644 src/main/deploy/choreo/3NB-2ndN.1.traj delete mode 100644 src/main/deploy/choreo/3NB-2ndN.2.traj delete mode 100644 src/main/deploy/choreo/3NB-2ndN.3.traj delete mode 100644 src/main/deploy/choreo/3NB-2ndN.traj create mode 100644 src/main/deploy/choreo/3NB-Close.1.traj create mode 100644 src/main/deploy/choreo/3NB-Close.2.traj create mode 100644 src/main/deploy/choreo/3NB-Close.3.traj create mode 100644 src/main/deploy/choreo/3NB-Close.traj create mode 100644 src/main/deploy/choreo/3NB-Skip.1.traj create mode 100644 src/main/deploy/choreo/3NB-Skip.2.traj create mode 100644 src/main/deploy/choreo/3NB-Skip.3.traj create mode 100644 src/main/deploy/choreo/3NB-Skip.traj delete mode 100644 src/main/deploy/choreo/3NBClose.1.traj delete mode 100644 src/main/deploy/choreo/3NBClose.2.traj delete mode 100644 src/main/deploy/choreo/3NBClose.3.traj delete mode 100644 src/main/deploy/choreo/3NBClose.traj delete mode 100644 src/main/deploy/choreo/3NBMid1st.1.traj delete mode 100644 src/main/deploy/choreo/3NBMid1st.2.traj delete mode 100644 src/main/deploy/choreo/3NBMid1st.3.traj delete mode 100644 src/main/deploy/choreo/3NBMid1st.4.traj delete mode 100644 src/main/deploy/choreo/3NBMid1st.5.traj delete mode 100644 src/main/deploy/choreo/3NBMid1st.traj rename src/main/deploy/choreo/{3NM.1.traj => 3NM-Close.1.traj} (100%) rename src/main/deploy/choreo/{3NM.2.traj => 3NM-Close.2.traj} (100%) rename src/main/deploy/choreo/{3NM.3.traj => 3NM-Close.3.traj} (100%) rename src/main/deploy/choreo/{3NM.traj => 3NM-Close.traj} (100%) delete mode 100644 src/main/deploy/choreo/3NT-2ndN.1.traj delete mode 100644 src/main/deploy/choreo/3NT-2ndN.2.traj delete mode 100644 src/main/deploy/choreo/3NT-2ndN.3.traj delete mode 100644 src/main/deploy/choreo/3NT-2ndN.traj create mode 100644 src/main/deploy/choreo/3NT-Close.1.traj create mode 100644 src/main/deploy/choreo/3NT-Close.2.traj create mode 100644 src/main/deploy/choreo/3NT-Close.3.traj create mode 100644 src/main/deploy/choreo/3NT-Close.traj delete mode 100644 src/main/deploy/choreo/3NTMid1st.1.traj delete mode 100644 src/main/deploy/choreo/3NTMid1st.2.traj delete mode 100644 src/main/deploy/choreo/3NTMid1st.3.traj delete mode 100644 src/main/deploy/choreo/3NTMid1st.4.traj delete mode 100644 src/main/deploy/choreo/3NTMid1st.5.traj delete mode 100644 src/main/deploy/choreo/3NTMid1st.6.traj delete mode 100644 src/main/deploy/choreo/3NTMid1st.traj create mode 100644 src/main/deploy/choreo/4NB-Skip.1.traj create mode 100644 src/main/deploy/choreo/4NB-Skip.2.traj create mode 100644 src/main/deploy/choreo/4NB-Skip.3.traj create mode 100644 src/main/deploy/choreo/4NB-Skip.4.traj create mode 100644 src/main/deploy/choreo/4NB-Skip.5.traj create mode 100644 src/main/deploy/choreo/4NB-Skip.traj create mode 100644 src/main/deploy/choreo/4NB-Sweep.1.traj create mode 100644 src/main/deploy/choreo/4NB-Sweep.2.traj create mode 100644 src/main/deploy/choreo/4NB-Sweep.3.traj create mode 100644 src/main/deploy/choreo/4NB-Sweep.traj create mode 100644 src/main/deploy/choreo/4NB-SweepStraight.1.traj create mode 100644 src/main/deploy/choreo/4NB-SweepStraight.2.traj create mode 100644 src/main/deploy/choreo/4NB-SweepStraight.3.traj create mode 100644 src/main/deploy/choreo/4NB-SweepStraight.traj create mode 100644 src/main/deploy/choreo/4NM-Sweep.1.traj create mode 100644 src/main/deploy/choreo/4NM-Sweep.2.traj create mode 100644 src/main/deploy/choreo/4NM-Sweep.3.traj create mode 100644 src/main/deploy/choreo/4NM-Sweep.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFender.1.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFender.2.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFender.3.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFender.4.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFender.5.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFender.6.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFender.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFenderStraight.1.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFenderStraight.2.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFenderStraight.3.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFenderStraight.4.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFenderStraight.5.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFenderStraight.6.traj create mode 100644 src/main/deploy/choreo/4NM-SweepFenderStraight.traj create mode 100644 src/main/deploy/choreo/4NT-Close.1.traj create mode 100644 src/main/deploy/choreo/4NT-Close.2.traj create mode 100644 src/main/deploy/choreo/4NT-Close.3.traj create mode 100644 src/main/deploy/choreo/4NT-Close.4.traj create mode 100644 src/main/deploy/choreo/4NT-Close.5.traj create mode 100644 src/main/deploy/choreo/4NT-Close.traj create mode 100644 src/main/deploy/choreo/4NT-Skip.5.traj create mode 100644 src/main/deploy/choreo/4NT-Sweep.1.traj create mode 100644 src/main/deploy/choreo/4NT-Sweep.2.traj create mode 100644 src/main/deploy/choreo/4NT-Sweep.3.traj create mode 100644 src/main/deploy/choreo/4NT-Sweep.traj delete mode 100644 src/main/deploy/choreo/4NT.1.traj delete mode 100644 src/main/deploy/choreo/4NT.2.traj delete mode 100644 src/main/deploy/choreo/4NT.3.traj delete mode 100644 src/main/deploy/choreo/4NT.4.traj delete mode 100644 src/main/deploy/choreo/4NT.5.traj delete mode 100644 src/main/deploy/choreo/4NT.traj create mode 100644 src/main/deploy/choreo/New Path.1.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottom-Straight.1.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottom-Straight.2.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottom-Straight.3.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottom-Straight.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottom.1.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottom.2.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottom.3.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottom.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottomStraight.1.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottomStraight.2.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottomStraight.3.traj delete mode 100644 src/main/deploy/choreo/NoTeamBottomStraight.traj delete mode 100644 src/main/deploy/choreo/NoTeamMiddle.1.traj delete mode 100644 src/main/deploy/choreo/NoTeamMiddle.2.traj delete mode 100644 src/main/deploy/choreo/NoTeamMiddle.3.traj delete mode 100644 src/main/deploy/choreo/NoTeamMiddle.traj delete mode 100644 src/main/deploy/choreo/NoTeamTop.1.traj delete mode 100644 src/main/deploy/choreo/NoTeamTop.2.traj delete mode 100644 src/main/deploy/choreo/NoTeamTop.3.traj delete mode 100644 src/main/deploy/choreo/NoTeamTop.4.traj delete mode 100644 src/main/deploy/choreo/NoTeamTop.5.traj delete mode 100644 src/main/deploy/choreo/NoTeamTop.traj delete mode 100644 src/main/deploy/choreo/SpeakerLineSweep.1.traj delete mode 100644 src/main/deploy/choreo/SpeakerLineSweep.2.traj delete mode 100644 src/main/deploy/choreo/SpeakerLineSweep.3.traj delete mode 100644 src/main/deploy/choreo/SpeakerLineSweep.4.traj delete mode 100644 src/main/deploy/choreo/SpeakerLineSweep.traj delete mode 100644 src/main/deploy/choreo/straight.1.traj delete mode 100644 src/main/deploy/pathplanner/autos/3NB-Sweep.auto delete mode 100644 src/main/deploy/pathplanner/autos/3NB.auto delete mode 100644 src/main/deploy/pathplanner/autos/3NM.auto delete mode 100644 src/main/deploy/pathplanner/autos/4NT.auto create mode 100644 src/main/deploy/pathplanner/autos/New Auto.auto delete mode 100644 src/main/deploy/pathplanner/autos/NoScoring_3NBSweep.auto create mode 100644 src/main/java/org/robolancers321/commands/PathAndAutoPickup create mode 100644 src/main/java/org/robolancers321/commands/PathAndIntake.java create mode 100644 src/main/java/org/robolancers321/commands/PathAndRetract.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto3NBClose.java delete mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto3NBSweep.java delete mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto3NBSweepStraight.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto3NMClose.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto3NTClose.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto4NBSkip.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto4NBSweep.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto4NBSweepStraight.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweep.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweepFender.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweepFenderStraight.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto4NTClose.java create mode 100644 src/main/java/org/robolancers321/commands/autonomous/Auto4NTSweep.java diff --git a/Autos.chor b/Autos.chor new file mode 100644 index 0000000..f6529a7 --- /dev/null +++ b/Autos.chor @@ -0,0 +1,10529 @@ +{ + "version": "v0.2.2", + "robotConfiguration": { + "mass": 53.837263288913476, + "rotationalInertia": 5.506392685924157, + "motorMaxTorque": 0.7248618784530386, + "motorMaxVelocity": 4704, + "gearing": 6.12, + "wheelbase": 0.7873995748042296, + "trackWidth": 0.7111996159522074, + "bumperLength": 0.939799492508274, + "bumperWidth": 0.8508995405142481, + "wheelRadius": 0.050799972568014815 + }, + "paths": { + "3NT-Close": { + "waypoints": [ + { + "x": 0.735096275806427, + "y": 6.688129901885986, + "heading": 1.030377205028743, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.309788465499878, + "y": 7.003068923950195, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 7.836877822875977, + "y": 7.436314105987549, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 3.1197965145111084, + "y": 7.003068923950195, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.735096275806427, + "y": 6.688129901885986, + "heading": 1.030377205028743, + "angularVelocity": 1.0391167973510024e-17, + "velocityX": -2.2461159941573586e-16, + "velocityY": -4.4539976704222156e-17, + "timestamp": 0 + }, + { + "x": 0.7596859244793368, + "y": 6.693079217230425, + "heading": 1.0146123815882935, + "angularVelocity": -0.2516963968106555, + "velocityX": 0.39259088395060876, + "velocityY": 0.07901926993222823, + "timestamp": 0.06263428336762733 + }, + { + "x": 0.8088699810683173, + "y": 6.702981947714709, + "heading": 0.9831479860517711, + "angularVelocity": -0.5023510104178023, + "velocityX": 0.7852577525362323, + "velocityY": 0.1581039959563583, + "timestamp": 0.12526856673525466 + }, + { + "x": 0.882652643462703, + "y": 6.717843001519701, + "heading": 0.9360140194508846, + "angularVelocity": -0.7525266366382326, + "velocityX": 1.177991643351673, + "velocityY": 0.23726708450970968, + "timestamp": 0.187902850102882 + }, + { + "x": 0.9810383785491245, + "y": 6.737666993774571, + "heading": 0.8732159109864319, + "angularVelocity": -1.0026155818829086, + "velocityX": 1.570796851126302, + "velocityY": 0.31650385681776916, + "timestamp": 0.2505371334705093 + }, + { + "x": 1.1040342419063196, + "y": 6.76245681515317, + "heading": 0.7947797985022379, + "angularVelocity": -1.2522872182286973, + "velocityX": 1.96371470613434, + "velocityY": 0.3957867807490719, + "timestamp": 0.31317141683813665 + }, + { + "x": 1.2516562548035544, + "y": 6.792212632567885, + "heading": 0.7009027883845247, + "angularVelocity": -1.4988119137040175, + "velocityX": 2.356888351875573, + "velocityY": 0.4750723695530488, + "timestamp": 0.375805700205764 + }, + { + "x": 1.42394926193708, + "y": 6.826935222209109, + "heading": 0.592493836735336, + "angularVelocity": -1.7308244913235529, + "velocityX": 2.7507779744563265, + "velocityY": 0.5543703507777238, + "timestamp": 0.4384399835733913 + }, + { + "x": 1.6210817820852441, + "y": 6.866668873496111, + "heading": 0.474890142906328, + "angularVelocity": -1.8776249604189084, + "velocityX": 3.1473581168177396, + "velocityY": 0.6343754434578357, + "timestamp": 0.5010742669410186 + }, + { + "x": 1.7933561506575397, + "y": 6.900954234351732, + "heading": 0.3592267918392059, + "angularVelocity": -1.846646035498528, + "velocityX": 2.7504803968322524, + "velocityY": 0.5473896883977395, + "timestamp": 0.563708550308646 + }, + { + "x": 1.9409491865805881, + "y": 6.930222536079952, + "heading": 0.2578351683787445, + "angularVelocity": -1.6187879545990924, + "velocityX": 2.356425714281139, + "velocityY": 0.46728884174232477, + "timestamp": 0.6263428336762733 + }, + { + "x": 2.0639150364587264, + "y": 6.954550982387355, + "heading": 0.17246455594955176, + "angularVelocity": -1.3630013442976032, + "velocityX": 1.9632355200170402, + "velocityY": 0.38842060608578943, + "timestamp": 0.6889771170439006 + }, + { + "x": 2.1622742220184694, + "y": 6.973979114751976, + "heading": 0.1037576955884256, + "angularVelocity": -1.0969529252511232, + "velocityX": 1.5703729694236497, + "velocityY": 0.3101836776927658, + "timestamp": 0.751611400411528 + }, + { + "x": 2.2360360999783158, + "y": 6.988531733503946, + "heading": 0.05200847775326007, + "angularVelocity": -0.8262123401560716, + "velocityX": 1.1776598053642056, + "velocityY": 0.23234270386001926, + "timestamp": 0.8142456837791553 + }, + { + "x": 2.285206028756279, + "y": 6.998224972989151, + "heading": 0.017380497120516848, + "angularVelocity": -0.5528598519998558, + "velocityX": 0.7850321921840192, + "velocityY": 0.15475932610756862, + "timestamp": 0.8768799671467826 + }, + { + "x": 2.309788465499878, + "y": 7.003068923950195, + "heading": -1.979989072741127e-18, + "angularVelocity": -0.27749175349390415, + "velocityX": 0.39247574047129724, + "velocityY": 0.0773370540956569, + "timestamp": 0.93951425051441 + }, + { + "x": 2.309788465499878, + "y": 7.003068923950195, + "heading": -1.2084937260384568e-18, + "angularVelocity": -8.586750612633434e-18, + "velocityX": 2.255152192952613e-16, + "velocityY": 4.541807880719648e-17, + "timestamp": 1.0021485338820373 + }, + { + "x": 2.3737994944405516, + "y": 7.008086478053747, + "heading": -4.812715970258146e-18, + "angularVelocity": -3.623090443805201e-17, + "velocityX": 0.6434612838287803, + "velocityY": 0.05043821132986589, + "timestamp": 1.1016277574956006 + }, + { + "x": 2.5018215509422213, + "y": 7.018121586152704, + "heading": -9.261038230044222e-18, + "angularVelocity": -4.4716093454457826e-17, + "velocityX": 1.2869225537885558, + "velocityY": 0.10087642157259902, + "timestamp": 1.201106981109164 + }, + { + "x": 2.69385463293537, + "y": 7.033174248084845, + "heading": -2.3019112293489812e-17, + "angularVelocity": -1.383009794787412e-16, + "velocityX": 1.930383802944823, + "velocityY": 0.15131463018463287, + "timestamp": 1.3005862047227272 + }, + { + "x": 2.9498987369708023, + "y": 7.0532444635798015, + "heading": -3.165953990708067e-17, + "angularVelocity": -8.685660482484638e-17, + "velocityX": 2.573845017428572, + "velocityY": 0.20175283607883435, + "timestamp": 1.4000654283362906 + }, + { + "x": 3.269953856150127, + "y": 7.078332232096839, + "heading": -4.072669650039425e-17, + "angularVelocity": -9.114623399787232e-17, + "velocityX": 3.217306162567271, + "velocityY": 0.25219103653737, + "timestamp": 1.4995446519498539 + }, + { + "x": 3.6540199697781635, + "y": 7.108437552013749, + "heading": -5.11540764570592e-17, + "angularVelocity": -1.0481967568691581e-16, + "velocityX": 3.860767099670771, + "velocityY": 0.3026292206889044, + "timestamp": 1.5990238755634172 + }, + { + "x": 4.059538019609511, + "y": 7.140224398572357, + "heading": -5.1600847084745084e-17, + "angularVelocity": -4.491094839223168e-18, + "velocityX": 4.0764094762804115, + "velocityY": 0.3195325154736649, + "timestamp": 1.6985030991769805 + }, + { + "x": 4.465056069440878, + "y": 7.17201124513096, + "heading": 2.166741952023924e-17, + "angularVelocity": 7.36518279329432e-16, + "velocityX": 4.076409476280607, + "velocityY": 0.31953251547360895, + "timestamp": 1.7979823227905438 + }, + { + "x": 4.870574119272244, + "y": 7.203798091689569, + "heading": 3.259173052122981e-17, + "angularVelocity": 1.0981500060085112e-16, + "velocityX": 4.076409476280603, + "velocityY": 0.3195325154736528, + "timestamp": 1.8974615464041071 + }, + { + "x": 5.2760921691036105, + "y": 7.235584938248175, + "heading": 1.0710070167127387e-16, + "angularVelocity": 7.489902759935735e-16, + "velocityX": 4.076409476280604, + "velocityY": 0.3195325154736477, + "timestamp": 1.9969407700176705 + }, + { + "x": 5.681610218934977, + "y": 7.267371784806784, + "heading": 9.77928108998134e-17, + "angularVelocity": -9.356617827771563e-17, + "velocityX": 4.076409476280603, + "velocityY": 0.3195325154736528, + "timestamp": 2.0964199936312338 + }, + { + "x": 6.087128268766344, + "y": 7.299158631365387, + "heading": 1.6513103694525856e-16, + "angularVelocity": 6.769074345305727e-16, + "velocityX": 4.076409476280607, + "velocityY": 0.31953251547360895, + "timestamp": 2.195899217244797 + }, + { + "x": 6.492646318597691, + "y": 7.330945477923995, + "heading": 7.396242511112854e-17, + "angularVelocity": -9.16458819465443e-16, + "velocityX": 4.0764094762804115, + "velocityY": 0.31953251547366457, + "timestamp": 2.2953784408583604 + }, + { + "x": 6.876712432225728, + "y": 7.361050797840905, + "heading": 5.635610906288318e-17, + "angularVelocity": -1.7698485581961922e-16, + "velocityX": 3.860767099670771, + "velocityY": 0.3026292206889045, + "timestamp": 2.3948576644719237 + }, + { + "x": 7.196767551405052, + "y": 7.386138566357943, + "heading": 4.322206142605105e-17, + "angularVelocity": -1.3202804726242542e-16, + "velocityX": 3.2173061625672705, + "velocityY": 0.2521910365373701, + "timestamp": 2.494336888085487 + }, + { + "x": 7.452811655440485, + "y": 7.406208781852899, + "heading": 2.809460919160464e-17, + "angularVelocity": -1.520664485004353e-16, + "velocityX": 2.5738450174285714, + "velocityY": 0.20175283607883435, + "timestamp": 2.5938161116990504 + }, + { + "x": 7.644844737433633, + "y": 7.42126144378504, + "heading": 1.597946349618214e-17, + "angularVelocity": -1.2178568806018996e-16, + "velocityX": 1.9303838029448228, + "velocityY": 0.15131463018463284, + "timestamp": 2.6932953353126137 + }, + { + "x": 7.772866793935303, + "y": 7.431296551883997, + "heading": 8.20050547918977e-18, + "angularVelocity": -7.81968106954602e-17, + "velocityX": 1.2869225537885556, + "velocityY": 0.10087642157259895, + "timestamp": 2.792774558926177 + }, + { + "x": 7.836877822875977, + "y": 7.436314105987549, + "heading": -5.795186749052117e-29, + "angularVelocity": -8.243435343999241e-17, + "velocityX": 0.6434612838287801, + "velocityY": 0.05043821132986585, + "timestamp": 2.8922537825397403 + }, + { + "x": 7.836877822875977, + "y": 7.436314105987549, + "heading": -2.878299648600445e-30, + "angularVelocity": 6.318523951927698e-29, + "velocityX": -2.685897796857194e-25, + "velocityY": -2.998497537131158e-25, + "timestamp": 2.9917330061533036 + }, + { + "x": 7.772829081039793, + "y": 7.430431483450002, + "heading": 2.3106084226064385e-18, + "angularVelocity": 2.320700449568332e-17, + "velocityX": -0.6432848704351871, + "velocityY": -0.05908316023699325, + "timestamp": 3.0912981362843035 + }, + { + "x": 7.644731598748364, + "y": 7.418666238501744, + "heading": 5.038306873108203e-18, + "angularVelocity": 2.7396121984342994e-17, + "velocityX": -1.2865697270007066, + "velocityY": -0.11816631920009113, + "timestamp": 3.1908632664153034 + }, + { + "x": 7.452585378073089, + "y": 7.401018371333026, + "heading": 8.895741825302513e-18, + "angularVelocity": 3.874283041736699e-17, + "velocityX": -1.929854562761723, + "velocityY": -0.1772494762523457, + "timestamp": 3.2904283965463033 + }, + { + "x": 7.196390422466309, + "y": 7.377487882260941, + "heading": 1.1475783692536467e-17, + "angularVelocity": 2.591310696619824e-17, + "velocityX": -2.5731393638485627, + "velocityY": -0.236332630119861, + "timestamp": 3.389993526677303 + }, + { + "x": 6.876146738832702, + "y": 7.3480747719196655, + "heading": 1.4866814385424972e-17, + "angularVelocity": 3.405841672060239e-17, + "velocityX": -3.216424095587036, + "velocityY": -0.29541577761789645, + "timestamp": 3.489558656808303 + }, + { + "x": 6.491854347886312, + "y": 7.312779042211733, + "heading": 1.3228269289281303e-17, + "angularVelocity": -1.6457017572655323e-17, + "velocityX": -3.8597086192803536, + "velocityY": -0.35449890600748984, + "timestamp": 3.589123786939303 + }, + { + "x": 6.086447476209309, + "y": 7.27554403131357, + "heading": 1.2208912555752519e-17, + "angularVelocity": -1.0238089703026631e-17, + "velocityX": -4.071775642171123, + "velocityY": -0.3739764197482683, + "timestamp": 3.6886889170703028 + }, + { + "x": 5.681040604532077, + "y": 7.2383090204177005, + "heading": 7.247479687391899e-17, + "angularVelocity": 6.05291071671654e-16, + "velocityX": -4.071775642173424, + "velocityY": -0.3739764197252527, + "timestamp": 3.7882540472013027 + }, + { + "x": 5.275633732854971, + "y": 7.201074009520451, + "heading": 1.0354124283363177e-16, + "angularVelocity": 3.1202134641708547e-16, + "velocityX": -4.0717756421721525, + "velocityY": -0.3739764197391007, + "timestamp": 3.8878191773323025 + }, + { + "x": 4.8702268611778505, + "y": 7.163838998623373, + "heading": 8.838275427260066e-18, + "angularVelocity": -9.511660084390495e-16, + "velocityX": -4.071775642172309, + "velocityY": -0.3739764197373963, + "timestamp": 3.9873843074633024 + }, + { + "x": 4.464819989500739, + "y": 7.12660398772638, + "heading": -4.197248226534058e-17, + "angularVelocity": -5.103268345634044e-16, + "velocityX": -4.071775642172202, + "velocityY": -0.37397641973651996, + "timestamp": 4.086949437594302 + }, + { + "x": 4.080527598554363, + "y": 7.091308258018297, + "heading": -2.975056105500731e-17, + "angularVelocity": 1.2275302803554645e-16, + "velocityX": -3.859708619280215, + "velocityY": -0.3544989060089996, + "timestamp": 4.186514567725301 + }, + { + "x": 3.7602839149207647, + "y": 7.061895147676932, + "heading": -1.550761080609776e-17, + "angularVelocity": 1.4305159075441368e-16, + "velocityX": -3.2164240955869543, + "velocityY": -0.2954157776187877, + "timestamp": 4.286079697856301 + }, + { + "x": 3.50408895931399, + "y": 7.038364658604788, + "heading": -1.759561477000234e-18, + "angularVelocity": 1.3808096580609213e-16, + "velocityX": -2.5731393638485094, + "velocityY": -0.23633263012044387, + "timestamp": 4.3856448279873 + }, + { + "x": 3.3119427386387184, + "y": 7.020716791436034, + "heading": -4.625194214758358e-19, + "angularVelocity": 1.3027071363418387e-17, + "velocityX": -1.9298545627616883, + "velocityY": -0.17724947625272364, + "timestamp": 4.4852099581183 + }, + { + "x": 3.1838452563472903, + "y": 7.0089515464877525, + "heading": 1.6084993167813846e-19, + "angularVelocity": 6.260920387830223e-18, + "velocityX": -1.2865697270006862, + "velocityY": -0.11816631920031569, + "timestamp": 4.584775088249299 + }, + { + "x": 3.1197965145111084, + "y": 7.003068923950195, + "heading": 4.3763650818108325e-31, + "angularVelocity": -1.6155247471267731e-18, + "velocityX": -0.6432848704351777, + "velocityY": -0.05908316023709528, + "timestamp": 4.6843402183802985 + }, + { + "x": 3.1197965145111084, + "y": 7.003068923950195, + "heading": 1.7233142036729875e-31, + "angularVelocity": -9.337973840266154e-31, + "velocityX": 1.535724623502671e-24, + "velocityY": 1.8094635509087927e-25, + "timestamp": 4.783905348511298 + } + ], + "constraints": [ + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4NT-Close": { + "waypoints": [ + { + "x": 0.7644821405410767, + "y": 6.707253456115723, + "heading": 1.0390723224103011, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.3547892570495605, + "y": 7.003068923950195, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 7.667381286621094, + "y": 7.427369117736816, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 3.0072953701019287, + "y": 7.003068923950195, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 5.127899646759033, + "y": 6.643808746337891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 7.7319159507751465, + "y": 5.835511207580566, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 5.127899646759033, + "y": 6.643808746337891, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 3.0297956466674805, + "y": 7.003068923950195, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7644821405410767, + "y": 6.707253456115723, + "heading": 1.0390723224103011, + "angularVelocity": -4.4871567857707046e-18, + "velocityX": 3.460724762099423e-16, + "velocityY": 6.79633296625553e-17, + "timestamp": 0 + }, + { + "x": 0.7893073618901386, + "y": 6.711898127758422, + "heading": 1.0229599251421253, + "angularVelocity": -0.2562974082798586, + "velocityX": 0.3948909517207538, + "velocityY": 0.0738820725755683, + "timestamp": 0.06286601716469933 + }, + { + "x": 0.8389638482798748, + "y": 6.721192561704798, + "heading": 0.9908267229593493, + "angularVelocity": -0.5111378711748218, + "velocityX": 0.7898780395080967, + "velocityY": 0.14784512150697973, + "timestamp": 0.12573203432939867 + }, + { + "x": 0.9134574420677571, + "y": 6.735142975200245, + "heading": 0.9427373156582869, + "angularVelocity": -0.7649507551133621, + "velocityX": 1.1849580607710317, + "velocityY": 0.22190706719814038, + "timestamp": 0.188598051494098 + }, + { + "x": 1.0127948463812606, + "y": 6.753755537156801, + "heading": 0.8787447001001124, + "angularVelocity": -1.0179206261879956, + "velocityX": 1.5801447076447082, + "velocityY": 0.296067140817775, + "timestamp": 0.25146406865879734 + }, + { + "x": 1.136986588346673, + "y": 6.777034858063683, + "heading": 0.798950116173758, + "angularVelocity": -1.2692800900268044, + "velocityX": 1.9754988078860747, + "velocityY": 0.3703005527754691, + "timestamp": 0.31433008582349664 + }, + { + "x": 1.2860552286500786, + "y": 6.804983168108553, + "heading": 0.70370039577791, + "angularVelocity": -1.5151225525614787, + "velocityX": 2.3712117774673787, + "velocityY": 0.44456943998292453, + "timestamp": 0.37719610298819595 + }, + { + "x": 1.4600612279623586, + "y": 6.8376056601235335, + "heading": 0.5943219599600833, + "angularVelocity": -1.739865840894617, + "velocityX": 2.7678864855778302, + "velocityY": 0.5189209287668323, + "timestamp": 0.44006212015289525 + }, + { + "x": 1.6592143751431372, + "y": 6.8749781984647464, + "heading": 0.47881641796683205, + "angularVelocity": -1.837328770020157, + "velocityX": 3.167898272591771, + "velocityY": 0.5944791801156865, + "timestamp": 0.5029281373175946 + }, + { + "x": 1.833246757687688, + "y": 6.907207190100634, + "heading": 0.3632682477784989, + "angularVelocity": -1.8380068501150066, + "velocityX": 2.7683061595681933, + "velocityY": 0.5126615791717365, + "timestamp": 0.5657941544822939 + }, + { + "x": 1.98232093363545, + "y": 6.934701065225548, + "heading": 0.2612435638075881, + "angularVelocity": -1.6228908490179, + "velocityX": 2.371299832105078, + "velocityY": 0.43734081408223097, + "timestamp": 0.6286601716469932 + }, + { + "x": 2.1065071082491382, + "y": 6.957543379697764, + "heading": 0.1749925302220398, + "angularVelocity": -1.371981834947508, + "velocityX": 1.975410248884438, + "velocityY": 0.3633491590913955, + "timestamp": 0.6915261888116925 + }, + { + "x": 2.2058348256223446, + "y": 6.9757778777630985, + "heading": 0.10539352393852991, + "angularVelocity": -1.1071006152833613, + "velocityX": 1.5799906189855346, + "velocityY": 0.29005333704479014, + "timestamp": 0.7543922059763918 + }, + { + "x": 2.28031886152934, + "y": 6.9894324604556495, + "heading": 0.052874348065941004, + "angularVelocity": -0.8354143978130386, + "velocityX": 1.1848060250398966, + "velocityY": 0.2172013324269997, + "timestamp": 0.8172582231410911 + }, + { + "x": 2.329968057638108, + "y": 6.998525566093112, + "heading": 0.017682384897765975, + "angularVelocity": -0.5597931085081341, + "velocityX": 0.7897620741376922, + "velocityY": 0.14464262327357277, + "timestamp": 0.8801242403057904 + }, + { + "x": 2.3547892570495605, + "y": 7.003068923950195, + "heading": -1.6526519468843367e-17, + "angularVelocity": -0.28127095838489014, + "velocityX": 0.39482697538202266, + "velocityY": 0.07227048987661186, + "timestamp": 0.9429902574704897 + }, + { + "x": 2.35478925704956, + "y": 7.003068923950195, + "heading": -7.945307467511743e-18, + "angularVelocity": 9.541751832566194e-18, + "velocityX": -2.469200856331852e-16, + "velocityY": -5.7488464838897e-17, + "timestamp": 1.0058562746351891 + }, + { + "x": 2.415475621976421, + "y": 7.007915755074314, + "heading": -3.517124144434822e-19, + "angularVelocity": 7.839197585347938e-17, + "velocityX": 0.6264916515173242, + "velocityY": 0.05003593870277661, + "timestamp": 1.1027232716332853 + }, + { + "x": 2.536848350474334, + "y": 7.017609417214267, + "heading": 2.0883695559124673e-17, + "angularVelocity": 2.1922232683494886e-16, + "velocityX": 1.2529832890380508, + "velocityY": 0.1000718762876886, + "timestamp": 1.1995902686313813 + }, + { + "x": 2.7189074405095877, + "y": 7.03214991020763, + "heading": 4.970708271093658e-17, + "angularVelocity": 2.975563285402562e-16, + "velocityX": 1.8794749055638844, + "velocityY": 0.1501078121958038, + "timestamp": 1.2964572656294773 + }, + { + "x": 2.9616528886926634, + "y": 7.051537233783689, + "heading": 1.0287632521899536e-16, + "angularVelocity": 5.488891512697579e-16, + "velocityX": 2.505966487098253, + "velocityY": 0.20014374530925957, + "timestamp": 1.3933242626275733 + }, + { + "x": 3.2650846882445492, + "y": 7.075771387401028, + "heading": 1.9068454520432853e-16, + "angularVelocity": 9.064823379151022e-16, + "velocityX": 3.1324579986499463, + "velocityY": 0.25017967283341686, + "timestamp": 1.4901912596256692 + }, + { + "x": 3.629202818829201, + "y": 7.104852369435471, + "heading": 3.733614670935224e-16, + "angularVelocity": 1.8858530856090668e-15, + "velocityX": 3.7589493002638363, + "velocityY": 0.300215583590495, + "timestamp": 1.5870582566237652 + }, + { + "x": 4.024026376830976, + "y": 7.136385698409197, + "heading": 4.457439879437158e-16, + "angularVelocity": 7.472361390272464e-16, + "velocityX": 4.07593473770572, + "velocityY": 0.3255322240901749, + "timestamp": 1.6839252536218612 + }, + { + "x": 4.418849934832716, + "y": 7.167919027382921, + "heading": 4.558091019327557e-16, + "angularVelocity": 1.0390653460947907e-16, + "velocityX": 4.075934737705357, + "velocityY": 0.32553222409014865, + "timestamp": 1.7807922506199572 + }, + { + "x": 4.8136734928344564, + "y": 7.199452356356644, + "heading": 4.22612711995065e-16, + "angularVelocity": -3.427007234567253e-16, + "velocityX": 4.075934737705359, + "velocityY": 0.32553222409012916, + "timestamp": 1.8776592476180531 + }, + { + "x": 5.208497050836196, + "y": 7.230985685330376, + "heading": 4.132801040379339e-16, + "angularVelocity": -9.634455769312537e-17, + "velocityX": 4.07593473770535, + "velocityY": 0.3255322240902326, + "timestamp": 1.9745262446161491 + }, + { + "x": 5.6033206088379375, + "y": 7.262519014304082, + "heading": 4.974719423376743e-16, + "angularVelocity": 8.691488422559714e-16, + "velocityX": 4.0759347377053725, + "velocityY": 0.32553222408995947, + "timestamp": 2.071393241614245 + }, + { + "x": 5.998144166839676, + "y": 7.294052343277824, + "heading": 4.787346935143853e-16, + "angularVelocity": -1.9343274205936813e-16, + "velocityX": 4.075934737705342, + "velocityY": 0.3255322240903373, + "timestamp": 2.168260238612341 + }, + { + "x": 6.392967724841452, + "y": 7.325585672251541, + "heading": 4.705780860197851e-16, + "angularVelocity": -8.420419481072571e-17, + "velocityX": 4.075934737705728, + "velocityY": 0.32553222409008153, + "timestamp": 2.265127235610437 + }, + { + "x": 6.757085855426104, + "y": 7.3546666542859835, + "heading": 2.57250007761421e-16, + "angularVelocity": -2.20227825947358e-15, + "velocityX": 3.7589493002638363, + "velocityY": 0.3002155835905016, + "timestamp": 2.361994232608533 + }, + { + "x": 7.06051765497799, + "y": 7.378900807903323, + "heading": 1.3962913393362377e-16, + "angularVelocity": -1.2142512866829015e-15, + "velocityX": 3.132457998649947, + "velocityY": 0.25017967283342074, + "timestamp": 2.458861229606629 + }, + { + "x": 7.303263103161066, + "y": 7.398288131479383, + "heading": 7.249700096797418e-17, + "angularVelocity": -6.930341220083241e-16, + "velocityX": 2.5059664870982536, + "velocityY": 0.20014374530926216, + "timestamp": 2.555728226604725 + }, + { + "x": 7.485322193196319, + "y": 7.412828624472744, + "heading": 2.3826173239994654e-17, + "angularVelocity": -5.024500609510776e-16, + "velocityX": 1.8794749055638846, + "velocityY": 0.1501078121958055, + "timestamp": 2.652595223602821 + }, + { + "x": 7.606694921694233, + "y": 7.422522286612698, + "heading": 6.1848492309357054e-18, + "angularVelocity": -1.8211904002605062e-16, + "velocityX": 1.2529832890380512, + "velocityY": 0.10007187628768963, + "timestamp": 2.749462220600917 + }, + { + "x": 7.667381286621095, + "y": 7.427369117736816, + "heading": -2.892642650950664e-26, + "angularVelocity": -6.384888230976486e-17, + "velocityX": 0.6264916515173246, + "velocityY": 0.05003593870277713, + "timestamp": 2.846329217599013 + }, + { + "x": 7.667381286621094, + "y": 7.427369117736816, + "heading": -2.807242222383161e-26, + "angularVelocity": -9.314848061399685e-27, + "velocityX": 2.930418041341978e-17, + "velocityY": 1.8573166632838487e-18, + "timestamp": 2.943196214597109 + }, + { + "x": 7.6043184218722315, + "y": 7.4216272524122235, + "heading": -5.4231338022379995e-19, + "angularVelocity": -5.489423433560598e-18, + "velocityX": -0.6383377994270321, + "velocityY": -0.05812057048950791, + "timestamp": 3.041988523004143 + }, + { + "x": 7.478192693747755, + "y": 7.410143521888074, + "heading": 9.1638326322218e-18, + "angularVelocity": 9.824800347715444e-17, + "velocityX": -1.276675584953701, + "velocityY": -0.11624113971338963, + "timestamp": 3.140780831411177 + }, + { + "x": 7.289004104307538, + "y": 7.392917926351917, + "heading": 3.509462172627408e-17, + "angularVelocity": 2.624778291044605e-16, + "velocityX": -1.915013349629827, + "velocityY": -0.17436170703883222, + "timestamp": 3.2395731398182113 + }, + { + "x": 7.0367526569847, + "y": 7.369950466116338, + "heading": 8.808387501704071e-17, + "angularVelocity": 5.363702518284207e-16, + "velocityX": -2.5533510795550636, + "velocityY": -0.23248227120021098, + "timestamp": 3.3383654482252454 + }, + { + "x": 6.721438358645465, + "y": 7.341241141806506, + "heading": 1.8168852174846708e-16, + "angularVelocity": 9.474892406751412e-16, + "velocityX": -3.191688739978706, + "velocityY": -0.2906028290334789, + "timestamp": 3.4371577566322795 + }, + { + "x": 6.343061229887728, + "y": 7.306789955297856, + "heading": 3.542313739589584e-16, + "angularVelocity": 1.7465211410774172e-15, + "velocityX": -3.830026191905404, + "velocityY": -0.3487233678831281, + "timestamp": 3.5359500650393136 + }, + { + "x": 5.940772069277239, + "y": 7.270161581516117, + "heading": 3.929421150957015e-16, + "angularVelocity": 3.9183962579211066e-16, + "velocityX": -4.072069648914557, + "velocityY": -0.37076139197828895, + "timestamp": 3.6347423734463478 + }, + { + "x": 5.5384829086667535, + "y": 7.233533207734375, + "heading": 3.5944692196834336e-16, + "angularVelocity": -3.3904656795171214e-16, + "velocityX": -4.072069648914502, + "velocityY": -0.37076139197830654, + "timestamp": 3.733534681853382 + }, + { + "x": 5.136193748056268, + "y": 7.1969048339526385, + "heading": 2.816331867279604e-16, + "angularVelocity": -7.876497311722583e-16, + "velocityX": -4.072069648914507, + "velocityY": -0.3707613919782439, + "timestamp": 3.832326990260416 + }, + { + "x": 4.7339045874457835, + "y": 7.1602764601708975, + "heading": 3.7044270006052305e-16, + "angularVelocity": 8.989516982312421e-16, + "velocityX": -4.072069648914502, + "velocityY": -0.3707613919782984, + "timestamp": 3.93111929866745 + }, + { + "x": 4.3316154268352935, + "y": 7.123648086389155, + "heading": 3.627714094587869e-16, + "angularVelocity": -7.765068710911665e-17, + "velocityX": -4.072069648914555, + "velocityY": -0.37076139197830443, + "timestamp": 4.029911607074484 + }, + { + "x": 3.9532382980775562, + "y": 7.0891968998805055, + "heading": 2.0138393758473373e-16, + "angularVelocity": -1.6336036426615943e-15, + "velocityX": -3.8300261919054046, + "velocityY": -0.34872336788312547, + "timestamp": 4.128703915481519 + }, + { + "x": 3.6379239997383217, + "y": 7.060487575570674, + "heading": 1.144803164813082e-16, + "angularVelocity": -8.79659814093205e-16, + "velocityX": -3.191688739978706, + "velocityY": -0.29060282903347734, + "timestamp": 4.227496223888553 + }, + { + "x": 3.385672552415484, + "y": 7.037520115335095, + "heading": 6.046472103882109e-17, + "angularVelocity": -5.467591390886901e-16, + "velocityX": -2.553351079555064, + "velocityY": -0.23248227120021, + "timestamp": 4.326288532295588 + }, + { + "x": 3.196483962975267, + "y": 7.020294519798938, + "heading": 3.2682810287652467e-17, + "angularVelocity": -2.8121533703852023e-16, + "velocityX": -1.9150133496298272, + "velocityY": -0.17436170703883164, + "timestamp": 4.4250808407026225 + }, + { + "x": 3.07035823485079, + "y": 7.008810789274788, + "heading": 1.6547008414870972e-17, + "angularVelocity": -1.6333055858508895e-16, + "velocityX": -1.276675584953701, + "velocityY": -0.11624113971338933, + "timestamp": 4.523873149109657 + }, + { + "x": 3.0072953701019274, + "y": 7.003068923950195, + "heading": -5.572543390873087e-26, + "angularVelocity": -1.6749288756381388e-16, + "velocityX": -0.638337799427032, + "velocityY": -0.058120570489507827, + "timestamp": 4.622665457516692 + }, + { + "x": 3.007295370101928, + "y": 7.003068923950195, + "heading": -2.701724786958032e-26, + "angularVelocity": 1.8342141297599884e-26, + "velocityX": 1.650330335862223e-16, + "velocityY": -1.072378048093932e-16, + "timestamp": 4.721457765923726 + }, + { + "x": 3.036322603690972, + "y": 6.998184073615173, + "heading": -6.659182533814151e-19, + "angularVelocity": -9.886580304682387e-18, + "velocityX": 0.430953927728845, + "velocityY": -0.07252311632756721, + "timestamp": 4.788813535813145 + }, + { + "x": 3.094377070111199, + "y": 6.988414372048806, + "heading": -6.272481275693907e-18, + "angularVelocity": -8.323805132134451e-17, + "velocityX": 0.8619078442060418, + "velocityY": -0.145046245962409, + "timestamp": 4.856169305702564 + }, + { + "x": 3.1814587683881745, + "y": 6.973759818098456, + "heading": -1.1245385282688892e-17, + "angularVelocity": -7.383040860905551e-17, + "velocityX": 1.292861746216284, + "velocityY": -0.2175693927099312, + "timestamp": 4.923525075591983 + }, + { + "x": 3.2975676972225916, + "y": 6.954220410226885, + "heading": -1.425981430781261e-17, + "angularVelocity": -4.475383529525523e-17, + "velocityX": 1.7238156289363036, + "velocityY": -0.2900925622801421, + "timestamp": 4.990880845481402 + }, + { + "x": 3.442703854795298, + "y": 6.9297961462812125, + "heading": -2.2571965647820338e-17, + "angularVelocity": -1.2340667118105605e-16, + "velocityX": 2.154769484648219, + "velocityY": -0.3626157638131432, + "timestamp": 5.058236615370821 + }, + { + "x": 3.61686723837731, + "y": 6.900487023030509, + "heading": -2.846165657687679e-17, + "angularVelocity": -8.74415093652977e-17, + "velocityX": 2.5857232998441617, + "velocityY": -0.43513901331428134, + "timestamp": 5.12559238526024 + }, + { + "x": 3.820057843419671, + "y": 6.866293035085623, + "heading": -3.614720128617906e-17, + "angularVelocity": -1.141037318539728e-16, + "velocityX": 3.0166770475038507, + "velocityY": -0.5076623428256568, + "timestamp": 5.192948155149659 + }, + { + "x": 4.052275660822413, + "y": 6.827214171652438, + "heading": -4.8269174033446284e-17, + "angularVelocity": -1.7996931628267424e-16, + "velocityX": 3.4476306600605273, + "velocityY": -0.5801858325923079, + "timestamp": 5.260303925039078 + }, + { + "x": 4.313520663286318, + "y": 6.783250400223824, + "heading": -6.49281895371743e-17, + "angularVelocity": -2.4732870697850985e-16, + "velocityX": 3.878583867318316, + "velocityY": -0.6527098049772873, + "timestamp": 5.327659694928497 + }, + { + "x": 4.585110952119839, + "y": 6.737529090718732, + "heading": -9.015940488845162e-17, + "angularVelocity": -3.745962038515257e-16, + "velocityX": 4.032175554958435, + "velocityY": -0.6788031608898043, + "timestamp": 5.395015464817916 + }, + { + "x": 4.85667163042734, + "y": 6.691632237173498, + "heading": -4.751315953731664e-17, + "angularVelocity": 6.331491122848668e-16, + "velocityX": 4.031735941157495, + "velocityY": -0.6814093821593625, + "timestamp": 5.4623712347073345 + }, + { + "x": 5.127899646759034, + "y": 6.643808746337891, + "heading": 8.852905771561079e-23, + "angularVelocity": 7.054072252152007e-16, + "velocityX": 4.026797062478613, + "velocityY": -0.7100132759837658, + "timestamp": 5.5297270045967535 + }, + { + "x": 5.435225736063499, + "y": 6.550301165877784, + "heading": -3.054048980043161e-19, + "angularVelocity": -3.89086507141557e-18, + "velocityX": 3.9118505934900796, + "velocityY": -1.190226592691475, + "timestamp": 5.608289843836051 + }, + { + "x": 5.741994583635364, + "y": 6.454981350959253, + "heading": -3.5555327560181985e-17, + "angularVelocity": -4.486866408095523e-16, + "velocityX": 3.9047576505906223, + "velocityY": -1.213293916587032, + "timestamp": 5.686852683075348 + }, + { + "x": 6.048713801922417, + "y": 6.3595019612227075, + "heading": 7.714193179741933e-18, + "angularVelocity": 5.507616890435799e-16, + "velocityX": 3.9041259360904785, + "velocityY": -1.2153250908580402, + "timestamp": 5.765415522314646 + }, + { + "x": 6.35543181351245, + "y": 6.264018695159109, + "heading": 2.1299456161841412e-17, + "angularVelocity": 1.7292071017284973e-16, + "velocityX": 3.904110576449497, + "velocityY": -1.2153744313228805, + "timestamp": 5.843978361553943 + }, + { + "x": 6.661317138890106, + "y": 6.168794814888757, + "heading": -2.6772613029830932e-14, + "angularVelocity": -3.41050623150326e-13, + "velocityX": 3.8935115932605218, + "velocityY": -1.212072796659924, + "timestamp": 5.9225412007932405 + }, + { + "x": 6.928966825594151, + "y": 6.085473915495383, + "heading": -3.073303152925181e-14, + "angularVelocity": -5.041082116318115e-14, + "velocityX": 3.406822987759037, + "velocityY": -1.0605637499881233, + "timestamp": 6.001104040032538 + }, + { + "x": 7.158380853190387, + "y": 6.014056000162917, + "heading": -2.7188130909040473e-14, + "angularVelocity": 4.512183847842235e-14, + "velocityX": 2.920134122157474, + "velocityY": -0.9090546627891893, + "timestamp": 6.079666879271835 + }, + { + "x": 7.34955921485854, + "y": 5.954541069923522, + "heading": -2.0785853473910866e-14, + "angularVelocity": 8.149241754648641e-14, + "velocityX": 2.4334451697429427, + "velocityY": -0.7575455624521772, + "timestamp": 6.158229718511133 + }, + { + "x": 7.502501907186795, + "y": 5.906929125287755, + "heading": -1.3764492993566052e-14, + "angularVelocity": 8.937251498866303e-14, + "velocityX": 1.9467561739005792, + "velocityY": -0.6060364556164637, + "timestamp": 6.23679255775043 + }, + { + "x": 7.617208928127525, + "y": 5.871220166560173, + "heading": -7.419672395660612e-15, + "angularVelocity": 8.076106815549843e-14, + "velocityX": 1.4600671519946562, + "velocityY": -0.4545273449041617, + "timestamp": 6.315355396989728 + }, + { + "x": 7.693680276315422, + "y": 5.847414193943067, + "heading": -2.6268163161796944e-15, + "angularVelocity": 6.100663836778702e-14, + "velocityX": 0.973378112710156, + "velocityY": -0.3030182316169395, + "timestamp": 6.393918236229025 + }, + { + "x": 7.731915950775149, + "y": 5.8355112075805655, + "heading": 4.745265421419366e-24, + "angularVelocity": 3.343585339172883e-14, + "velocityX": 0.4866890610109727, + "velocityY": -0.15150911649515025, + "timestamp": 6.472481075468322 + }, + { + "x": 7.731915950775149, + "y": 5.8355112075805655, + "heading": 2.3401335118799086e-24, + "angularVelocity": -8.262952430355052e-25, + "velocityX": -7.030605668713669e-16, + "velocityY": 2.492642804337347e-16, + "timestamp": 6.55104391470762 + }, + { + "x": 7.699082568912615, + "y": 5.845746923563925, + "heading": -2.39940234007706e-18, + "angularVelocity": -3.295608955934343e-17, + "velocityX": -0.4509701424121041, + "velocityY": 0.14058869458014864, + "timestamp": 6.623850024856996 + }, + { + "x": 7.633415805912784, + "y": 5.8662183553377405, + "heading": -7.195977584044839e-18, + "angularVelocity": -6.588149336242636e-17, + "velocityX": -0.901940274862986, + "velocityY": 0.28117738651074886, + "timestamp": 6.696656135006371 + }, + { + "x": 7.534915662742616, + "y": 5.8969255026447325, + "heading": -1.2575357650155535e-17, + "angularVelocity": -7.388638205297987e-17, + "velocityX": -1.3529103940325595, + "velocityY": 0.42176607490759116, + "timestamp": 6.769462245155747 + }, + { + "x": 7.40358214075581, + "y": 5.93786836512457, + "heading": -1.2739401484069817e-17, + "angularVelocity": -2.253160375109903e-18, + "velocityX": -1.8038804946089275, + "velocityY": 0.5623547583552307, + "timestamp": 6.842268355305123 + }, + { + "x": 7.239415241982809, + "y": 5.98904694223645, + "heading": -1.5244617828039866e-17, + "angularVelocity": -3.4409424685999395e-17, + "velocityX": -2.254850567296915, + "velocityY": 0.7029434343748852, + "timestamp": 6.915074465454499 + }, + { + "x": 7.042414969807385, + "y": 6.050461233078238, + "heading": -2.121155671219581e-17, + "angularVelocity": -8.195656807520109e-17, + "velocityX": -2.705820593508446, + "velocityY": 0.8435320980036471, + "timestamp": 6.987880575603874 + }, + { + "x": 6.812581330995641, + "y": 6.122111235842821, + "heading": -3.0333248024884946e-17, + "angularVelocity": -1.2528744281701554e-16, + "velocityX": -3.156790526786791, + "velocityY": 0.9841207368114688, + "timestamp": 7.06068668575325 + }, + { + "x": 6.549914345820485, + "y": 6.2039969450909735, + "heading": -3.5945148234933864e-17, + "angularVelocity": -7.708007265759047e-17, + "velocityX": -3.6077601816144935, + "velocityY": 1.124709300910954, + "timestamp": 7.133492795902626 + }, + { + "x": 6.265706841223026, + "y": 6.29259789219859, + "heading": -5.062522731893286e-17, + "angularVelocity": -2.0163251482297616e-16, + "velocityX": -3.9036216055816446, + "velocityY": 1.21694383789813, + "timestamp": 7.206298906052002 + }, + { + "x": 5.981499386126825, + "y": 6.381198998076362, + "heading": -3.5793594630502314e-17, + "angularVelocity": 2.0371412039541545e-16, + "velocityX": -3.90362092567637, + "velocityY": 1.2169460186238399, + "timestamp": 7.2791050162013775 + }, + { + "x": 5.697290967881506, + "y": 6.46979701439404, + "heading": -7.376404563418145e-17, + "angularVelocity": -5.215283580351126e-16, + "velocityX": -3.90363415463632, + "velocityY": 1.216903583173247, + "timestamp": 7.351911126350753 + }, + { + "x": 5.41304748356572, + "y": 6.558282465917216, + "heading": -8.687949694144753e-18, + "angularVelocity": 8.938274014785796e-16, + "velocityX": -3.9041157910044126, + "velocityY": 1.2153574932329017, + "timestamp": 7.424717236500129 + }, + { + "x": 5.127899646759033, + "y": 6.643808746337891, + "heading": -1.0157183102941835e-28, + "angularVelocity": 1.1932995438677598e-16, + "velocityX": -3.9165371727956653, + "velocityY": 1.1747129498500006, + "timestamp": 7.497523346649505 + }, + { + "x": 4.835131855746803, + "y": 6.69650604863309, + "heading": -2.645085913591072e-17, + "angularVelocity": -3.635805599807214e-16, + "velocityX": -4.024242721584003, + "velocityY": 0.7243513177298383, + "timestamp": 7.570274373679255 + }, + { + "x": 4.54187305912769, + "y": 6.7463985666619255, + "heading": 4.8500280831595865e-17, + "angularVelocity": 1.0302416754610894e-15, + "velocityX": -4.030991844269019, + "velocityY": 0.6857981263744645, + "timestamp": 7.643025400709005 + }, + { + "x": 4.248595942277107, + "y": 6.796183281920838, + "heading": 1.565041210121607e-17, + "angularVelocity": -4.515382128821962e-16, + "velocityX": -4.031243665201547, + "velocityY": 0.6843163222774555, + "timestamp": 7.7157764277387555 + }, + { + "x": 3.9777514636703137, + "y": 6.84215783779885, + "heading": 1.1250247092738965e-17, + "angularVelocity": -6.048251553582641e-17, + "velocityX": -3.7228956025052042, + "velocityY": 0.6319437368109796, + "timestamp": 7.788527454768506 + }, + { + "x": 3.74076252273117, + "y": 6.882385596024865, + "heading": 7.740492042931928e-18, + "angularVelocity": -4.824337500544115e-17, + "velocityX": -3.2575339567678285, + "velocityY": 0.5529510698119757, + "timestamp": 7.861278481798256 + }, + { + "x": 3.5376291362506724, + "y": 6.916866540158376, + "heading": 4.67781503303015e-18, + "angularVelocity": -4.20980577468297e-17, + "velocityX": -2.792172080229617, + "velocityY": 0.4739581768297859, + "timestamp": 7.934029508828006 + }, + { + "x": 3.368351309817325, + "y": 6.9456006646442425, + "heading": 1.1688792842416151e-18, + "angularVelocity": -4.823211315826823e-17, + "velocityX": -2.326810126874584, + "velocityY": 0.3949652074893709, + "timestamp": 8.006780535857756 + }, + { + "x": 3.2329290462231546, + "y": 6.968587966690773, + "heading": -1.3220258587467443e-18, + "angularVelocity": -3.4238762839143694e-17, + "velocityX": -1.861448135141702, + "velocityY": 0.3159721997757365, + "timestamp": 8.079531562887505 + }, + { + "x": 3.131362347142629, + "y": 6.985828444618419, + "heading": -1.9579482778740535e-18, + "angularVelocity": -8.74107836798576e-18, + "velocityX": -1.396086120392406, + "velocityY": 0.2369791689758226, + "timestamp": 8.152282589917254 + }, + { + "x": 3.063651213691743, + "y": 6.997322097305583, + "heading": -1.6053614795746217e-18, + "angularVelocity": 4.846485264019486e-18, + "velocityX": -0.9307240903032074, + "velocityY": 0.1579861227590071, + "timestamp": 8.225033616947004 + }, + { + "x": 3.029795646667476, + "y": 7.003068923950196, + "heading": 1.137912307807085e-28, + "angularVelocity": 2.2066513092231192e-17, + "velocityX": -0.4653620492590992, + "velocityY": 0.07899306551733094, + "timestamp": 8.297784643976753 + }, + { + "x": 3.0297956466674782, + "y": 7.003068923950195, + "heading": 5.621189806598705e-29, + "angularVelocity": -1.879533730837788e-29, + "velocityX": -1.7673666788727504e-16, + "velocityY": 5.511519448895902e-17, + "timestamp": 8.370535671006502 + } + ], + "constraints": [ + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 7 + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4NT-Sweep": { + "waypoints": [ + { + "x": 0.7823778390884399, + "y": 6.68265962600708, + "heading": 1.0516504568237233, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.357928514480591, + "y": 6.761153697967529, + "heading": 0.48364772207543033, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.2180731296539307, + "y": 6.21727180480957, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.3937551975250244, + "y": 5.565863609313965, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.1248362064361572, + "y": 5.020731449127197, + "heading": -0.35924157629701275, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.3423891067504883, + "y": 4.305914878845215, + "heading": -0.5532943925729576, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7823778390884399, + "y": 6.68265962600708, + "heading": 1.0516504568237233, + "angularVelocity": -1.305568560622299e-21, + "velocityX": 1.822131686996459e-19, + "velocityY": 9.010541927457129e-21, + "timestamp": 0 + }, + { + "x": 0.8145359389114578, + "y": 6.684261194082208, + "heading": 1.040561037250096, + "angularVelocity": -0.14616328569842307, + "velocityX": 0.42385748872992884, + "velocityY": 0.021109351177139767, + "timestamp": 0.07587007585822825 + }, + { + "x": 0.8788523386671915, + "y": 6.687464759358471, + "heading": 1.0183708391377924, + "angularVelocity": -0.29247628740702164, + "velocityX": 0.847717614991134, + "velocityY": 0.04222435841831154, + "timestamp": 0.1517401517164565 + }, + { + "x": 0.9753269412813598, + "y": 6.692270939975434, + "heading": 0.9850418553098709, + "angularVelocity": -0.4392902399386075, + "velocityX": 1.271576461771859, + "velocityY": 0.06334751300295363, + "timestamp": 0.22761022757468474 + }, + { + "x": 1.1039593198138598, + "y": 6.698680457702348, + "heading": 0.9405064375600677, + "angularVelocity": -0.5869958247178071, + "velocityX": 1.6954296812996996, + "velocityY": 0.08448018081451894, + "timestamp": 0.303480303432913 + }, + { + "x": 1.2647486754140431, + "y": 6.7066940112498585, + "heading": 0.8846637199397575, + "angularVelocity": -0.7360308657745194, + "velocityX": 2.119272371634848, + "velocityY": 0.10562205793077338, + "timestamp": 0.37935037929114124 + }, + { + "x": 1.4576937911793324, + "y": 6.71631209605501, + "heading": 0.8173761180571837, + "angularVelocity": -0.8868793278697673, + "velocityX": 2.5430990226743524, + "velocityY": 0.1267704651188646, + "timestamp": 0.4552204551493695 + }, + { + "x": 1.6827929759984845, + "y": 6.727534757965461, + "heading": 0.7384664173131257, + "angularVelocity": -1.040063554061942, + "velocityX": 2.9669033841454855, + "velocityY": 0.147919476598685, + "timestamp": 0.5310905310075977 + }, + { + "x": 1.875690822215849, + "y": 6.7371431845238074, + "heading": 0.6658970717001427, + "angularVelocity": -0.9564949657963566, + "velocityX": 2.542475989846324, + "velocityY": 0.1266431653014338, + "timestamp": 0.606960606865826 + }, + { + "x": 2.0364373939926854, + "y": 6.745148361535974, + "heading": 0.6052481909952064, + "angularVelocity": -0.7993781476937682, + "velocityX": 2.1187084625723753, + "velocityY": 0.10551165161776474, + "timestamp": 0.6828306827240542 + }, + { + "x": 2.165033879772669, + "y": 6.751551231704223, + "heading": 0.5566362685774989, + "angularVelocity": -0.6407258971052618, + "velocityX": 1.6949565994935976, + "velocityY": 0.08439256315249083, + "timestamp": 0.7587007585822825 + }, + { + "x": 2.2614810401187784, + "y": 6.756352675949364, + "heading": 0.5201406088325774, + "angularVelocity": -0.4810283808483043, + "velocityX": 1.2712147609596647, + "velocityY": 0.06328508559965212, + "timestamp": 0.8345708344405107 + }, + { + "x": 2.32577924734246, + "y": 6.759553371141778, + "heading": 0.4958065317915587, + "angularVelocity": -0.32073352722738296, + "velocityX": 0.8474778296495941, + "velocityY": 0.04218652948753564, + "timestamp": 0.910440910298739 + }, + { + "x": 2.357928514480591, + "y": 6.761153697967529, + "heading": 0.48364772207543033, + "angularVelocity": -0.1602583044578536, + "velocityX": 0.4237410701711366, + "velocityY": 0.021092990980281293, + "timestamp": 0.9863109861569672 + }, + { + "x": 2.357928514480591, + "y": 6.761153697967529, + "heading": 0.48364772207543033, + "angularVelocity": 1.1064970967631094e-21, + "velocityX": -1.8225634104070948e-19, + "velocityY": -9.012713579557001e-21, + "timestamp": 1.0621810620151955 + }, + { + "x": 2.352635709829095, + "y": 6.749572893800152, + "heading": 0.46948048681204285, + "angularVelocity": -0.29258597212150655, + "velocityX": -0.10930858176748662, + "velocityY": -0.23917022497801835, + "timestamp": 1.1106018224524183 + }, + { + "x": 2.3424174496797017, + "y": 6.726214578494324, + "heading": 0.4417934028197234, + "angularVelocity": -0.5718019242637775, + "velocityX": -0.2110305591470682, + "velocityY": -0.4824029010471924, + "timestamp": 1.1590225828896412 + }, + { + "x": 2.327759577018962, + "y": 6.690845088074086, + "heading": 0.40137593071489563, + "angularVelocity": -0.8347137000714533, + "velocityX": -0.30271876212567034, + "velocityY": -0.7304612753055373, + "timestamp": 1.207443343326864 + }, + { + "x": 2.3093233873244357, + "y": 6.64319111632961, + "heading": 0.3491865327700304, + "angularVelocity": -1.0778310268903832, + "velocityX": -0.38074969347969373, + "velocityY": -0.9841640510016261, + "timestamp": 1.255864103764087 + }, + { + "x": 2.288046078154732, + "y": 6.582940438286813, + "heading": 0.2865195619339567, + "angularVelocity": -1.2942169901961953, + "velocityX": -0.4394253410639758, + "velocityY": -1.2443149900735475, + "timestamp": 1.3042848642013098 + }, + { + "x": 2.2653395566100536, + "y": 6.509769721522354, + "heading": 0.21545532238914708, + "angularVelocity": -1.4676398904751573, + "velocityX": -0.4689418617065527, + "velocityY": -1.5111434868794307, + "timestamp": 1.3527056246385327 + }, + { + "x": 2.243480972819474, + "y": 6.423521182774951, + "heading": 0.13956927847959602, + "angularVelocity": -1.5672212337089715, + "velocityX": -0.4514299980670268, + "velocityY": -1.7812305707016642, + "timestamp": 1.4011263850757556 + }, + { + "x": 2.2261544695623017, + "y": 6.324920657670576, + "heading": 0.06468345124510128, + "angularVelocity": -1.5465644603327438, + "velocityX": -0.35783211789157504, + "velocityY": -2.0363274804865874, + "timestamp": 1.4495471455129785 + }, + { + "x": 2.2180731296539307, + "y": 6.21727180480957, + "heading": -6.945757678881964e-28, + "angularVelocity": -1.335861945600024, + "velocityX": -0.16689824437698478, + "velocityY": -2.223196246588727, + "timestamp": 1.4979679059502014 + }, + { + "x": 2.2352227511494878, + "y": 6.037365041726398, + "heading": -0.04735082051046012, + "angularVelocity": -0.6135224632970496, + "velocityX": 0.2222068785955233, + "velocityY": -2.3310438818310217, + "timestamp": 1.575146531580302 + }, + { + "x": 2.2764918846129354, + "y": 5.877967183230441, + "heading": -0.04961558249433564, + "angularVelocity": -0.029344419719651398, + "velocityX": 0.5347223162698054, + "velocityY": -2.0653109224814865, + "timestamp": 1.6523251572104025 + }, + { + "x": 2.319236637610397, + "y": 5.75175219753006, + "heading": -0.03647975719615842, + "angularVelocity": 0.1702003008078197, + "velocityX": 0.5538418525658589, + "velocityY": -1.6353619239775097, + "timestamp": 1.729503782840503 + }, + { + "x": 2.3550484780260685, + "y": 5.658236249707955, + "heading": -0.020643050195192494, + "angularVelocity": 0.20519550421728966, + "velocityX": 0.4640124143608037, + "velocityY": -1.2116819528544773, + "timestamp": 1.8066824084706037 + }, + { + "x": 2.3804879836257373, + "y": 5.596496310620883, + "heading": -0.007498878272735541, + "angularVelocity": 0.17030844764525793, + "velocityX": 0.32961853611639047, + "velocityY": -0.7999616290522918, + "timestamp": 1.8838610341007043 + }, + { + "x": 2.3937551975250244, + "y": 5.565863609313965, + "heading": 2.4640341825929873e-23, + "angularVelocity": 0.09716263034633371, + "velocityX": 0.1719026970352361, + "velocityY": -0.3969065405975678, + "timestamp": 1.9610396597308049 + }, + { + "x": 2.3937551975250244, + "y": 5.565863609313965, + "heading": 2.4913308967565905e-23, + "angularVelocity": 3.537030839600793e-24, + "velocityX": -2.1338399459553326e-23, + "velocityY": -4.571258849613942e-23, + "timestamp": 2.0382182853609057 + }, + { + "x": 2.3837015705887254, + "y": 5.555193042220565, + "heading": -0.00946464391198462, + "angularVelocity": -0.18411639657129, + "velocityX": -0.1955739255694025, + "velocityY": -0.20757530667595975, + "timestamp": 2.089624048716262 + }, + { + "x": 2.3641582098837484, + "y": 5.5333437080108325, + "heading": -0.028253803373095267, + "angularVelocity": -0.3655068660536286, + "velocityX": -0.38017839692173266, + "velocityY": -0.42503666483256275, + "timestamp": 2.141029812071618 + }, + { + "x": 2.3359331924350997, + "y": 5.499671258409721, + "heading": -0.056164170438548724, + "angularVelocity": -0.5429423715102839, + "velocityX": -0.549063287972907, + "velocityY": -0.6550325761790774, + "timestamp": 2.192435575426974 + }, + { + "x": 2.300248237017326, + "y": 5.453359822013785, + "heading": -0.09289041649224676, + "angularVelocity": -0.7144382975079676, + "velocityX": -0.6941819961137773, + "velocityY": -0.9008996924293653, + "timestamp": 2.2438413387823304 + }, + { + "x": 2.25906191216666, + "y": 5.393424337289651, + "heading": -0.13788233192980848, + "angularVelocity": -0.8752309566252937, + "velocityX": -0.8012005301031048, + "velocityY": -1.1659292813106188, + "timestamp": 2.2952471021376866 + }, + { + "x": 2.215650672687085, + "y": 5.318928923927703, + "heading": -0.18994798908185376, + "angularVelocity": -1.0128369613369528, + "velocityX": -0.8444819538907223, + "velocityY": -1.44916461695119, + "timestamp": 2.3466528654930427 + }, + { + "x": 2.1751906895479802, + "y": 5.229944253664982, + "heading": -0.24676651261993965, + "angularVelocity": -1.105294811893225, + "velocityX": -0.7870709527142773, + "velocityY": -1.7310251702244053, + "timestamp": 2.398058628848399 + }, + { + "x": 2.143630529202551, + "y": 5.129096717824508, + "heading": -0.3045822393435162, + "angularVelocity": -1.1246934769533508, + "velocityX": -0.6139420618513388, + "velocityY": -1.9617943447962864, + "timestamp": 2.449464392203755 + }, + { + "x": 2.1248362064361572, + "y": 5.020731449127197, + "heading": -0.35924157629701275, + "angularVelocity": -1.063292000464021, + "velocityX": -0.3656073082014722, + "velocityY": -2.1080373410313484, + "timestamp": 2.5008701555591113 + }, + { + "x": 2.1236034975576716, + "y": 4.879581187775217, + "heading": -0.41834324922038263, + "angularVelocity": -0.9214808419098464, + "velocityX": -0.019219720170182603, + "velocityY": -2.200737394270732, + "timestamp": 2.5650078676366372 + }, + { + "x": 2.145285525498013, + "y": 4.739114283134644, + "heading": -0.4654704427453386, + "angularVelocity": -0.734781332205791, + "velocityX": 0.33805427786593684, + "velocityY": -2.190082871537182, + "timestamp": 2.629145579714163 + }, + { + "x": 2.1854463786837415, + "y": 4.612083049790391, + "heading": -0.49888391882293226, + "angularVelocity": -0.520964577551588, + "velocityX": 0.6261659776261528, + "velocityY": -1.9806012598439038, + "timestamp": 2.693283291791689 + }, + { + "x": 2.23085672934491, + "y": 4.507371852155421, + "heading": -0.5212416835862062, + "angularVelocity": -0.3485899954811167, + "velocityX": 0.7080132606894286, + "velocityY": -1.6325995150622288, + "timestamp": 2.757421003869215 + }, + { + "x": 2.2725752093899105, + "y": 4.425381093870058, + "heading": -0.5360315135324354, + "angularVelocity": -0.23059491003283958, + "velocityX": 0.6504516405975544, + "velocityY": -1.278354896511699, + "timestamp": 2.821558715946741 + }, + { + "x": 2.306393030453338, + "y": 4.365049187904869, + "heading": -0.5454513613849756, + "angularVelocity": -0.1468690969386946, + "velocityX": 0.5272689026161482, + "velocityY": -0.9406619601937548, + "timestamp": 2.885696428024267 + }, + { + "x": 2.330102947478555, + "y": 4.325455758257715, + "heading": -0.5508966378561475, + "angularVelocity": -0.08489976169698503, + "velocityX": 0.36967201132085403, + "velocityY": -0.6173190212849581, + "timestamp": 2.949834140101793 + }, + { + "x": 2.3423891067504883, + "y": 4.305914878845215, + "heading": -0.5532943925729576, + "angularVelocity": -0.037384475360018043, + "velocityX": 0.19155905120349095, + "velocityY": -0.30467066534709186, + "timestamp": 3.013971852179319 + }, + { + "x": 2.3423891067504883, + "y": 4.305914878845215, + "heading": -0.5532943925729576, + "angularVelocity": -2.509202302365357e-24, + "velocityX": 1.871020058653073e-23, + "velocityY": -1.3414864759418838e-22, + "timestamp": 3.078109564256845 + } + ], + "constraints": [ + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "3NB-Skip": { + "waypoints": [ + { + "x": 0.71, + "y": 4.38, + "heading": -1.0303768814651526, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 2.37, + "y": 2.8567728996276855, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "x": 7.745757102966309, + "y": 0.7672333121299744, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 24 + }, + { + "x": 1.724782943725586, + "y": 3.313033103942871, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.3322887420654297, + "y": 4.145541191101074, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7100000000000002, + "y": 4.38, + "heading": -1.0303768814651526, + "angularVelocity": 1.5560405504853757e-19, + "velocityX": 3.6579942357642265e-17, + "velocityY": 3.5772569104609715e-17, + "timestamp": 0 + }, + { + "x": 0.7196231633442646, + "y": 4.37055163049096, + "heading": -1.0120510397413214, + "angularVelocity": 0.3659111239569298, + "velocityX": 0.19214519957043122, + "velocityY": -0.1886551001974958, + "timestamp": 0.05008276743722991 + }, + { + "x": 0.7389303360988976, + "y": 4.351635791048551, + "heading": -0.9762703828685111, + "angularVelocity": 0.7144305058152697, + "velocityX": 0.3855053093628394, + "velocityY": -0.3776915775693403, + "timestamp": 0.10016553487445982 + }, + { + "x": 0.7679871206287426, + "y": 4.323231755138663, + "heading": -0.9240353862773414, + "angularVelocity": 1.0429734470371541, + "velocityX": 0.5801752981455696, + "velocityY": -0.5671419005645384, + "timestamp": 0.15024830231168973 + }, + { + "x": 0.806863824332317, + "y": 4.285312709598439, + "heading": -0.8565791228845684, + "angularVelocity": 1.3468956857728758, + "velocityX": 0.7762491110800954, + "velocityY": -0.7571276005812321, + "timestamp": 0.20033106974891965 + }, + { + "x": 0.855633414600585, + "y": 4.2378416688864, + "heading": -0.7754660421005081, + "angularVelocity": 1.6195806448938828, + "velocityX": 0.9737798600965913, + "velocityY": -0.9478517889716715, + "timestamp": 0.25041383718614957 + }, + { + "x": 0.914367608814468, + "y": 4.180769559172441, + "heading": -0.6826698176675856, + "angularVelocity": 1.8528573635477696, + "velocityX": 1.1727425863098562, + "velocityY": -1.1395558319631454, + "timestamp": 0.30049660462337946 + }, + { + "x": 0.9831324058657855, + "y": 4.114037722550235, + "heading": -0.5806325593431202, + "angularVelocity": 2.0373726043064977, + "velocityX": 1.3730231089468503, + "velocityY": -1.3324310943047128, + "timestamp": 0.35057937206060935 + }, + { + "x": 1.0619841550917224, + "y": 4.037586277197585, + "heading": -0.47236160213245165, + "angularVelocity": 2.161840544182557, + "velocityX": 1.5744287558544843, + "velocityY": -1.5265020138605254, + "timestamp": 0.40066213949783924 + }, + { + "x": 1.1509626755897528, + "y": 3.9513700864804493, + "heading": -0.3616923537124617, + "angularVelocity": 2.209727099419903, + "velocityX": 1.7766294686003532, + "velocityY": -1.721474174229327, + "timestamp": 0.45074490693506913 + }, + { + "x": 1.2500676182895643, + "y": 3.855395460973611, + "heading": -0.2538729536839052, + "angularVelocity": 2.1528243255265638, + "velocityX": 1.9788232114775315, + "velocityY": -1.9163203316812512, + "timestamp": 0.500827674372299 + }, + { + "x": 1.3591936256250599, + "y": 3.749835630519891, + "heading": -0.15652201954824588, + "angularVelocity": 1.9438010141446913, + "velocityX": 2.1789132853386093, + "velocityY": -2.1077076179149334, + "timestamp": 0.550910441809529 + }, + { + "x": 1.4780432446812957, + "y": 3.6354509708518212, + "heading": -0.08074916244880044, + "angularVelocity": 1.5129526776732942, + "velocityX": 2.3730641323924204, + "velocityY": -2.283912521635818, + "timestamp": 0.6009932092467589 + }, + { + "x": 1.6064830934074645, + "y": 3.512581098747126, + "heading": -0.02955142499135005, + "angularVelocity": 1.0222625481233227, + "velocityX": 2.564551747008494, + "velocityY": -2.453336314904971, + "timestamp": 0.6510759766839889 + }, + { + "x": 1.7444408206294664, + "y": 3.3811278178903486, + "heading": -0.002660437876514541, + "angularVelocity": 0.5369309343486035, + "velocityX": 2.7545947295123865, + "velocityY": -2.6247207888730513, + "timestamp": 0.7011587441212188 + }, + { + "x": 1.8918929861971419, + "y": 3.2410564135466466, + "heading": -1.0357533070062969e-7, + "angularVelocity": 0.053118755957712996, + "velocityX": 2.944169683762683, + "velocityY": -2.796798410137877, + "timestamp": 0.7512415115584488 + }, + { + "x": 2.042423702860939, + "y": 3.102214682631318, + "heading": -7.051190961599685e-8, + "angularVelocity": 6.601756010788189e-7, + "velocityX": 3.005638952608048, + "velocityY": -2.772245585058687, + "timestamp": 0.8013242789956787 + }, + { + "x": 2.2019910957141193, + "y": 2.9738606026990846, + "heading": -3.641731647880258e-8, + "angularVelocity": 6.807649595630949e-7, + "velocityX": 3.1860737938089994, + "velocityY": -2.5628392059822205, + "timestamp": 0.8514070464329087 + }, + { + "x": 2.3699999999999997, + "y": 2.856772899627685, + "heading": 1.2045924074798219e-17, + "angularVelocity": 7.271426553754078e-7, + "velocityX": 3.3546250114165237, + "velocityY": -2.337884048003882, + "timestamp": 0.9014898138701386 + }, + { + "x": 2.6672211579606206, + "y": 2.6962515532804225, + "heading": 4.4196760900488604e-15, + "angularVelocity": 5.335264470419185e-14, + "velocityX": 3.597746231094708, + "velocityY": -1.9430483105374854, + "timestamp": 0.9841029663464744 + }, + { + "x": 2.9807027390289593, + "y": 2.5704071318299984, + "heading": 4.257815888008083e-15, + "angularVelocity": -1.9592546692313966e-15, + "velocityX": 3.7945723128987225, + "velocityY": -1.5232976551446145, + "timestamp": 1.0667161188228103 + }, + { + "x": 3.2966550111442214, + "y": 2.4509011416193687, + "heading": 4.252784018072507e-15, + "angularVelocity": -6.090882409757076e-17, + "velocityX": 3.8244790647055846, + "velocityY": -1.446573416325604, + "timestamp": 1.149329271299146 + }, + { + "x": 3.6125876520861304, + "y": 2.331343263293774, + "heading": 4.270047996286602e-15, + "angularVelocity": 2.089737255596764e-16, + "velocityX": 3.8242414370090176, + "velocityY": -1.447201501718981, + "timestamp": 1.231942423775482 + }, + { + "x": 3.928520259560843, + "y": 2.2117852965308926, + "heading": 4.2804411742991834e-15, + "angularVelocity": 1.2580536751887178e-16, + "velocityX": 3.8242410319016575, + "velocityY": -1.4472025722178512, + "timestamp": 1.3145555762518177 + }, + { + "x": 4.244452867036739, + "y": 2.0922273297711427, + "heading": 4.296224621909443e-15, + "angularVelocity": 1.9105247937253344e-16, + "velocityX": 3.824241031915982, + "velocityY": -1.447202572179951, + "timestamp": 1.3971687287281536 + }, + { + "x": 4.560385474513634, + "y": 1.9726693630140306, + "heading": 4.316587253622556e-15, + "angularVelocity": 2.4648171752648295e-16, + "velocityX": 3.824241031928069, + "velocityY": -1.4472025721480182, + "timestamp": 1.4797818812044894 + }, + { + "x": 4.876318081990444, + "y": 1.8531113962566956, + "heading": 4.307806953716989e-15, + "angularVelocity": -1.0628210693875022e-16, + "velocityX": 3.8242410319270483, + "velocityY": -1.447202572150715, + "timestamp": 1.5623950336808252 + }, + { + "x": 5.1922506894672535, + "y": 1.7335534294993578, + "heading": 4.330504238512068e-15, + "angularVelocity": 2.747417828097679e-16, + "velocityX": 3.824241031927034, + "velocityY": -1.4472025721507527, + "timestamp": 1.645008186157161 + }, + { + "x": 5.508183296944064, + "y": 1.6139954627420232, + "heading": 4.320958307300627e-15, + "angularVelocity": -1.1554977538132229e-16, + "velocityX": 3.8242410319270497, + "velocityY": -1.4472025721507116, + "timestamp": 1.7276213386334969 + }, + { + "x": 5.824115904420868, + "y": 1.4944374959846716, + "heading": 4.32750725592863e-15, + "angularVelocity": 7.927246971250498e-17, + "velocityX": 3.8242410319269715, + "velocityY": -1.4472025721509183, + "timestamp": 1.8102344911098327 + }, + { + "x": 6.14004851189783, + "y": 1.374879529227738, + "heading": 4.3338322343448905e-15, + "angularVelocity": 7.656139762494459e-17, + "velocityX": 3.8242410319288864, + "velocityY": -1.4472025721458581, + "timestamp": 1.8928476435861685 + }, + { + "x": 6.455981119863945, + "y": 1.2553215637651396, + "heading": 4.319457948267045e-15, + "angularVelocity": -1.7399512846812765e-16, + "velocityX": 3.824241037849887, + "velocityY": -1.4472025564784359, + "timestamp": 1.9754607960625044 + }, + { + "x": 6.742597970324816, + "y": 1.1468575210026002, + "heading": 4.7282996807897215e-15, + "angularVelocity": 4.948869815524412e-15, + "velocityX": 3.469385223412319, + "velocityY": -1.312914947695717, + "timestamp": 2.05807394853884 + }, + { + "x": 6.993387738830674, + "y": 1.0519514743449843, + "heading": 4.134595926485456e-15, + "angularVelocity": -7.186552464221235e-15, + "velocityX": 3.0357123652644447, + "velocityY": -1.1488006910851218, + "timestamp": 2.140687101015176 + }, + { + "x": 7.208350406940146, + "y": 0.9706034307897187, + "heading": 3.2355739078561107e-15, + "angularVelocity": -1.0882311014677393e-14, + "velocityX": 2.6020392838909574, + "velocityY": -0.9846863497733962, + "timestamp": 2.223300253491512 + }, + { + "x": 7.387485968506043, + "y": 0.9028133926693046, + "heading": 2.30776491712744e-15, + "angularVelocity": -1.1230766104875401e-14, + "velocityX": 2.168366128108148, + "velocityY": -0.8205719802276565, + "timestamp": 2.3059134059678477 + }, + { + "x": 7.530794420454761, + "y": 0.8485813611499973, + "heading": 1.4584800676415084e-15, + "angularVelocity": -1.0280261998735142e-14, + "velocityX": 1.7346929351205342, + "velocityY": -0.6564575965648514, + "timestamp": 2.3885265584441835 + }, + { + "x": 7.63827576094213, + "y": 0.8079073369315515, + "heading": 7.577076994785405e-16, + "angularVelocity": -8.482576307571573e-15, + "velocityX": 1.301019719809994, + "velocityY": -0.4923432044317867, + "timestamp": 2.4711397109205193 + }, + { + "x": 7.709929988738705, + "y": 0.7807913204804714, + "heading": 2.614341795751167e-16, + "angularVelocity": -6.0071974782633566e-15, + "velocityX": 0.8673464896174847, + "velocityY": -0.32822880665187276, + "timestamp": 2.553752863396855 + }, + { + "x": 7.745757102966309, + "y": 0.7672333121299746, + "heading": -9.721098396623117e-28, + "angularVelocity": -3.1645588171616285e-15, + "velocityX": 0.4336732487949893, + "velocityY": -0.16411440483848933, + "timestamp": 2.636366015873191 + }, + { + "x": 7.745757102966309, + "y": 0.7672333121299745, + "heading": -4.7866198406740125e-28, + "angularVelocity": 1.5898414573642954e-28, + "velocityX": -1.603276029328449e-17, + "velocityY": -6.86367715897337e-17, + "timestamp": 2.718979168349527 + }, + { + "x": 7.697012773089809, + "y": 0.7878434827717467, + "heading": 9.784997624243692e-20, + "angularVelocity": 1.0076940613114793e-18, + "velocityX": -0.5019865449737438, + "velocityY": 0.2122509095518505, + "timestamp": 2.8160820297002216 + }, + { + "x": 7.599524114232943, + "y": 0.8290638236763864, + "heading": 2.93708175506948e-19, + "angularVelocity": 2.0170177947228716e-18, + "velocityX": -1.003973080718789, + "velocityY": 0.424501815201605, + "timestamp": 2.9131848910509164 + }, + { + "x": 7.453291127650296, + "y": 0.8908943343134272, + "heading": 7.722005480992539e-18, + "angularVelocity": 7.64992627627732e-17, + "velocityX": -1.5059596035436553, + "velocityY": 0.6367527153884249, + "timestamp": 3.010287752401611 + }, + { + "x": 7.258313815223747, + "y": 0.973335013887169, + "heading": 1.3656857911178107e-17, + "angularVelocity": 6.111923322990157e-17, + "velocityX": -2.007946106988254, + "velocityY": 0.8490036073808429, + "timestamp": 3.107390613752306 + }, + { + "x": 7.014592180089764, + "y": 1.0763858610714456, + "heading": 1.9690795477324688e-17, + "angularVelocity": 6.213964740290064e-17, + "velocityX": -2.5099325781324064, + "velocityY": 1.0612544857159245, + "timestamp": 3.2044934751030008 + }, + { + "x": 6.722126228521277, + "y": 1.200046873213924, + "heading": 2.5824842503133833e-17, + "angularVelocity": 6.317061042778013e-17, + "velocityX": -3.0119189846756718, + "velocityY": 1.273505336736336, + "timestamp": 3.3015963364536955 + }, + { + "x": 6.38091597933707, + "y": 1.34431804235761, + "heading": 3.0656082796974605e-17, + "angularVelocity": 4.9753840686431446e-17, + "velocityX": -3.5139051974163613, + "velocityY": 1.4857561058127722, + "timestamp": 3.3986991978043903 + }, + { + "x": 6.015216878005737, + "y": 1.4989436347306673, + "heading": 1.124404487622351e-17, + "angularVelocity": -1.999121102172735e-16, + "velocityX": -3.7661001565193835, + "velocityY": 1.5923896600185086, + "timestamp": 3.495802059155085 + }, + { + "x": 5.649517776674369, + "y": 1.65356922710374, + "heading": -1.3050160652934763e-17, + "angularVelocity": -2.501904186058887e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.59290492050578 + }, + { + "x": 5.283818675343, + "y": 1.808194819476813, + "heading": -3.0278972898890853e-17, + "angularVelocity": -1.7742847127577924e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.6900077818564747 + }, + { + "x": 4.918119574011632, + "y": 1.9628204118498858, + "heading": -2.1847399359503068e-17, + "angularVelocity": 8.68313602926161e-17, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.7871106432071695 + }, + { + "x": 4.5524204726802635, + "y": 2.1174460042229586, + "heading": -4.272133931768823e-18, + "angularVelocity": 1.8099637006841738e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.8842135045578643 + }, + { + "x": 4.186721371348895, + "y": 2.2720715965960316, + "heading": -1.2968663792925179e-17, + "angularVelocity": -8.955997526941852e-17, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.981316365908559 + }, + { + "x": 3.8210222700175263, + "y": 2.4266971889691047, + "heading": -1.2904793423104192e-17, + "angularVelocity": 6.57759914907778e-19, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 4.078419227259254 + }, + { + "x": 3.455323168686157, + "y": 2.5813227813421777, + "heading": -2.5257394960019227e-18, + "angularVelocity": 1.0688720994138185e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 4.1755220886099496 + }, + { + "x": 3.089624067354825, + "y": 2.7359483737152357, + "heading": 1.1768908098193852e-18, + "angularVelocity": 3.8131011324654024e-17, + "velocityX": -3.766100156519383, + "velocityY": 1.5923896600185086, + "timestamp": 4.272624949960645 + }, + { + "x": 2.748413818170618, + "y": 2.8802195428589217, + "heading": 8.82496336912766e-19, + "angularVelocity": -3.03177958726717e-18, + "velocityX": -3.5139051974163618, + "velocityY": 1.485756105812772, + "timestamp": 4.36972781131134 + }, + { + "x": 2.4559478666021306, + "y": 3.0038805550014, + "heading": 4.913896997517123e-19, + "angularVelocity": -4.02775604881706e-18, + "velocityX": -3.011918984675672, + "velocityY": 1.2735053367363358, + "timestamp": 4.466830672662035 + }, + { + "x": 2.212226231468147, + "y": 3.1069314021856766, + "heading": 3.397185296681489e-21, + "angularVelocity": -5.025521469385519e-18, + "velocityX": -2.509932578132407, + "velocityY": 1.0612544857159247, + "timestamp": 4.5639335340127305 + }, + { + "x": 2.017248919041599, + "y": 3.1893720817594184, + "heading": -5.814891269033512e-19, + "angularVelocity": -6.023368457591792e-18, + "velocityX": -2.007946106988254, + "velocityY": 0.849003607380843, + "timestamp": 4.661036395363426 + }, + { + "x": 1.8710159324589521, + "y": 3.251202592396459, + "heading": -2.9073580616535128e-19, + "angularVelocity": 2.994281699756195e-18, + "velocityX": -1.5059596035436553, + "velocityY": 0.6367527153884249, + "timestamp": 4.758139256714121 + }, + { + "x": 1.7735272736020857, + "y": 3.292422933301099, + "heading": -9.69089983169116e-20, + "angularVelocity": 1.9960977992890128e-18, + "velocityX": -1.0039730807187888, + "velocityY": 0.424501815201605, + "timestamp": 4.855242118064816 + }, + { + "x": 1.724782943725586, + "y": 3.313033103942871, + "heading": -8.658446510793699e-29, + "angularVelocity": 9.980035282243734e-19, + "velocityX": -0.5019865449737438, + "velocityY": 0.21225090955185055, + "timestamp": 4.952344979415511 + }, + { + "x": 1.724782943725586, + "y": 3.313033103942871, + "heading": -8.802662084224178e-29, + "angularVelocity": -3.186156327432381e-29, + "velocityX": -3.740246502381023e-28, + "velocityY": -1.8632531177340128e-29, + "timestamp": 5.049447840766207 + }, + { + "x": 1.7551582357609965, + "y": 3.3546585112037994, + "heading": -2.6221860421386343e-18, + "angularVelocity": -2.7366661467732622e-17, + "velocityX": 0.3170142472018904, + "velocityY": 0.43442700507741666, + "timestamp": 5.1452646449114985 + }, + { + "x": 1.8159088186549184, + "y": 3.437909324112868, + "heading": 2.022452950709123e-18, + "angularVelocity": 4.847415893658119e-17, + "velocityX": 0.634028482120975, + "velocityY": 0.868853993322837, + "timestamp": 5.24108144905679 + }, + { + "x": 1.9070346900535533, + "y": 3.562785539444501, + "heading": 9.965469770186681e-18, + "angularVelocity": 8.289795188242313e-17, + "velocityX": 0.9510426924744488, + "velocityY": 1.303280947904267, + "timestamp": 5.336898253202082 + }, + { + "x": 2.028535842895508, + "y": 3.729287147521972, + "heading": 1.7089484611640945e-17, + "angularVelocity": 7.435037001169609e-17, + "velocityX": 1.268056829131106, + "velocityY": 1.737707801493746, + "timestamp": 5.432715057347374 + }, + { + "x": 2.1500369957374623, + "y": 3.8957887555994435, + "heading": 1.4813846609443293e-17, + "angularVelocity": -2.3749884192136335e-17, + "velocityX": 1.2680568291311063, + "velocityY": 1.737707801493746, + "timestamp": 5.528531861492666 + }, + { + "x": 2.2411628671360972, + "y": 4.020664970931077, + "heading": 1.3332074215599954e-17, + "angularVelocity": -1.5464640122103033e-17, + "velocityX": 0.951042692474449, + "velocityY": 1.3032809479042669, + "timestamp": 5.6243486656379575 + }, + { + "x": 2.3019134500300193, + "y": 4.103915783840145, + "heading": 8.539272261435287e-18, + "angularVelocity": -5.002047393284737e-17, + "velocityX": 0.634028482120975, + "velocityY": 0.868853993322837, + "timestamp": 5.720165469783249 + }, + { + "x": 2.3322887420654297, + "y": 4.145541191101074, + "heading": 1.747304188459868e-28, + "angularVelocity": -8.912082110592716e-17, + "velocityX": 0.3170142472018904, + "velocityY": 0.43442700507741666, + "timestamp": 5.815982273928541 + }, + { + "x": 2.3322887420654297, + "y": 4.145541191101074, + "heading": 8.637491834911917e-29, + "angularVelocity": -2.0670507700607165e-29, + "velocityX": -3.920785157230939e-28, + "velocityY": 7.068097925324523e-28, + "timestamp": 5.911799078073833 + } + ], + "constraints": [ + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 0 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "3NB-Close": { + "waypoints": [ + { + "x": 0.756615161895752, + "y": 4.431221961975098, + "heading": -1.034985029325618, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 2.4176580905914307, + "y": 4.1052751541137695, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.489790439605713, + "y": 2.975529670715332, + "heading": 0.031239585322267525, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 4.547727584838867, + "y": 1.6888097524642944, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 7.887538433074951, + "y": 2.4107484817504883, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 4.42242956161499, + "y": 1.7425090074539185, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 2.4447901248931885, + "y": 3.3805336952209473, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.756615161895752, + "y": 4.431221961975098, + "heading": -1.034985029325618, + "angularVelocity": -1.2153572582704763e-25, + "velocityX": -8.099282150741186e-25, + "velocityY": 3.9386843631996907e-25, + "timestamp": 0 + }, + { + "x": 0.7825301409073798, + "y": 4.426111948589589, + "heading": -1.0184480247142116, + "angularVelocity": 0.25729744903186014, + "velocityX": 0.4032083287203581, + "velocityY": -0.079506140289953, + "timestamp": 0.06427193380125104 + }, + { + "x": 0.8343681989936829, + "y": 4.4158863120476, + "heading": -0.9855108542950051, + "angularVelocity": 0.5124658380601789, + "velocityX": 0.8065426854371999, + "velocityY": -0.1590995623938954, + "timestamp": 0.12854386760250208 + }, + { + "x": 0.912137967019861, + "y": 4.4005383558351046, + "heading": -0.9362991408222285, + "angularVelocity": 0.765679676372499, + "velocityX": 1.210011328843265, + "velocityY": -0.2387971748283722, + "timestamp": 0.19281580140375312 + }, + { + "x": 1.0158503745896044, + "y": 4.380061388038885, + "heading": -0.8709611243682479, + "angularVelocity": 1.0165870635855798, + "velocityX": 1.6136500247597099, + "velocityY": -0.31859890600991186, + "timestamp": 0.25708773520500416 + }, + { + "x": 1.145523011747702, + "y": 4.354449737528758, + "heading": -0.7897691851783357, + "angularVelocity": 1.2632565163043505, + "velocityX": 2.0175624022623317, + "velocityY": -0.39848887368670943, + "timestamp": 0.32135966900625523 + }, + { + "x": 1.3011913004178595, + "y": 4.32369771075132, + "heading": -0.6934250833799015, + "angularVelocity": 1.499007359827703, + "velocityX": 2.422025905608079, + "velocityY": -0.4784674267392117, + "timestamp": 0.3856316028075063 + }, + { + "x": 1.4829411037935518, + "y": 4.287786779701849, + "heading": -0.58418273386785, + "angularVelocity": 1.6996897876118522, + "velocityX": 2.827825345005485, + "velocityY": -0.558734254994066, + "timestamp": 0.44990353660875737 + }, + { + "x": 1.6909603916353093, + "y": 4.2465633189263965, + "heading": -0.47548948460818546, + "angularVelocity": 1.6911463967425988, + "velocityX": 3.236549385382092, + "velocityY": -0.6413913249121974, + "timestamp": 0.5141754704100084 + }, + { + "x": 1.8728106274219034, + "y": 4.211006125740984, + "heading": -0.36224313486482546, + "angularVelocity": 1.761987590003954, + "velocityX": 2.829387961920238, + "velocityY": -0.5532304861927155, + "timestamp": 0.5784474042112595 + }, + { + "x": 2.0285640030407976, + "y": 4.180677932000477, + "heading": -0.2611966487318228, + "angularVelocity": 1.572171244224113, + "velocityX": 2.4233497641526145, + "velocityY": -0.47187305479700664, + "timestamp": 0.6427193380125106 + }, + { + "x": 2.1583029244219367, + "y": 4.155482855052808, + "heading": -0.17529966695510088, + "angularVelocity": 1.3364617601571205, + "velocityX": 2.018593711250895, + "velocityY": -0.39200745111512536, + "timestamp": 0.7069912718137616 + }, + { + "x": 2.262065112690402, + "y": 4.135371482146998, + "heading": -0.10573833466683616, + "angularVelocity": 1.082297173496758, + "velocityX": 1.6144245572154519, + "velocityY": -0.3129106550302491, + "timestamp": 0.7712632056150127 + }, + { + "x": 2.339870608422664, + "y": 4.120312466180706, + "heading": -0.05311299136785256, + "angularVelocity": 0.8187919700956509, + "velocityX": 1.2105672123210285, + "velocityY": -0.2343015850878145, + "timestamp": 0.8355351394162638 + }, + { + "x": 2.3917318142501975, + "y": 4.110284912416539, + "heading": -0.017780765488657927, + "angularVelocity": 0.5497302444400217, + "velocityX": 0.8069028386154424, + "velocityY": -0.15601761408291742, + "timestamp": 0.8998070732175149 + }, + { + "x": 2.4176580905914307, + "y": 4.1052751541137695, + "heading": 8.497464730716836e-25, + "angularVelocity": 0.27664898871164556, + "velocityX": 0.4033841026381037, + "velocityY": -0.0779462824047118, + "timestamp": 0.9640790070187659 + }, + { + "x": 2.4176580905914307, + "y": 4.1052751541137695, + "heading": 4.243816717185121e-25, + "angularVelocity": -1.1560345853556316e-25, + "velocityX": 1.998362528445031e-20, + "velocityY": -7.254658147615057e-21, + "timestamp": 1.0283509408200169 + }, + { + "x": 2.4083001000392614, + "y": 4.07855200651975, + "heading": 5.678182580423427e-18, + "angularVelocity": 8.595410324058872e-17, + "velocityX": -0.14165760527850227, + "velocityY": -0.40452456887715366, + "timestamp": 1.094411570136539 + }, + { + "x": 2.391723371861385, + "y": 4.0244502470643715, + "heading": 1.2092714840241192e-17, + "angularVelocity": 9.710068345931569e-17, + "velocityX": -0.2509320354556493, + "velocityY": -0.818971299776089, + "timestamp": 1.160472199453061 + }, + { + "x": 2.370959981958984, + "y": 3.9423454455218585, + "heading": 2.0506186000436857e-17, + "angularVelocity": 1.2735983962200107e-16, + "velocityX": -0.31430808512156017, + "velocityY": -1.242870411499066, + "timestamp": 1.226532828769583 + }, + { + "x": 2.3504243304381753, + "y": 3.831927280299648, + "heading": 2.7876588197896166e-17, + "angularVelocity": 1.1157026914706097e-16, + "velocityX": -0.3108606704670367, + "velocityY": -1.6714670502630964, + "timestamp": 1.292593458086105 + }, + { + "x": 2.3364844874655226, + "y": 3.6939737986961876, + "heading": 3.612951501320603e-17, + "angularVelocity": 1.2492958211265208e-16, + "velocityX": -0.21101589731853923, + "velocityY": -2.088285912967496, + "timestamp": 1.358654087402627 + }, + { + "x": 2.3373423735310555, + "y": 3.531880609628321, + "heading": 3.956479825579656e-17, + "angularVelocity": 5.2001975734663045e-17, + "velocityX": 0.012986344126731107, + "velocityY": -2.4537033743837897, + "timestamp": 1.424714716719149 + }, + { + "x": 2.3607492318969396, + "y": 3.3526631075737106, + "heading": 4.165260110772871e-17, + "angularVelocity": 3.1604343976295837e-17, + "velocityX": 0.35432387805046023, + "velocityY": -2.7129245347620055, + "timestamp": 1.490775346035671 + }, + { + "x": 2.4111324989473553, + "y": 3.1648449262631324, + "heading": 4.251470247939207e-17, + "angularVelocity": 1.3050153801581261e-17, + "velocityX": 0.7626822143793112, + "velocityY": -2.8431182574823626, + "timestamp": 1.556835975352193 + }, + { + "x": 2.489790439605713, + "y": 2.975529670715332, + "heading": 4.331854475647563e-17, + "angularVelocity": 1.2168250381293626e-17, + "velocityX": 1.1906931779513854, + "velocityY": -2.865780382453187, + "timestamp": 1.622896604668715 + }, + { + "x": 2.566881258362406, + "y": 2.832040035804707, + "heading": 4.346891950915645e-17, + "angularVelocity": 2.9604733656461275e-18, + "velocityX": 1.5177103424948193, + "velocityY": -2.8249239852022456, + "timestamp": 1.6736907625512862 + }, + { + "x": 2.6605214587645087, + "y": 2.691067428150466, + "heading": 4.410412763377102e-17, + "angularVelocity": 1.2505535086604585e-17, + "velocityX": 1.8435230409486294, + "velocityY": -2.775370505800075, + "timestamp": 1.7244849204338575 + }, + { + "x": 2.770613075448226, + "y": 2.5531880922477597, + "heading": 4.417340863018206e-17, + "angularVelocity": 1.3639559691841993e-18, + "velocityX": 2.167407065557281, + "velocityY": -2.7144723261573476, + "timestamp": 1.7752790783164287 + }, + { + "x": 2.8969902149108995, + "y": 2.419181627819772, + "heading": 4.438492564713358e-17, + "angularVelocity": 4.164199663614473e-18, + "velocityX": 2.488025094437815, + "velocityY": -2.63822593019046, + "timestamp": 1.826073236199 + }, + { + "x": 3.039350168869116, + "y": 2.290151805534909, + "heading": 4.425906230668265e-17, + "angularVelocity": -2.477909823337866e-18, + "velocityX": 2.802683613484297, + "velocityY": -2.5402492661294147, + "timestamp": 1.8768673940815712 + }, + { + "x": 3.197082247467654, + "y": 2.1677488242794944, + "heading": 4.264246918702522e-17, + "angularVelocity": -3.182635938410446e-17, + "velocityX": 3.1053192960338345, + "velocityY": -2.409784635831394, + "timestamp": 1.9276615519641425 + }, + { + "x": 3.368783708739533, + "y": 2.054569369929602, + "heading": 4.142429145389646e-17, + "angularVelocity": -2.398263475091366e-17, + "velocityX": 3.3803387718097024, + "velocityY": -2.2281982627125525, + "timestamp": 1.9784557098467137 + }, + { + "x": 3.5508140800737693, + "y": 1.9545630137345602, + "heading": 3.816577991230304e-17, + "angularVelocity": -6.41513055323665e-17, + "velocityX": 3.5836871585717445, + "velocityY": -1.9688554818891133, + "timestamp": 2.029249867729285 + }, + { + "x": 3.7403070217648606, + "y": 1.8695408729970555, + "heading": 4.394413528240289e-17, + "angularVelocity": 1.137602356449689e-16, + "velocityX": 3.7306050457450586, + "velocityY": -1.673856685134212, + "timestamp": 2.080044025611856 + }, + { + "x": 3.936031548275179, + "y": 1.8000552607614357, + "heading": 3.256197745041857e-17, + "angularVelocity": -2.240839952163188e-16, + "velocityX": 3.853288147089766, + "velocityY": -1.3679843338728146, + "timestamp": 2.1308381834944274 + }, + { + "x": 4.13671621691862, + "y": 1.7465575488326603, + "heading": 2.5319682073900963e-17, + "angularVelocity": -1.4258126679157706e-16, + "velocityX": 3.9509399704468953, + "velocityY": -1.0532256889159286, + "timestamp": 2.1816323413769987 + }, + { + "x": 4.341057366167047, + "y": 1.7093952475260255, + "heading": -7.734065432963739e-18, + "angularVelocity": -6.507391575084453e-16, + "velocityX": 4.022926213696339, + "velocityY": -0.7316255029278887, + "timestamp": 2.23242649925957 + }, + { + "x": 4.547727584838867, + "y": 1.6888097524642944, + "heading": -3.3452466298014664e-29, + "angularVelocity": 1.5226289312285738e-16, + "velocityX": 4.068779310203581, + "velocityY": -0.40527288806168965, + "timestamp": 2.283220657142141 + }, + { + "x": 4.872720718401865, + "y": 1.6975768387772854, + "heading": -1.982534191916707e-17, + "angularVelocity": -2.493425929833114e-16, + "velocityX": 4.0874266358219495, + "velocityY": 0.11026332070896885, + "timestamp": 2.3627311077309736 + }, + { + "x": 5.1940235017101015, + "y": 1.74719458876398, + "heading": 1.0615577094344061e-17, + "angularVelocity": 3.828543139690045e-16, + "velocityX": 4.041013236986538, + "velocityY": 0.6240406087406034, + "timestamp": 2.442241558319806 + }, + { + "x": 5.509692276544731, + "y": 1.8249803490868903, + "heading": -3.172576507949871e-18, + "angularVelocity": -1.734130985315765e-16, + "velocityX": 3.970154520530006, + "velocityY": 0.9783086342342504, + "timestamp": 2.5217520089086385 + }, + { + "x": 5.8253663131661115, + "y": 1.902744753011146, + "heading": 2.832120477993219e-17, + "angularVelocity": 3.960961239018336e-16, + "velocityX": 3.970220697827599, + "velocityY": 0.9780400355972578, + "timestamp": 2.601262459497471 + }, + { + "x": 6.141040349651611, + "y": 1.980509157486996, + "heading": 4.3091388724994025e-17, + "angularVelocity": 1.8576405787818657e-16, + "velocityX": 3.9702206961186195, + "velocityY": 0.9780400425346409, + "timestamp": 2.6807729100863034 + }, + { + "x": 6.456714386011232, + "y": 2.0582735624738286, + "heading": 3.5113592050441506e-17, + "angularVelocity": -1.0033645408255158e-16, + "velocityX": 3.9702206945354614, + "velocityY": 0.9780400489612482, + "timestamp": 2.760283360675136 + }, + { + "x": 6.772388422403809, + "y": 2.1360379673249357, + "heading": 3.856790983929373e-17, + "angularVelocity": 4.3444827230051585e-17, + "velocityX": 3.9702206949499432, + "velocityY": 0.9780400472542335, + "timestamp": 2.8397938112639682 + }, + { + "x": 7.051175893570039, + "y": 2.20471558817087, + "heading": 2.5601706470255025e-17, + "angularVelocity": -1.630754608117844e-16, + "velocityX": 3.5062997266599183, + "velocityY": 0.8637559004800854, + "timestamp": 2.9193042618528007 + }, + { + "x": 7.290136606671851, + "y": 2.263582126121373, + "heading": 1.755827518657081e-17, + "angularVelocity": -1.0116193813703912e-16, + "velocityX": 3.0054000616539693, + "velocityY": 0.7403622733182037, + "timestamp": 2.998814712441633 + }, + { + "x": 7.489270543021993, + "y": 2.312637576572798, + "heading": 1.0387054455853603e-17, + "angularVelocity": -9.019217823090925e-17, + "velocityX": 2.5045001616191533, + "velocityY": 0.6169685882564364, + "timestamp": 3.0783251630304655 + }, + { + "x": 7.648577696391329, + "y": 2.35188193799059, + "heading": 7.528064188393706e-18, + "angularVelocity": -3.595741498540887e-17, + "velocityX": 2.003600183240717, + "velocityY": 0.49357488389462933, + "timestamp": 3.157835613619298 + }, + { + "x": 7.768058063665282, + "y": 2.381315209607472, + "heading": 4.376696429652505e-18, + "angularVelocity": -3.9634635892779204e-17, + "velocityX": 1.502700165690348, + "velocityY": 0.37018116988278865, + "timestamp": 3.2373460642081304 + }, + { + "x": 7.847711642975101, + "y": 2.400937390963075, + "heading": 3.6531453006199625e-20, + "angularVelocity": -5.4586094589255846e-17, + "velocityX": 1.0018001246367811, + "velocityY": 0.24678745008092345, + "timestamp": 3.316856514796963 + }, + { + "x": 7.887538433074951, + "y": 2.4107484817504883, + "heading": -1.0863094495902712e-28, + "angularVelocity": -4.594547368911346e-19, + "velocityX": 0.5009000679143996, + "velocityY": 0.12339372641904009, + "timestamp": 3.3963669653857953 + }, + { + "x": 7.887538433074951, + "y": 2.4107484817504883, + "heading": -7.163298591279974e-29, + "angularVelocity": 1.88053587914437e-30, + "velocityX": -1.0646456405257399e-17, + "velocityY": 2.0398963035439954e-17, + "timestamp": 3.4758774159746277 + }, + { + "x": 7.850990393264518, + "y": 2.398572533679078, + "heading": -1.177521215275177e-18, + "angularVelocity": -1.5281620709395144e-17, + "velocityX": -0.47431269588407104, + "velocityY": -0.1580168672423615, + "timestamp": 3.552932152496727 + }, + { + "x": 7.77789431437577, + "y": 2.3742206377658586, + "heading": -3.5352413601672825e-18, + "angularVelocity": -3.059799124525414e-17, + "velocityX": -0.9486253822668896, + "velocityY": -0.3160337315050108, + "timestamp": 3.6299868890188263 + }, + { + "x": 7.668250197384856, + "y": 2.3376927943169763, + "heading": -7.072611394418981e-18, + "angularVelocity": -4.590723742808682e-17, + "velocityX": -1.4229380559814202, + "velocityY": -0.47405059179455206, + "timestamp": 3.7070416255409255 + }, + { + "x": 7.522058043658382, + "y": 2.2889890037610603, + "heading": -2.3232935434204652e-17, + "angularVelocity": -2.0972525521788622e-16, + "velocityX": -1.8972507119604403, + "velocityY": -0.6320674465214426, + "timestamp": 3.784096362063025 + }, + { + "x": 7.339317855246242, + "y": 2.2281092667411033, + "heading": -3.444177035375071e-17, + "angularVelocity": -1.4546587686435312e-16, + "velocityX": -2.3715633413363952, + "velocityY": -0.7900842929037087, + "timestamp": 3.861151098585124 + }, + { + "x": 7.1200296355648796, + "y": 2.155053584328892, + "heading": -4.681767867288413e-17, + "angularVelocity": -1.606119068108914e-16, + "velocityX": -2.8458759263744398, + "velocityY": -0.9481011253765523, + "timestamp": 3.9382058351072233 + }, + { + "x": 6.864193391447034, + "y": 2.069821958668493, + "heading": -6.091862473343017e-17, + "angularVelocity": -1.8299908088539646e-16, + "velocityX": -3.320188422738658, + "velocityY": -1.1061179300241564, + "timestamp": 4.015260571629322 + }, + { + "x": 6.571809143389688, + "y": 1.9724143961960199, + "heading": -6.906000748724748e-17, + "angularVelocity": -1.0565713593171376e-16, + "velocityX": -3.794500653097392, + "velocityY": -1.2641346511452527, + "timestamp": 4.092315308151421 + }, + { + "x": 6.272890863117038, + "y": 1.8728299317270023, + "heading": -7.894686935840572e-17, + "angularVelocity": -1.2830959286049874e-16, + "velocityX": -3.8792979350065835, + "velocityY": -1.2923860227649977, + "timestamp": 4.16937004467352 + }, + { + "x": 5.973997771041301, + "y": 1.7731698928043986, + "heading": -9.67999949293296e-17, + "angularVelocity": -2.316940706917987e-16, + "velocityX": -3.87897104793804, + "velocityY": -1.2933668119685562, + "timestamp": 4.246424781195619 + }, + { + "x": 5.669879935237952, + "y": 1.690819752200994, + "heading": -1.1050901644346994e-16, + "angularVelocity": -1.779127686741746e-16, + "velocityX": -3.9467766620175, + "velocityY": -1.0687226291385734, + "timestamp": 4.323479517717717 + }, + { + "x": 5.357985582093433, + "y": 1.6461978364052787, + "heading": -1.2113052448910788e-16, + "angularVelocity": -1.3784367483440315e-16, + "velocityX": -4.0476986519195854, + "velocityY": -0.5790937430940615, + "timestamp": 4.400534254239816 + }, + { + "x": 5.042976936505313, + "y": 1.639972541371015, + "heading": -5.4820283875488555e-17, + "angularVelocity": 8.605602148108709e-16, + "velocityX": -4.0881152776088845, + "velocityY": -0.0807905563660157, + "timestamp": 4.477588990761915 + }, + { + "x": 4.729563151516996, + "y": 1.6722370314267299, + "heading": 4.449704756389741e-18, + "angularVelocity": 7.691933203211217e-16, + "velocityX": -4.06741751557905, + "velocityY": 0.41872169722451913, + "timestamp": 4.554643727284014 + }, + { + "x": 4.42242956161499, + "y": 1.7425090074539185, + "heading": -1.5670272990102803e-28, + "angularVelocity": -5.774732297268101e-17, + "velocityX": -3.9859144780012232, + "velocityY": 0.9119747753214509, + "timestamp": 4.631698463806113 + }, + { + "x": 4.128704472216037, + "y": 1.8484554786573326, + "heading": 1.5665974518926906e-17, + "angularVelocity": 2.0514694884473714e-16, + "velocityX": -3.846349029561837, + "velocityY": 1.3873758878840463, + "timestamp": 4.708063111720662 + }, + { + "x": 3.8499499444697896, + "y": 1.9891501856691707, + "heading": -4.951052254344938e-18, + "angularVelocity": -2.699812981022409e-16, + "velocityX": -3.650308557150352, + "velocityY": 1.842406281624709, + "timestamp": 4.784427759635212 + }, + { + "x": 3.590258853943372, + "y": 2.1625273105920915, + "heading": -4.443248682637518e-18, + "angularVelocity": 6.64972059401854e-18, + "velocityX": -3.4006716146588456, + "velocityY": 2.2703846564828614, + "timestamp": 4.860792407549761 + }, + { + "x": 3.35344412752997, + "y": 2.3660411333508096, + "heading": -1.2020557304661416e-17, + "angularVelocity": -9.92253461355458e-17, + "velocityX": -3.1011041480660335, + "velocityY": 2.665026662421532, + "timestamp": 4.9371570554643105 + }, + { + "x": 3.149014297898049, + "y": 2.58911892320566, + "heading": -3.113544050291928e-17, + "angularVelocity": -2.5031060068627764e-16, + "velocityX": -2.6770218316292542, + "velocityY": 2.9212180759932416, + "timestamp": 5.01352170337886 + }, + { + "x": 2.9718701800847187, + "y": 2.785985113880848, + "heading": -3.7052285700782595e-17, + "angularVelocity": -7.748146793988773e-17, + "velocityX": -2.319713671849138, + "velocityY": 2.5779754906417227, + "timestamp": 5.089886351293409 + }, + { + "x": 2.8207247945797818, + "y": 2.955362663178613, + "heading": -3.796704916740075e-17, + "angularVelocity": -1.1978890626359567e-17, + "velocityX": -1.9792585919345465, + "velocityY": 2.2180099551732835, + "timestamp": 5.166250999207959 + }, + { + "x": 2.6951291405768862, + "y": 3.096833739929104, + "heading": -3.0938817386489246e-17, + "angularVelocity": 9.203514802265678e-17, + "velocityX": -1.6446832065987498, + "velocityY": 1.8525728935305812, + "timestamp": 5.242615647122508 + }, + { + "x": 2.594855061076495, + "y": 3.2101911526058116, + "heading": -1.8551698396752348e-17, + "angularVelocity": 1.6221011125667273e-16, + "velocityX": -1.313095551918033, + "velocityY": 1.4844226454571197, + "timestamp": 5.318980295037058 + }, + { + "x": 2.519764506253801, + "y": 3.2953111702184255, + "heading": -8.201545305515358e-18, + "angularVelocity": 1.3553591742048043e-16, + "velocityX": -0.9833156686155428, + "velocityY": 1.1146521320686176, + "timestamp": 5.395344942951607 + }, + { + "x": 2.469764964962585, + "y": 3.352111556180341, + "heading": -9.057115232772252e-20, + "angularVelocity": 1.0621372327118296e-16, + "velocityX": -0.654747224751943, + "velocityY": 0.7438047252634438, + "timestamp": 5.471709590866157 + }, + { + "x": 2.4447901248931885, + "y": 3.3805336952209473, + "heading": 3.7188233361746653e-28, + "angularVelocity": 1.186034116335578e-18, + "velocityX": -0.32704714486917663, + "velocityY": 0.37218974770122537, + "timestamp": 5.548074238780706 + }, + { + "x": 2.4447901248931885, + "y": 3.3805336952209473, + "heading": 1.8360534693167372e-28, + "angularVelocity": -6.116044573310473e-29, + "velocityX": -9.224611572050135e-18, + "velocityY": -5.199995140976138e-18, + "timestamp": 5.6244388866952555 + } + ], + "constraints": [ + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4NB-Sweep": { + "waypoints": [ + { + "x": 0.7265444397926331, + "y": 4.389017105102539, + "heading": -1.051650239140039, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.8526127338409424, + "y": 3.84, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 1.90418541431427, + "y": 4.952258110046387, + "heading": -0.029402891635568452, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 7 + }, + { + "x": 2.5253705978393555, + "y": 5.429551124572754, + "heading": -0.023251268502449494, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 1.645802617073059, + "y": 6.586877346038818, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.733689308166504, + "y": 6.957221508026123, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7265444397926323, + "y": 4.389017105102539, + "heading": -1.05165023914004, + "angularVelocity": -2.4052301236587744e-16, + "velocityX": -2.752276616089782e-15, + "velocityY": 5.134530261429221e-16, + "timestamp": 0 + }, + { + "x": 0.7559653796741465, + "y": 4.38140433106999, + "heading": -1.0337573727433125, + "angularVelocity": 0.2597755888304552, + "velocityX": 0.4271446403391007, + "velocityY": -0.11052521228797353, + "timestamp": 0.06887816702594797 + }, + { + "x": 0.8148295651894585, + "y": 4.366167962208403, + "heading": -0.9984651090693958, + "angularVelocity": 0.5123868011850714, + "velocityX": 0.8546131242594956, + "velocityY": -0.22120752510512612, + "timestamp": 0.13775633405189594 + }, + { + "x": 0.9031640621745638, + "y": 4.34329522415188, + "heading": -0.9463836724391982, + "angularVelocity": 0.7561385396707215, + "velocityX": 1.2824745605066483, + "velocityY": -0.3320753011314312, + "timestamp": 0.20663450107784392 + }, + { + "x": 1.0210050595189104, + "y": 4.312770957025727, + "heading": -0.8783834326740236, + "angularVelocity": 0.987253910801006, + "velocityX": 1.7108614011164693, + "velocityY": -0.4431631741108122, + "timestamp": 0.2755126681037919 + }, + { + "x": 1.1684052822191313, + "y": 4.274574303329794, + "heading": -0.7959106115927722, + "angularVelocity": 1.1973724714564178, + "velocityX": 2.1400137237202137, + "velocityY": -0.5545538643841008, + "timestamp": 0.34439083512973984 + }, + { + "x": 1.3454446954006436, + "y": 4.228667204890838, + "heading": -0.7017429838335593, + "angularVelocity": 1.367162220268308, + "velocityX": 2.5703270110950878, + "velocityY": -0.6664970980087429, + "timestamp": 0.4132690021556878 + }, + { + "x": 1.552227621121227, + "y": 4.174960199018195, + "heading": -0.6022461552568997, + "angularVelocity": 1.4445336290550153, + "velocityX": 3.002154886652024, + "velocityY": -0.779739185748209, + "timestamp": 0.48214716918163575 + }, + { + "x": 1.7883744114232694, + "y": 4.113221891555757, + "heading": -0.5202856320014997, + "angularVelocity": 1.1899347325041563, + "velocityX": 3.428470885601271, + "velocityY": -0.8963407437828599, + "timestamp": 0.5510253362075838 + }, + { + "x": 2.02506710191313, + "y": 4.0518739432063136, + "heading": -0.4352693076854395, + "angularVelocity": 1.2343000400117028, + "velocityX": 3.436396476704921, + "velocityY": -0.8906733584581512, + "timestamp": 0.6199035032335317 + }, + { + "x": 2.232210219377279, + "y": 3.9985845821566475, + "heading": -0.3384412768078663, + "angularVelocity": 1.4057869867689714, + "velocityX": 3.0073842903820833, + "velocityY": -0.773675655880783, + "timestamp": 0.6887816702594797 + }, + { + "x": 2.409618392670172, + "y": 3.9530993682814772, + "heading": -0.2475826023369981, + "angularVelocity": 1.319121550325896, + "velocityX": 2.5756808137193876, + "velocityY": -0.6603720139363624, + "timestamp": 0.7576598372854276 + }, + { + "x": 2.5573654312197593, + "y": 3.9153053743884563, + "heading": -0.16800100720308944, + "angularVelocity": 1.1553965294101876, + "velocityX": 2.1450489310194327, + "velocityY": -0.5487078928622399, + "timestamp": 0.8265380043113756 + }, + { + "x": 2.67550632370007, + "y": 3.885136654824466, + "heading": -0.10223202695817188, + "angularVelocity": 0.95485961785463, + "velocityX": 1.7152153952616722, + "velocityY": -0.4380011964114144, + "timestamp": 0.8954161713373235 + }, + { + "x": 2.764078093861892, + "y": 3.8625489746450645, + "heading": -0.05172765313938082, + "angularVelocity": 0.7332421288123194, + "velocityX": 1.2859193847080135, + "velocityY": -0.3279367200769452, + "timestamp": 0.9642943383632715 + }, + { + "x": 2.8231068724147113, + "y": 3.847510964463322, + "heading": -0.017424843362383667, + "angularVelocity": 0.49802152493516094, + "velocityX": 0.857002749950966, + "velocityY": -0.21832767669437975, + "timestamp": 1.0331725053892196 + }, + { + "x": 2.8526127338409437, + "y": 3.84, + "heading": 1.0917587940036658e-15, + "angularVelocity": 0.2529806485097509, + "velocityX": 0.4283775643320707, + "velocityY": -0.10904710139125373, + "timestamp": 1.1020506724151675 + }, + { + "x": 2.852612733840943, + "y": 3.84, + "heading": 5.446844655867544e-16, + "angularVelocity": -1.5954070335877785e-17, + "velocityX": 2.949719479275081e-15, + "velocityY": -7.8346554708601e-16, + "timestamp": 1.1709288394411155 + }, + { + "x": 2.8147339330756247, + "y": 3.8590487798371766, + "heading": -0.0009023184418920604, + "angularVelocity": -0.011161938023000674, + "velocityX": -0.4685716338028561, + "velocityY": 0.23563887213740328, + "timestamp": 1.2517677081065088 + }, + { + "x": 2.739502624002538, + "y": 3.8981587006634606, + "heading": -0.002706656651554298, + "angularVelocity": -0.022320181361403398, + "velocityX": -0.9306328788999082, + "velocityY": 0.4838009421948592, + "timestamp": 1.3326065767719022 + }, + { + "x": 2.627837725833638, + "y": 3.95895392824561, + "heading": -0.00541087612487252, + "angularVelocity": -0.033451970790332144, + "velocityX": -1.3813268296851184, + "velocityY": 0.7520544087992126, + "timestamp": 1.4134454454372956 + }, + { + "x": 2.481698476909063, + "y": 4.044430690887332, + "heading": -0.009004988998051864, + "angularVelocity": -0.04446020747836087, + "velocityX": -1.8077844400503824, + "velocityY": 1.0573720792101367, + "timestamp": 1.494284314102689 + }, + { + "x": 2.3073008390105594, + "y": 4.161516417890298, + "heading": -0.013430705174834927, + "angularVelocity": -0.054747378950859515, + "velocityX": -2.1573488196670145, + "velocityY": 1.4483840377282375, + "timestamp": 1.5751231827680823 + }, + { + "x": 2.142960643566143, + "y": 4.319791387587546, + "heading": -0.01814503786647799, + "angularVelocity": -0.05831764805067327, + "velocityX": -2.0329353707886444, + "velocityY": 1.9579067880375274, + "timestamp": 1.6559620514334756 + }, + { + "x": 2.0202818027696092, + "y": 4.4859414730946, + "heading": -0.02220768336419149, + "angularVelocity": -0.050256090477088365, + "velocityX": -1.5175724601530889, + "velocityY": 2.0553242301643135, + "timestamp": 1.736800920098869 + }, + { + "x": 1.939891618406941, + "y": 4.649035227213643, + "heading": -0.02546081479381014, + "angularVelocity": -0.04024216918576554, + "velocityX": -0.9944496464370033, + "velocityY": 2.017516533959881, + "timestamp": 1.8176397887642624 + }, + { + "x": 1.9013190323048412, + "y": 4.805132132072189, + "heading": -0.027864639292684273, + "angularVelocity": -0.029735998765940026, + "velocityX": -0.4771539574824876, + "velocityY": 1.9309635010437805, + "timestamp": 1.8984786574296557 + }, + { + "x": 1.9041854143142702, + "y": 4.952258110046387, + "heading": -0.02940289163556845, + "angularVelocity": -0.019028622842945542, + "velocityX": 0.03545796789034449, + "velocityY": 1.8199905615104273, + "timestamp": 1.979317526095049 + }, + { + "x": 1.9596283985749436, + "y": 5.1050826044619315, + "heading": -0.030026698581035755, + "angularVelocity": -0.006851792079561006, + "velocityX": 0.6089765482491194, + "velocityY": 1.678598912705159, + "timestamp": 2.07036041505371 + }, + { + "x": 2.0658010117719163, + "y": 5.240057978189405, + "heading": -0.029521094444683208, + "angularVelocity": 0.005553472018908819, + "velocityX": 1.1661823829555888, + "velocityY": 1.4825471299440058, + "timestamp": 2.1614033040123712 + }, + { + "x": 2.2166986800614077, + "y": 5.345171108591057, + "heading": -0.027897424478062212, + "angularVelocity": 0.017834121755058083, + "velocityX": 1.657434973949558, + "velocityY": 1.1545451995639058, + "timestamp": 2.2524461929710324 + }, + { + "x": 2.367639272503086, + "y": 5.396505549692408, + "heading": -0.025755785506199724, + "angularVelocity": 0.02352340744409937, + "velocityX": 1.6579064457215766, + "velocityY": 0.5638489912667554, + "timestamp": 2.3434890819296936 + }, + { + "x": 2.47239628749368, + "y": 5.420285820571503, + "heading": -0.024116176964614645, + "angularVelocity": 0.01800918842030109, + "velocityX": 1.1506336869226512, + "velocityY": 0.26119855324333596, + "timestamp": 2.434531970888355 + }, + { + "x": 2.525370597839355, + "y": 5.429551124572755, + "heading": -0.023251268502449497, + "angularVelocity": 0.009500011171194953, + "velocityX": 0.581861043202709, + "velocityY": 0.10176856322583427, + "timestamp": 2.525574859847016 + }, + { + "x": 2.525370597839355, + "y": 5.429551124572754, + "heading": -0.023251268502449497, + "angularVelocity": 6.144566740274697e-19, + "velocityX": -1.3368031084954793e-16, + "velocityY": -2.9798986610214504e-16, + "timestamp": 2.616617748805677 + }, + { + "x": 2.486815565414766, + "y": 5.451901360603296, + "heading": -0.022679468371633073, + "angularVelocity": 0.00689933086837991, + "velocityX": -0.4652043799964615, + "velocityY": 0.2696775762204275, + "timestamp": 2.699495370088783 + }, + { + "x": 2.4102574903491494, + "y": 5.497528005659964, + "heading": -0.021525109258417094, + "angularVelocity": 0.013928477861023088, + "velocityX": -0.9237484604450404, + "velocityY": 0.550530340401665, + "timestamp": 2.7823729913718886 + }, + { + "x": 2.296665771182315, + "y": 5.567944363601739, + "heading": -0.019771445415369154, + "angularVelocity": 0.0211596787636741, + "velocityX": -1.3705957942351075, + "velocityY": 0.8496426037764246, + "timestamp": 2.8652506126549944 + }, + { + "x": 2.148145849920551, + "y": 5.666038112101672, + "heading": -0.01738940046354127, + "angularVelocity": 0.028741714771119673, + "velocityX": -1.7920388997945242, + "velocityY": 1.1835975379270423, + "timestamp": 2.9481282339381 + }, + { + "x": 1.9718789441987798, + "y": 5.799004690569831, + "heading": -0.014325680038043673, + "angularVelocity": 0.036966799698945195, + "velocityX": -2.126833552807363, + "velocityY": 1.6043725243265818, + "timestamp": 3.031005855221206 + }, + { + "x": 1.8191804764247788, + "y": 5.969793912810861, + "heading": -0.010780972710273075, + "angularVelocity": 0.04277037966210476, + "velocityX": -1.8424571725145245, + "velocityY": 2.0607399150323507, + "timestamp": 3.1138834765043115 + }, + { + "x": 1.7110457921611644, + "y": 6.140250891131977, + "heading": -0.007470809475740908, + "angularVelocity": 0.039940374533974615, + "velocityX": -1.304751301853027, + "velocityY": 2.056731065420472, + "timestamp": 3.1967610977874172 + }, + { + "x": 1.646568244424728, + "y": 6.301758541946275, + "heading": -0.004541955985918456, + "angularVelocity": 0.03533949749616519, + "velocityX": -0.7779850186117685, + "velocityY": 1.9487486285664002, + "timestamp": 3.279638719070523 + }, + { + "x": 1.6249822101406382, + "y": 6.451168602079967, + "heading": -0.0020436909380878126, + "angularVelocity": 0.030144024516542172, + "velocityX": -0.2604567306577584, + "velocityY": 1.802779300618623, + "timestamp": 3.3625163403536287 + }, + { + "x": 1.6458026170730593, + "y": 6.586877346038818, + "heading": -8.583319940280204e-19, + "angularVelocity": 0.024659140868759512, + "velocityX": 0.2512186837662745, + "velocityY": 1.6374594475301991, + "timestamp": 3.4453939616367344 + }, + { + "x": 1.7175854091318359, + "y": 6.716530775561853, + "heading": 0.001662521310123723, + "angularVelocity": 0.0185413273788438, + "velocityX": 0.8005601128987638, + "velocityY": 1.4459644324183467, + "timestamp": 3.535059673058275 + }, + { + "x": 1.8379608407999335, + "y": 6.827214719472097, + "heading": 0.002751500963366046, + "angularVelocity": 0.012144883880112818, + "velocityX": 1.342491235051747, + "velocityY": 1.2344065770012334, + "timestamp": 3.6247253844798153 + }, + { + "x": 2.005463287958749, + "y": 6.915535164819774, + "heading": 0.003222193903074653, + "angularVelocity": 0.005249419563468997, + "velocityX": 1.8680769326788147, + "velocityY": 0.9849968728008089, + "timestamp": 3.7143910959013557 + }, + { + "x": 2.215062055192105, + "y": 6.97305024616571, + "heading": 0.002979401978971678, + "angularVelocity": -0.0027077454720852664, + "velocityX": 2.3375576227570494, + "velocityY": 0.6414389674057639, + "timestamp": 3.804056807322896 + }, + { + "x": 2.4217154464231823, + "y": 6.978484776468596, + "heading": 0.0019991193069041372, + "angularVelocity": -0.010932636974896442, + "velocityX": 2.304709213308401, + "velocityY": 0.06060879032495919, + "timestamp": 3.8937225187444366 + }, + { + "x": 2.5778319799768425, + "y": 6.970991314979555, + "heading": 0.0010539012563375312, + "angularVelocity": -0.01054157755045176, + "velocityX": 1.741095130776553, + "velocityY": -0.08357109278720787, + "timestamp": 3.983388230165977 + }, + { + "x": 2.6817937055315064, + "y": 6.962507949629537, + "heading": 0.0003631357733237097, + "angularVelocity": -0.007703786342210105, + "velocityX": 1.1594368003830842, + "velocityY": -0.09461103040977602, + "timestamp": 4.073053941587518 + }, + { + "x": 2.733689308166504, + "y": 6.957221508026124, + "heading": 2.397688145356894e-19, + "angularVelocity": -0.004049884482782016, + "velocityX": 0.5787675334557183, + "velocityY": -0.0589572258960861, + "timestamp": 4.162719653009058 + }, + { + "x": 2.733689308166504, + "y": 6.957221508026123, + "heading": 1.2133947797260325e-19, + "angularVelocity": 3.240031726812772e-20, + "velocityX": 1.3625265205891428e-17, + "velocityY": -9.676993787863113e-17, + "timestamp": 4.252385364430599 + } + ], + "constraints": [ + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "3NM-Close": { + "waypoints": [ + { + "x": 1.3615734577178955, + "y": 5.537254333496094, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.311396360397339, + "y": 5.537254333496094, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 4.288896560668945, + "y": 4.896035671234131, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "x": 5.409705638885498, + "y": 4.271584510803223, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 7.773779392242432, + "y": 4.118379592895508, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 13 + }, + { + "x": 5.409705638885498, + "y": 4.271584510803223, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 10 + }, + { + "x": 4.288896560668945, + "y": 4.896035671234131, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 13 + }, + { + "x": 2.311396360397339, + "y": 5.537254333496094, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.3615734577178955, + "y": 5.537254333496094, + "heading": 2.214441679929809e-48, + "angularVelocity": 2.3240077409867487e-47, + "velocityX": 0, + "velocityY": -1.721342681226222e-38, + "timestamp": 0 + }, + { + "x": 1.4090646063019332, + "y": 5.537254333496094, + "heading": 1.9456046497276407e-25, + "angularVelocity": 2.1151263730329146e-24, + "velocityX": 0.5162908141167563, + "velocityY": 1.0163081598312787e-23, + "timestamp": 0.09198526738323455 + }, + { + "x": 1.5040469015533051, + "y": 5.537254333496094, + "heading": 7.769111318759041e-25, + "angularVelocity": 6.330912367487192e-24, + "velocityX": 1.0325816073964444, + "velocityY": 2.0326162787461842e-23, + "timestamp": 0.1839705347664691 + }, + { + "x": 1.6465203396386052, + "y": 5.537254333496094, + "heading": 1.8917582624880275e-24, + "angularVelocity": 1.2119844430819319e-23, + "velocityX": 1.5488723590019993, + "velocityY": 3.048924315511678e-23, + "timestamp": 0.27595580214970367 + }, + { + "x": 1.8364849090576172, + "y": 5.537254333496094, + "heading": 3.898859380784999e-24, + "angularVelocity": 2.1819810665276264e-23, + "velocityX": 2.0651629855851796, + "velocityY": 4.065232106205135e-23, + "timestamp": 0.3679410695329382 + }, + { + "x": 2.026449478476629, + "y": 5.537254333496094, + "heading": 2.587084214379721e-24, + "angularVelocity": -1.4260709390994972e-23, + "velocityX": 2.06516298558518, + "velocityY": 4.0652321054688834e-23, + "timestamp": 0.45992633691617274 + }, + { + "x": 2.1689229165619293, + "y": 5.537254333496094, + "heading": 1.4784843245036944e-24, + "angularVelocity": -1.2051928764389099e-23, + "velocityX": 1.5488723590019995, + "velocityY": 3.0489243147666574e-23, + "timestamp": 0.5519116042994073 + }, + { + "x": 2.2639052118133014, + "y": 5.537254333496094, + "heading": 4.9814836533956335e-25, + "angularVelocity": -1.0657532309818666e-23, + "velocityX": 1.0325816073964444, + "velocityY": 2.0326162780553615e-23, + "timestamp": 0.6438968716826419 + }, + { + "x": 2.311396360397339, + "y": 5.537254333496094, + "heading": 7.949798578706025e-32, + "angularVelocity": -5.415522507165855e-24, + "velocityX": 0.5162908141167563, + "velocityY": 1.0163081591336611e-23, + "timestamp": 0.7358821390658764 + }, + { + "x": 2.311396360397339, + "y": 5.537254333496094, + "heading": 8.439039261116126e-32, + "angularVelocity": 5.318685223491289e-32, + "velocityX": 1.5271297522513056e-32, + "velocityY": -7.173666161189126e-33, + "timestamp": 0.8278674064491109 + }, + { + "x": 2.333988702088453, + "y": 5.530687639848115, + "heading": 2.7357905411745392e-18, + "angularVelocity": 4.225566579883271e-17, + "velocityX": 0.34894980130013487, + "velocityY": -0.10142580503518746, + "timestamp": 0.8926112223416762 + }, + { + "x": 2.3791733842230687, + "y": 5.5175542496687795, + "heading": 4.548394071480566e-18, + "angularVelocity": 2.799657011171827e-17, + "velocityX": 0.6978995833302994, + "velocityY": -0.20285165460557242, + "timestamp": 0.9573550382342415 + }, + { + "x": 2.4469504052763287, + "y": 5.4978541594339525, + "heading": 7.8862048960196e-18, + "angularVelocity": 5.155416680773327e-17, + "velocityX": 1.0468493418082916, + "velocityY": -0.304277558607872, + "timestamp": 1.0220988541268068 + }, + { + "x": 2.537319763342161, + "y": 5.47158736473847, + "heading": 5.032433303714057e-18, + "angularVelocity": -4.4077881290119006e-17, + "velocityX": 1.3957990708460906, + "velocityY": -0.40570353065007425, + "timestamp": 1.086842670019372 + }, + { + "x": 2.6502814559699055, + "y": 5.438753859918548, + "heading": 7.530601737573416e-19, + "angularVelocity": -6.609699862808569e-17, + "velocityX": 1.7447487620322484, + "velocityY": -0.5071295901721661, + "timestamp": 1.1515864859119374 + }, + { + "x": 2.785835479892019, + "y": 5.399353637422471, + "heading": -9.328301380807417e-18, + "angularVelocity": -1.557115793573686e-16, + "velocityX": 2.0936984027496157, + "velocityY": -0.6085557663341342, + "timestamp": 1.2163303018045026 + }, + { + "x": 2.943981830533949, + "y": 5.353386686677836, + "heading": -2.1917419574313143e-17, + "angularVelocity": -1.9444511294798906e-16, + "velocityX": 2.4426479728108004, + "velocityY": -0.7099821057919715, + "timestamp": 1.281074117697068 + }, + { + "x": 3.1247205010338854, + "y": 5.300852991826032, + "heading": -3.425033697426937e-17, + "angularVelocity": -1.9048797438260707e-16, + "velocityX": 2.7915974368879897, + "velocityY": -0.8114086901936944, + "timestamp": 1.3458179335896332 + }, + { + "x": 3.328051479955525, + "y": 5.241752526436032, + "heading": -4.82546070777501e-17, + "angularVelocity": -2.1630285080372957e-16, + "velocityX": 3.14054672432594, + "velocityY": -0.9128356828354128, + "timestamp": 1.4105617494821985 + }, + { + "x": 3.553974744426448, + "y": 5.176085237645743, + "heading": -6.627429722777717e-17, + "angularVelocity": -2.7832298821571975e-16, + "velocityX": 3.4894956584882664, + "velocityY": -1.0142634919576448, + "timestamp": 1.4753055653747638 + }, + { + "x": 3.802490225830772, + "y": 5.103850966868795, + "heading": -8.837633885982683e-17, + "angularVelocity": -3.413769151303276e-16, + "velocityX": 3.8384435328450017, + "velocityY": -1.1156937505229125, + "timestamp": 1.540049381267329 + }, + { + "x": 4.050315104768665, + "y": 5.01076075944265, + "heading": -8.243350807926677e-17, + "angularVelocity": 9.17899369790455e-17, + "velocityX": 3.82777683892875, + "velocityY": -1.4378239240925197, + "timestamp": 1.6047931971598943 + }, + { + "x": 4.288896560668945, + "y": 4.896035671234131, + "heading": -7.62003507301664e-17, + "angularVelocity": 9.627417326532366e-17, + "velocityX": 3.6850076352559227, + "velocityY": -1.7719852719227334, + "timestamp": 1.6695370130524596 + }, + { + "x": 4.402340886082199, + "y": 4.835318762030556, + "heading": -7.303407335542255e-17, + "angularVelocity": 1.0061831085733754e-16, + "velocityX": 3.605046257294911, + "velocityY": -1.9294686224845516, + "timestamp": 1.701005215487622 + }, + { + "x": 4.513057352461556, + "y": 4.7697594586676155, + "heading": -7.06099226407047e-17, + "angularVelocity": 7.70349281854855e-17, + "velocityX": 3.5183600527186023, + "velocityY": -2.0833507569763077, + "timestamp": 1.7324734179227845 + }, + { + "x": 4.620839460727155, + "y": 4.6994801912241115, + "heading": -7.060357135873025e-17, + "angularVelocity": 2.0183173729755417e-19, + "velocityX": 3.425111697657642, + "velocityY": -2.2333422949506527, + "timestamp": 1.763941620357947 + }, + { + "x": 4.7254864149518045, + "y": 4.624612485674306, + "heading": -7.099625319518912e-17, + "angularVelocity": -1.247868661272944e-17, + "velocityX": 3.3254824275253214, + "velocityY": -2.379154186040543, + "timestamp": 1.7954098227931095 + }, + { + "x": 4.832808704383543, + "y": 4.553633020226032, + "heading": -7.165943794882052e-17, + "angularVelocity": -2.1074758083014144e-17, + "velocityX": 3.410499524188173, + "velocityY": -2.2555932641481897, + "timestamp": 1.826878025228272 + }, + { + "x": 4.943095924051047, + "y": 4.487354159347409, + "heading": -7.348529763539e-17, + "angularVelocity": -5.802237005203753e-17, + "velocityX": 3.5047194035043416, + "velocityY": -2.1062169348406337, + "timestamp": 1.8583462276634344 + }, + { + "x": 5.056142456827311, + "y": 4.425899790074916, + "heading": -7.035642800696723e-17, + "angularVelocity": 9.942956337816296e-17, + "velocityX": 3.5924051591307293, + "velocityY": -1.952903709654341, + "timestamp": 1.8898144300985968 + }, + { + "x": 5.171737422557606, + "y": 4.369384643600972, + "heading": -6.629352517662628e-17, + "angularVelocity": 1.2911137325711957e-16, + "velocityX": 3.673389541993163, + "velocityY": -1.7959445440095247, + "timestamp": 1.9212826325337593 + }, + { + "x": 5.28966515888657, + "y": 4.317914199222627, + "heading": -6.55548292961571e-17, + "angularVelocity": 2.3474358981676347e-17, + "velocityX": 3.7475205827935785, + "velocityY": -1.635633445655282, + "timestamp": 1.9527508349689218 + }, + { + "x": 5.409705638885498, + "y": 4.271584510803223, + "heading": -6.571802328863205e-17, + "angularVelocity": -5.185996652043943e-18, + "velocityX": 3.8146595836400685, + "velocityY": -1.4722699370612031, + "timestamp": 1.9842190374040842 + }, + { + "x": 5.708749554118693, + "y": 4.190657909294355, + "heading": -8.63128139069911e-17, + "angularVelocity": -2.71821079393033e-16, + "velocityX": 3.946941793718872, + "velocityY": -1.0681126398297116, + "timestamp": 2.059985017968765 + }, + { + "x": 6.01458130018582, + "y": 4.141227935433476, + "heading": -6.519812472542959e-17, + "angularVelocity": 2.786829791452436e-16, + "velocityX": 4.0365312213681594, + "velocityY": -0.652403275109303, + "timestamp": 2.1357509985334455 + }, + { + "x": 6.3238928676995245, + "y": 4.1238286836853835, + "heading": -2.2948800283958053e-18, + "angularVelocity": 8.302307213384378e-16, + "velocityX": 4.082459768993199, + "velocityY": -0.2296446455231499, + "timestamp": 2.211516979098126 + }, + { + "x": 6.613870132060174, + "y": 4.122738763203337, + "heading": -9.066080252607654e-19, + "angularVelocity": 1.8323178126471314e-17, + "velocityX": 3.8272752784233455, + "velocityY": -0.014385354383583656, + "timestamp": 2.287282959662807 + }, + { + "x": 6.871627727395008, + "y": 4.121770013181932, + "heading": -3.3199950345434843e-18, + "angularVelocity": -3.185312321183995e-17, + "velocityX": 3.4020228262582535, + "velocityY": -0.012786081739125113, + "timestamp": 2.3630489402274875 + }, + { + "x": 7.097165634009429, + "y": 4.12092238390025, + "heading": -2.8404447967139716e-18, + "angularVelocity": 6.329381483051276e-18, + "velocityX": 2.9767701141533016, + "velocityY": -0.011187465334170248, + "timestamp": 2.438814920792168 + }, + { + "x": 7.290483845335008, + "y": 4.1201958587843395, + "heading": 4.211138438186873e-19, + "angularVelocity": 4.3047776169510437e-17, + "velocityX": 2.551517315354746, + "velocityY": -0.009589067679885944, + "timestamp": 2.5145809013568488 + }, + { + "x": 7.45158235808693, + "y": 4.11959042954717, + "heading": 6.3288304367769226e-18, + "angularVelocity": 7.797317559894418e-17, + "velocityX": 2.1262644732014313, + "velocityY": -0.007990779401695184, + "timestamp": 2.5903468819215294 + }, + { + "x": 7.58046117029412, + "y": 4.119106091216504, + "heading": 7.410357641228221e-18, + "angularVelocity": 1.4274566493954026e-17, + "velocityX": 1.701011605032828, + "velocityY": -0.006392556749398596, + "timestamp": 2.66611286248621 + }, + { + "x": 7.677120280642456, + "y": 4.118742840477511, + "heading": 3.662728059919033e-18, + "angularVelocity": -4.9463211035666574e-17, + "velocityX": 1.275758719519713, + "velocityY": -0.004794377847795854, + "timestamp": 2.7418788430508907 + }, + { + "x": 7.741559688193241, + "y": 4.118500674962447, + "heading": 2.4564209438434204e-18, + "angularVelocity": -1.5921497994971635e-17, + "velocityX": 0.850505821617187, + "velocityY": -0.0031962301967359986, + "timestamp": 2.8176448236155713 + }, + { + "x": 7.773779392242432, + "y": 4.118379592895508, + "heading": 3.1269636712640265e-33, + "angularVelocity": -3.242117401081041e-17, + "velocityX": 0.42525291442234653, + "velocityY": -0.0015981059836088808, + "timestamp": 2.893410804180252 + }, + { + "x": 7.773779392242432, + "y": 4.118379592895508, + "heading": 1.1844971908267807e-33, + "angularVelocity": -2.5637705565001737e-32, + "velocityX": -4.084582127410334e-32, + "velocityY": -3.5675440722041014e-30, + "timestamp": 2.9691767847449326 + }, + { + "x": 7.74609430870524, + "y": 4.117438747396381, + "heading": -2.05497935642393e-18, + "angularVelocity": -2.925144275486426e-17, + "velocityX": -0.3940811427571139, + "velocityY": -0.013392391211521543, + "timestamp": 3.039429026056197 + }, + { + "x": 7.690721455157199, + "y": 4.115639760645022, + "heading": -5.897998326291808e-18, + "angularVelocity": -5.4703150051541466e-17, + "velocityX": -0.7882005259120873, + "velocityY": -0.02560753532956242, + "timestamp": 3.109681267367461 + }, + { + "x": 7.607657826994539, + "y": 4.113085801554182, + "heading": -8.353793315278077e-18, + "angularVelocity": -3.495681937113195e-17, + "velocityX": -1.182362677862939, + "velocityY": -0.036354129678597254, + "timestamp": 3.1799335086787255 + }, + { + "x": 7.4969001342407955, + "y": 4.109909165473227, + "heading": -1.526679962466129e-17, + "angularVelocity": -9.840264115672304e-17, + "velocityX": -1.5765716607248677, + "velocityY": -0.0452175762888664, + "timestamp": 3.2501857499899898 + }, + { + "x": 7.358444983880178, + "y": 4.106285621312532, + "heading": -2.0305706370262314e-17, + "angularVelocity": -7.172591932681999e-17, + "velocityX": -1.9708289412030204, + "velocityY": -0.051579054177674104, + "timestamp": 3.320437991301254 + }, + { + "x": 7.1922895089935714, + "y": 4.10246001625686, + "heading": -2.298438007674344e-17, + "angularVelocity": -3.8129369021947994e-17, + "velocityX": -2.365127030615676, + "velocityY": -0.05445527408475893, + "timestamp": 3.3906902326125183 + }, + { + "x": 6.998433450931076, + "y": 4.098796843405572, + "heading": -2.2611777629214343e-17, + "angularVelocity": 5.303779741165941e-18, + "velocityX": -2.7594288017599875, + "velocityY": -0.05214314565508088, + "timestamp": 3.4609424739237826 + }, + { + "x": 6.776886824035131, + "y": 4.095895839339149, + "heading": -2.465545569001984e-17, + "angularVelocity": -2.909057420246661e-17, + "velocityX": -3.1535880245349874, + "velocityY": -0.041294114070628656, + "timestamp": 3.531194715235047 + }, + { + "x": 6.52770649933481, + "y": 4.094925348650892, + "heading": -2.807407218443882e-17, + "angularVelocity": -4.866202704245043e-17, + "velocityX": -3.546937721122466, + "velocityY": -0.013814373323087034, + "timestamp": 3.601446956546311 + }, + { + "x": 6.251309534448763, + "y": 4.099112534148996, + "heading": -2.5835306002252158e-17, + "angularVelocity": 3.186753600665183e-17, + "velocityX": -3.93435084386026, + "velocityY": 0.059602162435704605, + "timestamp": 3.6716991978575755 + }, + { + "x": 5.965656271322792, + "y": 4.12940837356358, + "heading": -8.624661629595893e-18, + "angularVelocity": 2.4498356282189034e-16, + "velocityX": -4.066108892673412, + "velocityY": 0.4312437418239143, + "timestamp": 3.7419514391688398 + }, + { + "x": 5.684249331081993, + "y": 4.187077783503906, + "heading": -1.9936792288925207e-17, + "angularVelocity": -1.6102163359044556e-16, + "velocityX": -4.005664943755716, + "velocityY": 0.8208906771360378, + "timestamp": 3.812203680480104 + }, + { + "x": 5.409705638885498, + "y": 4.271584510803223, + "heading": -2.6078962645628085e-17, + "angularVelocity": -8.743024054547926e-17, + "velocityX": -3.907970579615868, + "velocityY": 1.2029043589498862, + "timestamp": 3.8824559217913683 + }, + { + "x": 5.288298232295607, + "y": 4.314786208760466, + "heading": -2.909568780009548e-17, + "angularVelocity": -9.572142426185266e-17, + "velocityX": -3.8522865954532466, + "velocityY": 1.3708004035015684, + "timestamp": 3.913971595296342 + }, + { + "x": 5.168872926450536, + "y": 4.36319841734604, + "heading": -3.3051402703085907e-17, + "angularVelocity": -1.2551579779394563e-16, + "velocityX": -3.7893940558250763, + "velocityY": 1.5361311754271532, + "timestamp": 3.945487268801316 + }, + { + "x": 5.0516532214814776, + "y": 4.416730527614229, + "heading": -3.747448089829932e-17, + "angularVelocity": -1.4034534894249094e-16, + "velocityX": -3.7194098025720335, + "velocityY": 1.698586903425662, + "timestamp": 3.9770029423062896 + }, + { + "x": 4.936858487147447, + "y": 4.475282345718925, + "heading": -4.0016033176428666e-17, + "angularVelocity": -8.064407310629592e-17, + "velocityX": -3.642464893409777, + "velocityY": 1.8578634562721388, + "timestamp": 4.008518615811264 + }, + { + "x": 4.824703549738644, + "y": 4.538744277266208, + "heading": -3.9215963888302267e-17, + "angularVelocity": 2.5386393471797817e-17, + "velocityX": -3.5587034937109068, + "velocityY": 2.013662552293777, + "timestamp": 4.040034289316237 + }, + { + "x": 4.715398282405222, + "y": 4.606997522786201, + "heading": -3.873623413321436e-17, + "angularVelocity": 1.5221942028687378e-17, + "velocityX": -3.468282767816198, + "velocityY": 2.1656921121873096, + "timestamp": 4.07154996282121 + }, + { + "x": 4.609147174630297, + "y": 4.679914251228507, + "heading": -3.880924721736098e-17, + "angularVelocity": -2.3167229516803445e-18, + "velocityX": -3.371373540792568, + "velocityY": 2.313665561702092, + "timestamp": 4.103065636326184 + }, + { + "x": 4.5055479081696435, + "y": 4.756551965520632, + "heading": -3.815016865781766e-17, + "angularVelocity": 2.0912723297480083e-17, + "velocityX": -3.287229969694346, + "velocityY": 2.4317333494406586, + "timestamp": 4.134581309831157 + }, + { + "x": 4.398731100283088, + "y": 4.828637447090138, + "heading": -3.47447014727406e-17, + "angularVelocity": 1.0805630362110442e-16, + "velocityX": -3.3893233431834044, + "velocityY": 2.287289895871401, + "timestamp": 4.16609698333613 + }, + { + "x": 4.288896560668945, + "y": 4.896035671234131, + "heading": -3.0301452538996985e-17, + "angularVelocity": 1.409853713912325e-16, + "velocityX": -3.485076706255612, + "velocityY": 2.1385620755767296, + "timestamp": 4.197612656841104 + }, + { + "x": 4.031863105840825, + "y": 5.0215689729975965, + "heading": -1.9138186658356363e-17, + "angularVelocity": 1.5957200472787174e-16, + "velocityX": -3.6741348013740933, + "velocityY": 1.7944211699950958, + "timestamp": 4.267570202963796 + }, + { + "x": 3.7639740075166586, + "y": 5.121869712419655, + "heading": 8.866585751357036e-20, + "angularVelocity": 2.7483600528454067e-16, + "velocityX": -3.829309533732605, + "velocityY": 1.4337372446734074, + "timestamp": 4.3375277490864885 + }, + { + "x": 3.499869141493399, + "y": 5.197394590829224, + "heading": 1.115946844167211e-17, + "angularVelocity": 1.5825030223112458e-16, + "velocityX": -3.775216265591548, + "velocityY": 1.0795815833378437, + "timestamp": 4.407485295209181 + }, + { + "x": 3.262174659757514, + "y": 5.26536672558899, + "heading": 1.8977549587337083e-17, + "angularVelocity": 1.117546520506224e-16, + "velocityX": -3.39769610156156, + "velocityY": 0.9716197683742961, + "timestamp": 4.477442841331873 + }, + { + "x": 3.0508906346572426, + "y": 5.3257862975836385, + "heading": 2.54298959314255e-17, + "angularVelocity": 9.223231358389955e-17, + "velocityX": -3.0201749033580736, + "velocityY": 0.8636605390458314, + "timestamp": 4.547400387454566 + }, + { + "x": 2.8660170903084694, + "y": 5.378653367108372, + "heading": 2.5485282912423973e-17, + "angularVelocity": 7.917219258078879e-19, + "velocityX": -2.6426533604328997, + "velocityY": 0.7557021716000921, + "timestamp": 4.617357933577258 + }, + { + "x": 2.7075540387690964, + "y": 5.42396796431084, + "heading": 2.7012682013486872e-17, + "angularVelocity": 2.1833229254105427e-17, + "velocityX": -2.2651316451474477, + "velocityY": 0.6477442350964383, + "timestamp": 4.687315479699951 + }, + { + "x": 2.575501487273851, + "y": 5.4617301072796485, + "heading": 2.8840678474853086e-17, + "angularVelocity": 2.6130083844828922e-17, + "velocityX": -1.887609826446018, + "velocityY": 0.539786557158247, + "timestamp": 4.757273025822643 + }, + { + "x": 2.4698594406458803, + "y": 5.491939808073871, + "heading": 2.55057361888535e-17, + "angularVelocity": -4.7670944616029463e-17, + "velocityX": -1.5100879388006847, + "velocityY": 0.4318290515971121, + "timestamp": 4.827230571945336 + }, + { + "x": 2.390627902330286, + "y": 5.514597075307139, + "heading": 1.555218460712861e-17, + "angularVelocity": -1.4227988679695002e-16, + "velocityX": -1.1325660019097468, + "velocityY": 0.3238716691624835, + "timestamp": 4.897188118068028 + }, + { + "x": 2.337806874910892, + "y": 5.529701915439674, + "heading": 5.452841277859862e-18, + "angularVelocity": -1.4436388867795342e-16, + "velocityX": -0.7550440280846282, + "velocityY": 0.2159143790727539, + "timestamp": 4.967145664190721 + }, + { + "x": 2.311396360397339, + "y": 5.537254333496094, + "heading": -9.963384565748625e-36, + "angularVelocity": -7.794500474656568e-17, + "velocityX": -0.3775220255329383, + "velocityY": 0.10795716080684535, + "timestamp": 5.037103210313413 + }, + { + "x": 2.311396360397339, + "y": 5.537254333496094, + "heading": -6.535365453130042e-35, + "angularVelocity": -7.91769987677621e-34, + "velocityX": 8.47721531223007e-33, + "velocityY": 4.36786565310015e-32, + "timestamp": 5.1070607564361055 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4NT-Skip": { + "waypoints": [ + { + "x": 0.7644821405410767, + "y": 6.707253456115723, + "heading": 1.0390723224103011, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 2.763329029083252, + "y": 7.743913650512695, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 20 + }, + { + "x": 7.778790473937988, + "y": 7.453383922576904, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 5.19636344909668, + "y": 6.875411510467529, + "heading": 0.4089083102765838, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 7.763500213623047, + "y": 5.786660194396973, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 22 + }, + { + "x": 4.935596466064453, + "y": 6.19089937210083, + "heading": 0.18998832965561516, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.7644821405410767, + "y": 6.707253456115723, + "heading": 1.0390723224103011, + "angularVelocity": -1.7547937866246597e-32, + "velocityX": 2.2763161736314877e-32, + "velocityY": 0, + "timestamp": 0 + }, + { + "x": 0.7760278692353412, + "y": 6.71433952046308, + "heading": 1.0217643429776795, + "angularVelocity": -0.34522656494644127, + "velocityX": 0.2302921766484186, + "velocityY": 0.1413392974697392, + "timestamp": 0.05013513208436539 + }, + { + "x": 0.7991801780145195, + "y": 6.728500218028103, + "heading": 0.9877924180710587, + "angularVelocity": -0.6776071687505348, + "velocityX": 0.4617981007852623, + "velocityY": 0.28245058856519034, + "timestamp": 0.10027026416873078 + }, + { + "x": 0.8340045462176232, + "y": 6.749720753781454, + "heading": 0.9378562449760159, + "angularVelocity": -0.9960315455240489, + "velocityX": 0.69461008189831, + "velocityY": 0.4232667766316656, + "timestamp": 0.15040539625309618 + }, + { + "x": 0.8805749581377974, + "y": 6.777983973346064, + "heading": 0.872813574235644, + "angularVelocity": -1.2973471503160856, + "velocityX": 0.928897760592456, + "velocityY": 0.5637408019001573, + "timestamp": 0.20054052833746155 + }, + { + "x": 0.9389764076927801, + "y": 6.813272866000001, + "heading": 0.7938060911799183, + "angularVelocity": -1.5758905935018794, + "velocityX": 1.1648807358621667, + "velocityY": 0.7038755297294303, + "timestamp": 0.25067566042182693 + }, + { + "x": 1.0093049798201754, + "y": 6.85557383915721, + "heading": 0.70239868590727, + "angularVelocity": -1.8232205934718893, + "velocityX": 1.402780230219579, + "velocityY": 0.8437391385750465, + "timestamp": 0.3008107925061923 + }, + { + "x": 1.0916647821261949, + "y": 6.904878200303725, + "heading": 0.6007135393739693, + "angularVelocity": -2.0282213750267752, + "velocityX": 1.6427562645577014, + "velocityY": 0.983429365729964, + "timestamp": 0.3509459245905577 + }, + { + "x": 1.1861619165582793, + "y": 6.961177477000806, + "heading": 0.49155865682829086, + "angularVelocity": -2.177213423153984, + "velocityX": 1.88484861819189, + "velocityY": 1.1229506008350447, + "timestamp": 0.40108105667492305 + }, + { + "x": 1.2928950044716645, + "y": 7.024447544021754, + "heading": 0.37862860943476007, + "angularVelocity": -2.252513211763292, + "velocityX": 2.1289080825353928, + "velocityY": 1.2619906319281369, + "timestamp": 0.45121618875928843 + }, + { + "x": 1.411932213492286, + "y": 7.094613399845792, + "heading": 0.2670084248827377, + "angularVelocity": -2.2263865658953943, + "velocityX": 2.3743272246757017, + "velocityY": 1.3995346757233116, + "timestamp": 0.5013513208436539 + }, + { + "x": 1.5432310429314056, + "y": 7.171450777112641, + "heading": 0.16443069802118257, + "angularVelocity": -2.0460248651373116, + "velocityX": 2.618898644131923, + "velocityY": 1.5326054619254017, + "timestamp": 0.5514864529280192 + }, + { + "x": 1.686355782886626, + "y": 7.254093500212897, + "heading": 0.08456860071028449, + "angularVelocity": -1.5929368087932674, + "velocityX": 2.854779353415799, + "velocityY": 1.6483994289909887, + "timestamp": 0.6016215850123846 + }, + { + "x": 1.8412149510943983, + "y": 7.341984821518931, + "heading": 0.030968755693609747, + "angularVelocity": -1.0691074858739578, + "velocityX": 3.0888353489760694, + "velocityY": 1.7530884561774895, + "timestamp": 0.65175671709675 + }, + { + "x": 2.008057166165621, + "y": 7.434716608733129, + "heading": 0.002936894677818538, + "angularVelocity": -0.5591261028019302, + "velocityX": 3.3278503144355365, + "velocityY": 1.849636838657414, + "timestamp": 0.7018918491811154 + }, + { + "x": 2.187462439154994, + "y": 7.530834332052905, + "heading": 1.3401221666517954e-7, + "angularVelocity": -0.058576900937657404, + "velocityX": 3.578434234250707, + "velocityY": 1.9171730346303149, + "timestamp": 0.7520269812654807 + }, + { + "x": 2.3743734313488285, + "y": 7.615027816034377, + "heading": 9.157823134987268e-8, + "angularVelocity": -8.463922114317623e-7, + "velocityX": 3.7281440064685323, + "velocityY": 1.67933104952818, + "timestamp": 0.8021621133498461 + }, + { + "x": 2.5666324715448003, + "y": 7.68616657630836, + "heading": 4.894275703505732e-8, + "angularVelocity": -8.504111297257818e-7, + "velocityX": 3.834816668527906, + "velocityY": 1.4189403182238092, + "timestamp": 0.8522972454342115 + }, + { + "x": 2.763329029083252, + "y": 7.743913650512695, + "heading": 0, + "angularVelocity": -9.762167765450472e-7, + "velocityX": 3.923327801499733, + "velocityY": 1.1518285043541625, + "timestamp": 0.9024323775185769 + }, + { + "x": 3.0930925857719416, + "y": 7.801578883603503, + "heading": -2.025054003603945e-14, + "angularVelocity": -2.4734392492358033e-13, + "velocityX": 4.027794432293094, + "velocityY": 0.704334060174232, + "timestamp": 0.9843043703007283 + }, + { + "x": 3.427244052945336, + "y": 7.821878655200531, + "heading": -2.0171148368757895e-14, + "angularVelocity": 9.697048353617892e-16, + "velocityX": 4.081389200608798, + "velocityY": 0.24794524851792668, + "timestamp": 1.0661763630828798 + }, + { + "x": 3.7615632056259036, + "y": 7.804558480607454, + "heading": -2.0136813522881885e-14, + "angularVelocity": 4.1937229947938353e-16, + "velocityX": 4.083437343098987, + "velocityY": -0.21155188733665228, + "timestamp": 1.1480483558650312 + }, + { + "x": 4.095052943678329, + "y": 7.7753372729513455, + "heading": -2.0212632475990418e-14, + "angularVelocity": -9.260670289323022e-16, + "velocityX": 4.073306716007144, + "velocityY": -0.3569133553871345, + "timestamp": 1.2299203486471826 + }, + { + "x": 4.428549167930835, + "y": 7.746190184376778, + "heading": -2.0152082358748023e-14, + "angularVelocity": 7.395705806686352e-16, + "velocityX": 4.0733859396813115, + "velocityY": -0.35600805091092386, + "timestamp": 1.311792341429334 + }, + { + "x": 4.762045392031515, + "y": 7.71704309406487, + "heading": -2.015572108540389e-14, + "angularVelocity": -4.4444095376460173e-17, + "velocityX": 4.0733859378268935, + "velocityY": -0.3560080721311419, + "timestamp": 1.3936643342114854 + }, + { + "x": 5.095541616206554, + "y": 7.68789600460369, + "heading": -2.0229754033283868e-14, + "angularVelocity": -9.04252423377198e-16, + "velocityX": 4.0733859387351234, + "velocityY": -0.3560080617401835, + "timestamp": 1.4755363269936368 + }, + { + "x": 5.42903784040285, + "y": 7.658748915385721, + "heading": -2.0309309367944147e-14, + "angularVelocity": -9.717038996712115e-16, + "velocityX": 4.073385938994756, + "velocityY": -0.35600805876956854, + "timestamp": 1.5574083197757882 + }, + { + "x": 5.762534064600043, + "y": 7.629601826178022, + "heading": -2.0390490605011424e-14, + "angularVelocity": -9.915629790920958e-16, + "velocityX": 4.07338593900572, + "velocityY": -0.3560080586441164, + "timestamp": 1.6392803125579396 + }, + { + "x": 6.09603028879551, + "y": 7.60045473695057, + "heading": -2.0426978309746098e-14, + "angularVelocity": -4.456677246361619e-16, + "velocityX": 4.073385938984632, + "velocityY": -0.3560080588853828, + "timestamp": 1.721152305340091 + }, + { + "x": 6.4295265129813, + "y": 7.57130764761672, + "heading": -2.0431228864965123e-14, + "angularVelocity": -5.1917085129884607e-17, + "velocityX": 4.073385938866449, + "velocityY": -0.3560080601849525, + "timestamp": 1.8030242981222424 + }, + { + "x": 6.729362912053891, + "y": 7.545102378587393, + "heading": -1.7809391327471565e-14, + "angularVelocity": 3.202361940795799e-14, + "velocityX": 3.662258470615334, + "velocityY": -0.32007611075321446, + "timestamp": 1.8848962909043938 + }, + { + "x": 6.991719787015596, + "y": 7.522172765937965, + "heading": -1.4158080705052285e-14, + "angularVelocity": 4.459779832223585e-14, + "velocityX": 3.204476476586995, + "velocityY": -0.2800666243759492, + "timestamp": 1.9667682836865452 + }, + { + "x": 7.216597118349246, + "y": 7.502518811371365, + "heading": -1.0522354480483052e-14, + "angularVelocity": 4.440744754194895e-14, + "velocityX": 2.746694244172289, + "velocityY": -0.24005711719874057, + "timestamp": 2.0486402764686966 + }, + { + "x": 7.403994899549076, + "y": 7.486140515455231, + "heading": -7.2265963678814e-15, + "angularVelocity": 4.025501274271611e-14, + "velocityX": 2.2889119322949267, + "velocityY": -0.2000476030883275, + "timestamp": 2.1305122692508482 + }, + { + "x": 7.553913127362196, + "y": 7.473037878473377, + "heading": -4.440408288437967e-15, + "angularVelocity": 3.4031028009627144e-14, + "velocityX": 1.8311295806861385, + "velocityY": -0.16003808551133375, + "timestamp": 2.212384262033 + }, + { + "x": 7.666351799836867, + "y": 7.463210900596095, + "heading": -2.2615780344771733e-15, + "angularVelocity": 2.6612644691277202e-14, + "velocityX": 1.3733472052384643, + "velocityY": -0.12002856585439871, + "timestamp": 2.2942562548151515 + }, + { + "x": 7.7413109156719315, + "y": 7.456659581936907, + "heading": -7.614087443589096e-16, + "angularVelocity": 1.8323351358986854e-14, + "velocityX": 0.9155648138981872, + "velocityY": -0.08001904481083937, + "timestamp": 2.376128247597303 + }, + { + "x": 7.778790473937988, + "y": 7.453383922576904, + "heading": 1.2753058277402247e-32, + "angularVelocity": 9.299990369536053e-15, + "velocityX": 0.4577824112060448, + "velocityY": -0.04000952277683576, + "timestamp": 2.4580002403794547 + }, + { + "x": 7.778790473937988, + "y": 7.453383922576904, + "heading": -1.9205360939372803e-33, + "angularVelocity": -1.7921313580146165e-31, + "velocityX": 0, + "velocityY": -2.7149722697711243e-33, + "timestamp": 2.5398722331616064 + }, + { + "x": 7.738439660582295, + "y": 7.44435478084036, + "heading": 0.006378483370257001, + "angularVelocity": 0.07429220524317, + "velocityX": -0.4699786349727835, + "velocityY": -0.10516525827894567, + "timestamp": 2.6257289303666393 + }, + { + "x": 7.657738063777793, + "y": 7.4262963371792745, + "heading": 0.019137718520693816, + "angularVelocity": 0.14861083137133396, + "velocityX": -0.9399569216106665, + "velocityY": -0.21033238231795398, + "timestamp": 2.7115856275716723 + }, + { + "x": 7.536685745097366, + "y": 7.399208360987091, + "heading": 0.038283130587402035, + "angularVelocity": 0.2229926457686509, + "velocityX": -1.4099344910897795, + "velocityY": -0.3155021923041767, + "timestamp": 2.7974423247767053 + }, + { + "x": 7.375282797714109, + "y": 7.363090535376708, + "heading": 0.06382240791742924, + "angularVelocity": 0.29746400876611084, + "velocityX": -1.8799109753524952, + "velocityY": -0.4206756931742923, + "timestamp": 2.8832990219817383 + }, + { + "x": 7.173529348282375, + "y": 7.317942425556285, + "heading": 0.09576422280862332, + "angularVelocity": 0.3720363807486605, + "velocityX": -2.349885984432058, + "velocityY": -0.5258542582019549, + "timestamp": 2.9691557191867712 + }, + { + "x": 6.931425567675906, + "y": 7.263763399408429, + "heading": 0.1341164540627939, + "angularVelocity": 0.4467005196179663, + "velocityX": -2.819859003291312, + "velocityY": -0.6310401857000439, + "timestamp": 3.055012416391804 + }, + { + "x": 6.648971729733199, + "y": 7.200552321235508, + "heading": 0.17888339892270952, + "angularVelocity": 0.5214147098275685, + "velocityX": -3.2898288326673564, + "velocityY": -0.73623934102623, + "timestamp": 3.140869113596837 + }, + { + "x": 6.32616907238056, + "y": 7.128303578050117, + "heading": 0.23005243262398928, + "angularVelocity": 0.5959818554292148, + "velocityX": -3.759784243525683, + "velocityY": -0.8415038725849725, + "timestamp": 3.22672581080187 + }, + { + "x": 6.043717325555703, + "y": 7.065082680229889, + "heading": 0.2748002023996994, + "angularVelocity": 0.5211913715810514, + "velocityX": -3.2898044767589747, + "velocityY": -0.7363537135519165, + "timestamp": 3.312582508006903 + }, + { + "x": 5.801616036461219, + "y": 7.010892017513263, + "heading": 0.3131359109312741, + "angularVelocity": 0.44650807426269673, + "velocityX": -2.8198299838663337, + "velocityY": -0.6311757204823929, + "timestamp": 3.398439205211936 + }, + { + "x": 5.599865072469117, + "y": 6.965732394898873, + "heading": 0.345068551946627, + "angularVelocity": 0.3719295297266695, + "velocityX": -2.349857035733676, + "velocityY": -0.5259883513402064, + "timestamp": 3.484295902416969 + }, + { + "x": 5.438464367683131, + "y": 6.929604278069296, + "heading": 0.37060660034883014, + "angularVelocity": 0.29744969505659097, + "velocityX": -1.8798848551155938, + "velocityY": -0.4207955582463213, + "timestamp": 3.570152599622002 + }, + { + "x": 5.317413881627067, + "y": 6.902507972751596, + "heading": 0.38975689670647157, + "angularVelocity": 0.22304953464386104, + "velocityX": -1.4099131459364944, + "velocityY": -0.31559920425301746, + "timestamp": 3.656009296827035 + }, + { + "x": 5.2367135843204595, + "y": 6.884443678448797, + "heading": 0.40252374557523257, + "angularVelocity": 0.14869951074722523, + "velocityX": -0.9399417859492977, + "velocityY": -0.21040052658513142, + "timestamp": 3.741865994032068 + }, + { + "x": 5.19636344909668, + "y": 6.875411510467529, + "heading": 0.4089083102765838, + "angularVelocity": 0.07436303642224136, + "velocityX": -0.46997073655676014, + "velocityY": -0.10520050590460155, + "timestamp": 3.827722691237101 + }, + { + "x": 5.19636344909668, + "y": 6.875411510467529, + "heading": 0.4089083102765838, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -2.0717354478922107e-39, + "timestamp": 3.913579388442134 + }, + { + "x": 5.232018908472479, + "y": 6.860290267658786, + "heading": 0.4034159167471209, + "angularVelocity": -0.06610093533675257, + "velocityX": 0.4291133186212821, + "velocityY": -0.181984099964854, + "timestamp": 3.996670399204415 + }, + { + "x": 5.3033298450653605, + "y": 6.830047764220078, + "heading": 0.39242803594978326, + "angularVelocity": -0.13223910380357973, + "velocityX": 0.858226851962353, + "velocityY": -0.36396841440803684, + "timestamp": 4.079761409966696 + }, + { + "x": 5.410296246847904, + "y": 6.784683995320563, + "heading": 0.37593441821627777, + "angularVelocity": -0.19850062698951831, + "velocityX": 1.2873402405534362, + "velocityY": -0.5459527869903824, + "timestamp": 4.162852420728976 + }, + { + "x": 5.552918069928713, + "y": 6.72419896473783, + "heading": 0.3539176492261319, + "angularVelocity": -0.2649717314564233, + "velocityX": 1.7164531009117598, + "velocityY": -0.7279371141094613, + "timestamp": 4.2459434314912565 + }, + { + "x": 5.731195235870137, + "y": 6.64859267901311, + "heading": 0.3263531755047263, + "angularVelocity": -0.33173833689743104, + "velocityX": 2.1455650172732543, + "velocityY": -0.9099213625048691, + "timestamp": 4.329034442253537 + }, + { + "x": 5.945127628296537, + "y": 6.557865140131203, + "heading": 0.29320941514251314, + "angularVelocity": -0.39888503050030044, + "velocityX": 2.57467553305435, + "velocityY": -1.0919055870131655, + "timestamp": 4.412125453015817 + }, + { + "x": 6.194715088245195, + "y": 6.452016336978496, + "heading": 0.25444802631157254, + "angularVelocity": -0.4664931678570496, + "velocityX": 3.003784135719744, + "velocityY": -1.2738899452738088, + "timestamp": 4.495216463778098 + }, + { + "x": 6.47995740296407, + "y": 6.331046237930042, + "heading": 0.2100244721238566, + "angularVelocity": -0.5346373064928706, + "velocityX": 3.4328901779151324, + "velocityY": -1.455874684140549, + "timestamp": 4.578307474540378 + }, + { + "x": 6.765189822342828, + "y": 6.210072243447842, + "heading": 0.16347583941488503, + "angularVelocity": -0.5602126184521319, + "velocityX": 3.4327710875342943, + "velocityY": -1.4559215656709357, + "timestamp": 4.661398485302659 + }, + { + "x": 7.0147678146535055, + "y": 6.104219702621471, + "heading": 0.1226760205647685, + "angularVelocity": -0.49102566542177156, + "velocityX": 3.0036701927324945, + "velocityY": -1.2739349281621999, + "timestamp": 4.744489496064939 + }, + { + "x": 7.22869154945277, + "y": 6.013488715324614, + "heading": 0.0876591508055317, + "angularVelocity": -0.421427895003207, + "velocityX": 2.574571338544547, + "velocityY": -1.091947088674045, + "timestamp": 4.8275805068272195 + }, + { + "x": 7.406961171745674, + "y": 5.937879380234634, + "heading": 0.058451527477084785, + "angularVelocity": -0.3515136361983664, + "velocityX": 2.1454742294918456, + "velocityY": -0.9099580616042243, + "timestamp": 4.9106715175895 + }, + { + "x": 7.5495767925597805, + "y": 5.877391789669617, + "heading": 0.035072287662159383, + "angularVelocity": -0.2813690626752902, + "velocityX": 1.7163784566554727, + "velocityY": -0.7279679234865629, + "timestamp": 4.99376252835178 + }, + { + "x": 7.656538486904284, + "y": 5.832026023472794, + "heading": 0.017533973815731525, + "angularVelocity": -0.21107354075405452, + "velocityX": 1.287283586554457, + "velocityY": -0.54597682445592, + "timestamp": 5.076853539114061 + }, + { + "x": 7.7278462932659, + "y": 5.801782144275187, + "heading": 0.005843001619637481, + "angularVelocity": -0.14070080612620453, + "velocityX": 0.8581891796409112, + "velocityY": -0.36398497166117527, + "timestamp": 5.159944549876341 + }, + { + "x": 7.763500213623047, + "y": 5.786660194396973, + "heading": 0, + "angularVelocity": -0.07032050237484791, + "velocityX": 0.42909479653763305, + "velocityY": -0.18199260954325794, + "timestamp": 5.243035560638622 + }, + { + "x": 7.763500213623047, + "y": 5.786660194396973, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -4.001772889855341e-38, + "timestamp": 5.326126571400902 + }, + { + "x": 7.714022075284393, + "y": 5.791512130406288, + "heading": 0.0034123876701157535, + "angularVelocity": 0.03625557509808546, + "velocityX": -0.5256900837968682, + "velocityY": 0.0515503358242044, + "timestamp": 5.420246928363491 + }, + { + "x": 7.615065798767571, + "y": 5.801215987589317, + "heading": 0.010236903417415826, + "angularVelocity": 0.07250839210068752, + "velocityX": -1.0513801658886168, + "velocityY": 0.103100514024672, + "timestamp": 5.514367285326079 + }, + { + "x": 7.466631382770048, + "y": 5.8157717439660175, + "heading": 0.020472681304287838, + "angularVelocity": 0.10875200878106045, + "velocityX": -1.5770702618193642, + "velocityY": 0.15465045869391852, + "timestamp": 5.608487642288668 + }, + { + "x": 7.268718824587655, + "y": 5.83517937052045, + "heading": 0.03411811725863541, + "angularVelocity": 0.14497858268611724, + "velocityX": -2.1027603864811066, + "velocityY": 0.20620009507770087, + "timestamp": 5.702607999251256 + }, + { + "x": 7.021328120470755, + "y": 5.8594388313479895, + "heading": 0.05117069065282414, + "angularVelocity": 0.1811783756936537, + "velocityX": -2.6284505509815856, + "velocityY": 0.25774934998581056, + "timestamp": 5.796728356213845 + }, + { + "x": 6.724459267218483, + "y": 5.888550083771315, + "heading": 0.07162673839699378, + "angularVelocity": 0.21733924949201522, + "velocityX": -3.1541407494902667, + "velocityY": 0.3092981514604363, + "timestamp": 5.8908487131764335 + }, + { + "x": 6.378112271590919, + "y": 5.922513077815848, + "heading": 0.09548116624563086, + "angularVelocity": 0.25344599849019656, + "velocityX": -3.679830876175178, + "velocityY": 0.36084642197046873, + "timestamp": 5.984969070139022 + }, + { + "x": 5.995099579809893, + "y": 5.9600738684572026, + "heading": 0.09548116733610806, + "angularVelocity": 1.1585986600028837e-8, + "velocityX": -4.06939268125882, + "velocityY": 0.3990719101956213, + "timestamp": 6.079089427101611 + }, + { + "x": 5.61208688806035, + "y": 5.997634659420377, + "heading": 0.09548116842656575, + "angularVelocity": 1.1585779406391748e-8, + "velocityX": -4.069392680924314, + "velocityY": 0.3990719136148622, + "timestamp": 6.173209784064199 + }, + { + "x": 5.22907419631129, + "y": 6.035195450388469, + "heading": 0.09548116951702341, + "angularVelocity": 1.158577911551983e-8, + "velocityX": -4.069392680919191, + "velocityY": 0.3990719136671072, + "timestamp": 6.267330141026788 + }, + { + "x": 4.846061504562215, + "y": 6.072756241356416, + "heading": 0.09548117060748103, + "angularVelocity": 1.1585778663343773e-8, + "velocityX": -4.06939268091934, + "velocityY": 0.3990719136655779, + "timestamp": 6.3614504979893765 + }, + { + "x": 4.463048812813157, + "y": 6.110317032324537, + "heading": 0.0954811716979387, + "angularVelocity": 1.1585779124921689e-8, + "velocityX": -4.069392680919158, + "velocityY": 0.39907191366742895, + "timestamp": 6.455570854951965 + }, + { + "x": 4.080036121063514, + "y": 6.147877823286686, + "heading": 0.09548117278839631, + "angularVelocity": 1.1585778495719892e-8, + "velocityX": -4.0693926809253815, + "velocityY": 0.3990719136039721, + "timestamp": 6.549691211914554 + }, + { + "x": 3.697023429276966, + "y": 6.185438613871755, + "heading": 0.0954811738788733, + "angularVelocity": 1.1585984482237455e-8, + "velocityX": -4.069392681317477, + "velocityY": 0.39907190959759625, + "timestamp": 6.643811568877142 + }, + { + "x": 3.3506759158353345, + "y": 6.219399783259486, + "heading": 0.1191237323968371, + "angularVelocity": 0.25119495166557093, + "velocityX": -3.679836377791235, + "velocityY": 0.36082703555013734, + "timestamp": 6.737931925839731 + }, + { + "x": 3.0538065721545458, + "y": 6.248509296515882, + "heading": 0.13937914766365392, + "angularVelocity": 0.21520759079640947, + "velocityX": -3.1541459601432313, + "velocityY": 0.3092796733438538, + "timestamp": 6.832052282802319 + }, + { + "x": 2.806415429883936, + "y": 6.272767183419743, + "heading": 0.15625241023006078, + "angularVelocity": 0.17927325300214964, + "velocityX": -2.6284552062307136, + "velocityY": 0.25773262752820747, + "timestamp": 6.926172639764908 + }, + { + "x": 2.608502504943331, + "y": 6.2921734683030435, + "heading": 0.16974760497245162, + "angularVelocity": 0.14338231576995536, + "velocityX": -2.102764283174911, + "velocityY": 0.2061858402323502, + "timestamp": 7.0202929967274965 + }, + { + "x": 2.460067807083176, + "y": 6.306728169431489, + "heading": 0.17986771256728493, + "angularVelocity": 0.10752304731330184, + "velocityX": -1.5770732565236112, + "velocityY": 0.1546392470040303, + "timestamp": 7.114413353690085 + }, + { + "x": 2.361111341639132, + "y": 6.3164312990743445, + "heading": 0.1866144786841838, + "angularVelocity": 0.0716823260623689, + "velocityX": -1.051382173182552, + "velocityY": 0.10309278413290339, + "timestamp": 7.208533710652674 + }, + { + "x": 2.3116331100463867, + "y": 6.321282863616943, + "heading": 0.18998832965561516, + "angularVelocity": 0.035846134463476516, + "velocityX": -0.5256910745930495, + "velocityY": 0.05154638910397498, + "timestamp": 7.302654067615262 + }, + { + "x": 2.3116331100463867, + "y": 6.321282863616943, + "heading": 0.18998832965561516, + "angularVelocity": 0, + "velocityX": 5.252647651099752e-37, + "velocityY": 0, + "timestamp": 7.396774424577851 + } + ], + "constraints": [ + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 0 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4NB-SweepStraight": { + "waypoints": [ + { + "x": 0.727, + "y": 4.389017105102539, + "heading": -1.052, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 26 + }, + { + "x": 2.470987558364868, + "y": 2.642529249191284, + "heading": 1.5707963267948966, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 2.82627534866333, + "y": 3.8324406147003174, + "heading": 1.57, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 2.82627534866333, + "y": 5.474045753479004, + "heading": 1.57, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.82627534866333, + "y": 6.957221508026123, + "heading": 1.57, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.727, + "y": 4.389017105102539, + "heading": -1.052, + "angularVelocity": 7.985342564187253e-19, + "velocityX": -9.880504926804143e-18, + "velocityY": 4.541723618252651e-18, + "timestamp": 0 + }, + { + "x": 0.7320210369883334, + "y": 4.379190444759289, + "heading": -1.0132964977053183, + "angularVelocity": 0.8277410042322235, + "velocityX": 0.10738351706200194, + "velocityY": -0.2101600428523119, + "timestamp": 0.04675798600860395 + }, + { + "x": 0.7423767585467538, + "y": 4.35932167780024, + "heading": -0.9383484333304679, + "angularVelocity": 1.6028933402086725, + "velocityX": 0.22147492743864414, + "velocityY": -0.4249277750202023, + "timestamp": 0.0935159720172079 + }, + { + "x": 0.7584185968395437, + "y": 4.329046618725402, + "heading": -0.831097731487712, + "angularVelocity": 2.293740834408298, + "velocityX": 0.3430823194535392, + "velocityY": -0.6474842408585403, + "timestamp": 0.14027395802581186 + }, + { + "x": 0.7803726052330767, + "y": 4.287745603688506, + "heading": -0.6974263117481472, + "angularVelocity": 2.858793355958877, + "velocityX": 0.46952425173997964, + "velocityY": -0.8832932844737105, + "timestamp": 0.1870319440344158 + }, + { + "x": 0.8081702179949359, + "y": 4.234654918624728, + "heading": -0.5439140483187206, + "angularVelocity": 3.2831239438120146, + "velocityX": 0.5944997878390745, + "velocityY": -1.135435667695887, + "timestamp": 0.23378993004301976 + }, + { + "x": 0.8416609953282266, + "y": 4.169186742844337, + "heading": -0.3760252466197787, + "angularVelocity": 3.590590956331753, + "velocityX": 0.7162579099789393, + "velocityY": -1.400149607991124, + "timestamp": 0.2805479160516237 + }, + { + "x": 0.8808506607064412, + "y": 4.0909034261415895, + "heading": -0.20048698115793326, + "angularVelocity": 3.7541879034178507, + "velocityX": 0.838138438449452, + "velocityY": -1.6742234511200584, + "timestamp": 0.3273059020602277 + }, + { + "x": 0.925668806197045, + "y": 3.9996480546729565, + "heading": -0.02893711316846682, + "angularVelocity": 3.6688891595497974, + "velocityX": 0.9585131721127651, + "velocityY": -1.9516531668372759, + "timestamp": 0.3740638880688316 + }, + { + "x": 0.9754949593831373, + "y": 3.896282996276425, + "heading": 0.12253671854863268, + "angularVelocity": 3.2395285735590016, + "velocityX": 1.0656180353247193, + "velocityY": -2.2106396622276985, + "timestamp": 0.42082187407743554 + }, + { + "x": 1.0309514620598907, + "y": 3.781043042077509, + "heading": 0.2541852806699216, + "angularVelocity": 2.815531064508889, + "velocityX": 1.1860327488595344, + "velocityY": -2.4646047453299187, + "timestamp": 0.46757986008603947 + }, + { + "x": 1.0929356681499833, + "y": 3.6545108746667574, + "heading": 0.36498378647979907, + "angularVelocity": 2.3696167279161107, + "velocityX": 1.3256389203520678, + "velocityY": -2.7061081584525786, + "timestamp": 0.5143378460946434 + }, + { + "x": 1.1629696592047158, + "y": 3.5179953972400937, + "heading": 0.45299398894595083, + "angularVelocity": 1.882249642867833, + "velocityX": 1.4977974252790578, + "velocityY": -2.9196184241444185, + "timestamp": 0.5610958321032473 + }, + { + "x": 1.2444134330422476, + "y": 3.375806208333299, + "heading": 0.5186903236816397, + "angularVelocity": 1.4050291798979542, + "velocityX": 1.7418152660097566, + "velocityY": -3.0409605084502798, + "timestamp": 0.6078538181118512 + }, + { + "x": 1.3382986621093096, + "y": 3.240381730316091, + "heading": 0.5812504134266591, + "angularVelocity": 1.3379551833759529, + "velocityX": 2.00789719749598, + "velocityY": -2.896285524197266, + "timestamp": 0.6546118041204552 + }, + { + "x": 1.438124876158876, + "y": 3.1172857608464914, + "heading": 0.6580009966257694, + "angularVelocity": 1.6414433073585977, + "velocityX": 2.1349553856095276, + "velocityY": -2.632619151880301, + "timestamp": 0.7013697901290591 + }, + { + "x": 1.5413035249725158, + "y": 3.007118135101827, + "heading": 0.7526515078768561, + "angularVelocity": 2.024264074038882, + "velocityX": 2.206652972484545, + "velocityY": -2.3561242719990694, + "timestamp": 0.748127776137663 + }, + { + "x": 1.6457474356666648, + "y": 2.910401613533236, + "heading": 0.8633543586975786, + "angularVelocity": 2.3675709813660726, + "velocityX": 2.2337127752831343, + "velocityY": -2.068449260213815, + "timestamp": 0.7948857621462669 + }, + { + "x": 1.749997834476469, + "y": 2.8273646542502275, + "heading": 0.9880259417022461, + "angularVelocity": 2.666316358922014, + "velocityX": 2.2295741906093727, + "velocityY": -1.7758882785866068, + "timestamp": 0.8416437481548709 + }, + { + "x": 1.8521934124271529, + "y": 2.758362183574911, + "heading": 1.1133113758268665, + "angularVelocity": 2.6794446215418817, + "velocityX": 2.185628310247731, + "velocityY": -1.4757365867426573, + "timestamp": 0.8884017341634748 + }, + { + "x": 1.95145157552739, + "y": 2.7031033818343477, + "heading": 1.231381145561192, + "angularVelocity": 2.525125220594789, + "velocityX": 2.122806638461338, + "velocityY": -1.181804574097173, + "timestamp": 0.9351597201720787 + }, + { + "x": 2.0473220216152592, + "y": 2.6612321563456565, + "heading": 1.3371714258033476, + "angularVelocity": 2.2625072051406683, + "velocityX": 2.0503544799845086, + "velocityY": -0.8954882163008596, + "timestamp": 0.9819177061806826 + }, + { + "x": 2.139580608011859, + "y": 2.6324205589828935, + "heading": 1.4268444625557797, + "angularVelocity": 1.9178122157799795, + "velocityX": 1.973108644575945, + "velocityY": -0.6161855935675581, + "timestamp": 1.0286756921892866 + }, + { + "x": 2.2281178382739433, + "y": 2.6163741225527475, + "heading": 1.4972552356256885, + "angularVelocity": 1.5058555571031602, + "velocityX": 1.8935210392881243, + "velocityY": -0.3431806585331752, + "timestamp": 1.0754336781978906 + }, + { + "x": 2.312878410436143, + "y": 2.6128372160061697, + "heading": 1.5458711009980197, + "angularVelocity": 1.0397339475538696, + "velocityX": 1.8127507062977068, + "velocityY": -0.07564283341668805, + "timestamp": 1.1221916642064946 + }, + { + "x": 2.393836402463875, + "y": 2.621604064143261, + "heading": 1.5707963267948966, + "angularVelocity": 0.5330688492938996, + "velocityX": 1.7314259859865824, + "velocityY": 0.18749413491648587, + "timestamp": 1.1689496502150987 + }, + { + "x": 2.470987558364868, + "y": 2.642529249191284, + "heading": 1.5707963267948966, + "angularVelocity": -1.4908159190516835e-18, + "velocityX": 1.6500102439562863, + "velocityY": 0.44752109391968486, + "timestamp": 1.2157076362237027 + }, + { + "x": 2.602813969514969, + "y": 2.734000351322527, + "heading": 1.57075571228945, + "angularVelocity": -0.0004452490507007872, + "velocityX": 1.4451877174529841, + "velocityY": 1.0027801875851559, + "timestamp": 1.3069251365927887 + }, + { + "x": 2.713939411568514, + "y": 2.8753302073709235, + "heading": 1.570674784882514, + "angularVelocity": -0.0008871916749376329, + "velocityX": 1.2182469548490686, + "velocityY": 1.5493721651707555, + "timestamp": 1.3981426369618748 + }, + { + "x": 2.8004764091180525, + "y": 3.064720789521499, + "heading": 1.5705543763017473, + "angularVelocity": -0.001320016228019663, + "velocityX": 0.9486885433102401, + "velocityY": 2.076252707914343, + "timestamp": 1.4893601373309608 + }, + { + "x": 2.8524060828186255, + "y": 3.2955451367079904, + "heading": 1.5703982906404752, + "angularVelocity": -0.0017111372339782035, + "velocityX": 0.5692950748534504, + "velocityY": 2.5304831447084086, + "timestamp": 1.580577637700047 + }, + { + "x": 2.852926988468019, + "y": 3.509890434151461, + "heading": 1.5702419946817163, + "angularVelocity": -0.0017134426850825867, + "velocityX": 0.005710588946470057, + "velocityY": 2.3498264760185585, + "timestamp": 1.671795138069133 + }, + { + "x": 2.8426506520730745, + "y": 3.671341086515572, + "heading": 1.5701215497677061, + "angularVelocity": -0.0013204145424167311, + "velocityX": -0.11265750928735839, + "velocityY": 1.7699526046084872, + "timestamp": 1.763012638438219 + }, + { + "x": 2.832422777582534, + "y": 3.778806345465944, + "heading": 1.570040615176241, + "angularVelocity": -0.0008872704375541147, + "velocityX": -0.11212623070348748, + "velocityY": 1.1781210679481708, + "timestamp": 1.854230138807305 + }, + { + "x": 2.82627534866333, + "y": 3.8324406147003174, + "heading": 1.57, + "angularVelocity": -0.0004452564044901184, + "velocityX": -0.06739308679091216, + "velocityY": 0.5879822294775167, + "timestamp": 1.945447639176391 + }, + { + "x": 2.82627534866333, + "y": 3.8324406147003174, + "heading": 1.57, + "angularVelocity": -2.8295730504754553e-24, + "velocityX": -1.0072066101835765e-17, + "velocityY": 4.887478241972941e-18, + "timestamp": 2.036665139545477 + }, + { + "x": 2.82627534866333, + "y": 3.887160790336596, + "heading": 1.57, + "angularVelocity": -7.223726232843347e-17, + "velocityX": -1.4022322773048193e-16, + "velocityY": 0.595845723766963, + "timestamp": 2.1285012868124826 + }, + { + "x": 2.82627534866333, + "y": 3.9966011399802817, + "heading": 1.57, + "angularVelocity": -1.4517284366324747e-16, + "velocityX": -2.7085764742439913e-16, + "velocityY": 1.1916914297972172, + "timestamp": 2.220337434079488 + }, + { + "x": 2.82627534866333, + "y": 4.16076166091659, + "heading": 1.57, + "angularVelocity": -2.1926697371411643e-16, + "velocityX": -4.0231930155833035e-16, + "velocityY": 1.7875371062662906, + "timestamp": 2.3121735813464936 + }, + { + "x": 2.82627534866333, + "y": 4.379642347715951, + "heading": 1.57, + "angularVelocity": -2.958096299138811e-16, + "velocityX": -5.355279601742931e-16, + "velocityY": 2.3833827236130065, + "timestamp": 2.404009728613499 + }, + { + "x": 2.82627534866333, + "y": 4.653243184089661, + "heading": 1.57, + "angularVelocity": -3.783476918941958e-16, + "velocityX": -6.748965699405944e-16, + "velocityY": 2.979228163592689, + "timestamp": 2.4958458758805047 + }, + { + "x": 2.82627534866333, + "y": 4.9268440204633706, + "heading": 1.57, + "angularVelocity": -3.641716874889364e-16, + "velocityX": -6.159722745525841e-16, + "velocityY": 2.979228163592689, + "timestamp": 2.58768202314751 + }, + { + "x": 2.82627534866333, + "y": 5.1457247072627315, + "heading": 1.57, + "angularVelocity": -2.879074384838342e-16, + "velocityX": -4.995322375549383e-16, + "velocityY": 2.3833827236130065, + "timestamp": 2.6795181704145157 + }, + { + "x": 2.82627534866333, + "y": 5.30988522819904, + "heading": 1.57, + "angularVelocity": -2.1475953045710015e-16, + "velocityX": -3.7669901554704897e-16, + "velocityY": 1.7875371062662906, + "timestamp": 2.771354317681521 + }, + { + "x": 2.82627534866333, + "y": 5.419325577842725, + "heading": 1.57, + "angularVelocity": -1.426506525121705e-16, + "velocityX": -2.519305992761992e-16, + "velocityY": 1.1916914297972172, + "timestamp": 2.8631904649485267 + }, + { + "x": 2.82627534866333, + "y": 5.474045753479004, + "heading": 1.57, + "angularVelocity": -7.113697583370428e-17, + "velocityX": -1.262362732724396e-16, + "velocityY": 0.5958457237669631, + "timestamp": 2.955026612215532 + }, + { + "x": 2.82627534866333, + "y": 5.474045753479004, + "heading": 1.57, + "angularVelocity": 1.033945689540126e-28, + "velocityX": -3.228114037955519e-28, + "velocityY": 3.3712113107609656e-23, + "timestamp": 3.0468627594825377 + }, + { + "x": 2.82627534866333, + "y": 5.533372789188761, + "heading": 1.57, + "angularVelocity": -8.890513472633016e-17, + "velocityX": -2.241316397130285e-16, + "velocityY": 0.6204208876250313, + "timestamp": 3.142486612407383 + }, + { + "x": 2.82627534866333, + "y": 5.652026858634037, + "heading": 1.57, + "angularVelocity": -1.7783037339245997e-16, + "velocityX": -4.482632719673846e-16, + "velocityY": 1.2408417546041586, + "timestamp": 3.238110465332228 + }, + { + "x": 2.82627534866333, + "y": 5.830007958261196, + "heading": 1.57, + "angularVelocity": -2.666888193558669e-16, + "velocityX": -6.723948907964149e-16, + "velocityY": 1.8612625844206605, + "timestamp": 3.3337343182570733 + }, + { + "x": 2.82627534866333, + "y": 6.067316079778427, + "heading": 1.57, + "angularVelocity": -3.5609571634279607e-16, + "velocityX": -8.965264783008826e-16, + "velocityY": 2.4816833275243777, + "timestamp": 3.4293581711819185 + }, + { + "x": 2.82627534866333, + "y": 6.3639511817267, + "heading": 1.57, + "angularVelocity": -4.462891867937391e-16, + "velocityX": -1.1206579091688093e-15, + "velocityY": 3.102103637064379, + "timestamp": 3.5249820241067638 + }, + { + "x": 2.82627534866333, + "y": 6.601259303243931, + "heading": 1.57, + "angularVelocity": -3.515360973110671e-16, + "velocityX": -8.965264782972495e-16, + "velocityY": 2.4816833275243777, + "timestamp": 3.620605877031609 + }, + { + "x": 2.82627534866333, + "y": 6.77924040287109, + "heading": 1.57, + "angularVelocity": -2.6388235640692793e-16, + "velocityX": -6.723948907917458e-16, + "velocityY": 1.8612625844206607, + "timestamp": 3.716229729956454 + }, + { + "x": 2.82627534866333, + "y": 6.8978944723163655, + "heading": 1.57, + "angularVelocity": -1.7617660412854568e-16, + "velocityX": -4.482632719646236e-16, + "velocityY": 1.2408417546041588, + "timestamp": 3.8118535828812994 + }, + { + "x": 2.82627534866333, + "y": 6.957221508026123, + "heading": 1.57, + "angularVelocity": -8.819964498314495e-17, + "velocityX": -2.241316397116374e-16, + "velocityY": 0.6204208876250313, + "timestamp": 3.9074774358061446 + }, + { + "x": 2.82627534866333, + "y": 6.957221508026123, + "heading": 1.57, + "angularVelocity": 2.2261004408365928e-29, + "velocityX": 5.7622977196112705e-39, + "velocityY": -1.7124712130623078e-23, + "timestamp": 4.00310128873099 + } + ], + "constraints": [ + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 0 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 1, + 1 + ], + "type": "ZeroAngularVelocity" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4NB-Skip": { + "waypoints": [ + { + "x": 0.8085346817970276, + "y": 4.440213680267334, + "heading": -0.9944211947753461, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 23 + }, + { + "x": 4.03019905090332, + "y": 1.7862433195114136, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 18 + }, + { + "x": 7.989865779876709, + "y": 0.6650473475456238, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 5.31951379776001, + "y": 0.9000114798545837, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 2.4222898483276367, + "y": 3.3805336952209473, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 15 + }, + { + "x": 4.85231351852417, + "y": 1.6030162572860718, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 7.989865779876709, + "y": 2.396514654159546, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 17 + }, + { + "x": 4.424809455871582, + "y": 1.6480166912078857, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 16 + }, + { + "x": 1.8147839307785034, + "y": 3.560535430908203, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 8 + }, + { + "x": 2.377289295196533, + "y": 4.123040676116943, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 0.8085346817970276, + "y": 4.440213680267334, + "heading": -0.994421194775346, + "angularVelocity": 9.676628377640253e-20, + "velocityX": 2.353549276621379e-17, + "velocityY": 2.6128743115300274e-17, + "timestamp": 0 + }, + { + "x": 0.822974919123913, + "y": 4.427726715046675, + "heading": -0.9705493797852635, + "angularVelocity": 0.40182084288808634, + "velocityX": 0.2430643977679354, + "velocityY": -0.21018606637847403, + "timestamp": 0.05940910087815508 + }, + { + "x": 0.8519383277486233, + "y": 4.402735349568012, + "heading": -0.9240064477296293, + "angularVelocity": 0.78343101254963, + "velocityX": 0.4875247764498045, + "velocityY": -0.4206656069399718, + "timestamp": 0.11881820175631017 + }, + { + "x": 0.8955174447652127, + "y": 4.365218664734886, + "heading": -0.856257826909993, + "angularVelocity": 1.1403744513586813, + "velocityX": 0.7335427800188714, + "velocityY": -0.6314972668931192, + "timestamp": 0.17822730263446523 + }, + { + "x": 0.9538148065445263, + "y": 4.315143986084932, + "heading": -0.7692489012801026, + "angularVelocity": 1.4645723356147615, + "velocityX": 0.981286720680567, + "velocityY": -0.842878917704278, + "timestamp": 0.23763640351262033 + }, + { + "x": 1.026938864152823, + "y": 4.2524594827203375, + "heading": -0.6656141360189669, + "angularVelocity": 1.7444257483998427, + "velocityX": 1.230856157178015, + "velocityY": -1.0551330088830848, + "timestamp": 0.29704550439077543 + }, + { + "x": 1.1149962326719975, + "y": 4.177093081809827, + "heading": -0.5488592608060379, + "angularVelocity": 1.9652691841337226, + "velocityX": 1.4822201854184571, + "velocityY": -1.2686002615171224, + "timestamp": 0.3564546052689305 + }, + { + "x": 1.2180827278312385, + "y": 4.088964134511281, + "heading": -0.4236140095866624, + "angularVelocity": 2.108182910834597, + "velocityX": 1.735197025968326, + "velocityY": -1.483425030776192, + "timestamp": 0.4158637061470856 + }, + { + "x": 1.3362653476838338, + "y": 3.988016447914785, + "heading": -0.29631937092086613, + "angularVelocity": 2.1426790977171852, + "velocityX": 1.9893016070882061, + "velocityY": -1.6991956637006456, + "timestamp": 0.4752728070252407 + }, + { + "x": 1.4695036425602637, + "y": 3.874326638244391, + "heading": -0.17703470987105152, + "angularVelocity": 2.007851647081105, + "velocityX": 2.2427253216586585, + "velocityY": -1.9136766587931984, + "timestamp": 0.5346819079033958 + }, + { + "x": 1.61738357366469, + "y": 3.7486761953014893, + "heading": -0.08320124029329157, + "angularVelocity": 1.5794460476721217, + "velocityX": 2.4891797539185125, + "velocityY": -2.115003275350498, + "timestamp": 0.5940910087815509 + }, + { + "x": 1.7795783215287542, + "y": 3.612066748468329, + "heading": -0.024373238316333756, + "angularVelocity": 0.9902186888437958, + "velocityX": 2.7301330177794627, + "velocityY": -2.299470027552942, + "timestamp": 0.653500109659706 + }, + { + "x": 1.9560238490010582, + "y": 3.464363802586248, + "heading": -3.251899068917287e-7, + "angularVelocity": 0.41025554613951887, + "velocityX": 2.9700083802675254, + "velocityY": -2.486200661158664, + "timestamp": 0.712909210537861 + }, + { + "x": 2.1407664058154356, + "y": 3.306630906711477, + "heading": -2.960600612657896e-7, + "angularVelocity": 4.903263169257326e-7, + "velocityX": 3.1096676112512296, + "velocityY": -2.655029171343601, + "timestamp": 0.7723183114160161 + }, + { + "x": 2.3254947652525764, + "y": 3.1488813838204237, + "heading": -2.6697171512847604e-7, + "angularVelocity": 4.896277793613143e-7, + "velocityX": 3.109428634780798, + "velocityY": -2.655309044562341, + "timestamp": 0.8317274122941712 + }, + { + "x": 2.5102231286576613, + "y": 2.9911318655758214, + "heading": -2.3788336891991952e-7, + "angularVelocity": 4.896277805605067e-7, + "velocityX": 3.109428701570963, + "velocityY": -2.655308966351258, + "timestamp": 0.8911365131723263 + }, + { + "x": 2.694951491657384, + "y": 2.8333823468564927, + "heading": -2.0879502283644788e-7, + "angularVelocity": 4.89627778455023e-7, + "velocityX": 3.1094286947477254, + "velocityY": -2.6553089743420633, + "timestamp": 0.9505456140504814 + }, + { + "x": 2.8796798410785964, + "y": 2.675632812236456, + "heading": -1.7970667738519104e-7, + "angularVelocity": 4.896277678133068e-7, + "velocityX": 3.1094284661883007, + "velocityY": -2.6553092419897357, + "timestamp": 1.0099547149286365 + }, + { + "x": 3.0644080904838464, + "y": 2.5178831604959693, + "heading": -1.5061833606690155e-7, + "angularVelocity": 4.896276982453926e-7, + "velocityX": 3.109426782675891, + "velocityY": -2.655311213412356, + "timestamp": 1.0693638158067915 + }, + { + "x": 3.2491359906412702, + "y": 2.360133099780758, + "heading": -1.215300088943934e-7, + "angularVelocity": 4.896274601374268e-7, + "velocityX": 3.109420903984017, + "velocityY": -2.6553180974534567, + "timestamp": 1.1287729166849465 + }, + { + "x": 3.4338955523253594, + "y": 2.202420122735163, + "heading": -9.244031535060432e-8, + "angularVelocity": 4.896504594972081e-7, + "velocityX": 3.1099538446664665, + "velocityY": -2.654693888886324, + "timestamp": 1.1881820175631015 + }, + { + "x": 3.6212771735981955, + "y": 2.0478316149306797, + "heading": -6.319311636602291e-8, + "angularVelocity": 4.923016600450938e-7, + "velocityX": 3.1540894998099027, + "velocityY": -2.6021014544794228, + "timestamp": 1.2475911184412565 + }, + { + "x": 3.820400967204548, + "y": 1.908694296792687, + "heading": -3.281153410671686e-8, + "angularVelocity": 5.113960960484033e-7, + "velocityX": 3.3517388861820665, + "velocityY": -2.342020264256331, + "timestamp": 1.3070002193194115 + }, + { + "x": 4.03019905090332, + "y": 1.7862433195114136, + "heading": -4.8954376995112275e-19, + "angularVelocity": 5.522981095615313e-7, + "velocityX": 3.5314132110677283, + "velocityY": -2.0611484683535886, + "timestamp": 1.3664093201975664 + }, + { + "x": 4.322951930029737, + "y": 1.6552955500710258, + "heading": 5.175704079338812e-15, + "angularVelocity": 6.599529748424669e-14, + "velocityX": 3.7325329660882653, + "velocityY": -1.669554430106589, + "timestamp": 1.4448420846999053 + }, + { + "x": 4.628082081291311, + "y": 1.556567853907556, + "heading": 5.190662725666785e-15, + "angularVelocity": 1.9071943375934453e-16, + "velocityX": 3.8903403851384977, + "velocityY": -1.2587557864383778, + "timestamp": 1.523274849202244 + }, + { + "x": 4.938074644435486, + "y": 1.4743718801236945, + "heading": 5.169631100164441e-15, + "angularVelocity": -2.681484041715186e-16, + "velocityX": 3.952335036397641, + "velocityY": -1.0479800668204275, + "timestamp": 1.601707613704583 + }, + { + "x": 5.248064134540723, + "y": 1.3921643177396372, + "heading": 5.1832756916526305e-15, + "angularVelocity": 1.7396546747950823e-16, + "velocityX": 3.952295855847418, + "velocityY": -1.0481278188474166, + "timestamp": 1.6801403782069217 + }, + { + "x": 5.558053461013338, + "y": 1.3099561383310128, + "heading": 5.1652975147552064e-15, + "angularVelocity": -2.2921768707255205e-16, + "velocityX": 3.9522937695684903, + "velocityY": -1.0481356857710855, + "timestamp": 1.7585731427092606 + }, + { + "x": 5.868042788859202, + "y": 1.227747964100929, + "heading": 5.17893915410426e-15, + "angularVelocity": 1.7392781023474698e-16, + "velocityX": 3.952293787077097, + "velocityY": -1.0481356197458775, + "timestamp": 1.8370059072115994 + }, + { + "x": 6.1780321171916235, + "y": 1.1455397917056027, + "heading": 5.203641884469278e-15, + "angularVelocity": 3.1495423062461055e-16, + "velocityX": 3.95229379328059, + "velocityY": -1.0481355963531385, + "timestamp": 1.9154386717139382 + }, + { + "x": 6.488021446386602, + "y": 1.0633316225647949, + "heading": 5.179220620544195e-15, + "angularVelocity": -3.1136561843441626e-16, + "velocityX": 3.952293804278011, + "velocityY": -1.048135554858763, + "timestamp": 1.993871436216277 + }, + { + "x": 6.788390273210204, + "y": 0.9836747790194914, + "heading": 1.4385064984300214e-14, + "angularVelocity": 1.1737244138358137e-13, + "velocityX": 3.8296345759259354, + "velocityY": -1.015606730818312, + "timestamp": 2.072304200718616 + }, + { + "x": 7.055384812513254, + "y": 0.9128686882327177, + "heading": 1.5703193582509516e-14, + "angularVelocity": 1.680584250857847e-14, + "velocityX": 3.40411996181899, + "velocityY": -0.9027616358546965, + "timestamp": 2.150736965220955 + }, + { + "x": 7.289005044919871, + "y": 0.8509133557721549, + "heading": 1.4172123019917492e-14, + "angularVelocity": -1.9520804221347698e-14, + "velocityX": 2.97860510067396, + "velocityY": -0.7899164699044072, + "timestamp": 2.229169729723294 + }, + { + "x": 7.48925096397141, + "y": 0.7978087834938102, + "heading": 1.1452140462826918e-14, + "angularVelocity": -3.4679163908121556e-14, + "velocityX": 2.5530901571826567, + "velocityY": -0.6770712802904464, + "timestamp": 2.307602494225633 + }, + { + "x": 7.656122566438546, + "y": 0.7535549723257088, + "heading": 8.361039725202098e-15, + "angularVelocity": -3.9410836943919146e-14, + "velocityX": 2.127575172518181, + "velocityY": -0.5642260788443735, + "timestamp": 2.386035258727972 + }, + { + "x": 7.789619850383681, + "y": 0.7181519228246735, + "heading": 5.39075057643342e-15, + "angularVelocity": -3.787051642035781e-14, + "velocityX": 1.7020601631497934, + "velocityY": -0.4513808702989361, + "timestamp": 2.464468023230311 + }, + { + "x": 7.889742814515085, + "y": 0.6915996353619231, + "heading": 2.8547195474114465e-15, + "angularVelocity": -3.233382334683438e-14, + "velocityX": 1.2765451373121304, + "velocityY": -0.33853565702054167, + "timestamp": 2.54290078773265 + }, + { + "x": 7.9564914579100945, + "y": 0.6738981102026166, + "heading": 1.001573018444963e-15, + "angularVelocity": -2.3627200058002107e-14, + "velocityX": 0.8510300997107006, + "velocityY": -0.22569044036143487, + "timestamp": 2.621333552234989 + }, + { + "x": 7.989865779876709, + "y": 0.6650473475456241, + "heading": -9.790956081407498e-26, + "angularVelocity": -1.2769829956117919e-14, + "velocityX": 0.4255150532864473, + "velocityY": -0.11284522116677391, + "timestamp": 2.6997663167373283 + }, + { + "x": 7.989865779876709, + "y": 0.665047347545624, + "heading": -4.586051220974463e-26, + "angularVelocity": 2.5919631848423095e-26, + "velocityX": -1.4783880366896059e-18, + "velocityY": 2.7911510207865606e-17, + "timestamp": 2.7781990812396673 + }, + { + "x": 7.961825021408631, + "y": 0.6653260998514925, + "heading": -3.8642001004622653e-17, + "angularVelocity": -5.466910762418722e-16, + "velocityX": -0.39670913756880827, + "velocityY": 0.003943673170767399, + "timestamp": 2.848882500935829 + }, + { + "x": 7.905743504974589, + "y": 0.6658836070474405, + "heading": -1.1420692112030604e-16, + "angularVelocity": -1.0690612857146226e-15, + "velocityX": -0.7934182680338816, + "velocityY": 0.007887382901887431, + "timestamp": 2.9195659206319906 + }, + { + "x": 7.821621231202305, + "y": 0.6667198723730994, + "heading": -2.271131022785688e-16, + "angularVelocity": -1.597350021728402e-15, + "velocityX": -1.190127389618255, + "velocityY": 0.011831138465963963, + "timestamp": 2.9902493403281523 + }, + { + "x": 7.709458200898961, + "y": 0.6678349000090077, + "heading": -3.725115104027876e-16, + "angularVelocity": -2.0570366115956903e-15, + "velocityX": -1.586836499782901, + "velocityY": 0.015774953174573097, + "timestamp": 3.060932760024314 + }, + { + "x": 7.569254415141015, + "y": 0.6692286955560068, + "heading": -5.529032952361946e-16, + "angularVelocity": -2.5521084387850704e-15, + "velocityX": -1.983545594718296, + "velocityY": 0.019718847121565148, + "timestamp": 3.1316161797204756 + }, + { + "x": 7.4010098754359035, + "y": 0.6709012669067533, + "heading": -7.630982661410159e-16, + "angularVelocity": -2.9737515734334796e-15, + "velocityX": -2.3802546683270744, + "velocityY": 0.02366285273059252, + "timestamp": 3.202299599416637 + }, + { + "x": 7.204724584045661, + "y": 0.6728526260129781, + "heading": -1.0096410558455573e-15, + "angularVelocity": -3.487985554177256e-15, + "velocityX": -2.7769637099335265, + "velocityY": 0.0276070274277582, + "timestamp": 3.272983019112799 + }, + { + "x": 6.980398544742687, + "y": 0.6750827930954573, + "heading": -1.423077463628514e-15, + "angularVelocity": -5.849127749576447e-15, + "velocityX": -3.173672698169647, + "velocityY": 0.03155148820008718, + "timestamp": 3.3436664388089605 + }, + { + "x": 6.7280317650805745, + "y": 0.6775918097291393, + "heading": -1.9851623705937264e-15, + "angularVelocity": -7.952145739517459e-15, + "velocityX": -3.5703815795405807, + "velocityY": 0.03549653715757366, + "timestamp": 3.414349858505122 + }, + { + "x": 6.447624267790984, + "y": 0.6803798097800222, + "heading": -6.968574786204946e-17, + "angularVelocity": 2.7099372702904392e-14, + "velocityX": -3.96709013931336, + "velocityY": 0.03944347999579872, + "timestamp": 3.485033278201284 + }, + { + "x": 6.158924341772469, + "y": 0.693943817311479, + "heading": 9.565698789607162e-18, + "angularVelocity": 1.1212169263035645e-15, + "velocityX": -4.084408016187073, + "velocityY": 0.19189800931673312, + "timestamp": 3.5557166978974455 + }, + { + "x": 5.872883545252504, + "y": 0.7353236661281272, + "heading": 6.282540250170779e-18, + "angularVelocity": -4.644877904443354e-17, + "velocityX": -4.046787743851827, + "velocityY": 0.58542511093175, + "timestamp": 3.626400117593607 + }, + { + "x": 5.592199247715388, + "y": 0.8042290602455044, + "heading": 2.5226905744349242e-17, + "angularVelocity": 2.6801710459780227e-16, + "velocityX": -3.971006195564076, + "velocityY": 0.9748452241499008, + "timestamp": 3.697083537289769 + }, + { + "x": 5.31951379776001, + "y": 0.9000114798545834, + "heading": 1.0213561793054143e-23, + "angularVelocity": -3.568997601327789e-16, + "velocityX": -3.8578417842194788, + "velocityY": 1.3550903453853254, + "timestamp": 3.7677669569859304 + }, + { + "x": 5.028358906099587, + "y": 1.039292217399747, + "heading": 1.939930717479169e-17, + "angularVelocity": 2.45765977450937e-16, + "velocityX": -3.688587353353505, + "velocityY": 1.7645218465846235, + "timestamp": 3.846700940044708 + }, + { + "x": 4.753982028496478, + "y": 1.2092558179345199, + "heading": 5.251717131242591e-17, + "angularVelocity": 4.1956390743611424e-16, + "velocityX": -3.476029803269934, + "velocityY": 2.1532373503589888, + "timestamp": 3.9256349231034857 + }, + { + "x": 4.499604125223824, + "y": 1.407906716987468, + "heading": 4.7661466331843347e-17, + "angularVelocity": -6.151615583577767e-17, + "velocityX": -3.222666504529982, + "velocityY": 2.5166714177468545, + "timestamp": 4.004568906162263 + }, + { + "x": 4.26534834796377, + "y": 1.629930428449429, + "heading": 6.383046336001005e-17, + "angularVelocity": 2.0484189779951308e-16, + "velocityX": -2.9677430199566506, + "velocityY": 2.8127772457221822, + "timestamp": 4.083502889221041 + }, + { + "x": 4.031326138540414, + "y": 1.852200317188001, + "heading": 2.3677757755478083e-17, + "angularVelocity": -5.086873150258546e-16, + "velocityX": -2.964783992333329, + "velocityY": 2.815896020002203, + "timestamp": 4.162436872279819 + }, + { + "x": 3.797312456863741, + "y": 2.0744791835519405, + "heading": 4.512669218284662e-17, + "angularVelocity": 2.717323991439214e-16, + "velocityX": -2.9646759558849944, + "velocityY": 2.816009755880509, + "timestamp": 4.241370855338596 + }, + { + "x": 3.563297360395116, + "y": 2.29675656041435, + "heading": 2.2571410949696857e-18, + "angularVelocity": -5.431063958209394e-16, + "velocityX": -2.964693879622175, + "velocityY": 2.815990885661535, + "timestamp": 4.320304838397374 + }, + { + "x": 3.335095888371744, + "y": 2.5135119589023023, + "heading": -5.89134026984256e-14, + "angularVelocity": -7.463915767059434e-13, + "velocityX": -2.8910421491511853, + "velocityY": 2.7460339652011654, + "timestamp": 4.3992388214561515 + }, + { + "x": 3.1322501152043865, + "y": 2.706183443191518, + "heading": -7.201295573540544e-14, + "angularVelocity": -1.6595581004068512e-13, + "velocityX": -2.569815500330909, + "velocityY": 2.440919320463041, + "timestamp": 4.478172804514929 + }, + { + "x": 2.9547600556706652, + "y": 2.8747709994570525, + "heading": -6.793308622264907e-14, + "angularVelocity": 5.168711075034906e-14, + "velocityX": -2.2485886642956747, + "velocityY": 2.1358045005792574, + "timestamp": 4.557106787573707 + }, + { + "x": 2.802625714692311, + "y": 3.019274623086207, + "heading": -5.6227190966394764e-14, + "angularVelocity": 1.482998177559053e-13, + "velocityX": -1.927361765907952, + "velocityY": 1.830689622258048, + "timestamp": 4.636040770632484 + }, + { + "x": 2.6758470947294035, + "y": 3.1396943117718017, + "heading": -4.1672293686354096e-14, + "angularVelocity": 1.843932964768755e-13, + "velocityX": -1.6061348363539316, + "velocityY": 1.5255747147076162, + "timestamp": 4.714974753691262 + }, + { + "x": 2.57442419725774, + "y": 3.236030064129265, + "heading": -2.7119123795716724e-14, + "angularVelocity": 1.8437141252050512e-13, + "velocityX": -1.2849078881033131, + "velocityY": 1.2204597896162928, + "timestamp": 4.79390873675004 + }, + { + "x": 2.498357023261081, + "y": 3.308281879235439, + "heading": -1.4470412940019133e-14, + "angularVelocity": 1.6024417392647582e-13, + "velocityX": -0.9636809273896104, + "velocityY": 0.9153448528296537, + "timestamp": 4.872842719808817 + }, + { + "x": 2.4476455734420637, + "y": 3.356449756430872, + "heading": -5.093331671182619e-15, + "angularVelocity": 1.1879650495017822e-13, + "velocityX": -0.6424539577743393, + "velocityY": 0.6102299076885483, + "timestamp": 4.951776702867595 + }, + { + "x": 2.422289848327637, + "y": 3.3805336952209477, + "heading": 8.600144701827338e-26, + "angularVelocity": 6.45264751057182e-14, + "velocityX": -0.3212269814832313, + "velocityY": 0.30511495628123525, + "timestamp": 5.030710685926373 + }, + { + "x": 2.422289848327637, + "y": 3.3805336952209477, + "heading": 4.234321675050891e-26, + "angularVelocity": -1.6322812413801202e-26, + "velocityX": 1.82145222049907e-18, + "velocityY": 1.2398127508278904e-17, + "timestamp": 5.10964466898515 + }, + { + "x": 2.4422092097338126, + "y": 3.359397040789022, + "heading": 1.4828318603888107e-18, + "angularVelocity": 2.061358359803718e-17, + "velocityX": 0.2769090563806016, + "velocityY": -0.29383125866924514, + "timestamp": 5.181579338318127 + }, + { + "x": 2.4821232633957244, + "y": 3.317194979333885, + "heading": -2.399084815386753e-18, + "angularVelocity": -5.3964448898673995e-17, + "velocityX": 0.5548653247699834, + "velocityY": -0.5866720712936807, + "timestamp": 5.253514007651103 + }, + { + "x": 2.5421257730353313, + "y": 3.2540169079394587, + "heading": -4.312212643467297e-18, + "angularVelocity": -2.6595363961942612e-17, + "velocityX": 0.8341250497936068, + "velocityY": -0.878270130114354, + "timestamp": 5.325448676984079 + }, + { + "x": 2.6223366317399224, + "y": 3.169978308273709, + "heading": -6.87359010796808e-18, + "angularVelocity": -3.560699654803604e-17, + "velocityX": 1.1150514688942472, + "velocityY": -1.1682628202156935, + "timestamp": 5.3973833463170555 + }, + { + "x": 2.722914524907397, + "y": 3.0652340847546355, + "heading": -1.0050987029108054e-17, + "angularVelocity": -4.417054911796212e-17, + "velocityX": 1.3981838534826794, + "velocityY": -1.4561021061315331, + "timestamp": 5.469318015650032 + }, + { + "x": 2.844079320063175, + "y": 2.94000283277744, + "heading": -1.3800277943608368e-17, + "angularVelocity": -5.212076319266894e-17, + "velocityX": 1.6843727270700168, + "velocityY": -1.7409025875701625, + "timestamp": 5.541252684983008 + }, + { + "x": 2.9861556801651536, + "y": 2.7946160556206303, + "heading": -1.8054024423421994e-17, + "angularVelocity": -5.913346837366306e-17, + "velocityX": 1.9750749036440556, + "velocityY": -2.0210946752866197, + "timestamp": 5.6131873543159845 + }, + { + "x": 3.1496705290943545, + "y": 2.629635153477409, + "heading": -1.9755148692229212e-17, + "angularVelocity": -2.3648200854179725e-17, + "velocityX": 2.2731021139742653, + "velocityY": -2.2934824566998935, + "timestamp": 5.685122023648961 + }, + { + "x": 3.3356217659937375, + "y": 2.446211186910791, + "heading": -2.1620491181128583e-17, + "angularVelocity": -2.5931074864358486e-17, + "velocityX": 2.5850016219372725, + "velocityY": -2.5498687665852504, + "timestamp": 5.757056692981937 + }, + { + "x": 3.546522888895758, + "y": 2.247919800418475, + "heading": -2.298290975372596e-17, + "angularVelocity": -1.8939700380288258e-17, + "velocityX": 2.931842529590256, + "velocityY": -2.756548244821444, + "timestamp": 5.828991362314913 + }, + { + "x": 3.779551655861922, + "y": 2.0684406150868386, + "heading": -2.356725503120944e-17, + "angularVelocity": -8.123277452624726e-18, + "velocityX": 3.2394500333071203, + "velocityY": -2.495030379582669, + "timestamp": 5.90092603164789 + }, + { + "x": 4.029145048692618, + "y": 1.912818243294336, + "heading": -7.243904171356102e-18, + "angularVelocity": 2.2691910606172613e-16, + "velocityX": 3.4697232244967258, + "velocityY": -2.163384821739647, + "timestamp": 5.972860700980866 + }, + { + "x": 4.292869509297096, + "y": 1.7825700534328885, + "heading": 2.7690024039408224e-18, + "angularVelocity": 1.391944477977289e-16, + "velocityX": 3.666166301315592, + "velocityY": -1.8106455630985843, + "timestamp": 6.044795370313842 + }, + { + "x": 4.568153688292314, + "y": 1.6789660022880506, + "heading": 1.6169391405873456e-17, + "angularVelocity": 1.8628554390059454e-16, + "velocityX": 3.8268637577418185, + "velocityY": -1.4402519967735667, + "timestamp": 6.116730039646819 + }, + { + "x": 4.85231351852417, + "y": 1.6030162572860713, + "heading": 1.104418666333468e-28, + "angularVelocity": -2.2477883829523805e-16, + "velocityX": 3.9502486473765894, + "velocityY": -1.055815585244564, + "timestamp": 6.188664708979795 + }, + { + "x": 5.158309378699811, + "y": 1.5545385083826373, + "heading": -1.1441689912277495e-17, + "angularVelocity": -1.5100789546254797e-16, + "velocityX": 4.038545985692146, + "velocityY": -0.6398113298648395, + "timestamp": 6.264433528069543 + }, + { + "x": 5.467685385105672, + "y": 1.5381052997754503, + "heading": 2.911311456658326e-17, + "angularVelocity": 5.352439824969174e-16, + "velocityX": 4.083157295078998, + "velocityY": -0.21688616511744238, + "timestamp": 6.340202347159291 + }, + { + "x": 5.777094938781223, + "y": 1.553894378826709, + "heading": -2.420664700580125e-18, + "angularVelocity": -4.1618411961572785e-16, + "velocityX": 4.083600053329194, + "velocityY": 0.2083849166488678, + "timestamp": 6.415971166249039 + }, + { + "x": 6.083191086572635, + "y": 1.6017349192235426, + "heading": -5.882315170795842e-18, + "angularVelocity": -4.568700570848447e-17, + "velocityX": 4.039869585784915, + "velocityY": 0.6314014256909969, + "timestamp": 6.491739985338787 + }, + { + "x": 6.382662742491119, + "y": 1.6811093330008764, + "heading": -1.6858060845261797e-17, + "angularVelocity": -1.4485834418926946e-16, + "velocityX": 3.9524392687700143, + "velocityY": 1.0475867874182512, + "timestamp": 6.5675088044285355 + }, + { + "x": 6.672275412037942, + "y": 1.7911460204151195, + "heading": -2.888480028332242e-17, + "angularVelocity": -1.587294032373541e-16, + "velocityX": 3.8223199599175173, + "velocityY": 1.452268739806397, + "timestamp": 6.6432776235182835 + }, + { + "x": 6.935773457086466, + "y": 1.9122631966744312, + "heading": -2.1691436993771164e-17, + "angularVelocity": 9.493832977551161e-17, + "velocityX": 3.4776580684956064, + "velocityY": 1.5985094886787496, + "timestamp": 6.719046442608032 + }, + { + "x": 7.170007295221654, + "y": 2.0198939541094463, + "heading": -1.6299807800034943e-17, + "angularVelocity": 7.115894340654352e-17, + "velocityX": 3.0914278584382107, + "velocityY": 1.4205151766725412, + "timestamp": 6.79481526169778 + }, + { + "x": 7.374967182869676, + "y": 2.1140594081505015, + "heading": -1.0901160029750536e-17, + "angularVelocity": 7.125157266111349e-17, + "velocityX": 2.7050690522873455, + "velocityY": 1.2427995469958841, + "timestamp": 6.870584080787528 + }, + { + "x": 7.550649876686179, + "y": 2.1947665963601173, + "heading": -8.565573749391565e-18, + "angularVelocity": 3.0825158228438804e-17, + "velocityX": 2.318667440342544, + "velocityY": 1.0651767993626458, + "timestamp": 6.946352899877276 + }, + { + "x": 7.69705375584567, + "y": 2.26201903734599, + "heading": -6.268442595471868e-18, + "angularVelocity": 3.0317623225845786e-17, + "velocityX": 1.932244436675357, + "velocityY": 0.8876004904604238, + "timestamp": 7.022121718967024 + }, + { + "x": 7.814177848123867, + "y": 2.315818842216871, + "heading": -5.860377826964177e-18, + "angularVelocity": 5.385632685743263e-18, + "velocityX": 1.5458086015515728, + "velocityY": 0.7100520440594869, + "timestamp": 7.097890538056772 + }, + { + "x": 7.902021505484193, + "y": 2.3561674183552324, + "heading": -2.0084875089379725e-18, + "angularVelocity": 5.083743950397372e-17, + "velocityX": 1.1593642136140294, + "velocityY": 0.5325221723510412, + "timestamp": 7.17365935714652 + }, + { + "x": 7.960584265098705, + "y": 2.3830657710227903, + "heading": -5.354322583341586e-20, + "angularVelocity": 2.5801441111297213e-17, + "velocityX": 0.772913717254023, + "velocityY": 0.35500556812864675, + "timestamp": 7.249428176236268 + }, + { + "x": 7.989865779876708, + "y": 2.3965146541595463, + "heading": 4.464057669964841e-25, + "angularVelocity": 7.066509963979535e-19, + "velocityX": 0.3864586399765603, + "velocityY": 0.17749891443779842, + "timestamp": 7.325196995326016 + }, + { + "x": 7.989865779876708, + "y": 2.3965146541595463, + "heading": 4.577698718092902e-25, + "angularVelocity": 1.4924495313995464e-25, + "velocityX": 1.7316501925858062e-16, + "velocityY": 2.4957312477014177e-17, + "timestamp": 7.400965814415764 + }, + { + "x": 7.961948125314342, + "y": 2.3869413231673744, + "heading": 3.968152450731581e-15, + "angularVelocity": 5.4722575152696526e-14, + "velocityX": -0.38499664874452716, + "velocityY": -0.1320204152917069, + "timestamp": 7.473479834092087 + }, + { + "x": 7.9061128172497455, + "y": 2.3677946595682435, + "heading": 1.1360635138080996e-14, + "angularVelocity": 1.0194560175538186e-13, + "velocityX": -0.7699932828692636, + "velocityY": -0.26404085285209106, + "timestamp": 7.54599385376841 + }, + { + "x": 7.822359857005323, + "y": 2.3390746613517033, + "heading": 2.1504602727030266e-14, + "angularVelocity": 1.3988979395557398e-13, + "velocityX": -1.154989898757494, + "velocityY": -0.3960613181375172, + "timestamp": 7.618507873444733 + }, + { + "x": 7.71068924627678, + "y": 2.300781325945998, + "heading": 3.351912896052837e-14, + "angularVelocity": 1.6568562398720052e-13, + "velocityX": -1.539986491261158, + "velocityY": -0.5280818188886689, + "timestamp": 7.691021893121055 + }, + { + "x": 7.571100987317153, + "y": 2.2529146499450325, + "heading": 4.62437330221949e-14, + "angularVelocity": 1.754779109699489e-13, + "velocityX": -1.9249830526944358, + "velocityY": -0.6601023666114509, + "timestamp": 7.763535912797378 + }, + { + "x": 7.4035950832655315, + "y": 2.195474628624213, + "heading": 5.80518545778972e-14, + "angularVelocity": 1.628392124731043e-13, + "velocityX": -2.30997957083824, + "velocityY": -0.7921229794885999, + "timestamp": 7.836049932473701 + }, + { + "x": 7.20817153879745, + "y": 2.1284612549925805, + "heading": 6.651768983836094e-14, + "angularVelocity": 1.1674760820877797e-13, + "velocityX": -2.6949760245043035, + "velocityY": -0.9241436887743528, + "timestamp": 7.908563952150024 + }, + { + "x": 6.9848303616174565, + "y": 2.0518745176536126, + "heading": 6.760398615570593e-14, + "angularVelocity": 1.498050653761286e-14, + "velocityX": -3.0799723719211687, + "velocityY": -1.0561645552234988, + "timestamp": 7.9810779718263465 + }, + { + "x": 6.733571566806331, + "y": 1.965714394765822, + "heading": 5.327081908950384e-14, + "angularVelocity": -1.9766070480015549e-13, + "velocityX": -3.464968511367217, + "velocityY": -1.188185722877021, + "timestamp": 8.05359199150267 + }, + { + "x": 6.454395197014029, + "y": 1.8699808283188497, + "heading": -1.210358626215953e-16, + "angularVelocity": -7.362972657145883e-13, + "velocityX": -3.849964062651033, + "velocityY": -1.3202076905209181, + "timestamp": 8.126106011178992 + }, + { + "x": 6.1738479950442935, + "y": 1.7740244527442883, + "heading": -1.1104225188815767e-16, + "angularVelocity": 1.3781627642234675e-16, + "velocityX": -3.868868437050859, + "velocityY": -1.3232803257055905, + "timestamp": 8.198620030855315 + }, + { + "x": 5.89229592505603, + "y": 1.6810578437397081, + "heading": -1.3815766244496281e-16, + "angularVelocity": -3.7393339475245444e-16, + "velocityX": -3.882726005881283, + "velocityY": -1.2820501389883114, + "timestamp": 8.271134050531638 + }, + { + "x": 5.603047274828762, + "y": 1.615869067560432, + "heading": -1.375103171712517e-16, + "angularVelocity": 8.927167451460251e-18, + "velocityX": -3.9888652086644605, + "velocityY": -0.8989816930592814, + "timestamp": 8.34364807020796 + }, + { + "x": 5.3087508481141255, + "y": 1.5797585348826493, + "heading": -5.115064325941328e-17, + "angularVelocity": 1.1909376120637012e-15, + "velocityX": -4.058476250913794, + "velocityY": -0.49798001598624214, + "timestamp": 8.416162089884283 + }, + { + "x": 5.012322426011832, + "y": 1.5730842246634098, + "heading": -1.6843428292277085e-17, + "angularVelocity": 4.731114706232749e-16, + "velocityX": -4.087877398404444, + "velocityY": -0.09204165275748097, + "timestamp": 8.488676109560606 + }, + { + "x": 4.716698964119128, + "y": 1.5959123273167983, + "heading": 1.0780704155043204e-17, + "angularVelocity": 3.8094884400946135e-16, + "velocityX": -4.076776645568003, + "velocityY": 0.3148095051884876, + "timestamp": 8.561190129236929 + }, + { + "x": 4.424809455871582, + "y": 1.6480166912078853, + "heading": -4.7825786100317035e-25, + "angularVelocity": -1.4867063166908422e-16, + "velocityX": -4.025283794091286, + "velocityY": 0.7185419333227538, + "timestamp": 8.633704148913251 + }, + { + "x": 4.128740207867581, + "y": 1.7331899442926113, + "heading": 2.436084377884843e-17, + "angularVelocity": 3.233261816100211e-16, + "velocityX": -3.929541129144622, + "velocityY": 1.1304510797896916, + "timestamp": 8.709048632727956 + }, + { + "x": 3.843051522185569, + "y": 1.8484872151278047, + "heading": 4.0057524127252557e-17, + "angularVelocity": 2.0833224449518998e-16, + "velocityX": -3.7917664468268093, + "velocityY": 1.5302682425770875, + "timestamp": 8.784393116542661 + }, + { + "x": 3.5707992295594733, + "y": 1.992675206864091, + "heading": 4.3264327263683456e-17, + "angularVelocity": 4.256188342181214e-17, + "velocityX": -3.6134336429424168, + "velocityY": 1.9137166310769615, + "timestamp": 8.859737600357366 + }, + { + "x": 3.314895397994771, + "y": 2.1642115529138155, + "heading": 5.0710064414841386e-17, + "angularVelocity": 9.88225915723796e-17, + "velocityX": -3.39645078987007, + "velocityY": 2.2766941568200596, + "timestamp": 8.935082084172072 + }, + { + "x": 3.078073502264168, + "y": 2.361256926788227, + "heading": 7.635756849280407e-17, + "angularVelocity": 3.4040320908383547e-16, + "velocityX": -3.143188243401714, + "velocityY": 2.615259457584993, + "timestamp": 9.010426567986777 + }, + { + "x": 2.854646488956178, + "y": 2.573369772957938, + "heading": 6.019554858067844e-17, + "angularVelocity": -2.1450833681506433e-16, + "velocityX": -2.9654063840756333, + "velocityY": 2.815240551521766, + "timestamp": 9.085771051801482 + }, + { + "x": 2.6466740813199228, + "y": 2.7708029552314177, + "heading": 4.0534261647582457e-17, + "angularVelocity": -2.6095184862579493e-16, + "velocityX": -2.7602871120227874, + "velocityY": 2.620406594855673, + "timestamp": 9.161115535616187 + }, + { + "x": 2.4618096495885284, + "y": 2.946299083382676, + "heading": 1.8651869909581232e-17, + "angularVelocity": -2.9043121070808416e-16, + "velocityX": -2.4535894649644563, + "velocityY": 2.329249857008171, + "timestamp": 9.236460019430892 + }, + { + "x": 2.3000532444302113, + "y": 3.0998581821870363, + "heading": 1.2585166550915731e-17, + "angularVelocity": -8.051961119619559e-17, + "velocityX": -2.1468911454186292, + "velocityY": 2.0380934479880564, + "timestamp": 9.311804503245597 + }, + { + "x": 2.1614048826816776, + "y": 3.2314802598444294, + "heading": 1.044220916189497e-17, + "angularVelocity": -2.8442168781856105e-17, + "velocityX": -1.8401926024097508, + "velocityY": 1.7469371478004907, + "timestamp": 9.387148987060302 + }, + { + "x": 2.04586457275109, + "y": 3.341165320443523, + "heading": 6.144588170756101e-18, + "angularVelocity": -5.703966005365595e-17, + "velocityX": -1.5334939478045977, + "velocityY": 1.4557809018792693, + "timestamp": 9.462493470875007 + }, + { + "x": 1.9534323196800467, + "y": 3.4289133664338625, + "heading": 8.956175204831333e-18, + "angularVelocity": 3.7316381823207725e-17, + "velocityX": -1.2267952262854738, + "velocityY": 1.1646246884693368, + "timestamp": 9.537837954689712 + }, + { + "x": 1.8841081268282298, + "y": 3.4947243994469472, + "heading": 8.125508496541916e-18, + "angularVelocity": -1.1024919871707434e-17, + "velocityX": -0.920096460175393, + "velocityY": 0.8734684967132663, + "timestamp": 9.613182438504417 + }, + { + "x": 1.8378919965947327, + "y": 3.538598420647381, + "heading": 4.6134522575515235e-18, + "angularVelocity": -4.661329437022688e-17, + "velocityX": -0.6133976622236478, + "velocityY": 0.5823123204142531, + "timestamp": 9.688526922319122 + }, + { + "x": 1.8147839307785034, + "y": 3.5605354309082036, + "heading": 2.574108918698912e-28, + "angularVelocity": -6.123143521392433e-17, + "velocityX": -0.3066988403955926, + "velocityY": 0.2911561557025646, + "timestamp": 9.763871406133827 + }, + { + "x": 1.8147839307785032, + "y": 3.560535430908203, + "heading": 1.6039214960839928e-28, + "angularVelocity": -4.957933854028128e-29, + "velocityX": -1.2239411035355163e-16, + "velocityY": -5.776889799901916e-17, + "timestamp": 9.839215889948532 + }, + { + "x": 1.8499405192639373, + "y": 3.595692011943056, + "heading": 7.674497632021469e-18, + "angularVelocity": 8.154121514620013e-17, + "velocityX": 0.3735372767099104, + "velocityY": 0.37353719754779685, + "timestamp": 9.93333390889189 + }, + { + "x": 1.9202536945231747, + "y": 3.666005172301131, + "heading": 1.2913126269895033e-17, + "angularVelocity": 5.566020934800391e-17, + "velocityX": 0.7470745352338143, + "velocityY": 0.7470743769095909, + "timestamp": 10.02745192783525 + }, + { + "x": 2.02572345256241, + "y": 3.771474907988624, + "heading": 1.5083295615484505e-17, + "angularVelocity": 2.3057958187881123e-17, + "velocityX": 1.1206117513237068, + "velocityY": 1.1206115138373827, + "timestamp": 10.121569946778608 + }, + { + "x": 2.1663497734126267, + "y": 3.912101199036522, + "heading": 1.7930576325828894e-17, + "angularVelocity": 3.0252238011837634e-17, + "velocityX": 1.4941487552436352, + "velocityY": 1.494148438595255, + "timestamp": 10.215687965721967 + }, + { + "x": 2.2718195314518623, + "y": 4.017570934724015, + "heading": 1.2801642358185404e-17, + "angularVelocity": -5.44947080834505e-17, + "velocityX": 1.120611751323707, + "velocityY": 1.1206115138373829, + "timestamp": 10.309805984665326 + }, + { + "x": 2.3421327067110993, + "y": 4.0878840950820905, + "heading": 5.291529814049623e-18, + "angularVelocity": -7.979463049177405e-17, + "velocityX": 0.7470745352338144, + "velocityY": 0.747074376909591, + "timestamp": 10.403924003608685 + }, + { + "x": 2.377289295196533, + "y": 4.123040676116943, + "heading": -1.3564749018286617e-28, + "angularVelocity": -5.62222821268349e-17, + "velocityX": 0.37353727670991055, + "velocityY": 0.3735371975477969, + "timestamp": 10.498042022552044 + }, + { + "x": 2.377289295196533, + "y": 4.123040676116943, + "heading": -6.710895051043041e-29, + "angularVelocity": 1.5189324842732185e-29, + "velocityX": -2.8617362878477276e-23, + "velocityY": -2.866266175914819e-23, + "timestamp": 10.592160041495402 + } + ], + "constraints": [ + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 9 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" + }, + { + "scope": [ + 8 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + "first" + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4NM-Sweep": { + "waypoints": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 1.6146607398986816, + "y": 4.286937236785889, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 8 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 1.7022826671600342, + "y": 5.068050384521484, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "x": 2.6223177909851074, + "y": 5.706172466278076, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 9 + }, + { + "x": 1.7022826671600342, + "y": 6.463063716888428, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 9 + }, + { + "x": 2.6223177909851074, + "y": 7.111215591430664, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.1708032823161308e-29, + "angularVelocity": -9.137601985120096e-30, + "velocityX": -1.7245265837493291e-19, + "velocityY": 3.781835438274059e-19, + "timestamp": 0 + }, + { + "x": 1.2920113917886342, + "y": 5.554330110103596, + "heading": -9.481227429886463e-19, + "angularVelocity": -1.3571377455648403e-17, + "velocityX": -0.07539684918457451, + "velocityY": -0.44695930001512374, + "timestamp": 0.06986193895423295 + }, + { + "x": 1.282761551546609, + "y": 5.491689489432592, + "heading": -3.571081438475658e-18, + "angularVelocity": -3.754488861622517e-17, + "velocityX": -0.13240171086698216, + "velocityY": -0.896634442282502, + "timestamp": 0.1397238779084659 + }, + { + "x": 1.271458199578559, + "y": 5.397448925207257, + "heading": -6.053079981598685e-18, + "angularVelocity": -3.552719270340507e-17, + "velocityX": -0.16179556618740504, + "velocityY": -1.3489543181312649, + "timestamp": 0.20958581686269886 + }, + { + "x": 1.2612678627955922, + "y": 5.271561332803293, + "heading": -9.412252485751232e-18, + "angularVelocity": -4.8083012570774046e-17, + "velocityX": -0.14586392727580277, + "velocityY": -1.8019481607350594, + "timestamp": 0.2794477558169318 + }, + { + "x": 1.2580444533875073, + "y": 5.114783048267347, + "heading": -4.514723077948285e-18, + "angularVelocity": 7.010296729390708e-17, + "velocityX": -0.04613970720446646, + "velocityY": -2.2441158502436007, + "timestamp": 0.3493096947711648 + }, + { + "x": 1.2734562507703713, + "y": 4.932401999052926, + "heading": -1.9192225006314126e-18, + "angularVelocity": 3.715185125877048e-17, + "velocityX": 0.22060363072603112, + "velocityY": -2.610592433369192, + "timestamp": 0.4191716337253978 + }, + { + "x": 1.3200722448212343, + "y": 4.744629254191582, + "heading": 4.5333995240179614e-18, + "angularVelocity": 9.236247783325738e-17, + "velocityX": 0.6672588071367644, + "velocityY": -2.6877688720371014, + "timestamp": 0.48903357267963077 + }, + { + "x": 1.3953620513844092, + "y": 4.570294708742183, + "heading": 7.671914334286196e-18, + "angularVelocity": 4.492452874736247e-17, + "velocityX": 1.077694202167754, + "velocityY": -2.49541521548101, + "timestamp": 0.5588955116338638 + }, + { + "x": 1.4945697873897588, + "y": 4.416713614347291, + "heading": 1.2373124312426076e-17, + "angularVelocity": 6.729286327198698e-17, + "velocityX": 1.4200541452240707, + "velocityY": -2.198351444203463, + "timestamp": 0.6287574505880967 + }, + { + "x": 1.6146607398986816, + "y": 4.286937236785889, + "heading": 1.5881178098148638e-17, + "angularVelocity": 5.0214090683168825e-17, + "velocityX": 1.7189753720920211, + "velocityY": -1.8576120202792974, + "timestamp": 0.6986193895423297 + }, + { + "x": 1.7770335829472077, + "y": 4.171588935482394, + "heading": 1.388382460128691e-17, + "angularVelocity": -2.4968323513493456e-17, + "velocityX": 2.0297748281421804, + "velocityY": -1.4419349569732145, + "timestamp": 0.7786148850018645 + }, + { + "x": 1.9580373267940145, + "y": 4.0933452292031935, + "heading": 1.0101249557116561e-17, + "angularVelocity": -4.728484899999743e-17, + "velocityX": 2.262674201928864, + "velocityY": -0.9781014022068167, + "timestamp": 0.8586103804613993 + }, + { + "x": 2.1432715777255, + "y": 4.056404838041411, + "heading": 5.258068055729367e-18, + "angularVelocity": -6.054317546655827e-17, + "velocityX": 2.3155585182316285, + "velocityY": -0.4617808909061482, + "timestamp": 0.938605875920934 + }, + { + "x": 2.30556761109126, + "y": 4.0540722338730015, + "heading": 1.7344191572744589e-18, + "angularVelocity": -4.404808962124233e-17, + "velocityX": 2.0288146530432702, + "velocityY": -0.029159193964731635, + "timestamp": 1.0186013713804687 + }, + { + "x": 2.4288986578849183, + "y": 4.066078072388016, + "heading": -3.0892891018139647e-18, + "angularVelocity": -6.029974658902888e-17, + "velocityX": 1.5417248944478947, + "velocityY": 0.15008143203621216, + "timestamp": 1.0985968668400035 + }, + { + "x": 2.5107392491251295, + "y": 4.079634262670543, + "heading": -1.4571920319979493e-18, + "angularVelocity": 2.040236208636831e-17, + "velocityX": 1.0230649959735494, + "velocityY": 0.16946192038255684, + "timestamp": 1.1785923622995382 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -2.1781827010886517e-29, + "angularVelocity": 1.821592588016304e-17, + "velocityX": 0.5077389399960676, + "velocityY": 0.1076332542399146, + "timestamp": 1.258587857759073 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -8.20915977294843e-30, + "angularVelocity": 2.3308941769124188e-29, + "velocityX": -1.0859852689087957e-19, + "velocityY": -3.0453945703275425e-19, + "timestamp": 1.3385833532186078 + }, + { + "x": 2.509912633384866, + "y": 4.106948680299614, + "heading": -1.0058018540616292e-19, + "angularVelocity": -1.2014784070586274e-18, + "velocityX": -0.4950617653324921, + "velocityY": 0.22343112145323105, + "timestamp": 1.422297036462786 + }, + { + "x": 2.4277731042980624, + "y": 4.145931881900096, + "heading": -3.539291451520145e-19, + "angularVelocity": -3.0263744487771125e-18, + "velocityX": -0.9811959754203724, + "velocityY": 0.46567299502012466, + "timestamp": 1.5060107197069643 + }, + { + "x": 2.3064032345030743, + "y": 4.2079017677417365, + "heading": -8.502057746481102e-19, + "angularVelocity": -5.928261581023188e-18, + "velocityX": -1.4498211653281692, + "velocityY": 0.7402599364895339, + "timestamp": 1.5897244029511426 + }, + { + "x": 2.149653812858836, + "y": 4.298432011058232, + "heading": -1.775104690299589e-18, + "angularVelocity": -1.1048360705497396e-17, + "velocityX": -1.8724468398675924, + "velocityY": 1.081427071515116, + "timestamp": 1.6734380861953209 + }, + { + "x": 1.9757267559424647, + "y": 4.431061350920483, + "heading": 3.435416387971875e-18, + "angularVelocity": 6.224216507177934e-17, + "velocityX": -2.0776419120044674, + "velocityY": 1.5843209224875967, + "timestamp": 1.7571517694394991 + }, + { + "x": 1.8394339825531039, + "y": 4.5892066822176, + "heading": 1.6609097248007948e-18, + "angularVelocity": -2.1197330678659494e-17, + "velocityX": -1.6280823887753026, + "velocityY": 1.8891216485581488, + "timestamp": 1.8408654526836774 + }, + { + "x": 1.74837798651176, + "y": 4.751939027202194, + "heading": 3.07643951923686e-18, + "angularVelocity": 1.6909179328924933e-17, + "velocityX": -1.0877074393650743, + "velocityY": 1.9439157217576093, + "timestamp": 1.9245791359278557 + }, + { + "x": 1.7027420711484422, + "y": 4.9125681460047455, + "heading": 3.455775740084333e-18, + "angularVelocity": 4.531351906133862e-18, + "velocityX": -0.5451428439745697, + "velocityY": 1.918791678703507, + "timestamp": 2.008292819172034 + }, + { + "x": 1.7022826671600342, + "y": 5.068050384521484, + "heading": 3.498529240472229e-18, + "angularVelocity": 5.107104898810256e-19, + "velocityX": -0.005487800447964233, + "velocityY": 1.8573097311131817, + "timestamp": 2.0920065024162127 + }, + { + "x": 1.748998056779968, + "y": 5.2201514780459135, + "heading": 3.2452497925690024e-18, + "angularVelocity": -2.9529105301285464e-18, + "velocityX": 0.5446409206134261, + "velocityY": 1.7733016951678244, + "timestamp": 2.177779326036073 + }, + { + "x": 1.842485607000515, + "y": 5.362722985501461, + "heading": 2.572181733343048e-18, + "angularVelocity": -7.847101115669858e-18, + "velocityX": 1.089943717311651, + "velocityY": 1.6621990677072165, + "timestamp": 2.263552149655933 + }, + { + "x": 1.9816206378320913, + "y": 5.49133902203876, + "heading": 1.2483858256210483e-18, + "angularVelocity": -1.5433744368348303e-17, + "velocityX": 1.6221342024160712, + "velocityY": 1.4994963568801014, + "timestamp": 2.3493249732757935 + }, + { + "x": 2.1614899424975276, + "y": 5.595072018302606, + "heading": 8.735519607318585e-19, + "angularVelocity": -4.370076935236176e-18, + "velocityX": 2.097043061828134, + "velocityY": 1.2093923446380141, + "timestamp": 2.4350977968956538 + }, + { + "x": 2.3416630615403307, + "y": 5.651072922830644, + "heading": -1.580597953419591e-18, + "angularVelocity": -2.8612206866525546e-17, + "velocityX": 2.1005851438600054, + "velocityY": 0.6528979945469758, + "timestamp": 2.520870620515514 + }, + { + "x": 2.481211015710272, + "y": 5.682013046713306, + "heading": -5.273562689120438e-18, + "angularVelocity": -4.305518156048475e-17, + "velocityX": 1.626948353576521, + "velocityY": 0.36072175984069416, + "timestamp": 2.6066434441353743 + }, + { + "x": 2.5751424394052407, + "y": 5.698896862460529, + "heading": -3.238292974805406e-18, + "angularVelocity": 2.372860764105119e-17, + "velocityX": 1.095118706960925, + "velocityY": 0.1968434177012898, + "timestamp": 2.6924162677552346 + }, + { + "x": 2.6223177909851074, + "y": 5.706172466278076, + "heading": -1.6431536638622417e-29, + "angularVelocity": 3.7754299539056475e-17, + "velocityX": 0.5500034811602417, + "velocityY": 0.08482411456794345, + "timestamp": 2.778189091375095 + }, + { + "x": 2.6223177909851074, + "y": 5.706172466278076, + "heading": -1.2685688932181852e-29, + "angularVelocity": 2.8790313162701036e-30, + "velocityX": 1.0395697262890221e-19, + "velocityY": -8.557247579898581e-19, + "timestamp": 2.863961914994955 + }, + { + "x": 2.580562376655811, + "y": 5.717941500249003, + "heading": -2.5900187158353315e-19, + "angularVelocity": -3.1674270516013714e-18, + "velocityX": -0.5106420139476558, + "velocityY": 0.14392775896647922, + "timestamp": 2.945732339654092 + }, + { + "x": 2.4974776849678753, + "y": 5.742898235730052, + "heading": -7.872006902584448e-19, + "angularVelocity": -6.459533342935218e-18, + "velocityX": -1.016072645266023, + "velocityY": 0.30520491467523164, + "timestamp": 3.027502764313229 + }, + { + "x": 2.373951012372342, + "y": 5.783554351890171, + "heading": -1.601704058013041e-18, + "angularVelocity": -9.960854638783481e-18, + "velocityX": -1.5106522084293756, + "velocityY": 0.49719830035854506, + "timestamp": 3.1092731889723657 + }, + { + "x": 2.212584981311779, + "y": 5.845428543659977, + "heading": -2.553510477545141e-18, + "angularVelocity": -1.1639982927649009e-17, + "velocityX": -1.9734033635415664, + "velocityY": 0.7566817957437639, + "timestamp": 3.1910436136315026 + }, + { + "x": 2.029785893220335, + "y": 5.945020714468991, + "heading": -3.86959228607625e-18, + "angularVelocity": -1.6094840727170316e-17, + "velocityX": -2.235515944223699, + "velocityY": 1.2179485581023628, + "timestamp": 3.2728140382906394 + }, + { + "x": 1.8832983601749962, + "y": 6.0683508997995, + "heading": -5.262396045953343e-18, + "angularVelocity": -1.7033099337403357e-17, + "velocityX": -1.7914488478686192, + "velocityY": 1.5082492948350907, + "timestamp": 3.3545844629497763 + }, + { + "x": 1.7796069658898477, + "y": 6.198788435444483, + "heading": -6.5612261048200405e-18, + "angularVelocity": -1.5883860411297538e-17, + "velocityX": -1.2680794396921602, + "velocityY": 1.5951676439093452, + "timestamp": 3.436354887608913 + }, + { + "x": 1.7192548891917947, + "y": 6.3311567689867, + "heading": -7.717140781068942e-18, + "angularVelocity": -1.4136096934392038e-17, + "velocityX": -0.7380673018347763, + "velocityY": 1.6187800674143442, + "timestamp": 3.51812531226805 + }, + { + "x": 1.7022826671600342, + "y": 6.463063716888428, + "heading": -8.916494739837526e-18, + "angularVelocity": -1.4667331539141363e-17, + "velocityX": -0.207559421422963, + "velocityY": 1.613137616095136, + "timestamp": 3.599895736927187 + }, + { + "x": 1.7347406171289421, + "y": 6.603778103628165, + "heading": -1.000751929260003e-17, + "angularVelocity": -1.231976965868251e-17, + "velocityX": 0.36651280374474055, + "velocityY": 1.5889365921324623, + "timestamp": 3.688454580267114 + }, + { + "x": 1.8179248925163114, + "y": 6.740485682269317, + "heading": -1.0810197858207777e-17, + "angularVelocity": -9.063787776490836e-18, + "velocityX": 0.9393107706711105, + "velocityY": 1.5436920073177693, + "timestamp": 3.777013423607041 + }, + { + "x": 1.9514234110036923, + "y": 6.869597791652038, + "heading": -1.3656868184138031e-17, + "angularVelocity": -3.21443923528337e-17, + "velocityX": 1.5074555341125644, + "velocityY": 1.4579245224232664, + "timestamp": 3.8655722669469683 + }, + { + "x": 2.132885886706102, + "y": 6.981719550529973, + "heading": -1.737159453771359e-17, + "angularVelocity": -4.194641742998263e-17, + "velocityX": 2.049061040757697, + "velocityY": 1.266070723705848, + "timestamp": 3.9541311102868955 + }, + { + "x": 2.3247702498642075, + "y": 7.044035745793493, + "heading": -1.618624975339561e-17, + "angularVelocity": 1.3384826497101663e-17, + "velocityX": 2.1667442337923255, + "velocityY": 0.7036699319154751, + "timestamp": 4.042689953626823 + }, + { + "x": 2.472806977776645, + "y": 7.080533384911558, + "heading": -8.026020803635598e-18, + "angularVelocity": 9.214470826616307e-17, + "velocityX": 1.671619934603348, + "velocityY": 0.4121286789843381, + "timestamp": 4.131248796966751 + }, + { + "x": 2.572342101217046, + "y": 7.101642440594894, + "heading": -3.7650843441848865e-18, + "angularVelocity": 4.81141829629879e-17, + "velocityX": 1.1239433543451147, + "velocityY": 0.23836191719793084, + "timestamp": 4.219807640306678 + }, + { + "x": 2.6223177909851074, + "y": 7.111215591430664, + "heading": 1.8821880131630952e-29, + "angularVelocity": 4.251505701529514e-17, + "velocityX": 0.5643218439092824, + "velocityY": 0.10809932102460933, + "timestamp": 4.308366483646606 + }, + { + "x": 2.6223177909851074, + "y": 7.111215591430664, + "heading": 9.186803464292479e-30, + "angularVelocity": -5.0618681731929696e-30, + "velocityX": -6.572861886784797e-20, + "velocityY": -2.9604907575354983e-19, + "timestamp": 4.396925326986533 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4NM-SweepFender": { + "waypoints": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 1.6146607398986816, + "y": 4.286937236785889, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 8 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 11 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 1.7001316547393799, + "y": 6.864553928375244, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": false, + "controlIntervalCount": 8 + }, + { + "x": 2.58807373046875, + "y": 7.118251800537109, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 12 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.9517716307925408e-29, + "angularVelocity": 3.5304770094188047e-29, + "velocityX": -1.724526619664194e-19, + "velocityY": 3.7818353955730183e-19, + "timestamp": 0 + }, + { + "x": 1.2920113917886342, + "y": 5.554330110103596, + "heading": 2.1731437642535974e-18, + "angularVelocity": 3.110625940380434e-17, + "velocityX": -0.07539684918457464, + "velocityY": -0.44695930001512396, + "timestamp": 0.069861938954233 + }, + { + "x": 1.282761551546609, + "y": 5.491689489432592, + "heading": 3.294425072358551e-18, + "angularVelocity": 1.604996065096705e-17, + "velocityX": -0.1324017108669824, + "velocityY": -0.8966344422825023, + "timestamp": 0.139723877908466 + }, + { + "x": 1.271458199578559, + "y": 5.397448925207256, + "heading": -1.1426328921377949e-17, + "angularVelocity": -2.1071206657810978e-16, + "velocityX": -0.16179556618740545, + "velocityY": -1.3489543181312655, + "timestamp": 0.209585816862699 + }, + { + "x": 1.261267862795592, + "y": 5.271561332803292, + "heading": -2.391418921336277e-17, + "angularVelocity": -1.7875054858841103e-16, + "velocityX": -0.14586392727580333, + "velocityY": -1.8019481607350603, + "timestamp": 0.279447755816932 + }, + { + "x": 1.2580444533875073, + "y": 5.114783048267347, + "heading": -3.501816243214742e-17, + "angularVelocity": -1.5894166558012616e-16, + "velocityX": -0.046139707204466994, + "velocityY": -2.244115850243602, + "timestamp": 0.34930969477116497 + }, + { + "x": 1.2734562507703713, + "y": 4.932401999052925, + "heading": -4.116208538702014e-17, + "angularVelocity": -8.794377668506781e-17, + "velocityX": 0.2206036307260309, + "velocityY": -2.6105924333691934, + "timestamp": 0.41917163372539795 + }, + { + "x": 1.3200722448212343, + "y": 4.744629254191581, + "heading": -4.742296059721744e-17, + "angularVelocity": -8.961782560603248e-17, + "velocityX": 0.6672588071367647, + "velocityY": -2.6877688720371022, + "timestamp": 0.48903357267963093 + }, + { + "x": 1.395362051384409, + "y": 4.570294708742184, + "heading": -4.971944913704747e-17, + "angularVelocity": -3.2871811710653476e-17, + "velocityX": 1.077694202167754, + "velocityY": -2.4954152154810103, + "timestamp": 0.558895511633864 + }, + { + "x": 1.4945697873897588, + "y": 4.416713614347291, + "heading": -4.954660865117441e-17, + "angularVelocity": 2.4740284798965177e-18, + "velocityX": 1.420054145224071, + "velocityY": -2.1983514442034626, + "timestamp": 0.628757450588097 + }, + { + "x": 1.6146607398986816, + "y": 4.286937236785889, + "heading": -4.40878360826832e-17, + "angularVelocity": 7.813657120875944e-17, + "velocityX": 1.7189753720920216, + "velocityY": -1.8576120202792965, + "timestamp": 0.6986193895423299 + }, + { + "x": 1.7770335829472077, + "y": 4.171588935482394, + "heading": -3.677251742788914e-17, + "angularVelocity": 9.144662966049345e-17, + "velocityX": 2.029774828142181, + "velocityY": -1.4419349569732136, + "timestamp": 0.7786148850018647 + }, + { + "x": 1.9580373267940145, + "y": 4.0933452292031935, + "heading": -2.7494710502210263e-17, + "angularVelocity": 1.159791154548558e-16, + "velocityX": 2.262674201928864, + "velocityY": -0.9781014022068157, + "timestamp": 0.8586103804613995 + }, + { + "x": 2.1432715777255, + "y": 4.056404838041411, + "heading": -1.4102008166543997e-17, + "angularVelocity": 1.6741820232350694e-16, + "velocityX": 2.315558518231629, + "velocityY": -0.4617808909061473, + "timestamp": 0.9386058759209343 + }, + { + "x": 2.30556761109126, + "y": 4.054072233873002, + "heading": -6.5008381019634475e-18, + "angularVelocity": 9.50199751021012e-17, + "velocityX": 2.0288146530432702, + "velocityY": -0.029159193964730743, + "timestamp": 1.018601371380469 + }, + { + "x": 2.4288986578849183, + "y": 4.066078072388016, + "heading": 5.687906951263531e-19, + "angularVelocity": 8.8375334664513e-17, + "velocityX": 1.5417248944478947, + "velocityY": 0.15008143203621294, + "timestamp": 1.0985968668400037 + }, + { + "x": 2.5107392491251295, + "y": 4.079634262670543, + "heading": 1.429916772585085e-18, + "angularVelocity": 1.0764681677136246e-17, + "velocityX": 1.0230649959735494, + "velocityY": 0.16946192038255736, + "timestamp": 1.1785923622995385 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -4.8145646612541634e-29, + "angularVelocity": -1.7874965990600758e-17, + "velocityX": 0.5077389399960675, + "velocityY": 0.10763325423991486, + "timestamp": 1.2585878577590732 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -2.9775150183346403e-29, + "angularVelocity": -1.4341236571899655e-29, + "velocityX": -2.760379088955979e-19, + "velocityY": 2.12311250171415e-20, + "timestamp": 1.338583353218608 + }, + { + "x": 2.5095534969743727, + "y": 4.138154812312414, + "heading": 2.190510097540435e-18, + "angularVelocity": 2.186765826125146e-17, + "velocityX": -0.41731125706956324, + "velocityY": 0.4982506071172713, + "timestamp": 1.4387545791600944 + }, + { + "x": 2.4259483376755, + "y": 4.237975559232393, + "heading": 3.933266220970577e-18, + "angularVelocity": 1.739777229170673e-17, + "velocityX": -0.8346225027505352, + "velocityY": 0.9965012006370843, + "timestamp": 1.5389258051015808 + }, + { + "x": 2.300540601198944, + "y": 4.387706676661202, + "heading": 1.3527765530751777e-17, + "angularVelocity": 9.578099042485691e-17, + "velocityX": -1.2519337294505222, + "velocityY": 1.494751771494467, + "timestamp": 1.6390970310430673 + }, + { + "x": 2.133330291347402, + "y": 4.587348160058594, + "heading": 2.2617014617503804e-17, + "angularVelocity": 9.073712421559476e-17, + "velocityX": -1.669244918188541, + "velocityY": 1.9930022970269912, + "timestamp": 1.7392682569845537 + }, + { + "x": 1.9243174195289612, + "y": 4.836899995803833, + "heading": 3.880271021847084e-17, + "angularVelocity": 1.6158028652305301e-16, + "velocityX": -2.0865559930406836, + "velocityY": 2.4912526865849736, + "timestamp": 1.83943948292604 + }, + { + "x": 1.7153045477105202, + "y": 5.086451831549072, + "heading": 2.228690287655552e-17, + "angularVelocity": -1.648757621591215e-16, + "velocityX": -2.086555993040684, + "velocityY": 2.491252686584974, + "timestamp": 1.9396107088675265 + }, + { + "x": 1.548094237858978, + "y": 5.286093314946464, + "heading": 1.1611942212855913e-17, + "angularVelocity": -1.065671349922539e-16, + "velocityX": -1.6692449181885414, + "velocityY": 1.9930022970269914, + "timestamp": 2.039781934809013 + }, + { + "x": 1.422686501382422, + "y": 5.435824432375273, + "heading": 1.3869899594487062e-18, + "angularVelocity": -1.0207474326639434e-16, + "velocityX": -1.2519337294505222, + "velocityY": 1.494751771494467, + "timestamp": 2.1399531607504994 + }, + { + "x": 1.3390813420835495, + "y": 5.535645179295252, + "heading": 4.452399801737667e-19, + "angularVelocity": -9.40140233404049e-18, + "velocityX": -0.8346225027505352, + "velocityY": 0.9965012006370843, + "timestamp": 2.2401243866919858 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.2561037494800293e-29, + "angularVelocity": -4.4447889631061314e-18, + "velocityX": -0.4173112570695632, + "velocityY": 0.4982506071172713, + "timestamp": 2.340295612633472 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.2140312015726775e-29, + "angularVelocity": -1.6302535205902077e-30, + "velocityX": -1.8217183323138137e-19, + "velocityY": -1.1857377469208627e-19, + "timestamp": 2.4404668385749586 + }, + { + "x": 1.3545482975852345, + "y": 5.589358578088535, + "heading": 1.9334918613178174e-18, + "angularVelocity": 2.0557149745608903e-17, + "velocityX": 0.6088974989534299, + "velocityY": 0.040434275746134056, + "timestamp": 2.5345213154285515 + }, + { + "x": 1.469087367090714, + "y": 5.5969646272643825, + "heading": 2.3789667257742094e-18, + "angularVelocity": 4.736350526475188e-18, + "velocityX": 1.2177949773063277, + "velocityY": 0.08086855012427496, + "timestamp": 2.6285757922821444 + }, + { + "x": 1.6408959668925176, + "y": 5.608373700732222, + "heading": 4.870233499579861e-18, + "angularVelocity": 2.6487487710164578e-17, + "velocityX": 1.8266924185782696, + "velocityY": 0.12130282204002837, + "timestamp": 2.7226302691357374 + }, + { + "x": 1.8699740888528427, + "y": 5.623585797951656, + "heading": 2.7021941669251377e-18, + "angularVelocity": -2.3050888781904244e-17, + "velocityX": 2.435589773327992, + "velocityY": 0.16173708821021165, + "timestamp": 2.8166847459893303 + }, + { + "x": 2.156321692282717, + "y": 5.642600916220706, + "heading": -8.385603820696607e-19, + "angularVelocity": -3.764578410393351e-17, + "velocityX": 3.0444866954670173, + "velocityY": 0.20217132565257093, + "timestamp": 2.910739222842923 + }, + { + "x": 2.385399814243042, + "y": 5.657813013440141, + "heading": -1.979930432977169e-18, + "angularVelocity": -1.2135202438796547e-17, + "velocityX": 2.4355897733279916, + "velocityY": 0.16173708821021165, + "timestamp": 3.004793699696516 + }, + { + "x": 2.5572084140448452, + "y": 5.66922208690798, + "heading": -2.2142142923894204e-18, + "angularVelocity": -2.490938414552063e-18, + "velocityX": 1.8266924185782694, + "velocityY": 0.12130282204002837, + "timestamp": 3.098848176550109 + }, + { + "x": 2.671747483550325, + "y": 5.676828136083827, + "heading": -1.281622902160523e-18, + "angularVelocity": 9.915437612097848e-18, + "velocityX": 1.2177949773063277, + "velocityY": 0.08086855012427498, + "timestamp": 3.192902653403702 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.304583994113875e-29, + "angularVelocity": 1.3626388144391886e-17, + "velocityX": 0.60889749895343, + "velocityY": 0.040434275746134056, + "timestamp": 3.286957130257295 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.1449818357083717e-29, + "angularVelocity": -3.0489465233384347e-30, + "velocityX": -1.0348040233346454e-19, + "velocityY": -6.871642741102469e-21, + "timestamp": 3.3810116071108878 + }, + { + "x": 2.671747483550325, + "y": 5.676828136083827, + "heading": -2.37591069216883e-18, + "angularVelocity": -2.5261004796916857e-17, + "velocityX": -0.6088974989534299, + "velocityY": -0.04043427574613417, + "timestamp": 3.4750660839644807 + }, + { + "x": 2.5572084140448452, + "y": 5.66922208690798, + "heading": -5.149849388984227e-18, + "angularVelocity": -2.949289312618861e-17, + "velocityX": -1.2177949773063277, + "velocityY": -0.0808685501242752, + "timestamp": 3.5691205608180736 + }, + { + "x": 2.385399814243042, + "y": 5.6578130134401405, + "heading": -9.068994008626725e-18, + "angularVelocity": -4.166887776483435e-17, + "velocityX": -1.8266924185782696, + "velocityY": -0.12130282204002872, + "timestamp": 3.6631750376716665 + }, + { + "x": 2.1563216922827166, + "y": 5.642600916220706, + "heading": -1.1203751573212612e-17, + "angularVelocity": -2.2697031803123958e-17, + "velocityX": -2.435589773327992, + "velocityY": -0.16173708821021213, + "timestamp": 3.7572295145252594 + }, + { + "x": 1.8699740888528424, + "y": 5.623585797951656, + "heading": -1.1207624225292261e-17, + "angularVelocity": -4.1174476338185276e-20, + "velocityX": -3.044486695467018, + "velocityY": -0.20217132565257148, + "timestamp": 3.8512839913788524 + }, + { + "x": 1.6408959668925174, + "y": 5.608373700732222, + "heading": -8.810275051865213e-18, + "angularVelocity": 2.5488942127572833e-17, + "velocityX": -2.4355897733279925, + "velocityY": -0.16173708821021213, + "timestamp": 3.9453384682324453 + }, + { + "x": 1.4690873670907139, + "y": 5.596964627264383, + "heading": -5.012140744625911e-18, + "angularVelocity": 4.0382279181189287e-17, + "velocityX": -1.8266924185782698, + "velocityY": -0.12130282204002875, + "timestamp": 4.039392945086038 + }, + { + "x": 1.3545482975852343, + "y": 5.589358578088536, + "heading": -2.397406088315742e-18, + "angularVelocity": 2.780021402222337e-17, + "velocityX": -1.217794977306328, + "velocityY": -0.08086855012427523, + "timestamp": 4.133447421939631 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 3.5803605220083744e-29, + "angularVelocity": 2.5489546637834322e-17, + "velocityX": -0.6088974989534299, + "velocityY": -0.040434275746134174, + "timestamp": 4.227501898793224 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.6002222494720617e-29, + "angularVelocity": -2.492411377554893e-30, + "velocityX": 1.9318637428338628e-19, + "velocityY": -3.328737127233864e-19, + "timestamp": 4.321556375646817 + }, + { + "x": 1.2952633927726702, + "y": 5.615759088627331, + "heading": 7.301213567930375e-19, + "angularVelocity": 1.0689167577301925e-17, + "velocityX": -0.02950553143506986, + "velocityY": 0.44218766725785547, + "timestamp": 4.3898611643768 + }, + { + "x": 1.292420372281359, + "y": 5.67622200565353, + "heading": 1.0899052605697139e-18, + "angularVelocity": 5.267330361290212e-18, + "velocityX": -0.04162256474505727, + "velocityY": 0.8851929440147399, + "timestamp": 4.458165953106783 + }, + { + "x": 1.2904736626423332, + "y": 5.766942347295431, + "heading": 1.7572190046603685e-18, + "angularVelocity": 9.769647663647677e-18, + "velocityX": -0.028500339071703427, + "velocityY": 1.3281695665662407, + "timestamp": 4.526470741836766 + }, + { + "x": 1.2921156551461344, + "y": 5.887719904619932, + "heading": 2.495299931317049e-18, + "angularVelocity": 1.080569753938027e-17, + "velocityX": 0.024039200388896183, + "velocityY": 1.7682150778907955, + "timestamp": 4.594775530566749 + }, + { + "x": 1.3019663635740581, + "y": 6.037633895876457, + "heading": 3.044317794390009e-18, + "angularVelocity": 8.037763728377753e-18, + "velocityX": 0.1442169518577215, + "velocityY": 2.194780103180635, + "timestamp": 4.663080319296732 + }, + { + "x": 1.3285668981441072, + "y": 6.21276212488805, + "heading": 3.684680070354539e-18, + "angularVelocity": 9.375068279246926e-18, + "velocityX": 0.3894387943311595, + "velocityY": 2.5639231489887897, + "timestamp": 4.731385108026715 + }, + { + "x": 1.3835021976266313, + "y": 6.398541889884891, + "heading": 5.2477062652819604e-18, + "angularVelocity": 2.2883111141942298e-17, + "velocityX": 0.8042671751711217, + "velocityY": 2.719864426069018, + "timestamp": 4.799689896756698 + }, + { + "x": 1.4670924013533833, + "y": 6.574564051095573, + "heading": 6.5266221738388946e-18, + "angularVelocity": 1.872366295041098e-17, + "velocityX": 1.2237824796910453, + "velocityY": 2.577010550556237, + "timestamp": 4.867994685486681 + }, + { + "x": 1.5739126540179675, + "y": 6.731177977822192, + "heading": 7.49337156608948e-18, + "angularVelocity": 1.415346481933098e-17, + "velocityX": 1.5638764814405166, + "velocityY": 2.2928689135652283, + "timestamp": 4.936299474216664 + }, + { + "x": 1.70013165473938, + "y": 6.864553928375244, + "heading": 8.152149061201394e-18, + "angularVelocity": 9.644675830649179e-18, + "velocityX": 1.8478792346517696, + "velocityY": 1.9526588550079922, + "timestamp": 5.004604262946647 + }, + { + "x": 1.862901466815502, + "y": 6.9825514639396085, + "heading": 9.64595727993831e-18, + "angularVelocity": 1.9516578427319273e-17, + "velocityX": 2.126584724254197, + "velocityY": 1.541635721207726, + "timestamp": 5.081144741310709 + }, + { + "x": 2.0390721934747793, + "y": 7.064979265467772, + "heading": 1.0519046193421893e-17, + "angularVelocity": 1.1406891943933564e-17, + "velocityX": 2.301667436951849, + "velocityY": 1.0769177733133648, + "timestamp": 5.157685219674771 + }, + { + "x": 2.212264721041194, + "y": 7.109513524937173, + "heading": 9.225749764850672e-18, + "angularVelocity": -1.689689286868671e-17, + "velocityX": 2.2627573183254994, + "velocityY": 0.581839314585602, + "timestamp": 5.234225698038832 + }, + { + "x": 2.360897546409662, + "y": 7.125037248480103, + "heading": 6.641149339179356e-18, + "angularVelocity": -3.376775775195044e-17, + "velocityX": 1.9418852422309485, + "velocityY": 0.2028171743204049, + "timestamp": 5.310766176402894 + }, + { + "x": 2.47440631167586, + "y": 7.126032006105099, + "heading": 2.8458717511366033e-18, + "angularVelocity": -4.958523477788854e-17, + "velocityX": 1.4829900164237055, + "velocityY": 0.012996490827559389, + "timestamp": 5.387306654766956 + }, + { + "x": 2.5502443215325226, + "y": 7.121956765796971, + "heading": -1.96953206231535e-18, + "angularVelocity": -6.291316501648883e-17, + "velocityX": 0.9908222613391942, + "velocityY": -0.053242942756960426, + "timestamp": 5.463847133131018 + }, + { + "x": 2.58807373046875, + "y": 7.118251800537109, + "heading": -5.993434819714505e-29, + "angularVelocity": 2.573190102598612e-17, + "velocityX": 0.4942405606128222, + "velocityY": -0.048405305781324695, + "timestamp": 5.540387611495079 + }, + { + "x": 2.58807373046875, + "y": 7.118251800537109, + "heading": -4.321420643442313e-29, + "angularVelocity": 3.722543235019264e-30, + "velocityX": -2.2890622227193903e-19, + "velocityY": 2.3506515476551758e-20, + "timestamp": 5.616928089859141 + }, + { + "x": 2.552218311146659, + "y": 7.07567690059573, + "heading": -2.216399626364933e-18, + "angularVelocity": -2.3929278644058663e-17, + "velocityX": -0.3871117607863663, + "velocityY": -0.45965839455282403, + "timestamp": 5.70955100384464 + }, + { + "x": 2.4805074734431463, + "y": 6.990527101829926, + "heading": -5.2327325281333886e-18, + "angularVelocity": -3.256572939942303e-17, + "velocityX": -0.7742235114168355, + "velocityY": -0.9193167770464863, + "timestamp": 5.80217391783014 + }, + { + "x": 2.372941218836405, + "y": 6.862802405994913, + "heading": -1.452637734592403e-17, + "angularVelocity": -1.0033850407775787e-16, + "velocityX": -1.1613352460880377, + "velocityY": -1.3789751405900372, + "timestamp": 5.894796831815639 + }, + { + "x": 2.229519549987184, + "y": 6.692502816250075, + "heading": -9.320490492158675e-18, + "angularVelocity": 5.620517232807327e-17, + "velocityX": -1.5484469520325608, + "velocityY": -1.838633470023389, + "timestamp": 5.987419745801138 + }, + { + "x": 2.050242473103896, + "y": 6.479628339967314, + "heading": -8.790348049291345e-20, + "angularVelocity": 9.967929575374868e-17, + "velocityX": -1.9355585909481747, + "velocityY": -2.298291719866287, + "timestamp": 6.0800426597866375 + }, + { + "x": 1.8351100192285623, + "y": 6.224179014006075, + "heading": 1.878777588983223e-17, + "angularVelocity": 2.0379059864583081e-16, + "velocityX": -2.322669894719714, + "velocityY": -2.757949571757475, + "timestamp": 6.172665573772137 + }, + { + "x": 1.6558329423452747, + "y": 6.011304537723314, + "heading": 2.071185162570867e-17, + "angularVelocity": 2.0773214298256213e-17, + "velocityX": -1.9355585909481743, + "velocityY": -2.298291719866287, + "timestamp": 6.265288487757636 + }, + { + "x": 1.5124112734960538, + "y": 5.841004947978476, + "heading": 1.8006256380694997e-17, + "angularVelocity": -2.921086300145266e-17, + "velocityX": -1.5484469520325606, + "velocityY": -1.838633470023389, + "timestamp": 6.357911401743135 + }, + { + "x": 1.4048450188893125, + "y": 5.713280252143463, + "heading": 1.1160795488320093e-17, + "angularVelocity": -7.39067747686445e-17, + "velocityX": -1.1613352460880377, + "velocityY": -1.3789751405900372, + "timestamp": 6.450534315728635 + }, + { + "x": 1.3331341811857995, + "y": 5.628130453377659, + "heading": 2.2020929810261438e-18, + "angularVelocity": -9.672231256696494e-17, + "velocityX": -0.7742235114168354, + "velocityY": -0.9193167770464864, + "timestamp": 6.543157229714134 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 5.441647463444351e-29, + "angularVelocity": -2.377481856337291e-17, + "velocityX": -0.3871117607863663, + "velocityY": -0.4596583945528241, + "timestamp": 6.635780143699633 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.677896302038151e-29, + "angularVelocity": -9.269005124761017e-30, + "velocityX": 9.63963472983009e-20, + "velocityY": 1.144615001673186e-19, + "timestamp": 6.728403057685132 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 7 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 8 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + }, + "4NM-SweepFenderStraight": { + "waypoints": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.5176022052764893, + "y": 4.271198749542236, + "heading": -0.5713377241070188, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 10 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 2.545790910720825, + "y": 6.808176517486572, + "heading": 0.6970216139142698, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 14 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 0, + "isInitialGuess": false, + "translationConstrained": true, + "headingConstrained": true, + "controlIntervalCount": 40 + } + ], + "trajectory": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -3.4870573885213374e-23, + "angularVelocity": -5.958014181544604e-22, + "velocityX": 4.75095708718707e-20, + "velocityY": -5.100796288410356e-20, + "timestamp": 0 + }, + { + "x": 1.3221899506052506, + "y": 5.558731738657612, + "heading": -0.011172212869994504, + "angularVelocity": -0.14847792516259956, + "velocityX": 0.33106795052320837, + "velocityY": -0.35648661636038936, + "timestamp": 0.07524494201922415 + }, + { + "x": 1.3720125126335756, + "y": 5.5050840215864865, + "heading": -0.03353368538631347, + "angularVelocity": -0.29718240078656566, + "velocityX": 0.662138353639718, + "velocityY": -0.712974395772926, + "timestamp": 0.1504898840384483 + }, + { + "x": 1.4467463839809327, + "y": 5.424612630178231, + "heading": -0.06713200496181293, + "angularVelocity": -0.4465193097884971, + "velocityX": 0.9932079066293052, + "velocityY": -1.0694591456751465, + "timestamp": 0.22573482605767242 + }, + { + "x": 1.5463911448663232, + "y": 5.31731803920096, + "heading": -0.11204656505352982, + "angularVelocity": -0.5969113522639407, + "velocityX": 1.3242718807588763, + "velocityY": -1.4259375859424448, + "timestamp": 0.3009797680768966 + }, + { + "x": 1.6709458752175186, + "y": 5.183200865786002, + "heading": -0.1683893711840811, + "angularVelocity": -0.7487919402762832, + "velocityX": 1.6553236271930836, + "velocityY": -1.7824078245776684, + "timestamp": 0.37622471009612074 + }, + { + "x": 1.8204089760394484, + "y": 5.022261716216735, + "heading": -0.2363043631413436, + "angularVelocity": -0.9025854779702146, + "velocityX": 1.9863541230949968, + "velocityY": -2.1388700057493355, + "timestamp": 0.4514696521153449 + }, + { + "x": 1.99477795080174, + "y": 4.83450098026474, + "heading": -0.3159644115550863, + "angularVelocity": -1.0586764542045974, + "velocityX": 2.317351440283424, + "velocityY": -2.495327006884067, + "timestamp": 0.526714594134569 + }, + { + "x": 2.1441616151551486, + "y": 4.673559909893242, + "heading": -0.3887569416939099, + "angularVelocity": -0.9674076181788537, + "velocityX": 1.9852984180018707, + "velocityY": -2.138895533076221, + "timestamp": 0.6019595361537932 + }, + { + "x": 2.268644362208996, + "y": 4.53944104449319, + "heading": -0.449547316997939, + "angularVelocity": -0.8078998225355519, + "velocityX": 1.6543669742219151, + "velocityY": -1.782430310940861, + "timestamp": 0.6772044781730173 + }, + { + "x": 2.3682284247776337, + "y": 4.432144914414375, + "heading": -0.49824500481886397, + "angularVelocity": -0.6471888543482873, + "velocityX": 1.323465204388028, + "velocityY": -1.425958040493952, + "timestamp": 0.7524494201922415 + }, + { + "x": 2.4429154717792705, + "y": 4.351672139574372, + "heading": -0.5347927169900155, + "angularVelocity": -0.48571653044551594, + "velocityX": 0.9925856143600397, + "velocityY": -1.0694775313860099, + "timestamp": 0.8276943622114656 + }, + { + "x": 2.492706577536202, + "y": 4.298023281924518, + "heading": -0.5591597915473412, + "angularVelocity": -0.3238367111918331, + "velocityX": 0.6617203019999724, + "velocityY": -0.7129895539842044, + "timestamp": 0.9029393042306898 + }, + { + "x": 2.5176022052764893, + "y": 4.271198749542236, + "heading": -0.5713377241070188, + "angularVelocity": -0.16184386927384822, + "velocityX": 0.3308611459083442, + "velocityY": -0.3564961532620849, + "timestamp": 0.978184246249914 + }, + { + "x": 2.5176022052764893, + "y": 4.271198749542236, + "heading": -0.5713377241070188, + "angularVelocity": -1.965008466563789e-21, + "velocityX": 1.6720342558000003e-19, + "velocityY": -1.796953683889986e-19, + "timestamp": 1.053429188269138 + }, + { + "x": 2.4927076698068604, + "y": 4.298022743670389, + "heading": -0.5590626618027412, + "angularVelocity": 0.163131980525576, + "velocityX": -0.3308410804570447, + "velocityY": 0.3564830205551893, + "timestamp": 1.1286753923839292 + }, + { + "x": 2.4429176923283147, + "y": 4.351669893774747, + "heading": -0.5345293787694578, + "angularVelocity": 0.32604014145161125, + "velocityX": -0.6616942085555453, + "velocityY": 0.712954902316633, + "timestamp": 1.2039215964987204 + }, + { + "x": 2.368230774545632, + "y": 4.432139277372316, + "heading": -0.49778036282477456, + "angularVelocity": 0.48838365173373005, + "velocityX": -0.9925672485584068, + "velocityY": 1.0694145245494298, + "timestamp": 1.2791678006135117 + }, + { + "x": 2.2686448820648275, + "y": 4.539430029093104, + "heading": -0.4488890301403053, + "angularVelocity": 0.6497514826114488, + "velocityX": -1.3234673250611078, + "velocityY": 1.4258626462686734, + "timestamp": 1.3544140047283029 + }, + { + "x": 2.14415754877083, + "y": 4.673541531401143, + "heading": -0.38796534930924853, + "angularVelocity": 0.8096578631144655, + "velocityX": -1.6544001755103406, + "velocityY": 1.7823025611158592, + "timestamp": 1.429660208843094 + }, + { + "x": 1.9947660347325065, + "y": 4.83447364690962, + "heading": -0.31516105455016663, + "angularVelocity": 0.9675477403221032, + "velocityX": -1.9853694388413312, + "velocityY": 2.138740650132567, + "timestamp": 1.5049064129578853 + }, + { + "x": 1.8203962771969746, + "y": 5.02224100400193, + "heading": -0.2359849474173268, + "angularVelocity": 1.0522272593585371, + "velocityX": -2.3173229744522876, + "velocityY": 2.4953731460774122, + "timestamp": 1.5801526170726765 + }, + { + "x": 1.6709347760864326, + "y": 5.183186512420658, + "heading": -0.1683650413377472, + "angularVelocity": 0.8986487341796361, + "velocityX": -1.9862995465197482, + "velocityY": 2.1389186379847103, + "timestamp": 1.6553988211874677 + }, + { + "x": 1.546382775447489, + "y": 5.317309066145499, + "heading": -0.11216464144227759, + "angularVelocity": 0.7468868437500663, + "velocityX": -1.6552595855723733, + "velocityY": 1.7824494312062664, + "timestamp": 1.730645025302259 + }, + { + "x": 1.4467409698876759, + "y": 5.424607739615935, + "heading": -0.06728177554770955, + "angularVelocity": 0.5964801337499681, + "velocityX": -1.3242103934944731, + "velocityY": 1.4259679239998275, + "timestamp": 1.8058912294170502 + }, + { + "x": 1.3720096830963446, + "y": 5.505081872672054, + "heading": -0.0336471785574408, + "angularVelocity": 0.4469939365839344, + "velocityX": -0.9931568996799567, + "velocityY": 1.0694776434616193, + "timestamp": 1.8811374335318414 + }, + { + "x": 1.3221889841070316, + "y": 5.558731132858149, + "heading": -0.011222693307519035, + "angularVelocity": 0.2980148369439646, + "velocityX": -0.6621024884299752, + "velocityY": 0.7129829446845056, + "timestamp": 1.9563836376466326 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -5.804527907102298e-22, + "angularVelocity": 0.14914630498035963, + "velocityX": -0.33104955308205936, + "velocityY": 0.3564886879503997, + "timestamp": 2.031629841761424 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -1.7372853944454406e-22, + "angularVelocity": 2.7872047475617054e-21, + "velocityX": -2.1429242136387852e-19, + "velocityY": 2.3070553047501513e-19, + "timestamp": 2.106876045876215 + }, + { + "x": 1.3545482975852352, + "y": 5.589358578088536, + "heading": 9.197337540759096e-19, + "angularVelocity": 9.780581566888343e-18, + "velocityX": 0.6088974989534347, + "velocityY": 0.04043427574613332, + "timestamp": 2.2009305227298084 + }, + { + "x": 1.4690873670907165, + "y": 5.596964627264383, + "heading": 3.887468901363766e-18, + "angularVelocity": 3.155336403486356e-17, + "velocityX": 1.2177949773063368, + "velocityY": 0.08086855012427349, + "timestamp": 2.2949849995834017 + }, + { + "x": 1.6408959668925225, + "y": 5.608373700732223, + "heading": 3.6659024972997286e-18, + "angularVelocity": -2.3557241663966836e-18, + "velocityX": 1.8266924185782827, + "velocityY": 0.1213028220400261, + "timestamp": 2.389039476436995 + }, + { + "x": 1.8699740888528504, + "y": 5.623585797951658, + "heading": -8.067542241443972e-19, + "angularVelocity": -4.7553895051950526e-17, + "velocityX": 2.435589773328005, + "velocityY": 0.16173708821020832, + "timestamp": 2.4830939532905885 + }, + { + "x": 2.156321692282708, + "y": 5.642600916220705, + "heading": -2.6708058042599195e-18, + "angularVelocity": -1.981885012273154e-17, + "velocityX": 3.044486695466825, + "velocityY": 0.20217132565255283, + "timestamp": 2.577148430144182 + }, + { + "x": 2.385399814243036, + "y": 5.6578130134401405, + "heading": -2.770306305819978e-18, + "angularVelocity": -1.0579028761460627e-18, + "velocityX": 2.435589773328005, + "velocityY": 0.1617370882102083, + "timestamp": 2.671202906997775 + }, + { + "x": 2.557208414044842, + "y": 5.66922208690798, + "heading": -1.5286567962624567e-18, + "angularVelocity": 1.3201386590800127e-17, + "velocityX": 1.8266924185782827, + "velocityY": 0.12130282204002607, + "timestamp": 2.7652573838513685 + }, + { + "x": 2.6717474835503237, + "y": 5.676828136083827, + "heading": -1.1055203544056362e-18, + "angularVelocity": 4.4988442442533515e-18, + "velocityX": 1.217794977306337, + "velocityY": 0.08086855012427348, + "timestamp": 2.859311860704962 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.3994282719055762e-28, + "angularVelocity": 1.1754042882709242e-17, + "velocityX": 0.6088974989534347, + "velocityY": 0.04043427574613331, + "timestamp": 2.9533663375585553 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.2578191824600292e-28, + "angularVelocity": -3.8382941954344137e-29, + "velocityX": -7.718549111027777e-22, + "velocityY": -5.126840450701387e-23, + "timestamp": 3.0474208144121486 + }, + { + "x": 2.6717474835503237, + "y": 5.676828136083827, + "heading": 2.957978952433013e-18, + "angularVelocity": 3.144963484589081e-17, + "velocityX": -0.6088974989534347, + "velocityY": -0.04043427574613368, + "timestamp": 3.141475291265742 + }, + { + "x": 2.557208414044842, + "y": 5.66922208690798, + "heading": 7.803415503811453e-18, + "angularVelocity": 5.15173409451729e-17, + "velocityX": -1.217794977306337, + "velocityY": -0.08086855012427421, + "timestamp": 3.2355297681193353 + }, + { + "x": 2.385399814243036, + "y": 5.6578130134401405, + "heading": 1.0918258808355902e-17, + "angularVelocity": 3.3117437987572525e-17, + "velocityX": -1.8266924185782827, + "velocityY": -0.12130282204002718, + "timestamp": 3.3295842449729287 + }, + { + "x": 2.156321692282708, + "y": 5.642600916220706, + "heading": 1.4843907441415233e-17, + "angularVelocity": 4.173803060196763e-17, + "velocityX": -2.435589773328005, + "velocityY": -0.16173708821020977, + "timestamp": 3.423638721826522 + }, + { + "x": 1.8699740888528504, + "y": 5.623585797951657, + "heading": 1.4047394913563068e-17, + "angularVelocity": -8.468629611156265e-18, + "velocityX": -3.044486695466825, + "velocityY": -0.2021713256525547, + "timestamp": 3.5176931986801154 + }, + { + "x": 1.6408959668925225, + "y": 5.608373700732223, + "heading": 5.894670829565627e-18, + "angularVelocity": -8.668087215646387e-17, + "velocityX": -2.435589773328005, + "velocityY": -0.1617370882102098, + "timestamp": 3.611747675533709 + }, + { + "x": 1.4690873670907165, + "y": 5.596964627264383, + "heading": -3.5801552348274217e-19, + "angularVelocity": -6.647941238005475e-17, + "velocityX": -1.8266924185782827, + "velocityY": -0.12130282204002721, + "timestamp": 3.705802152387302 + }, + { + "x": 1.3545482975852352, + "y": 5.589358578088536, + "heading": -2.459449657169509e-18, + "angularVelocity": -2.234273374195548e-17, + "velocityX": -1.2177949773063372, + "velocityY": -0.08086855012427423, + "timestamp": 3.7998566292408955 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -2.170460361005601e-22, + "angularVelocity": 2.6146895857700576e-17, + "velocityX": -0.6088974989534347, + "velocityY": -0.04043427574613369, + "timestamp": 3.893911106094489 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -3.6388277145419484e-23, + "angularVelocity": 1.9207790315741447e-21, + "velocityX": 1.47076337525445e-19, + "velocityY": 1.4190124023616126e-19, + "timestamp": 3.9879655829480822 + }, + { + "x": 1.3227737240195483, + "y": 5.61050348195736, + "heading": 0.013550786900983634, + "angularVelocity": 0.18226929801485287, + "velocityX": 0.34292833980902226, + "velocityY": 0.33557028471401684, + "timestamp": 4.062310450945979 + }, + { + "x": 1.3737639106778228, + "y": 5.660399482511426, + "heading": 0.040679974016250266, + "angularVelocity": 0.36491001794547456, + "velocityX": 0.6858602083968518, + "velocityY": 0.6711424997819331, + "timestamp": 4.136655318943877 + }, + { + "x": 1.4502490112097923, + "y": 5.735243170344193, + "heading": 0.08146222479513072, + "angularVelocity": 0.548555023058672, + "velocityX": 1.0287878987710746, + "velocityY": 1.0067095395862875, + "timestamp": 4.211000186941774 + }, + { + "x": 1.5522278856647675, + "y": 5.835033815033583, + "heading": 0.1360214110793065, + "angularVelocity": 0.7338662069548484, + "velocityX": 1.3717002558651332, + "velocityY": 1.3422667546092502, + "timestamp": 4.285345054939672 + }, + { + "x": 1.6796982296330216, + "y": 5.959770632655783, + "heading": 0.20453087198468312, + "angularVelocity": 0.9215089454098471, + "velocityX": 1.7145816167413093, + "velocityY": 1.6778134252081405, + "timestamp": 4.359689922937569 + }, + { + "x": 1.8326561921573887, + "y": 6.109453229425897, + "heading": 0.28720867051263665, + "angularVelocity": 1.1120848116953013, + "velocityX": 2.0574111790566834, + "velocityY": 2.01335479907434, + "timestamp": 4.434034790935467 + }, + { + "x": 2.011096000176797, + "y": 6.28408222003575, + "heading": 0.38430411909211587, + "angularVelocity": 1.3060141364730837, + "velocityX": 2.400163088923032, + "velocityY": 2.3489044410542577, + "timestamp": 4.5083796589333645 + }, + { + "x": 2.1638766096575854, + "y": 6.433815495784424, + "heading": 0.4735075918913831, + "angularVelocity": 1.1998605310831978, + "velocityX": 2.05502563384886, + "velocityY": 2.0140364732762612, + "timestamp": 4.582724526931262 + }, + { + "x": 2.291186482527782, + "y": 6.5585980911554875, + "heading": 0.5479625204474381, + "angularVelocity": 1.0014804055898086, + "velocityX": 1.712423147671697, + "velocityY": 1.678429173814564, + "timestamp": 4.6570693949291595 + }, + { + "x": 2.393030162177412, + "y": 6.658427500466555, + "heading": 0.6075765143590145, + "angularVelocity": 0.8018575527400554, + "velocityX": 1.3698817738503593, + "velocityY": 1.3427881708511593, + "timestamp": 4.731414262927057 + }, + { + "x": 2.469410931527034, + "y": 6.733301422056823, + "heading": 0.6523015105520286, + "angularVelocity": 0.6015882117684191, + "velocityX": 1.027384558027357, + "velocityY": 1.007116208645172, + "timestamp": 4.8057591309249545 + }, + { + "x": 2.520330867960948, + "y": 6.783218097420482, + "heading": 0.6821175635862542, + "angularVelocity": 0.4010505881195312, + "velocityX": 0.6849152847423776, + "velocityY": 0.6714205930807574, + "timestamp": 4.880103998922852 + }, + { + "x": 2.545790910720825, + "y": 6.808176517486572, + "heading": 0.6970216139142698, + "angularVelocity": 0.20047181102584016, + "velocityX": 0.34245864503515044, + "velocityY": 0.3357114046768724, + "timestamp": 4.95444886692075 + }, + { + "x": 2.545790910720825, + "y": 6.808176517486572, + "heading": 0.6970216139142698, + "angularVelocity": 1.060990978792425e-20, + "velocityX": 9.444494751401817e-19, + "velocityY": 9.172807861306771e-19, + "timestamp": 5.028793734918647 + }, + { + "x": 2.52033376431852, + "y": 6.783218896026595, + "heading": 0.6819576043595674, + "angularVelocity": -0.20261789732027236, + "velocityX": -0.34241039592273365, + "velocityY": -0.3356915543615947, + "timestamp": 5.103140620134275 + }, + { + "x": 2.469417510751594, + "y": 6.733305671093222, + "heading": 0.6518402320176618, + "angularVelocity": -0.405092590692349, + "velocityX": -0.6848471650056825, + "velocityY": -0.6713559658701147, + "timestamp": 5.177487505349903 + }, + { + "x": 2.393038999153381, + "y": 6.65843948497798, + "heading": 0.6067059073438061, + "angularVelocity": -0.6070775465973136, + "velocityX": -1.0273263147029235, + "velocityY": -1.0069848373352566, + "timestamp": 5.251834390565532 + }, + { + "x": 2.2911938790627677, + "y": 6.558623270950903, + "heading": 0.5466314564942851, + "angularVelocity": -0.8080291551594541, + "velocityX": -1.3698639801147294, + "velocityY": -1.342574254961449, + "timestamp": 5.32618127578116 + }, + { + "x": 2.163876704340966, + "y": 6.433859756287583, + "heading": 0.471750868383845, + "angularVelocity": -1.0071785508332185, + "velocityX": -1.7124748986126783, + "velocityY": -1.6781269894692763, + "timestamp": 5.400528160996788 + }, + { + "x": 2.0110812200123074, + "y": 6.284150815002161, + "heading": 0.3822707555656717, + "angularVelocity": -1.2035489120849394, + "velocityX": -2.0551699494270133, + "velocityY": -2.0136545176199485, + "timestamp": 5.474875046212416 + }, + { + "x": 1.832635836912685, + "y": 6.109507130533716, + "heading": 0.28610948605635284, + "angularVelocity": -1.2934135603720627, + "velocityX": -2.4001729538779912, + "velocityY": -2.349038348572726, + "timestamp": 5.549221931428044 + }, + { + "x": 1.6796785281931887, + "y": 5.959809271214951, + "heading": 0.2040617419906055, + "angularVelocity": -1.1035801140529855, + "velocityX": -2.0573465623458747, + "velocityY": -2.0135054600417615, + "timestamp": 5.623568816643672 + }, + { + "x": 1.5522122020719726, + "y": 5.835058855915294, + "heading": 0.1359188756997469, + "angularVelocity": -0.9165530754008571, + "velocityX": -1.7144810539342084, + "velocityY": -1.6779507969680607, + "timestamp": 5.6979157018593005 + }, + { + "x": 1.4502385246804212, + "y": 5.735257410270185, + "heading": 0.08152405680169575, + "angularVelocity": -0.7316354779395241, + "velocityX": -1.3715931352846462, + "velocityY": -1.342375613391939, + "timestamp": 5.772262587074929 + }, + { + "x": 1.373758307379264, + "y": 5.6604060852535225, + "heading": 0.04077083075075002, + "angularVelocity": -0.5481497433651609, + "velocityX": -1.0286943034579255, + "velocityY": -1.0067849486844118, + "timestamp": 5.846609472290557 + }, + { + "x": 1.322771778723125, + "y": 5.6105054821115194, + "heading": 0.013600563583197408, + "angularVelocity": -0.3654526627275732, + "velocityX": -0.6857923974657818, + "velocityY": -0.6711861969371897, + "timestamp": 5.920956357506185 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -1.098795178581163e-21, + "angularVelocity": -0.18293387199412173, + "velocityX": -0.34289287016502545, + "velocityY": -0.3355880828480881, + "timestamp": 5.995303242721813 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -9.496010380322716e-22, + "angularVelocity": -6.404372441775509e-21, + "velocityX": -5.456635982947961e-19, + "velocityY": -5.295509774626706e-19, + "timestamp": 6.069650127937441 + } + ], + "constraints": [ + { + "scope": [ + "first" + ], + "type": "StopPoint" + }, + { + "scope": [ + "last" + ], + "type": "StopPoint" + }, + { + "scope": [ + 5 + ], + "type": "StopPoint" + }, + { + "scope": [ + 2 + ], + "type": "StopPoint" + }, + { + "scope": [ + 4 + ], + "type": "StopPoint" + }, + { + "scope": [ + 3 + ], + "type": "StopPoint" + }, + { + "scope": [ + 1 + ], + "type": "StopPoint" + }, + { + "scope": [ + 6 + ], + "type": "StopPoint" + } + ], + "usesControlIntervalGuessing": true, + "defaultControlIntervalCount": 40, + "usesDefaultFieldObstacles": true, + "circleObstacles": [] + } + }, + "splitTrajectoriesAtStopPoints": true, + "usesObstacles": false +} \ No newline at end of file diff --git a/NoTeamBottom.chor b/NoTeamBottom.chor deleted file mode 100644 index 0e73690..0000000 --- a/NoTeamBottom.chor +++ /dev/null @@ -1,11549 +0,0 @@ -{ - "version": "v0.2.2", - "robotConfiguration": { - "mass": 54.23239916626329, - "rotationalInertia": 5.506392685924157, - "motorMaxTorque": 0.7248618784530386, - "motorMaxVelocity": 4704, - "gearing": 6.12, - "wheelbase": 0.7873995748042296, - "trackWidth": 0.7111996159522074, - "bumperLength": 0.8635995336562519, - "bumperWidth": 0.7873995748042296, - "wheelRadius": 0.050799972568014815 - }, - "paths": { - "3NT-2ndN": { - "waypoints": [ - { - "x": 0.7773118615150452, - "y": 6.72802209854126, - "heading": 1.030377205028743, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 2.338470935821533, - "y": 6.983484268188477, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 5.347249984741211, - "y": 6.855753421783447, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 - }, - { - "x": 7.845104217529297, - "y": 5.791326522827148, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 - }, - { - "x": 5.347249984741211, - "y": 6.855753421783447, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 - }, - { - "x": 2.9345498085021973, - "y": 6.955099582672119, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.7773118615150452, - "y": 6.72802209854126, - "heading": 1.030377205028743, - "angularVelocity": -2.5603966606354566e-33, - "velocityX": 0, - "velocityY": 3.3983269045643113e-34, - "timestamp": 0 - }, - { - "x": 0.8017165578496347, - "y": 6.73204375441724, - "heading": 1.0153126355108713, - "angularVelocity": -0.2257566165421289, - "velocityX": 0.36572712321443296, - "velocityY": 0.0602682620556043, - "timestamp": 0.06672924917379079 - }, - { - "x": 0.8505266341075832, - "y": 6.740088761712056, - "heading": 0.9851666091775935, - "angularVelocity": -0.45176630497916986, - "velocityX": 0.7314644906437763, - "velocityY": 0.12056193340141036, - "timestamp": 0.13345849834758158 - }, - { - "x": 0.9237415389002549, - "y": 6.752159262105969, - "heading": 0.9398639932852465, - "angularVelocity": -0.678901927614384, - "velocityX": 1.0971935950004397, - "velocityY": 0.18088769982225175, - "timestamp": 0.20018774752137236 - }, - { - "x": 1.0213592682884212, - "y": 6.768257272086683, - "heading": 0.8792594067892606, - "angularVelocity": -0.9082162207182363, - "velocityX": 1.4628926684598107, - "velocityY": 0.24124368519101108, - "timestamp": 0.26691699669516317 - }, - { - "x": 1.1433761786203043, - "y": 6.788383770319312, - "heading": 0.8031252901056259, - "angularVelocity": -1.1409407063063117, - "velocityX": 1.8285371384008848, - "velocityY": 0.30161433676873595, - "timestamp": 0.333646245868954 - }, - { - "x": 1.2897869899026333, - "y": 6.812537367144202, - "heading": 0.7111466792887886, - "angularVelocity": -1.3783852202096063, - "velocityX": 2.1941024827210964, - "velocityY": 0.3619641630012081, - "timestamp": 0.4003754950427448 - }, - { - "x": 1.4605852210940866, - "y": 6.8407125983828605, - "heading": 0.6029335592009877, - "angularVelocity": -1.6216744745017098, - "velocityX": 2.55957070259585, - "velocityY": 0.4222321034255566, - "timestamp": 0.4671047442165356 - }, - { - "x": 1.6557643007257534, - "y": 6.872898115584218, - "heading": 0.4780638213571818, - "angularVelocity": -1.871289417907176, - "velocityX": 2.924940442883433, - "velocityY": 0.48232997673228606, - "timestamp": 0.5338339933903263 - }, - { - "x": 1.8264308864004113, - "y": 6.900588544051283, - "heading": 0.35839609824636437, - "angularVelocity": -1.793332378117321, - "velocityX": 2.5575978718143664, - "velocityY": 0.4149668819882522, - "timestamp": 0.6005632425641171 - }, - { - "x": 1.9727210620901834, - "y": 6.9242976491672845, - "heading": 0.2558451219964055, - "angularVelocity": -1.5368219711699938, - "velocityX": 2.192294645917135, - "velocityY": 0.3553030404141562, - "timestamp": 0.6672924917379078 - }, - { - "x": 2.0946339267218823, - "y": 6.944038578297596, - "heading": 0.1704674226074578, - "angularVelocity": -1.2794644094763992, - "velocityX": 1.8269779165982687, - "velocityY": 0.2958362243654028, - "timestamp": 0.7340217409116986 - }, - { - "x": 2.192167334429377, - "y": 6.959821625235924, - "heading": 0.10224171092320174, - "angularVelocity": -1.0224258856347668, - "velocityX": 1.4616290294751688, - "velocityY": 0.23652367041058378, - "timestamp": 0.8007509900854893 - }, - { - "x": 2.265318973651712, - "y": 6.971654176410272, - "heading": 0.05111626576067614, - "angularVelocity": -0.7661624519312907, - "velocityX": 1.0962455014564527, - "velocityY": 0.17732180896462885, - "timestamp": 0.8674802392592801 - }, - { - "x": 2.314087093590593, - "y": 6.979540946456479, - "heading": 0.01704319032984002, - "angularVelocity": -0.5106167962731848, - "velocityX": 0.7308357360933072, - "velocityY": 0.11819060073141874, - "timestamp": 0.9342094884330708 - }, - { - "x": 2.338470935821533, - "y": 6.983484268188477, - "heading": 0, - "angularVelocity": -0.2554080937648863, - "velocityX": 0.3654146050322646, - "velocityY": 0.059094351889496924, - "timestamp": 1.0009387376068617 - }, - { - "x": 2.338470935821533, - "y": 6.983484268188477, - "heading": 0, - "angularVelocity": 0, - "velocityX": -3.661539873949153e-32, - "velocityY": -1.6264049380013454e-33, - "timestamp": 1.0676679867806524 - }, - { - "x": 2.3668454143263347, - "y": 6.982856207850841, - "heading": -5.424481691988399e-19, - "angularVelocity": -7.628324804228275e-18, - "velocityX": 0.39902381550093635, - "velocityY": -0.008832269190277171, - "timestamp": 1.138777723599694 - }, - { - "x": 2.423594370858518, - "y": 6.981600087186138, - "heading": -1.6273462227067539e-18, - "angularVelocity": -1.5256673727660342e-17, - "velocityX": 0.7980476242880341, - "velocityY": -0.01766453823194559, - "timestamp": 1.2098874604187357 - }, - { - "x": 2.5087178048213095, - "y": 6.979715906207575, - "heading": -3.2546963346700688e-18, - "angularVelocity": -2.2885053225616023e-17, - "velocityX": 1.1970714246828327, - "velocityY": -0.02649680708785306, - "timestamp": 1.2809971972377774 - }, - { - "x": 2.6222157154474277, - "y": 6.977203664932138, - "heading": -5.22299810707614e-18, - "angularVelocity": -2.7679778613369908e-17, - "velocityX": 1.5960952142875302, - "velocityY": -0.03532907570492495, - "timestamp": 1.352106934056819 - }, - { - "x": 2.764088101713831, - "y": 6.974063363382471, - "heading": -7.733758581198458e-18, - "angularVelocity": -3.5308251533986704e-17, - "velocityX": 1.9951189895054233, - "velocityY": -0.04416134400354938, - "timestamp": 1.4232166708758607 - }, - { - "x": 2.9343349621882595, - "y": 6.970295001590276, - "heading": -1.1212959884643953e-17, - "angularVelocity": -4.892721389616843e-17, - "velocityX": 2.394142744581784, - "velocityY": -0.0529936118563472, - "timestamp": 1.4943264076949023 - }, - { - "x": 3.1329562947223244, - "y": 6.965898579603107, - "heading": -1.5058318284360188e-17, - "angularVelocity": -5.4076397575508025e-17, - "velocityX": 2.7931664694458354, - "velocityY": -0.06182587904040484, - "timestamp": 1.565436144513944 - }, - { - "x": 3.3599520957353746, - "y": 6.960874097500221, - "heading": -1.921107651348465e-17, - "angularVelocity": -5.839929122072696e-17, - "velocityX": 3.192190143956008, - "velocityY": -0.0706581451098949, - "timestamp": 1.6365458813329856 - }, - { - "x": 3.6153223580661016, - "y": 6.955221555440131, - "heading": -1.7381335638599224e-17, - "angularVelocity": 2.573122833433782e-17, - "velocityX": 3.5912137177583303, - "velocityY": -0.07949040895024774, - "timestamp": 1.7076556181520273 - }, - { - "x": 3.89906706023054, - "y": 6.948940953898378, - "heading": -1.600425363722977e-17, - "angularVelocity": 1.9365589903304247e-17, - "velocityX": 3.990236989436556, - "velocityY": -0.0883226661031768, - "timestamp": 1.778765354971069 - }, - { - "x": 4.189757421654959, - "y": 6.942506612329325, - "heading": -2.309450754421387e-17, - "angularVelocity": -9.970862253403092e-17, - "velocityX": 4.087912210449614, - "velocityY": -0.09048467702006271, - "timestamp": 1.8498750917901106 - }, - { - "x": 4.480447783078946, - "y": 6.936072270739366, - "heading": -3.567945640397763e-17, - "angularVelocity": -1.769792636385315e-16, - "velocityX": 4.08791221044355, - "velocityY": -0.09048467731405441, - "timestamp": 1.9209848286091522 - }, - { - "x": 4.77113812835654, - "y": 6.9296371997299495, - "heading": -5.239589681395825e-17, - "angularVelocity": -2.350794864073287e-16, - "velocityX": 4.0879119833805015, - "velocityY": -0.0904949349734297, - "timestamp": 1.9920945654281939 - }, - { - "x": 5.060999121162208, - "y": 6.906770374050763, - "heading": -6.413834291103341e-17, - "angularVelocity": -1.6513133956545303e-16, - "velocityX": 4.0762489888452125, - "velocityY": -0.3215709507880496, - "timestamp": 2.0632043022472355 - }, - { - "x": 5.347249984741211, - "y": 6.855753421783447, - "heading": -7.100878289298809e-17, - "angularVelocity": -9.661742947296258e-17, - "velocityX": 4.025480565445591, - "velocityY": -0.7174397564871019, - "timestamp": 2.134314039066277 - }, - { - "x": 5.646959680408469, - "y": 6.7691984973790005, - "heading": -7.924873015338203e-17, - "angularVelocity": -1.0800318016891037e-16, - "velocityX": 3.9283746893754534, - "velocityY": -1.134498413587298, - "timestamp": 2.2106076004920348 - }, - { - "x": 5.9359740719099685, - "y": 6.651774740866165, - "heading": -9.202999259689669e-17, - "angularVelocity": -1.6752740604398516e-16, - "velocityX": 3.788188493241898, - "velocityY": -1.5391044056463643, - "timestamp": 2.2869011619177924 - }, - { - "x": 6.220380450256188, - "y": 6.523592569288839, - "heading": -1.0221846207986328e-16, - "angularVelocity": -1.3354297915271803e-16, - "velocityX": 3.7277900393073033, - "velocityY": -1.68011781311405, - "timestamp": 2.36319472334355 - }, - { - "x": 6.50478671019526, - "y": 6.395410134994128, - "heading": -1.0951189331938173e-16, - "angularVelocity": -9.559694295587159e-17, - "velocityX": 3.727788487313321, - "velocityY": -1.6801212566206856, - "timestamp": 2.4394882847693076 - }, - { - "x": 6.772850174759086, - "y": 6.274593429192974, - "heading": -1.0951189787115588e-16, - "angularVelocity": -5.966131443872117e-23, - "velocityX": 3.5135791219378683, - "velocityY": -1.5835766943285492, - "timestamp": 2.515781846195065 - }, - { - "x": 7.011128834517843, - "y": 6.167200790731002, - "heading": -1.0951190457325102e-16, - "angularVelocity": -8.784614336894978e-23, - "velocityX": 3.1231817640421453, - "velocityY": -1.40762387356209, - "timestamp": 2.592075407620823 - }, - { - "x": 7.21962267153951, - "y": 6.073232227690204, - "heading": -1.0951190901705372e-16, - "angularVelocity": -5.824610389344753e-23, - "velocityX": 2.732784171106696, - "velocityY": -1.231670946862785, - "timestamp": 2.6683689690465804 - }, - { - "x": 7.398331679846744, - "y": 5.992687742764583, - "heading": -1.095119119553291e-16, - "angularVelocity": -3.85127569143241e-23, - "velocityX": 2.342386499824594, - "velocityY": -1.0557179848524971, - "timestamp": 2.744662530472338 - }, - { - "x": 7.54725585645087, - "y": 5.925567337301139, - "heading": -1.0951191376445854e-16, - "angularVelocity": -2.3712740836183491e-23, - "velocityX": 1.951988789369157, - "velocityY": -0.8797650051867136, - "timestamp": 2.8209560918980956 - }, - { - "x": 7.666395199558686, - "y": 5.8718710121080715, - "heading": -1.0951191500361807e-16, - "angularVelocity": -1.6241993379413765e-23, - "velocityX": 1.5615910554097168, - "velocityY": -0.7038120149276318, - "timestamp": 2.897249653323853 - }, - { - "x": 7.75574970797472, - "y": 5.831598767724182, - "heading": -1.0951191576780229e-16, - "angularVelocity": -1.0016365829374042e-23, - "velocityX": 1.1711933057809407, - "velocityY": -0.5278590176063511, - "timestamp": 2.973543214749611 - }, - { - "x": 7.815319380845067, - "y": 5.804750604534326, - "heading": -1.095119162439172e-16, - "angularVelocity": -6.240564641473135e-24, - "velocityX": 0.7807955449597819, - "velocityY": -0.3519060152406423, - "timestamp": 3.0498367761753684 - }, - { - "x": 7.845104217529297, - "y": 5.791326522827148, - "heading": -1.0951191646797131e-16, - "angularVelocity": -2.9367367517813784e-24, - "velocityX": 0.3903977757443359, - "velocityY": -0.17595300909161263, - "timestamp": 3.126130337601126 - }, - { - "x": 7.845104217529297, - "y": 5.791326522827148, - "heading": -1.0951191646797131e-16, - "angularVelocity": -3.8529205658537675e-42, - "velocityX": 0, - "velocityY": -4.029280467146476e-44, - "timestamp": 3.2024238990268836 - }, - { - "x": 7.819330555877964, - "y": 5.803041575931587, - "heading": -1.0951188196648234e-16, - "angularVelocity": 4.857879592349432e-22, - "velocityX": -0.3628983811361795, - "velocityY": 0.1649503226991147, - "timestamp": 3.2734456036998 - }, - { - "x": 7.767783233011056, - "y": 5.8264716819423965, - "heading": -1.0951180912999703e-16, - "angularVelocity": 1.0255524790956886e-21, - "velocityX": -0.7257967561367829, - "velocityY": 0.32990064260939006, - "timestamp": 3.3444673083727166 - }, - { - "x": 7.690462249473273, - "y": 5.861616840611991, - "heading": -1.0951169316662303e-16, - "angularVelocity": 1.6327878151424353e-21, - "velocityX": -1.0886951234679147, - "velocityY": 0.4948509590336158, - "timestamp": 3.415489013045633 - }, - { - "x": 7.587367605964943, - "y": 5.9084770516220475, - "heading": -1.0951152779403229e-16, - "angularVelocity": 2.3284796037986105e-21, - "velocityX": -1.4515934809382964, - "velocityY": 0.6598012709757769, - "timestamp": 3.4865107177185495 - }, - { - "x": 7.458499303419834, - "y": 5.967052314548134, - "heading": -1.0951130436615097e-16, - "angularVelocity": 3.1459098644356444e-21, - "velocityX": -1.8144918252610083, - "velocityY": 0.824751576941851, - "timestamp": 3.557532422391466 - }, - { - "x": 7.303857343145226, - "y": 6.037342628796045, - "heading": -1.0951101137046541e-16, - "angularVelocity": 4.1254386513769835e-21, - "velocityX": -2.1773901511769775, - "velocityY": 0.9897018745414005, - "timestamp": 3.6285541270643824 - }, - { - "x": 7.123441727102036, - "y": 6.119347993474474, - "heading": -1.0951063176976066e-16, - "angularVelocity": 5.344854879364384e-21, - "velocityX": -2.5402884494828224, - "velocityY": 1.154652159591159, - "timestamp": 3.699575831737299 - }, - { - "x": 6.917252458558463, - "y": 6.213068407097906, - "heading": -1.0951013731921648e-16, - "angularVelocity": 6.961963901804915e-21, - "velocityX": -2.9031867017717694, - "velocityY": 1.3196024237245876, - "timestamp": 3.7705975364102153 - }, - { - "x": 6.685289544050908, - "y": 6.318503866695314, - "heading": -1.0950947214506759e-16, - "angularVelocity": 9.365786866840166e-21, - "velocityX": -3.2660848620268434, - "velocityY": 1.4845526460253218, - "timestamp": 3.841619241083132 - }, - { - "x": 6.427553003188613, - "y": 6.435654363353594, - "heading": -1.1321193217013263e-16, - "angularVelocity": -5.213138775134091e-17, - "velocityX": -3.628982746179858, - "velocityY": 1.6495027428277718, - "timestamp": 3.9126409457560483 - }, - { - "x": 6.163180157507604, - "y": 6.555821298407878, - "heading": -1.1495995520201052e-16, - "angularVelocity": -2.4612518664938278e-17, - "velocityX": -3.722423263403093, - "velocityY": 1.6919748069650107, - "timestamp": 3.9836626504289647 - }, - { - "x": 5.898806927725833, - "y": 6.675987388419107, - "heading": -1.1629491145603269e-16, - "angularVelocity": -1.879645469184645e-17, - "velocityX": -3.7224286716196997, - "velocityY": 1.691962908587475, - "timestamp": 4.054684355101881 - }, - { - "x": 5.627414904076508, - "y": 6.779328876635568, - "heading": -1.0580061371759786e-16, - "angularVelocity": 1.4776183966247067e-16, - "velocityX": -3.821254712192482, - "velocityY": 1.4550691044715158, - "timestamp": 4.125706059774798 - }, - { - "x": 5.347249984741211, - "y": 6.855753421783447, - "heading": -9.637324583502604e-17, - "angularVelocity": 1.3273925099360297e-16, - "velocityX": -3.9447788619770323, - "velocityY": 1.076073089203438, - "timestamp": 4.196727764447714 - }, - { - "x": 5.061742585487608, - "y": 6.904479036217225, - "heading": -8.82951413614811e-17, - "angularVelocity": 1.1404224621492368e-16, - "velocityX": -4.030636794620884, - "velocityY": 0.6878814871023652, - "timestamp": 4.267562080165426 - }, - { - "x": 4.772852740000535, - "y": 6.925247237603686, - "heading": -7.9445886933944e-17, - "angularVelocity": 1.249289181080401e-16, - "velocityX": -4.078388314476751, - "velocityY": 0.29319407092496935, - "timestamp": 4.338396395883138 - }, - { - "x": 4.483255537650678, - "y": 6.9299505879565855, - "heading": -7.626016633533651e-17, - "angularVelocity": 4.4974255293199715e-17, - "velocityX": -4.0883743905137075, - "velocityY": 0.0663993193869991, - "timestamp": 4.40923071160085 - }, - { - "x": 4.201672722376627, - "y": 6.934523131728666, - "heading": -7.626013411314718e-17, - "angularVelocity": 4.548951873443926e-22, - "velocityX": -3.9752316715560787, - "velocityY": 0.0645526638571946, - "timestamp": 4.480065027318562 - }, - { - "x": 3.948248160256927, - "y": 6.938638421582927, - "heading": -7.626011484857826e-17, - "angularVelocity": 2.719666128336545e-22, - "velocityX": -3.5777089049556827, - "velocityY": 0.05809740395688512, - "timestamp": 4.550899343036274 - }, - { - "x": 3.7229818713544702, - "y": 6.942296457194531, - "heading": -7.626010057772757e-17, - "angularVelocity": 2.014680390074246e-22, - "velocityX": -3.1801858551183724, - "velocityY": 0.05164213947067224, - "timestamp": 4.621733658753986 - }, - { - "x": 3.525873862356881, - "y": 6.945497238455197, - "heading": -7.626008963589061e-17, - "angularVelocity": 1.544708499714273e-22, - "velocityX": -2.782662710868862, - "velocityY": 0.04518687345582658, - "timestamp": 4.692567974471698 - }, - { - "x": 3.3569241366079687, - "y": 6.9482407653107865, - "heading": -7.626008119077207e-17, - "angularVelocity": 1.1922354961091427e-22, - "velocityX": -2.385139519413282, - "velocityY": 0.03873160667666502, - "timestamp": 4.76340229018941 - }, - { - "x": 3.2161326961140184, - "y": 6.950527037728815, - "heading": -7.626007474301078e-17, - "angularVelocity": 9.102595569495179e-23, - "velocityX": -1.9876162996340712, - "velocityY": 0.03227633943891411, - "timestamp": 4.8342366059071225 - }, - { - "x": 3.1034995422125533, - "y": 6.952356055687626, - "heading": -7.626006995970547e-17, - "angularVelocity": 6.752807978535753e-23, - "velocityX": -1.5900930609724457, - "velocityY": 0.02582107189543705, - "timestamp": 4.9050709216248345 - }, - { - "x": 3.019024675858946, - "y": 6.9537278191717515, - "heading": -7.626006660307227e-17, - "angularVelocity": 4.7387105624513213e-23, - "velocityX": -1.1925698088233843, - "velocityY": 0.019365804133584213, - "timestamp": 4.9759052373425465 - }, - { - "x": 2.962708097769727, - "y": 6.954642328169591, - "heading": -7.626006449477479e-17, - "angularVelocity": 2.9763786792475846e-23, - "velocityX": -0.7950465465587467, - "velocityY": 0.012910536207949581, - "timestamp": 5.046739553060259 - }, - { - "x": 2.9345498085021973, - "y": 6.955099582672119, - "heading": -7.626006349610785e-17, - "angularVelocity": 1.4098631913740372e-23, - "velocityX": -0.3975232764264406, - "velocityY": 0.0064552681549291165, - "timestamp": 5.117573868777971 - }, - { - "x": 2.9345498085021973, - "y": 6.955099582672119, - "heading": -7.626006349610785e-17, - "angularVelocity": -5.7559710712214985e-49, - "velocityX": -1.1684104255391741e-43, - "velocityY": -3.507282078288517e-42, - "timestamp": 5.188408184495683 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 0 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "4NT": { - "waypoints": [ - { - "x": 0.7644821405410767, - "y": 6.707253456115723, - "heading": 1.0390723224103011, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 2.4094326496124268, - "y": 6.756406784057617, - "heading": 0.5485498201613986, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 24 - }, - { - "x": 7.745757579803467, - "y": 7.494409084320068, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 - }, - { - "x": 3.0764732360839844, - "y": 6.955099582672119, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 - }, - { - "x": 5.176941871643066, - "y": 6.855753421783447, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 15 - }, - { - "x": 7.788334846496582, - "y": 5.777134418487549, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 5.176941871643066, - "y": 6.855753421783447, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 - }, - { - "x": 3.0622808933258057, - "y": 6.969292163848877, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.7644821405410767, - "y": 6.707253456115723, - "heading": 1.0390723224103011, - "angularVelocity": -9.206160444329012e-19, - "velocityX": 9.05046145899678e-17, - "velocityY": 2.68861825331568e-18, - "timestamp": 0 - }, - { - "x": 0.7980550173951986, - "y": 6.708255980327082, - "heading": 1.0294370798834647, - "angularVelocity": -0.12440616498554202, - "velocityX": 0.4334787469355561, - "velocityY": 0.012944167424232692, - "timestamp": 0.07744987981870564 - }, - { - "x": 0.8652008914326297, - "y": 6.71026127914523, - "heading": 1.0101571456310194, - "angularVelocity": -0.24893433401802018, - "velocityX": 0.866959047510542, - "velocityY": 0.025891567848019475, - "timestamp": 0.1548997596374113 - }, - { - "x": 0.9659196942242209, - "y": 6.713269722660067, - "heading": 0.9812027432997417, - "angularVelocity": -0.37384696269450945, - "velocityX": 1.3004384645573803, - "velocityY": 0.038843746715683435, - "timestamp": 0.23234963945611692 - }, - { - "x": 1.1002111527349556, - "y": 6.717281753096185, - "heading": 0.9425220110355934, - "angularVelocity": -0.49942920963456483, - "velocityX": 1.7339143562918844, - "velocityY": 0.0518016353996883, - "timestamp": 0.3097995192748226 - }, - { - "x": 1.268074768329642, - "y": 6.722297816740347, - "heading": 0.8940388796768679, - "angularVelocity": -0.6259936293279543, - "velocityX": 2.167383809860272, - "velocityY": 0.06476528634910197, - "timestamp": 0.38724939909352823 - }, - { - "x": 1.469509791267818, - "y": 6.728318270649073, - "heading": 0.8356508730520583, - "angularVelocity": -0.7538811778854153, - "velocityX": 2.600843583097795, - "velocityY": 0.07773354746086393, - "timestamp": 0.4646992789122339 - }, - { - "x": 1.7045151768946503, - "y": 6.7353432595041784, - "heading": 0.7672266029593876, - "angularVelocity": -0.883465155179555, - "velocityX": 3.0342898681951693, - "velocityY": 0.09070367664288498, - "timestamp": 0.5421491587309395 - }, - { - "x": 1.9059214136950664, - "y": 6.741362767222263, - "heading": 0.7049265752872264, - "angularVelocity": -0.8043915344735578, - "velocityX": 2.6004719086958845, - "velocityY": 0.07772133064860018, - "timestamp": 0.6195990385496452 - }, - { - "x": 2.0737589181433664, - "y": 6.746378213892937, - "heading": 0.6528779373682576, - "angularVelocity": -0.6720299378230677, - "velocityX": 2.1670466738124516, - "velocityY": 0.0647573202491049, - "timestamp": 0.6970489183683508 - }, - { - "x": 2.208028432585087, - "y": 6.750389994490007, - "heading": 0.6111683036443215, - "angularVelocity": -0.5385371006587716, - "velocityX": 1.7336310237797354, - "velocityY": 0.051798409583853885, - "timestamp": 0.7744987981870565 - }, - { - "x": 2.308730438118727, - "y": 6.753398497687527, - "heading": 0.5798578891124007, - "angularVelocity": -0.4042667929919595, - "velocityX": 1.300221585486807, - "velocityY": 0.03884451731317741, - "timestamp": 0.8519486780057621 - }, - { - "x": 2.3758651742265764, - "y": 6.755404033176191, - "heading": 0.5589815311679106, - "angularVelocity": -0.26954667965085594, - "velocityX": 0.8668152392876279, - "velocityY": 0.02589462363734593, - "timestamp": 0.9293985578244678 - }, - { - "x": 2.4094326496124268, - "y": 6.756406784057617, - "heading": 0.5485498201613986, - "angularVelocity": -0.13468982819509207, - "velocityX": 0.43340900546811284, - "velocityY": 0.012947094091989499, - "timestamp": 1.0068484376431734 - }, - { - "x": 2.4094326496124268, - "y": 6.756406784057617, - "heading": 0.5485498201613986, - "angularVelocity": 7.885113031928889e-19, - "velocityX": -9.054050236249424e-17, - "velocityY": -2.6979151059026156e-18, - "timestamp": 1.084298317461879 - }, - { - "x": 2.449918473004672, - "y": 6.762011948706376, - "heading": 0.5401868465452816, - "angularVelocity": -0.09794900486670763, - "velocityX": 0.4741789576900468, - "velocityY": 0.0656489335805002, - "timestamp": 1.1696792116067214 - }, - { - "x": 2.5308915856088263, - "y": 6.7732229923798455, - "heading": 0.5235987360078687, - "angularVelocity": -0.1942836357425878, - "velocityX": 0.9483750833856259, - "velocityY": 0.13130623409085962, - "timestamp": 1.2550601057515638 - }, - { - "x": 2.652353832669549, - "y": 6.790040853204755, - "heading": 0.4989623873518077, - "angularVelocity": -0.2885463885429357, - "velocityX": 1.42259282099659, - "velocityY": 0.19697452214987327, - "timestamp": 1.3404409998964062 - }, - { - "x": 2.814307686694331, - "y": 6.812466902815374, - "heading": 0.4665261355043061, - "angularVelocity": -0.3799005875070355, - "velocityX": 1.8968395171645707, - "velocityY": 0.26265887509418356, - "timestamp": 1.4258218940412486 - }, - { - "x": 3.0167566936149885, - "y": 6.840503375672428, - "heading": 0.42667323499598936, - "angularVelocity": -0.4667660242666151, - "velocityX": 2.3711277440737293, - "velocityY": 0.32836939853888514, - "timestamp": 1.511202788186091 - }, - { - "x": 3.259706353846804, - "y": 6.874154379047141, - "heading": 0.3800673035647531, - "angularVelocity": -0.5458590226540936, - "velocityX": 2.8454803930686077, - "velocityY": 0.39412802725662344, - "timestamp": 1.5965836823309334 - }, - { - "x": 3.54316599484672, - "y": 6.913428880352818, - "heading": 0.3280843834092549, - "angularVelocity": -0.6088355091165131, - "velocityX": 3.3199422873113495, - "velocityY": 0.45999168431113957, - "timestamp": 1.6819645764757758 - }, - { - "x": 3.867149061852821, - "y": 6.958354860932515, - "heading": 0.274992628968794, - "angularVelocity": -0.621822422594857, - "velocityX": 3.7945616551694545, - "velocityY": 0.526183065071723, - "timestamp": 1.7673454706206182 - }, - { - "x": 4.212980616322228, - "y": 7.006124001931196, - "heading": 0.27499262530079555, - "angularVelocity": -4.296041295056196e-8, - "velocityX": 4.050455994086094, - "velocityY": 0.5594827915205997, - "timestamp": 1.8527263647654606 - }, - { - "x": 4.558812171652906, - "y": 7.053893136695068, - "heading": 0.27499262163285115, - "angularVelocity": -4.295977880952904e-8, - "velocityX": 4.050456004173501, - "velocityY": 0.5594827184971202, - "timestamp": 1.938107258910303 - }, - { - "x": 4.904643726983597, - "y": 7.1016622714588475, - "heading": 0.27499261796490687, - "angularVelocity": -4.295977836262281e-8, - "velocityX": 4.050456004173649, - "velocityY": 0.5594827184960538, - "timestamp": 2.0234881530551454 - }, - { - "x": 5.250475282314288, - "y": 7.149431406222625, - "heading": 0.27499261429696265, - "angularVelocity": -4.2959777948006654e-8, - "velocityX": 4.050456004173652, - "velocityY": 0.5594827184960296, - "timestamp": 2.108869047199988 - }, - { - "x": 5.59630683764498, - "y": 7.1972005409864055, - "heading": 0.2749926106290183, - "angularVelocity": -4.2959778676517735e-8, - "velocityX": 4.05045600417365, - "velocityY": 0.5594827184960434, - "timestamp": 2.19424994134483 - }, - { - "x": 5.942138392975663, - "y": 7.244969675750248, - "heading": 0.27499260696107397, - "angularVelocity": -4.2959778796089525e-8, - "velocityX": 4.0504560041735465, - "velocityY": 0.5594827184967942, - "timestamp": 2.2796308354896726 - }, - { - "x": 6.287969947721882, - "y": 7.292738814744901, - "heading": 0.2749926032930733, - "angularVelocity": -4.296043929094884e-8, - "velocityX": 4.05045599732818, - "velocityY": 0.5594827680489721, - "timestamp": 2.365011729634515 - }, - { - "x": 6.611957814067862, - "y": 7.337606380569157, - "heading": 0.21950497451670065, - "angularVelocity": -0.6498834350720434, - "velocityX": 3.7946178661043106, - "velocityY": 0.5254988984788682, - "timestamp": 2.4503926237793574 - }, - { - "x": 6.8954258290534405, - "y": 7.376830372069738, - "heading": 0.1669734117826824, - "angularVelocity": -0.6152613328797233, - "velocityX": 3.320040365291738, - "velocityY": 0.45940010225285227, - "timestamp": 2.5357735179242 - }, - { - "x": 7.138387345763902, - "y": 7.410436361963956, - "heading": 0.12043561907247709, - "angularVelocity": -0.5450609668160352, - "velocityX": 2.845619258780494, - "velocityY": 0.393600819373116, - "timestamp": 2.621154412069042 - }, - { - "x": 7.340849161615168, - "y": 7.438433533551551, - "heading": 0.08088918506026732, - "angularVelocity": -0.4631766205812063, - "velocityX": 2.3712777651145713, - "velocityY": 0.3279090933400053, - "timestamp": 2.7065353062138846 - }, - { - "x": 7.502815057013489, - "y": 7.460826777716827, - "heading": 0.04882589115457254, - "angularVelocity": -0.3755324212381965, - "velocityX": 1.8969805484065148, - "velocityY": 0.26227465043042614, - "timestamp": 2.791916200358727 - }, - { - "x": 7.62428740604637, - "y": 7.477619165692711, - "heading": 0.024536835467999913, - "angularVelocity": -0.28447881613148784, - "velocityX": 1.4227111375387078, - "velocityY": 0.19667617848317423, - "timestamp": 2.8772970945035694 - }, - { - "x": 7.705267842650796, - "y": 7.488812781962917, - "heading": 0.00821504269256905, - "angularVelocity": -0.1911644629504835, - "velocityX": 0.9484608637038761, - "velocityY": 0.1311021204722608, - "timestamp": 2.962677988648412 - }, - { - "x": 7.745757579803467, - "y": 7.494409084320068, - "heading": -3.4489084618021245e-23, - "angularVelocity": -0.09621640502654895, - "velocityX": 0.47422479652159877, - "velocityY": 0.06554513645240315, - "timestamp": 3.048058882793254 - }, - { - "x": 7.745757579803467, - "y": 7.494409084320068, - "heading": -1.6876431270400293e-23, - "angularVelocity": 9.102523324796537e-24, - "velocityX": 6.60258980472981e-23, - "velocityY": 9.24836920818837e-24, - "timestamp": 3.1334397769380966 - }, - { - "x": 7.691102928405584, - "y": 7.488096388275378, - "heading": 6.192852550462252e-16, - "angularVelocity": 6.254986697847185e-15, - "velocityX": -0.552030111111553, - "velocityY": -0.06376032432436478, - "timestamp": 3.232446425846852 - }, - { - "x": 7.581793626590513, - "y": 7.475470996299268, - "heading": 1.8570099149900534e-15, - "angularVelocity": 1.2501429687662208e-14, - "velocityX": -1.1040602123177685, - "velocityY": -0.1275206475046479, - "timestamp": 3.3314530747556073 - }, - { - "x": 7.417829675731226, - "y": 7.456532908550319, - "heading": 3.711983243078296e-15, - "angularVelocity": 1.8735846011693857e-14, - "velocityX": -1.6560902996565103, - "velocityY": -0.1912809690832165, - "timestamp": 3.4304597236643626 - }, - { - "x": 7.19921107788718, - "y": 7.431282125266402, - "heading": 6.179661952041429e-15, - "angularVelocity": 2.4924373627041796e-14, - "velocityX": -2.20812036619404, - "velocityY": -0.25504128825921324, - "timestamp": 3.529466372573118 - }, - { - "x": 6.9259378364908075, - "y": 7.399718646843969, - "heading": 9.255687598717214e-15, - "angularVelocity": 3.106887952051271e-14, - "velocityX": -2.7601503980628785, - "velocityY": -0.31880160343092295, - "timestamp": 3.6284730214818732 - }, - { - "x": 6.598009958406971, - "y": 7.36184247407592, - "heading": 1.2963393641775597e-14, - "angularVelocity": 3.744906108755832e-14, - "velocityX": -3.312180360594323, - "velocityY": -0.38256191059405736, - "timestamp": 3.7274796703906286 - }, - { - "x": 6.215427464230262, - "y": 7.317653609340963, - "heading": 1.7282751287499044e-14, - "angularVelocity": 4.362694519339008e-14, - "velocityX": -3.8642101151135524, - "velocityY": -0.4463221937314619, - "timestamp": 3.826486319299384 - }, - { - "x": 5.813271436087006, - "y": 7.27120397141853, - "heading": 1.7340324515409316e-14, - "angularVelocity": 5.81508702141454e-16, - "velocityX": -4.061909301807436, - "velocityY": -0.4691567529494063, - "timestamp": 3.925492968208139 - }, - { - "x": 5.411115407943726, - "y": 7.224754333496093, - "heading": 1.7396219297650977e-14, - "angularVelocity": 5.645558440545595e-16, - "velocityX": -4.061909301807676, - "velocityY": -0.469156752949434, - "timestamp": 4.024499617116894 - }, - { - "x": 5.008959379800445, - "y": 7.178304695573657, - "heading": 1.7361996647802476e-14, - "angularVelocity": -3.456601170296343e-16, - "velocityX": -4.061909301807676, - "velocityY": -0.46915675294943404, - "timestamp": 4.123506266025649 - }, - { - "x": 4.606803351657189, - "y": 7.131855057651224, - "heading": 1.7439558668245237e-14, - "angularVelocity": 7.834021381153129e-16, - "velocityX": -4.061909301807435, - "velocityY": -0.46915675294940606, - "timestamp": 4.222512914934405 - }, - { - "x": 4.22422085748048, - "y": 7.087666192916267, - "heading": 2.176954052963867e-14, - "angularVelocity": 4.37342532963012e-14, - "velocityX": -3.864210115113553, - "velocityY": -0.44632219373146187, - "timestamp": 4.32151956384316 - }, - { - "x": 3.896292979396644, - "y": 7.049790020148219, - "heading": 2.5486765461328337e-14, - "angularVelocity": 3.7545205020679964e-14, - "velocityX": -3.3121803605943234, - "velocityY": -0.3825619105940573, - "timestamp": 4.420526212751915 - }, - { - "x": 3.6230197380002713, - "y": 7.018226541725785, - "heading": 2.859002831120057e-14, - "angularVelocity": 3.134398430889467e-14, - "velocityX": -2.7601503980628785, - "velocityY": -0.3188016034309229, - "timestamp": 4.519532861660671 - }, - { - "x": 3.4044011401562257, - "y": 6.992975758441869, - "heading": 3.1065991495946517e-14, - "angularVelocity": 2.5008049580870388e-14, - "velocityX": -2.20812036619404, - "velocityY": -0.2550412882592132, - "timestamp": 4.618539510569426 - }, - { - "x": 3.240437189296938, - "y": 6.97403767069292, - "heading": 3.291706550760512e-14, - "angularVelocity": 1.8696461622134015e-14, - "velocityX": -1.6560902996565106, - "velocityY": -0.19128096908321646, - "timestamp": 4.717546159478181 - }, - { - "x": 3.131127887481867, - "y": 6.96141227871681, - "heading": 3.415005257379642e-14, - "angularVelocity": 1.2453578419037865e-14, - "velocityX": -1.1040602123177685, - "velocityY": -0.12752064750464787, - "timestamp": 4.816552808386937 - }, - { - "x": 3.0764732360839844, - "y": 6.955099582672119, - "heading": 3.4758328772534545e-14, - "angularVelocity": 6.143791406360488e-15, - "velocityX": -0.552030111111553, - "velocityY": -0.06376032432436476, - "timestamp": 4.915559457295692 - }, - { - "x": 3.0764732360839844, - "y": 6.955099582672119, - "heading": 3.475832877253451e-14, - "angularVelocity": -5.200407773157229e-29, - "velocityX": 6.353773828235079e-22, - "velocityY": -9.018528098234072e-21, - "timestamp": 5.014566106204447 - }, - { - "x": 3.1002974851018976, - "y": 6.954611325658584, - "heading": 3.504479981929914e-14, - "angularVelocity": 4.396565873129549e-15, - "velocityX": 0.3656386268981033, - "velocityY": -0.007493441823419454, - "timestamp": 5.07972401695372 - }, - { - "x": 3.1479459827206875, - "y": 6.953634810583847, - "heading": 3.561530727564337e-14, - "angularVelocity": 8.755766564394683e-15, - "velocityX": 0.7312772473958112, - "velocityY": -0.014986899725712866, - "timestamp": 5.1448819277029925 - }, - { - "x": 3.219418728430643, - "y": 6.952170036167429, - "heading": 3.6469746902170535e-14, - "angularVelocity": 1.311336745917341e-14, - "velocityX": 1.096915860070811, - "velocityY": -0.022480377279967864, - "timestamp": 5.210039838452265 - }, - { - "x": 3.314715721594625, - "y": 6.950217000808728, - "heading": 3.7610775824707776e-14, - "angularVelocity": 1.7511748142569967e-14, - "velocityX": 1.4625544629674223, - "velocityY": -0.029973879399181655, - "timestamp": 5.275197749201538 - }, - { - "x": 3.433836961393453, - "y": 6.947775702449826, - "heading": 3.903558275776818e-14, - "angularVelocity": 2.1866983098078997e-14, - "velocityX": 1.8281930532918131, - "velocityY": -0.03746741310192467, - "timestamp": 5.34035565995081 - }, - { - "x": 3.576782446734888, - "y": 6.944846138346835, - "heading": 4.0741719020202745e-14, - "angularVelocity": 2.6184637334365043e-14, - "velocityX": 2.1938316268532327, - "velocityY": -0.04496098891605818, - "timestamp": 5.405513570700083 - }, - { - "x": 3.7435521760897914, - "y": 6.941428304658305, - "heading": 4.2729089755284945e-14, - "angularVelocity": 3.0500835773105754e-14, - "velocityX": 2.5594701769464736, - "velocityY": -0.05245462368617194, - "timestamp": 5.470671481449355 - }, - { - "x": 3.934146147164455, - "y": 6.937522195622059, - "heading": 4.5003079615010204e-14, - "angularVelocity": 3.48996742463965e-14, - "velocityX": 2.925108691837403, - "velocityY": -0.05994834689032576, - "timestamp": 5.535829392198628 - }, - { - "x": 4.148564356136022, - "y": 6.933127801634456, - "heading": 4.756109468648397e-14, - "angularVelocity": 3.925870308084403e-14, - "velocityX": 3.2907471480577066, - "velocityY": -0.06744221748471975, - "timestamp": 5.6009873029479005 - }, - { - "x": 4.386806795358758, - "y": 6.928245103488179, - "heading": 5.0403457079820235e-14, - "angularVelocity": 4.362267544571434e-14, - "velocityX": 3.6563854869363728, - "velocityY": -0.07493638286018017, - "timestamp": 5.666145213697173 - }, - { - "x": 4.648873441895276, - "y": 6.922874043561024, - "heading": 5.3527133351455455e-14, - "angularVelocity": 4.7940092549239784e-14, - "velocityX": 4.022023473787371, - "velocityY": -0.08243143258264855, - "timestamp": 5.731303124446446 - }, - { - "x": 4.914409944960004, - "y": 6.90113291291604, - "heading": 5.351906829106693e-14, - "angularVelocity": -1.237771484037243e-16, - "velocityX": 4.075276509194888, - "velocityY": -0.33366832046908873, - "timestamp": 5.796461035195718 - }, - { - "x": 5.176941871643066, - "y": 6.855753421783447, - "heading": 5.3521968753757984e-14, - "angularVelocity": 4.45143599254819e-17, - "velocityX": 4.0291642820360565, - "velocityY": -0.6964540546306409, - "timestamp": 5.861618945944991 - }, - { - "x": 5.463817637938302, - "y": 6.77599939155945, - "heading": 5.354091246992271e-14, - "angularVelocity": 2.601435934873782e-16, - "velocityX": 3.9395064875027384, - "velocityY": -1.0952180573822985, - "timestamp": 5.934439175383701 - }, - { - "x": 5.741298450592208, - "y": 6.668004902444624, - "heading": 5.354780772967558e-14, - "angularVelocity": 9.468879466669219e-17, - "velocityX": 3.810490776981841, - "velocityY": -1.4830286851227334, - "timestamp": 6.007259404822411 - }, - { - "x": 6.014319653285819, - "y": 6.549186410545976, - "heading": 5.3548296476122484e-14, - "angularVelocity": 6.711685074013839e-18, - "velocityX": 3.7492494159662573, - "velocityY": -1.6316687384053477, - "timestamp": 6.0800796342611205 - }, - { - "x": 6.287340700556493, - "y": 6.430367561517044, - "heading": 5.354604536453578e-14, - "angularVelocity": -3.0913272385962676e-17, - "velocityX": 3.749247281628875, - "velocityY": -1.6316736426783327, - "timestamp": 6.15289986369983 - }, - { - "x": 6.560248685316027, - "y": 6.311597917328536, - "heading": 3.587309338739589e-14, - "angularVelocity": -2.4269289060693314e-13, - "velocityX": 3.747694656595805, - "velocityY": -1.6309979397754133, - "timestamp": 6.22572009313854 - }, - { - "x": 6.80586589819516, - "y": 6.204705225984495, - "heading": 2.502296079067399e-14, - "angularVelocity": -1.489988795744515e-13, - "velocityX": 3.3729255561582545, - "velocityY": -1.467898304742482, - "timestamp": 6.29854032257725 - }, - { - "x": 7.024192320386008, - "y": 6.1096894956701115, - "heading": 1.7418086030342693e-14, - "angularVelocity": -1.0443354571867126e-13, - "velocityX": 2.9981561974424364, - "velocityY": -1.304798557306861, - "timestamp": 6.37136055201596 - }, - { - "x": 7.215227945620654, - "y": 6.026550729113183, - "heading": 1.1866391794709625e-14, - "angularVelocity": -7.623835132661919e-14, - "velocityX": 2.623386752652774, - "velocityY": -1.141698772411907, - "timestamp": 6.44418078145467 - }, - { - "x": 7.378972770765402, - "y": 5.955288927677494, - "heading": 7.783117662427377e-15, - "angularVelocity": -5.6073348899830995e-14, - "velocityX": 2.248617264829748, - "velocityY": -0.9785989687888353, - "timestamp": 6.51700101089338 - }, - { - "x": 7.515426793940112, - "y": 5.895904092181282, - "heading": 4.802760291325747e-15, - "angularVelocity": -4.0927602042373195e-14, - "velocityX": 1.8738477511878469, - "velocityY": -0.8154991539293902, - "timestamp": 6.58982124033209 - }, - { - "x": 7.624590013891394, - "y": 5.848396223170021, - "heading": 2.683796378124848e-15, - "angularVelocity": -2.909856134117839e-14, - "velocityX": 1.499078220333837, - "velocityY": -0.6523993315792364, - "timestamp": 6.6626414697708 - }, - { - "x": 7.706462429723989, - "y": 5.8127653210333285, - "heading": 1.2533736642541387e-15, - "angularVelocity": -1.9643205258982397e-14, - "velocityX": 1.124308677185698, - "velocityY": -0.48929950387867815, - "timestamp": 6.73546169920951 - }, - { - "x": 7.76104404076646, - "y": 5.789011386063414, - "heading": 3.9416699766786147e-16, - "angularVelocity": -1.1799010703604574e-14, - "velocityX": 0.7495391248170898, - "velocityY": -0.32619967216537205, - "timestamp": 6.80828192864822 - }, - { - "x": 7.788334846496582, - "y": 5.777134418487549, - "heading": 9.668206947791558e-29, - "angularVelocity": -5.412877722384226e-15, - "velocityX": 0.37476956527708116, - "velocityY": -0.1630998373310729, - "timestamp": 6.88110215808693 - }, - { - "x": 7.788334846496582, - "y": 5.777134418487549, - "heading": 6.841375823115053e-29, - "angularVelocity": 8.529174821322553e-29, - "velocityX": -8.917160283036205e-22, - "velocityY": -7.355274998038012e-21, - "timestamp": 6.9539223875256395 - }, - { - "x": 7.764426386423816, - "y": 5.787558027926257, - "heading": 9.971110849937308e-19, - "angularVelocity": 1.462722327199216e-17, - "velocityX": -0.3507276058737257, - "velocityY": 0.15291020717662415, - "timestamp": 7.022090560410709 - }, - { - "x": 7.716609466703687, - "y": 5.808405246618205, - "heading": 5.480572379025607e-18, - "angularVelocity": 6.577059504431801e-17, - "velocityX": -0.7014552055069206, - "velocityY": 0.3058204116325032, - "timestamp": 7.090258733295779 - }, - { - "x": 7.644884087867956, - "y": 5.839676074331557, - "heading": 1.0960016322262206e-17, - "angularVelocity": 8.038126461333601e-17, - "velocityX": -1.0521827973394524, - "velocityY": 0.4587306126874509, - "timestamp": 7.158426906180849 - }, - { - "x": 7.5492502506003065, - "y": 5.88137051076824, - "heading": 1.743462997857394e-17, - "angularVelocity": 9.498000873247208e-17, - "velocityX": -1.4029103791425612, - "velocityY": 0.611640809369773, - "timestamp": 7.226595079065919 - }, - { - "x": 7.429707955812324, - "y": 5.933488555530821, - "heading": 2.450986571187997e-17, - "angularVelocity": 1.0379089586106166e-16, - "velocityX": -1.753637947573107, - "velocityY": 0.7645510002219281, - "timestamp": 7.294763251950989 - }, - { - "x": 7.286257204780225, - "y": 5.996030208062897, - "heading": 3.2577664546725194e-17, - "angularVelocity": 1.1835140202508417e-16, - "velocityX": -2.104365497282067, - "velocityY": 0.9174611829118506, - "timestamp": 7.3629314248360584 - }, - { - "x": 7.118897999418333, - "y": 6.068995467529859, - "heading": 3.9164721464432884e-17, - "angularVelocity": 9.662950668074327e-17, - "velocityX": -2.455093018908655, - "velocityY": 1.070371353358427, - "timestamp": 7.431099597721128 - }, - { - "x": 6.927630342917185, - "y": 6.1523843325407, - "heading": 4.673832569637582e-17, - "angularVelocity": 1.1110176363638367e-16, - "velocityX": -2.8058204937313085, - "velocityY": 1.2232815033994346, - "timestamp": 7.499267770606198 - }, - { - "x": 6.712454241657855, - "y": 6.246196800313401, - "heading": 5.529092126185662e-17, - "angularVelocity": 1.254631773564182e-16, - "velocityX": -3.156547874946174, - "velocityY": 1.37619161262934, - "timestamp": 7.567435943491268 - }, - { - "x": 6.473369714783506, - "y": 6.350432862501926, - "heading": 6.480025692264811e-17, - "angularVelocity": 1.3949817426000514e-16, - "velocityX": -3.507274975338426, - "velocityY": 1.5291015994262664, - "timestamp": 7.635604116376338 - }, - { - "x": 6.217863323353405, - "y": 6.461828528523879, - "heading": 7.581887128261551e-17, - "angularVelocity": 1.6163869286199958e-16, - "velocityX": -3.7481772008306624, - "velocityY": 1.6341301417857155, - "timestamp": 7.703772289261408 - }, - { - "x": 5.962356931918229, - "y": 6.573224194534609, - "heading": 7.994866139472889e-17, - "angularVelocity": 6.058237939191866e-17, - "velocityX": -3.7481772009051104, - "velocityY": 1.6341301416210885, - "timestamp": 7.771940462146477 - }, - { - "x": 5.706850260692564, - "y": 6.684619218792329, - "heading": 8.350055926035511e-17, - "angularVelocity": 5.210492984138971e-17, - "velocityX": -3.7481813053203648, - "velocityY": 1.6341207273595257, - "timestamp": 7.840108635031547 - }, - { - "x": 5.445903710147097, - "y": 6.782595788135376, - "heading": 7.181307218009646e-17, - "angularVelocity": -1.714507897986972e-16, - "velocityX": -3.8279821726396834, - "velocityY": 1.4372773274741362, - "timestamp": 7.908276807916617 - }, - { - "x": 5.176941871643066, - "y": 6.855753421783447, - "heading": 6.07611657923737e-17, - "angularVelocity": -1.621270736768902e-16, - "velocityX": -3.9455632609883127, - "velocityY": 1.0731934061283093, - "timestamp": 7.976444980801687 - }, - { - "x": 4.892034856694491, - "y": 6.904225124198338, - "heading": 4.348929794316467e-17, - "angularVelocity": -2.443700947187874e-16, - "velocityX": -4.030991600719193, - "velocityY": 0.6857992785549824, - "timestamp": 8.047124119626423 - }, - { - "x": 4.6037716259633, - "y": 6.92486061131334, - "heading": -4.780536109631277e-18, - "angularVelocity": -6.829431548752389e-16, - "velocityX": -4.078476839481642, - "velocityY": 0.2919600812648877, - "timestamp": 8.117803258451158 - }, - { - "x": 4.323500629705874, - "y": 6.932939131074572, - "heading": -5.6528899699478266e-18, - "angularVelocity": -1.2342451744525731e-17, - "velocityX": -3.9653991392342633, - "velocityY": 0.11429850300331837, - "timestamp": 8.188482397275893 - }, - { - "x": 4.071256703765887, - "y": 6.940209763425182, - "heading": -5.97740235993501e-18, - "angularVelocity": -4.591346121302763e-18, - "velocityX": -3.568859639977803, - "velocityY": 0.10286815135991294, - "timestamp": 8.259161536100628 - }, - { - "x": 3.8470398688675607, - "y": 6.946672533421318, - "heading": -6.017364404100811e-18, - "angularVelocity": -5.6540083456305495e-19, - "velocityX": -3.172319847505807, - "velocityY": 0.09143815422202402, - "timestamp": 8.329840674925363 - }, - { - "x": 3.6508501319189572, - "y": 6.952327449414996, - "heading": -6.01163708431682e-18, - "angularVelocity": 8.103267776729523e-20, - "velocityX": -2.775779957295443, - "velocityY": 0.08000827525220806, - "timestamp": 8.400519813750098 - }, - { - "x": 3.482687496374107, - "y": 6.95717451558222, - "heading": -6.001606208178643e-18, - "angularVelocity": 1.4192131369018897e-19, - "velocityX": -2.3792400182159317, - "velocityY": 0.06857845536633275, - "timestamp": 8.471198952574833 - }, - { - "x": 3.342551964305426, - "y": 6.961213734428586, - "heading": -5.990861342748415e-18, - "angularVelocity": 1.5202315284148297e-19, - "velocityX": -1.9827000498149467, - "velocityY": 0.05714867093078821, - "timestamp": 8.541878091399568 - }, - { - "x": 3.2304435370945255, - "y": 6.964445107624494, - "heading": -5.979520825641484e-18, - "angularVelocity": 1.6045069862761274e-19, - "velocityX": -1.5861600618663185, - "velocityY": 0.04571891012878258, - "timestamp": 8.612557230224303 - }, - { - "x": 3.1463622157282694, - "y": 6.966868636363084, - "heading": -5.967668668372028e-18, - "angularVelocity": 1.6768961196496423e-19, - "velocityX": -1.1896200599550915, - "velocityY": 0.03428916620786852, - "timestamp": 8.683236369049038 - }, - { - "x": 3.0903080009468056, - "y": 6.968484321539212, - "heading": -5.968425491191159e-18, - "angularVelocity": -1.070786702703606e-20, - "velocityX": -0.7930800475719175, - "velocityY": 0.02285943494776875, - "timestamp": 8.753915507873772 - }, - { - "x": 3.0622808933258057, - "y": 6.969292163848877, - "heading": -5.968784000627222e-18, - "angularVelocity": -5.072351465622455e-21, - "velocityX": -0.396540027043897, - "velocityY": 0.011429713534966315, - "timestamp": 8.824594646698507 - }, - { - "x": 3.0622808933258057, - "y": 6.969292163848877, - "heading": -5.968784000628849e-18, - "angularVelocity": -4.846357394740608e-33, - "velocityX": -1.592220183778683e-21, - "velocityY": 4.017271607125232e-21, - "timestamp": 8.895273785523242 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - }, - { - "scope": [ - 7 - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 5 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "NoTeamTop": { - "waypoints": [ - { - "x": 0.7823778390884399, - "y": 6.68265962600708, - "heading": 1.0516504568237233, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 2.633068561553955, - "y": 7.049606800079346, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 1.366418480873108, - "y": 5.724342346191406, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 2.3937551975250244, - "y": 5.565863609313965, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 8 - }, - { - "x": 2.4097094535827637, - "y": 4.704334735870361, - "heading": -0.2730086143988003, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 7 - }, - { - "x": 2.617114305496216, - "y": 4.1299824714660645, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 2.0746705532073975, - "y": 4.2257080078125, - "heading": -0.6078018818929833, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.7823778390884399, - "y": 6.68265962600708, - "heading": 1.0516504568237233, - "angularVelocity": 0.000011116709994913391, - "velocityX": 3.4550282315085217, - "velocityY": 0.6846486983804677, - "timestamp": 0 - }, - { - "x": 0.9751074188860586, - "y": 6.720850881415975, - "heading": 1.0516503577916685, - "angularVelocity": -0.00000190674765800839, - "velocityX": 3.710785114782621, - "velocityY": 0.7353284443156448, - "timestamp": 0.05193768268333393 - }, - { - "x": 1.167836999199548, - "y": 6.7590421342229945, - "heading": 1.051650258760242, - "angularVelocity": -0.0000019067355590266187, - "velocityX": 3.7107851247151196, - "velocityY": 0.7353283942195143, - "timestamp": 0.10387536536666786 - }, - { - "x": 1.3605665805829503, - "y": 6.797233381630862, - "heading": 1.0516501597288161, - "angularVelocity": -0.000001906735551697809, - "velocityX": 3.710785145315053, - "velocityY": 0.7353282902651058, - "timestamp": 0.1558130480500018 - }, - { - "x": 1.5532961655237487, - "y": 6.835424611086715, - "heading": 1.051650060697391, - "angularVelocity": -0.0000019067355329513512, - "velocityX": 3.710785213808596, - "velocityY": 0.7353279446198315, - "timestamp": 0.20775073073333572 - }, - { - "x": 1.7460254429890028, - "y": 6.873617391792323, - "heading": 1.0516499614844557, - "angularVelocity": -0.000001910230304114969, - "velocityX": 3.710779293722669, - "velocityY": 0.7353578121394269, - "timestamp": 0.25968841341666965 - }, - { - "x": 1.9209155764266739, - "y": 6.9133027716288264, - "heading": 1.0096996919934733, - "angularVelocity": -0.8077039121432283, - "velocityX": 3.3673072112974913, - "velocityY": 0.7640960818076123, - "timestamp": 0.3116260961000036 - }, - { - "x": 2.078239262369381, - "y": 6.947237798484431, - "heading": 0.9225295067368948, - "angularVelocity": -1.6783610810682208, - "velocityX": 3.029085585160111, - "velocityY": 0.653379687008914, - "timestamp": 0.3635637787833375 - }, - { - "x": 2.2178882418919197, - "y": 6.9754482977346735, - "heading": 0.7884918073872876, - "angularVelocity": -2.580740849891981, - "velocityX": 2.6887795586488554, - "velocityY": 0.5431605299420711, - "timestamp": 0.41550146146667144 - }, - { - "x": 2.3397525319384047, - "y": 6.998192697437133, - "heading": 0.60541415124654, - "angularVelocity": -3.5249484898465573, - "velocityX": 2.346355935621853, - "velocityY": 0.4379171061815228, - "timestamp": 0.46743914415000537 - }, - { - "x": 2.4395876082580847, - "y": 7.016459363314808, - "heading": 0.4250239365373652, - "angularVelocity": -3.473204913839163, - "velocityX": 1.922208908094313, - "velocityY": 0.3517035211032972, - "timestamp": 0.5193768268333393 - }, - { - "x": 2.5180690988084318, - "y": 7.03039819123932, - "heading": 0.265125600134493, - "angularVelocity": -3.0786575014865125, - "velocityX": 1.5110703153402525, - "velocityY": 0.26837600763782044, - "timestamp": 0.5713145095166732 - }, - { - "x": 2.5760585000102645, - "y": 7.040303686655802, - "heading": 0.1368038723234758, - "angularVelocity": -2.470686429993418, - "velocityX": 1.1165188396139325, - "velocityY": 0.1907188558426073, - "timestamp": 0.6232521922000072 - }, - { - "x": 2.6142221606288607, - "y": 7.046583625856348, - "heading": 0.04689928938368535, - "angularVelocity": -1.7310087453832381, - "velocityX": 0.7347971385493195, - "velocityY": 0.12091296484741729, - "timestamp": 0.6751898748833411 - }, - { - "x": 2.633068561553955, - "y": 7.049606800079346, - "heading": 4.3474215162415935e-31, - "angularVelocity": -0.9029915652885839, - "velocityX": 0.36286564881998395, - "velocityY": 0.05820772253990189, - "timestamp": 0.7271275575666751 - }, - { - "x": 2.633068561553955, - "y": 7.049606800079346, - "heading": 3.803214140819317e-31, - "angularVelocity": 3.020347919142555e-31, - "velocityX": 7.840324925442265e-34, - "velocityY": 5.62397113618381e-32, - "timestamp": 0.7790652402500091 - }, - { - "x": 2.609144744005489, - "y": 7.015299626278446, - "heading": 0.018678562047563966, - "angularVelocity": 0.2627050620237914, - "velocityX": -0.33647707767394147, - "velocityY": -0.4825140285572393, - "timestamp": 0.8501661238937067 - }, - { - "x": 2.5612954442990494, - "y": 6.946685760529339, - "heading": 0.056012535459477056, - "angularVelocity": 0.5250845207353814, - "velocityX": -0.6729775672862703, - "velocityY": -0.9650212800862848, - "timestamp": 0.9212670075374043 - }, - { - "x": 2.4895180668345147, - "y": 6.843766221469334, - "heading": 0.11197590720911785, - "angularVelocity": 0.7870981186406304, - "velocityX": -1.009514562775715, - "velocityY": -1.4475142049676655, - "timestamp": 0.9923678911811019 - }, - { - "x": 2.393809314296558, - "y": 6.706543018483128, - "heading": 0.1865585662991481, - "angularVelocity": 1.0489695101931586, - "velocityX": -1.346097933431799, - "velocityY": -1.929978868812128, - "timestamp": 1.0634687748247995 - }, - { - "x": 2.298062994828891, - "y": 6.56935427735818, - "heading": 0.26139370468794165, - "angularVelocity": 1.0525205110503135, - "velocityX": -1.3466262943717173, - "velocityY": -1.9294941791782931, - "timestamp": 1.1345696584684972 - }, - { - "x": 2.22625248415356, - "y": 6.466464285312162, - "heading": 0.3175528585708617, - "angularVelocity": 0.7898517009204281, - "velocityX": -1.0099805655748195, - "velocityY": -1.447098640315394, - "timestamp": 1.205670542112195 - }, - { - "x": 2.178378576021294, - "y": 6.397871445224483, - "heading": 0.355003104186284, - "angularVelocity": 0.5267198337941039, - "velocityX": -0.6733236730526801, - "velocityY": -0.9647255641914598, - "timestamp": 1.2767714257558926 - }, - { - "x": 2.154441595077514, - "y": 6.363574981689453, - "heading": 0.3737268100062681, - "angularVelocity": 0.2633399876408391, - "velocityX": -0.3366622145476251, - "velocityY": -0.48236339377858967, - "timestamp": 1.3478723093995904 - }, - { - "x": 2.154441595077514, - "y": 6.363574981689453, - "heading": 0.3737268100062681, - "angularVelocity": 3.1895226258919956e-35, - "velocityX": -6.441615179673898e-36, - "velocityY": -6.456245804980884e-36, - "timestamp": 1.418973193043288 - }, - { - "x": 2.1664141619492328, - "y": 6.323693868020063, - "heading": 0.35495101918303457, - "angularVelocity": -0.2646577261921422, - "velocityX": 0.16876159064530338, - "velocityY": -0.5621518135305482, - "timestamp": 1.489916864680527 - }, - { - "x": 2.1903575818390344, - "y": 6.243931303899423, - "heading": 0.3173922807828279, - "angularVelocity": -0.5294163317661137, - "velocityX": 0.3374990233411272, - "velocityY": -1.1243083742337978, - "timestamp": 1.560860536317766 - }, - { - "x": 2.226269330136065, - "y": 6.124286501815425, - "heading": 0.26105173416926286, - "angularVelocity": -0.7941588772238201, - "velocityX": 0.5062008699050788, - "velocityY": -1.6864760354635386, - "timestamp": 1.6318042079550048 - }, - { - "x": 2.2741463498064425, - "y": 5.964757852535621, - "heading": 0.18595590358446726, - "angularVelocity": -1.0585275451880791, - "velocityX": 0.674859625467234, - "velocityY": -2.248666379934951, - "timestamp": 1.7027478795922437 - }, - { - "x": 2.321990419372695, - "y": 5.805201381630825, - "heading": 0.11153311841802548, - "angularVelocity": -1.0490405056421792, - "velocityX": 0.6743951710153469, - "velocityY": -2.2490585449350284, - "timestamp": 1.7736915512294826 - }, - { - "x": 2.3578729357424715, - "y": 5.685532745421478, - "heading": 0.055757885713101384, - "angularVelocity": -0.7861903876377221, - "velocityX": 0.5057888257215818, - "velocityY": -1.6868119939049238, - "timestamp": 1.8446352228667215 - }, - { - "x": 2.381794458514866, - "y": 5.6057532815472815, - "heading": 0.018587215775969886, - "angularVelocity": -0.5239462390274757, - "velocityX": 0.33719036836314187, - "velocityY": -1.1245465879203078, - "timestamp": 1.9155788945039605 - }, - { - "x": 2.3937551975250244, - "y": 5.565863609313965, - "heading": -5.628092987966886e-31, - "angularVelocity": -0.26199963079178046, - "velocityX": 0.16859486877586724, - "velocityY": -0.5622724523941706, - "timestamp": 1.9865225661411994 - }, - { - "x": 2.3937551975250244, - "y": 5.565863609313965, - "heading": -5.174700324190939e-31, - "angularVelocity": -3.008085461468343e-31, - "velocityX": 1.034031579268112e-34, - "velocityY": 1.7448699710998943e-32, - "timestamp": 2.057466237778438 - }, - { - "x": 2.394754254757926, - "y": 5.5120190095853445, - "heading": -0.017115260393375086, - "angularVelocity": -0.21254841017002704, - "velocityX": 0.012406941036337042, - "velocityY": -0.6686771808034808, - "timestamp": 2.137990295237555 - }, - { - "x": 2.396751769962143, - "y": 5.4043296750978795, - "heading": -0.051339403058144904, - "angularVelocity": -0.4250176126823665, - "velocityX": 0.02480644005340539, - "velocityY": -1.337356038499925, - "timestamp": 2.218514352696672 - }, - { - "x": 2.3997467282028473, - "y": 5.242795162936243, - "heading": -0.10265156806320024, - "angularVelocity": -0.6372277630336057, - "velocityX": 0.03719333495117604, - "velocityY": -2.006040396606318, - "timestamp": 2.2990384101557892 - }, - { - "x": 2.403732200664872, - "y": 5.027411442372448, - "heading": -0.17081671584355101, - "angularVelocity": -0.8465190395423907, - "velocityX": 0.04949418332587571, - "velocityY": -2.674774810908508, - "timestamp": 2.3795624676149063 - }, - { - "x": 2.4067209273092627, - "y": 4.865873194470122, - "heading": -0.22191798416318598, - "angularVelocity": -0.6346087111367488, - "velocityX": 0.037115946944272976, - "velocityY": -2.0060867894584153, - "timestamp": 2.4600865250740234 - }, - { - "x": 2.4087132882328275, - "y": 4.758180884503706, - "heading": -0.2559781943076682, - "angularVelocity": -0.42298179226466653, - "velocityX": 0.024742430851509695, - "velocityY": -1.337392989928425, - "timestamp": 2.5406105825331404 - }, - { - "x": 2.4097094535827637, - "y": 4.704334735870361, - "heading": -0.2730086143988003, - "angularVelocity": -0.2114948082413471, - "velocityX": 0.012371027757036695, - "velocityY": -0.6686964161074856, - "timestamp": 2.6211346399922575 - }, - { - "x": 2.4097094535827637, - "y": 4.704334735870361, - "heading": -0.2730086143988003, - "angularVelocity": 8.309717858612118e-31, - "velocityX": 6.470358508109623e-32, - "velocityY": -3.482387330812075e-33, - "timestamp": 2.7016586974513745 - }, - { - "x": 2.426997620799911, - "y": 4.656468232431784, - "heading": -0.25045854641235116, - "angularVelocity": 0.2875344989961346, - "velocityX": 0.22044033314361022, - "velocityY": -0.6103427756039931, - "timestamp": 2.780084305735521 - }, - { - "x": 2.4615718445380113, - "y": 4.5607357498021335, - "heading": -0.20530912855642702, - "angularVelocity": 0.5756973881839936, - "velocityX": 0.4408537529327513, - "velocityY": -1.2206788665610262, - "timestamp": 2.8585099140196673 - }, - { - "x": 2.5134294609331516, - "y": 4.41713890238669, - "heading": -0.1374653380839415, - "angularVelocity": 0.8650719064451283, - "velocityX": 0.6612332059606509, - "velocityY": -1.8309943723378186, - "timestamp": 2.9369355223038136 - }, - { - "x": 2.5652719402701414, - "y": 4.273560233385289, - "heading": -0.06875061341189337, - "angularVelocity": 0.8761771336613093, - "velocityX": 0.6610401942839578, - "velocityY": -1.8307625805233965, - "timestamp": 3.01536113058796 - }, - { - "x": 2.599833500729999, - "y": 4.1778417623198285, - "heading": -0.02291575248408611, - "angularVelocity": 0.5844374296944103, - "velocityX": 0.44069228426812324, - "velocityY": -1.2205002059870416, - "timestamp": 3.0937867388721063 - }, - { - "x": 2.617114305496216, - "y": 4.1299824714660645, - "heading": -1.023528578657426e-34, - "angularVelocity": 0.2921973190320597, - "velocityX": 0.22034645499472394, - "velocityY": -0.6102508083885511, - "timestamp": 3.1722123471562527 - }, - { - "x": 2.617114305496216, - "y": 4.1299824714660645, - "heading": -1.8621051633589405e-34, - "angularVelocity": 8.300885938147206e-34, - "velocityX": 2.197325936488708e-35, - "velocityY": -1.68400260681791e-35, - "timestamp": 3.250637955440399 - }, - { - "x": 2.590026298993156, - "y": 4.134571386049753, - "heading": -0.03154497655949952, - "angularVelocity": -0.5370590154132491, - "velocityX": -0.461178282209251, - "velocityY": 0.07812711299635268, - "timestamp": 3.3093744735029933 - }, - { - "x": 2.5358680596167886, - "y": 4.143794140189051, - "heading": -0.09482876594361848, - "angularVelocity": -1.0774181288151696, - "velocityX": -0.9220539651099411, - "velocityY": 0.15701908188479646, - "timestamp": 3.3681109915655876 - }, - { - "x": 2.454656525048389, - "y": 4.157724678166265, - "heading": -0.1899314404675443, - "angularVelocity": -1.6191404880789242, - "velocityX": -1.382641280878349, - "velocityY": 0.23716996575057447, - "timestamp": 3.426847509628182 - }, - { - "x": 2.346386675307616, - "y": 4.176461325890866, - "heading": -0.3164723678981996, - "angularVelocity": -2.154382513716643, - "velocityX": -1.8433140627333764, - "velocityY": 0.31899486627099644, - "timestamp": 3.485584027690776 - }, - { - "x": 2.2377299397341766, - "y": 4.196152369291608, - "heading": -0.43354708531926917, - "angularVelocity": -1.9932185509584521, - "velocityX": -1.8499008650401365, - "velocityY": 0.3352436278186754, - "timestamp": 3.5443205457533704 - }, - { - "x": 2.1562058087373717, - "y": 4.21092916440678, - "heading": -0.5207849832743855, - "angularVelocity": -1.4852412235629702, - "velocityX": -1.387963292443145, - "velocityY": 0.2515776488389278, - "timestamp": 3.6030570638159647 - }, - { - "x": 2.101847539092883, - "y": 4.220781491832273, - "heading": -0.5787788056200436, - "angularVelocity": -0.9873554691113143, - "velocityX": -0.925459517136503, - "velocityY": 0.1677376826286145, - "timestamp": 3.661793581878559 - }, - { - "x": 2.0746705532073975, - "y": 4.2257080078125, - "heading": -0.6078018818929833, - "angularVelocity": -0.49412320018715394, - "velocityX": -0.4626931725255473, - "velocityY": 0.08387483873280513, - "timestamp": 3.7205300999411532 - }, - { - "x": 2.0746705532073975, - "y": 4.2257080078125, - "heading": -0.6078018818929833, - "angularVelocity": 9.526213155938014e-34, - "velocityX": 0, - "velocityY": 0, - "timestamp": 3.7792666180037475 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - }, - { - "scope": [ - 5 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "3NB-1stN": { - "waypoints": [ - { - "x": 0.71, - "y": 4.38, - "heading": -1.0303768814651526, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 2.37, - "y": 2.8567728996276855, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 22 - }, - { - "x": 7.745757102966309, - "y": 0.7672333121299744, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 - }, - { - "x": 2.37, - "y": 2.8567728996276855, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.71, - "y": 4.38, - "heading": -1.0303768814651526, - "angularVelocity": 0, - "velocityX": 2.0971536544825226e-38, - "velocityY": 1.1304744522998729e-38, - "timestamp": 0 - }, - { - "x": 0.809116701461151, - "y": 4.380000000002171, - "heading": -1.0303768755698945, - "angularVelocity": 4.436264736581951e-8, - "velocityX": 0.7458671604401304, - "velocityY": 1.6337868985579388e-11, - "timestamp": 0.1328878743001141 - }, - { - "x": 0.9420045739227598, - "y": 4.380000000000036, - "heading": -1.001743993813421, - "angularVelocity": 0.21546647432864371, - "velocityX": 0.9999999861649881, - "velocityY": -1.6060202539031698e-11, - "timestamp": 0.2657757486002282 - }, - { - "x": 1.0748924463843688, - "y": 4.380000000000185, - "heading": -0.9535209860370869, - "angularVelocity": 0.36288493611860506, - "velocityX": 0.9999999861649895, - "velocityY": 1.1165887293235233e-12, - "timestamp": 0.3986636229003423 - }, - { - "x": 1.2077803188459775, - "y": 4.380000000000181, - "heading": -0.894987542957539, - "angularVelocity": 0.440472416221783, - "velocityX": 0.9999999861649895, - "velocityY": -2.8685249700374206e-14, - "timestamp": 0.5315514972004564 - }, - { - "x": 1.3406681913075864, - "y": 4.3800000000000825, - "heading": -0.8311473610319392, - "angularVelocity": 0.48040637463598235, - "velocityX": 0.9999999861649895, - "velocityY": -7.450142676686956e-13, - "timestamp": 0.6644393715005704 - }, - { - "x": 1.4735560637691953, - "y": 4.379999999999938, - "heading": -0.7647977755611476, - "angularVelocity": 0.4992899903037624, - "velocityX": 0.9999999861649895, - "velocityY": -1.0891621656864383e-12, - "timestamp": 0.7973272458006845 - }, - { - "x": 1.6064439362308043, - "y": 4.379999999999788, - "heading": -0.6976974114003008, - "angularVelocity": 0.5049397058553835, - "velocityX": 0.9999999861649895, - "velocityY": -1.1281631993388294e-12, - "timestamp": 0.9302151201007985 - }, - { - "x": 1.7393318086924132, - "y": 4.379999999999669, - "heading": -0.6313021688387096, - "angularVelocity": 0.4996335663526692, - "velocityX": 0.9999999861649895, - "velocityY": -8.982783325849748e-13, - "timestamp": 1.0631029944009127 - }, - { - "x": 1.8722196811540222, - "y": 4.379999999999615, - "heading": -0.5673736940074112, - "angularVelocity": 0.4810707912064441, - "velocityX": 0.9999999861649895, - "velocityY": -4.0282423623910287e-13, - "timestamp": 1.1959908687010268 - }, - { - "x": 2.0051075536156313, - "y": 4.379999999999665, - "heading": -0.5087129398828393, - "angularVelocity": 0.44143044979478335, - "velocityX": 0.9999999861649895, - "velocityY": 3.753353729505819e-13, - "timestamp": 1.3288787430011408 - }, - { - "x": 2.1379954260772402, - "y": 4.379999999999856, - "heading": -0.4603202485132628, - "angularVelocity": 0.3641618290942509, - "velocityX": 0.9999999861649895, - "velocityY": 1.4392523553285558e-12, - "timestamp": 1.461766617301255 - }, - { - "x": 2.2708832985388487, - "y": 4.379999999998099, - "heading": -0.4314454065710539, - "angularVelocity": 0.21728725885853115, - "velocityX": 0.9999999861649881, - "velocityY": -1.3220835293967591e-11, - "timestamp": 1.594654491601369 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": 4.944810808316635e-8, - "velocityX": 0.7458671604401304, - "velocityY": 1.4301174702823493e-11, - "timestamp": 1.727542365901483 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": 0, - "velocityX": 4.042058444555445e-32, - "velocityY": -2.454352935526138e-31, - "timestamp": 1.860430240201597 - }, - { - "x": 2.3630236433782694, - "y": 4.362669397059245, - "heading": -0.4294233067682944, - "angularVelocity": 0.0350427544731118, - "velocityX": -0.1208998419972325, - "velocityY": -0.30033830993037086, - "timestamp": 1.9181338443297877 - }, - { - "x": 2.34987568315914, - "y": 4.327705672048482, - "heading": -0.4253632837544961, - "angularVelocity": 0.07035995541499669, - "velocityX": -0.22785336233062292, - "velocityY": -0.6059192582336417, - "timestamp": 1.9758374484579784 - }, - { - "x": 2.331570009798934, - "y": 4.274786157183859, - "heading": -0.41924975262264014, - "angularVelocity": 0.10594712798520795, - "velocityX": -0.3172362218423166, - "velocityY": -0.917092019885982, - "timestamp": 2.033541052586169 - }, - { - "x": 2.3094077007864797, - "y": 4.2035872375384535, - "heading": -0.41106959562082307, - "angularVelocity": 0.1417616304094878, - "velocityX": -0.3840714864768336, - "velocityY": -1.2338730088200456, - "timestamp": 2.0912446567143594 - }, - { - "x": 2.2850867554506067, - "y": 4.113831614360468, - "heading": -0.4008169264246363, - "angularVelocity": 0.17767814248291539, - "velocityX": -0.42148052454380325, - "velocityY": -1.5554595684977943, - "timestamp": 2.14894826084255 - }, - { - "x": 2.260845942760846, - "y": 4.00539414959916, - "heading": -0.38850351266806354, - "angularVelocity": 0.21339072216498506, - "velocityX": -0.4200918305908423, - "velocityY": -1.8792147630924392, - "timestamp": 2.2066518649707407 - }, - { - "x": 2.2396150278583664, - "y": 3.8785182297474, - "heading": -0.37418142990770303, - "angularVelocity": 0.24820083557341727, - "velocityX": -0.3679304823911001, - "velocityY": -2.1987520843574346, - "timestamp": 2.2643554690989314 - }, - { - "x": 2.2250552117068545, - "y": 3.734190340531852, - "heading": -0.3579887048437084, - "angularVelocity": 0.2806189545434901, - "velocityX": -0.2523207409924592, - "velocityY": -2.501193667122479, - "timestamp": 2.322059073227122 - }, - { - "x": 2.221244664883484, - "y": 3.57457876197761, - "heading": -0.34021831223188553, - "angularVelocity": 0.3079598385604548, - "velocityX": -0.06603654799218, - "velocityY": -2.7660590870483115, - "timestamp": 2.3797626773553127 - }, - { - "x": 2.2318839537226465, - "y": 3.4031176182672533, - "heading": -0.32135306694663757, - "angularVelocity": 0.3269335697506321, - "velocityX": 0.18437823772063575, - "velocityY": -2.971411340777662, - "timestamp": 2.4374662814835033 - }, - { - "x": 2.259517906960715, - "y": 3.223882144792273, - "heading": -0.3019785070706419, - "angularVelocity": 0.3357599610732849, - "velocityX": 0.4788947528626231, - "velocityY": -3.1061400094973273, - "timestamp": 2.495169885611694 - }, - { - "x": 2.305422788003848, - "y": 3.0407166406448676, - "heading": -0.2826352454002691, - "angularVelocity": 0.3352175650466901, - "velocityX": 0.7955288363161972, - "velocityY": -3.174247205429028, - "timestamp": 2.5528734897398846 - }, - { - "x": 2.37, - "y": 2.8567728996276855, - "heading": -0.2637444281681159, - "angularVelocity": 0.3273767300585669, - "velocityX": 1.1191192122579394, - "velocityY": -3.187734003722659, - "timestamp": 2.6105770938680752 - }, - { - "x": 2.5101147898329845, - "y": 2.582541241565645, - "heading": -0.2369267963688336, - "angularVelocity": 0.3075125984694063, - "velocityX": 1.6066692028575509, - "velocityY": -3.144561398428411, - "timestamp": 2.6977853316208464 - }, - { - "x": 2.692445936304254, - "y": 2.3145698313008403, - "heading": -0.2138681460623352, - "angularVelocity": 0.2644090845158318, - "velocityX": 2.090756001606738, - "velocityY": -3.072776347400009, - "timestamp": 2.7849935693736176 - }, - { - "x": 2.916145350701991, - "y": 2.057000183979087, - "heading": -0.1978775533130541, - "angularVelocity": 0.18336103516336708, - "velocityX": 2.565117931081324, - "velocityY": -2.9535013429847994, - "timestamp": 2.872201807126389 - }, - { - "x": 3.1783469526388295, - "y": 1.817614435910448, - "heading": -0.19483940838664568, - "angularVelocity": 0.0348378204206141, - "velocityX": 3.0066150709301063, - "velocityY": -2.744990086231003, - "timestamp": 2.95941004487916 - }, - { - "x": 3.4685657965160517, - "y": 1.6104254712653228, - "heading": -0.1948394011506557, - "angularVelocity": 8.297369784385518e-8, - "velocityX": 3.327883366933304, - "velocityY": -2.375795796181351, - "timestamp": 3.0466182826319312 - }, - { - "x": 3.7814630349981173, - "y": 1.4394004168841172, - "heading": -0.19483939812430692, - "angularVelocity": 3.4702556370809717e-8, - "velocityX": 3.5879321328443092, - "velocityY": -1.9611112297253483, - "timestamp": 3.1338265203847024 - }, - { - "x": 4.112554783061403, - "y": 1.3069900395824194, - "heading": -0.19483939551160215, - "angularVelocity": 2.995938037806074e-8, - "velocityX": 3.7965650561793693, - "velocityY": -1.518324194067512, - "timestamp": 3.2210347581374736 - }, - { - "x": 4.45709640896014, - "y": 1.2150916006881398, - "heading": -0.1948393931126124, - "angularVelocity": 2.7508751650457417e-8, - "velocityX": 3.950792204692831, - "velocityY": -1.0537816296109497, - "timestamp": 3.308242995890245 - }, - { - "x": 4.810149751561953, - "y": 1.1650157280273563, - "heading": -0.1948393907882322, - "angularVelocity": 2.6653218265092305e-8, - "velocityX": 4.048394414336398, - "velocityY": -0.5742103492351371, - "timestamp": 3.395451233643016 - }, - { - "x": 5.163509032960075, - "y": 1.1171463873442062, - "heading": -0.19483938846606072, - "angularVelocity": 2.6627891462865042e-8, - "velocityX": 4.051902555350812, - "velocityY": -0.5489084737505261, - "timestamp": 3.4826594713957872 - }, - { - "x": 5.516868318784233, - "y": 1.0692770793329736, - "heading": -0.19483938614388904, - "angularVelocity": 2.6627893716137373e-8, - "velocityX": 4.0519026061033125, - "velocityY": -0.5489080991079575, - "timestamp": 3.5698677091485584 - }, - { - "x": 5.870227604608456, - "y": 1.0214077713222123, - "heading": -0.1948393838217175, - "angularVelocity": 2.6627892144106447e-8, - "velocityX": 4.051902606104045, - "velocityY": -0.5489080991025519, - "timestamp": 3.6570759469013296 - }, - { - "x": 6.223586890335042, - "y": 0.9735384625909697, - "heading": -0.1948393814995219, - "angularVelocity": 2.6628167647885842e-8, - "velocityX": 4.051902604984475, - "velocityY": -0.5489081073641706, - "timestamp": 3.744284184654101 - }, - { - "x": 6.56185786613434, - "y": 0.9276752702257555, - "heading": -0.15388858338732217, - "angularVelocity": 0.46957488383510493, - "velocityX": 3.878887872476839, - "velocityY": -0.5259043588914193, - "timestamp": 3.831492422406872 - }, - { - "x": 6.857837819484865, - "y": 0.8875569901395891, - "heading": -0.11639807755802914, - "angularVelocity": 0.4298963812981445, - "velocityX": 3.393944895316064, - "velocityY": -0.4600285606054367, - "timestamp": 3.9187006601596432 - }, - { - "x": 7.1115317110378875, - "y": 0.8531747968258696, - "heading": -0.08363192967569993, - "angularVelocity": 0.37572308220727396, - "velocityX": 2.9090588009842437, - "velocityY": -0.39425396269720775, - "timestamp": 4.0059088979124144 - }, - { - "x": 7.322941572891862, - "y": 0.8245255713030246, - "heading": -0.05600648536782893, - "angularVelocity": 0.31677562830923217, - "velocityX": 2.4241960083324776, - "velocityY": -0.3285151295476042, - "timestamp": 4.093117135665186 - }, - { - "x": 7.492068478804665, - "y": 0.8016076744881854, - "heading": -0.03372719469686328, - "angularVelocity": 0.2554723182702995, - "velocityX": 1.9393455282546543, - "velocityY": -0.26279509144433477, - "timestamp": 4.180325373417958 - }, - { - "x": 7.618913088814633, - "y": 0.7844200882405312, - "heading": -0.016915967524029574, - "angularVelocity": 0.1927710914247968, - "velocityX": 1.4545026167088106, - "velocityY": -0.19708672816499287, - "timestamp": 4.267533611170729 - }, - { - "x": 7.703475852711533, - "y": 0.7729621267495633, - "heading": -0.005653945175052056, - "angularVelocity": 0.12913943268645117, - "velocityX": 0.9696648628152448, - "velocityY": -0.1313862289420392, - "timestamp": 4.354741848923501 - }, - { - "x": 7.745757102966309, - "y": 0.7672333121299744, - "heading": 2.3206028060930825e-30, - "angularVelocity": 0.06483269609323732, - "velocityX": 0.4848309213017403, - "velocityY": -0.06569120953750239, - "timestamp": 4.441950086676273 - }, - { - "x": 7.745757102966309, - "y": 0.7672333121299744, - "heading": -8.234811939268404e-31, - "angularVelocity": -3.633238130815655e-29, - "velocityX": -2.7472728518460047e-31, - "velocityY": -1.5587964590889528e-30, - "timestamp": 4.529158324429044 - }, - { - "x": 7.70654223065801, - "y": 0.7719092516777752, - "heading": -0.004953190228808496, - "angularVelocity": -0.059037568964601335, - "velocityX": -0.467405979054445, - "velocityY": 0.05573299041131227, - "timestamp": 4.613057277513962 - }, - { - "x": 7.628112492041216, - "y": 0.7812611877206381, - "heading": -0.014860324472800461, - "angularVelocity": -0.11808412238427937, - "velocityX": -0.9348118865966086, - "velocityY": 0.11146665958271756, - "timestamp": 4.696956230598881 - }, - { - "x": 7.510467900615875, - "y": 0.7952892053137158, - "heading": -0.029723096869892895, - "angularVelocity": -0.1771508684029673, - "velocityX": -1.4022176332315919, - "velocityY": 0.1672013425349818, - "timestamp": 4.780855183683799 - }, - { - "x": 7.353608476239863, - "y": 0.813993418196902, - "heading": -0.049543726815246006, - "angularVelocity": -0.2362440676141995, - "velocityX": -1.8696231431785302, - "velocityY": 0.2229373811643965, - "timestamp": 4.864754136768717 - }, - { - "x": 7.157534243577833, - "y": 0.8373739693753024, - "heading": -0.07432438646100914, - "angularVelocity": -0.29536315692381165, - "velocityX": -2.3370283591450236, - "velocityY": 0.2786751242859491, - "timestamp": 4.948653089853635 - }, - { - "x": 6.922245230336082, - "y": 0.8654310314879592, - "heading": -0.10406646621205466, - "angularVelocity": -0.3544988186079438, - "velocityX": -2.8044332448773686, - "velocityY": 0.33441492510948284, - "timestamp": 5.032552042938553 - }, - { - "x": 6.647741466165584, - "y": 0.8981648066152749, - "heading": -0.13876967015547761, - "angularVelocity": -0.413630953276612, - "velocityX": -3.2718377771968092, - "velocityY": 0.3901571345495152, - "timestamp": 5.116450996023471 - }, - { - "x": 6.334022988624143, - "y": 0.9355755244610184, - "heading": -0.1784308890646727, - "angularVelocity": -0.47272602876288267, - "velocityX": -3.739241861861053, - "velocityY": 0.44590208185170727, - "timestamp": 5.200349949108389 - }, - { - "x": 5.993381166833567, - "y": 0.9761991039310791, - "heading": -0.17843089111825428, - "angularVelocity": -2.4476843635546923e-8, - "velocityX": -4.0601438905417195, - "velocityY": 0.4841965003895071, - "timestamp": 5.2842489021933075 - }, - { - "x": 5.6527393449598975, - "y": 1.0168226827053695, - "heading": -0.17843089317177263, - "angularVelocity": -2.4476090712713582e-8, - "velocityX": -4.060143891532119, - "velocityY": 0.4841964920965512, - "timestamp": 5.368147855278226 - }, - { - "x": 5.312097523086226, - "y": 1.0574462614796487, - "heading": -0.17843089522529096, - "angularVelocity": -2.447609001089921e-8, - "velocityX": -4.060143891532136, - "velocityY": 0.48419649209641696, - "timestamp": 5.452046808363144 - }, - { - "x": 4.9714557012148255, - "y": 1.0980698402729543, - "heading": -0.17843089727880931, - "angularVelocity": -2.44760903452213e-8, - "velocityX": -4.060143891505091, - "velocityY": 0.48419649232319817, - "timestamp": 5.535945761448062 - }, - { - "x": 4.63081401567638, - "y": 1.1386945622457554, - "heading": -0.1784308993323284, - "angularVelocity": -2.4476099139063654e-8, - "velocityX": -4.060142266538957, - "velocityY": 0.4842101179937616, - "timestamp": 5.61984471453298 - }, - { - "x": 4.294909882891934, - "y": 1.2083762621040492, - "heading": -0.17843090142312937, - "angularVelocity": -2.4920465555084847e-8, - "velocityX": -4.003674902170267, - "velocityY": 0.8305431390516352, - "timestamp": 5.703743667617898 - }, - { - "x": 3.9692448455861586, - "y": 1.3162159960510575, - "heading": -0.1784309036504149, - "angularVelocity": -2.654723854820396e-8, - "velocityX": -3.881634100680077, - "velocityY": 1.2853525578305942, - "timestamp": 5.787642620702816 - }, - { - "x": 3.6581383145057944, - "y": 1.4607837972243456, - "heading": -0.17843090615389368, - "angularVelocity": -2.983921383921478e-8, - "velocityX": -3.7081098111615183, - "velocityY": 1.723118058779164, - "timestamp": 5.871541573787734 - }, - { - "x": 3.3657165804558278, - "y": 1.6401623049726775, - "heading": -0.17843090915540483, - "angularVelocity": -3.5775311105492386e-8, - "velocityX": -3.4854038494853836, - "velocityY": 2.1380303466573016, - "timestamp": 5.9554405268726525 - }, - { - "x": 3.0958581558807583, - "y": 1.851972375004246, - "heading": -0.1784309902750725, - "angularVelocity": -9.668734194205462e-7, - "velocityX": -3.2164695106735524, - "velocityY": 2.524585376139148, - "timestamp": 6.039339479957571 - }, - { - "x": 2.8575420142720582, - "y": 2.0873477427865708, - "heading": -0.1922018309233486, - "angularVelocity": -0.1641360248480786, - "velocityX": -2.8405138901732085, - "velocityY": 2.805462513269867, - "timestamp": 6.123238433042489 - }, - { - "x": 2.6562685264660293, - "y": 2.336347374122755, - "heading": -0.21363610908830247, - "angularVelocity": -0.25547730188310785, - "velocityX": -2.398998800405901, - "velocityY": 2.96785147109238, - "timestamp": 6.207137386127407 - }, - { - "x": 2.4935734358591812, - "y": 2.5938140264471015, - "heading": -0.23928053059906224, - "angularVelocity": -0.3056584208482637, - "velocityX": -1.9391790317356759, - "velocityY": 3.068770739770169, - "timestamp": 6.291036339212325 - }, - { - "x": 2.37, - "y": 2.8567728996276855, - "heading": -0.2670876942616293, - "angularVelocity": -0.3314363605278995, - "velocityX": -1.4728841220951399, - "velocityY": 3.1342330686108957, - "timestamp": 6.374935292297243 - }, - { - "x": 2.299791533091049, - "y": 3.0546011768493755, - "heading": -0.2882641191045993, - "angularVelocity": -0.33891320852752377, - "velocityX": -1.1236352199369757, - "velocityY": 3.1660970474403998, - "timestamp": 6.437418619326534 - }, - { - "x": 2.2514737936228975, - "y": 3.2514391795997852, - "heading": -0.30953916781592244, - "angularVelocity": -0.34049161148783, - "velocityX": -0.7732901201868099, - "velocityY": 3.150248428002838, - "timestamp": 6.499901946355825 - }, - { - "x": 2.2245033630694606, - "y": 3.4433305709133575, - "heading": -0.33040483151292044, - "angularVelocity": -0.3339397034222824, - "velocityX": -0.431642036935605, - "velocityY": 3.0710815258543787, - "timestamp": 6.562385273385116 - }, - { - "x": 2.217116881452816, - "y": 3.625393519094175, - "heading": -0.35022202500571886, - "angularVelocity": -0.3171597037960086, - "velocityX": -0.11821524185455658, - "velocityY": 2.913784473984096, - "timestamp": 6.624868600414407 - }, - { - "x": 2.225788569605819, - "y": 3.7925510322181357, - "heading": -0.36834815899940476, - "angularVelocity": -0.29009553196789717, - "velocityX": 0.13878403352206445, - "velocityY": 2.6752338755201226, - "timestamp": 6.687351927443698 - }, - { - "x": 2.2455610808640105, - "y": 3.940821796699816, - "heading": -0.38433795999185266, - "angularVelocity": -0.2559050830464319, - "velocityX": 0.31644459727508745, - "velocityY": 2.372965261791757, - "timestamp": 6.749835254472989 - }, - { - "x": 2.271296811862093, - "y": 4.068013754579051, - "heading": -0.39799791167793525, - "angularVelocity": -0.21861754704065436, - "velocityX": 0.41188157259965286, - "velocityY": 2.0356143618858633, - "timestamp": 6.81231858150228 - }, - { - "x": 2.298666674813771, - "y": 4.1733611816283265, - "heading": -0.40928906927102027, - "angularVelocity": -0.18070672817713285, - "velocityX": 0.4380346606519097, - "velocityY": 1.6860086051418306, - "timestamp": 6.8748019085315715 - }, - { - "x": 2.3243703381234817, - "y": 4.2568665137587525, - "heading": -0.41823553880068076, - "angularVelocity": -0.14318170870553068, - "velocityX": 0.41136835267529415, - "velocityY": 1.3364418333755004, - "timestamp": 6.9372852355608625 - }, - { - "x": 2.345964878201746, - "y": 4.318854956416003, - "heading": -0.42488052820760597, - "angularVelocity": -0.10634820075778077, - "velocityX": 0.3456048374015221, - "velocityY": 0.992079737178395, - "timestamp": 6.999768562590154 - }, - { - "x": 2.3616333943908905, - "y": 4.359754200754428, - "heading": -0.42926967074734346, - "angularVelocity": -0.0702450197262999, - "velocityX": 0.25076315449399394, - "velocityY": 0.6545625254438318, - "timestamp": 7.062251889619445 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": -0.03482095714327977, - "velocityX": 0.1339014102944245, - "velocityY": 0.32401922573810255, - "timestamp": 7.124735216648736 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": -1.1758721771275293e-31, - "velocityX": 1.7010814766774368e-33, - "velocityY": -2.1446217942541315e-33, - "timestamp": 7.187218543678027 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 0 - ], - "type": "StopPoint" - }, - { - "scope": [ - "first", - 1 - ], - "type": "MaxVelocity", - "velocity": 1 - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "3NBClose": { - "waypoints": [ - { - "x": 0.756615161895752, - "y": 4.431221961975098, - "heading": -1.034985029325618, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 2.5509378910064697, - "y": 4.086857318878174, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 2.2246973514556885, - "y": 3.4162516593933105, - "heading": -0.8455662927866701, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 4.6896257400512695, - "y": 2.2562854290008545, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 7.952030658721924, - "y": 0.715705394744873, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 - }, - { - "x": 4.870870113372803, - "y": 2.510028123855591, - "heading": -0.575, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 2.1703240871429443, - "y": 4.014359474182129, - "heading": -0.5754249075385721, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.756615161895752, - "y": 4.431221961975098, - "heading": -1.034985029325618, - "angularVelocity": 8.110335832445358e-7, - "velocityX": 3.46491583266257, - "velocityY": -0.6635831503447084, - "timestamp": 0 - }, - { - "x": 0.9453330335621133, - "y": 4.395079669962969, - "heading": -1.0349849176957346, - "angularVelocity": 0.0000021977330086686356, - "velocityX": 3.7154163650328855, - "velocityY": -0.7115577450399997, - "timestamp": 0.05079319600420905 - }, - { - "x": 1.1340509052288028, - "y": 4.358937377950767, - "heading": -1.0349848060666427, - "angularVelocity": 0.0000021977174247335837, - "velocityX": 3.715416365039343, - "velocityY": -0.7115577450414385, - "timestamp": 0.1015863920084181 - }, - { - "x": 1.322768776895351, - "y": 4.322795085937826, - "heading": -1.0349846944375516, - "angularVelocity": 0.0000021977174097972457, - "velocityX": 3.7154163650365644, - "velocityY": -0.7115577450559716, - "timestamp": 0.15237958801262713 - }, - { - "x": 1.5114866425477105, - "y": 4.28665276252143, - "heading": -1.0349845828083795, - "angularVelocity": 0.000002197719005657343, - "velocityX": 3.7154162466311615, - "velocityY": -0.7115583633170585, - "timestamp": 0.2031727840168362 - }, - { - "x": 1.69813453980795, - "y": 4.2495459266525195, - "heading": -1.030459064976107, - "angularVelocity": 0.08909693006711296, - "velocityX": 3.6746633790237286, - "velocityY": -0.7305473722469792, - "timestamp": 0.25396598002104526 - }, - { - "x": 1.8669872465749084, - "y": 4.212655978268133, - "heading": -0.9831886401407888, - "angularVelocity": 0.9306448216292826, - "velocityX": 3.3243174293062006, - "velocityY": -0.7262773616633765, - "timestamp": 0.3047591760252543 - }, - { - "x": 2.0190124095336994, - "y": 4.18140148614854, - "heading": -0.892440005402738, - "angularVelocity": 1.7866297432933886, - "velocityX": 2.993022194275648, - "velocityY": -0.615328323049506, - "timestamp": 0.3555523720294634 - }, - { - "x": 2.1541013042131363, - "y": 4.155546420509619, - "heading": -0.7564993339594785, - "angularVelocity": 2.6763559322393244, - "velocityX": 2.659586427052991, - "velocityY": -0.5090261624171132, - "timestamp": 0.40634556803367244 - }, - { - "x": 2.2707485094165962, - "y": 4.134387232003345, - "heading": -0.5827726511536211, - "angularVelocity": 3.4202746917414184, - "velocityX": 2.2965124146508487, - "velocityY": -0.41657525359346464, - "timestamp": 0.4571387640378815 - }, - { - "x": 2.3663024178277605, - "y": 4.117419152577522, - "heading": -0.4109047131229225, - "angularVelocity": 3.3836803263267097, - "velocityX": 1.8812344157915586, - "velocityY": -0.3340620547763231, - "timestamp": 0.5079319600420905 - }, - { - "x": 2.4413363437910904, - "y": 4.104502135595215, - "heading": -0.2574297741497486, - "angularVelocity": 3.0215649151208352, - "velocityX": 1.4772436441509222, - "velocityY": -0.2543060488108931, - "timestamp": 0.5587251560462996 - }, - { - "x": 2.4966779864989124, - "y": 4.095364710580153, - "heading": -0.13334577720883498, - "angularVelocity": 2.4429255629165603, - "velocityX": 1.0895483462634659, - "velocityY": -0.17989466570101328, - "timestamp": 0.6095183520505086 - }, - { - "x": 2.5330251703027096, - "y": 4.089609545054607, - "heading": -0.04586327123160215, - "angularVelocity": 1.7223272575717314, - "velocityX": 0.7155915883061368, - "velocityY": -0.11330583578692442, - "timestamp": 0.6603115480547177 - }, - { - "x": 2.5509378910064697, - "y": 4.086857318878174, - "heading": -2.5191104221446134e-30, - "angularVelocity": 0.9029412370074453, - "velocityX": 0.3526598464541567, - "velocityY": -0.05418493800242671, - "timestamp": 0.7111047440589268 - }, - { - "x": 2.5509378910064697, - "y": 4.086857318878174, - "heading": -2.52291805755639e-30, - "angularVelocity": -7.496319427174816e-32, - "velocityX": 0, - "velocityY": 7.845413232696036e-33, - "timestamp": 0.7618979400631358 - }, - { - "x": 2.540265709715395, - "y": 4.06437744223644, - "heading": -0.027677023929901675, - "angularVelocity": -0.49532482401432165, - "velocityX": -0.1909958358687293, - "velocityY": -0.4023135207612014, - "timestamp": 0.8177744529856972 - }, - { - "x": 2.5189263879945742, - "y": 4.0194411386787365, - "heading": -0.08346743892564222, - "angularVelocity": -0.998459139228317, - "velocityX": -0.3819014574226356, - "velocityY": -0.804207370992889, - "timestamp": 0.8736509659082585 - }, - { - "x": 2.4869145481158594, - "y": 3.952095228464412, - "heading": -0.16807523339881444, - "angularVelocity": -1.5141924584740862, - "velocityX": -0.5729033220644937, - "velocityY": -1.2052632974369473, - "timestamp": 0.9295274788308199 - }, - { - "x": 2.444192615028637, - "y": 3.8624080728656693, - "heading": -0.2822872457171867, - "angularVelocity": -2.044007514868679, - "velocityX": -0.7645776526253585, - "velocityY": -1.6050957890489632, - "timestamp": 0.9854039917533812 - }, - { - "x": 2.390671759835886, - "y": 3.750452426932401, - "heading": -0.42657055754485035, - "angularVelocity": -2.58218174830674, - "velocityX": -0.9578417190586789, - "velocityY": -2.0036262121157438, - "timestamp": 1.0412805046759426 - }, - { - "x": 2.3354143484351475, - "y": 3.6390964679555458, - "heading": -0.5674310004450425, - "angularVelocity": -2.520924007827922, - "velocityX": -0.9889201832855812, - "velocityY": -1.9928938502512255, - "timestamp": 1.097157017598504 - }, - { - "x": 2.29114926051941, - "y": 3.54997658752366, - "heading": -0.6791457851227963, - "angularVelocity": -1.9993156128511118, - "velocityX": -0.7921948883439481, - "velocityY": -1.5949434882487357, - "timestamp": 1.1530335305210653 - }, - { - "x": 2.257927443594339, - "y": 3.4831161723554422, - "heading": -0.7624378374394924, - "angularVelocity": -1.4906451380051187, - "velocityX": -0.5945578059087696, - "velocityY": -1.1965745833294734, - "timestamp": 1.2089100434436266 - }, - { - "x": 2.2357738106690572, - "y": 3.4385377084317534, - "heading": -0.8178321387402372, - "angularVelocity": -0.9913700480471095, - "velocityX": -0.3964748651366984, - "velocityY": -0.7978032556446372, - "timestamp": 1.264786556366188 - }, - { - "x": 2.2246973514556885, - "y": 3.4162516593933105, - "heading": -0.8455662927866701, - "angularVelocity": -0.49634725926561274, - "velocityX": -0.19823103901848363, - "velocityY": -0.39884466429264903, - "timestamp": 1.3206630692887493 - }, - { - "x": 2.2246973514556885, - "y": 3.4162516593933105, - "heading": -0.8455662927866701, - "angularVelocity": 1.7401147103630586e-35, - "velocityX": 0, - "velocityY": -2.9784720237642915e-35, - "timestamp": 1.3765395822113107 - }, - { - "x": 2.2478567142138566, - "y": 3.4045992699268264, - "heading": -0.801702995310435, - "angularVelocity": 0.7505801290697112, - "velocityX": 0.3962984656503782, - "velocityY": -0.1993933993326178, - "timestamp": 1.4349787758129322 - }, - { - "x": 2.294609413503049, - "y": 3.381442972938176, - "heading": -0.7165359103981246, - "angularVelocity": 1.4573624251712332, - "velocityX": 0.8000230052437824, - "velocityY": -0.39624600480468514, - "timestamp": 1.4934179694145537 - }, - { - "x": 2.365522904611265, - "y": 3.3469050328439485, - "heading": -0.5944096615423881, - "angularVelocity": 2.089800377607326, - "velocityX": 1.2134577282436745, - "velocityY": -0.591006445600051, - "timestamp": 1.5518571630161753 - }, - { - "x": 2.4613273363775683, - "y": 3.300956971286685, - "heading": -0.44341803786852796, - "angularVelocity": 2.5837390006297127, - "velocityX": 1.6393866147332483, - "velocityY": -0.7862542024534139, - "timestamp": 1.6102963566177968 - }, - { - "x": 2.582796668027654, - "y": 3.243418728875599, - "heading": -0.27720516334390327, - "angularVelocity": 2.8442020548348648, - "velocityX": 2.078559339441595, - "velocityY": -0.9845831002276041, - "timestamp": 1.6687355502194183 - }, - { - "x": 2.7301911975501905, - "y": 3.1745171553945775, - "heading": -0.12210660840345536, - "angularVelocity": 2.6540160016195737, - "velocityX": 2.522186232194111, - "velocityY": -1.1790301890666381, - "timestamp": 1.7271747438210399 - }, - { - "x": 2.8997764163925672, - "y": 3.0981960670393747, - "heading": -0.030345077601412643, - "angularVelocity": 1.5702052876974757, - "velocityX": 2.901908948272534, - "velocityY": -1.3059914699624646, - "timestamp": 1.7856139374226614 - }, - { - "x": 3.0915054018011863, - "y": 3.0138284358769285, - "heading": -7.683299452582235e-7, - "angularVelocity": 0.5192458588378841, - "velocityX": 3.280828731409792, - "velocityY": -1.4436823296635168, - "timestamp": 1.844053131024283 - }, - { - "x": 3.2912704992742374, - "y": 2.9191356764509986, - "heading": -6.722882817433478e-7, - "angularVelocity": 0.000001643446077808863, - "velocityX": 3.418341102289066, - "velocityY": -1.6203638960429139, - "timestamp": 1.9024923246259045 - }, - { - "x": 3.4910355335063255, - "y": 2.8244427836108152, - "heading": -5.762470893344414e-7, - "angularVelocity": 0.0000016434380163356612, - "velocityX": 3.418340020122122, - "velocityY": -1.6203661790014092, - "timestamp": 1.960931518227526 - }, - { - "x": 3.690800567737388, - "y": 2.72974989076847, - "heading": -4.802059000608906e-7, - "angularVelocity": 0.000001643437962684092, - "velocityX": 3.4183400201045884, - "velocityY": -1.6203661790384074, - "timestamp": 2.0193707118291475 - }, - { - "x": 3.8905656019684516, - "y": 2.635056997926124, - "heading": -3.8416471392980063e-7, - "angularVelocity": 0.0000016434379089109065, - "velocityX": 3.4183400201045884, - "velocityY": -1.6203661790384178, - "timestamp": 2.0778099054307693 - }, - { - "x": 4.090330636199514, - "y": 2.5403641050837775, - "heading": -2.881235309010655e-7, - "angularVelocity": 0.0000016434378558240006, - "velocityX": 3.4183400201045897, - "velocityY": -1.6203661790384272, - "timestamp": 2.136249099032391 - }, - { - "x": 4.290095670430578, - "y": 2.44567121224143, - "heading": -1.9208235097393964e-7, - "angularVelocity": 0.0000016434378027498543, - "velocityX": 3.418340020104591, - "velocityY": -1.6203661790384367, - "timestamp": 2.1946882926340128 - }, - { - "x": 4.48986070466166, - "y": 2.3509783193991223, - "heading": -9.604117422336942e-8, - "angularVelocity": 0.0000016434377483932399, - "velocityX": 3.4183400201049134, - "velocityY": -1.6203661790377692, - "timestamp": 2.2531274862356345 - }, - { - "x": 4.6896257400512695, - "y": 2.2562854290008545, - "heading": 0, - "angularVelocity": 0.0000016434377051482952, - "velocityX": 3.418340039929407, - "velocityY": -1.620366137215836, - "timestamp": 2.3115666798372563 - }, - { - "x": 4.968559869574538, - "y": 2.1245665376158462, - "heading": 1.3383629188384767e-17, - "angularVelocity": 1.6413040812646372e-16, - "velocityX": 3.420718848021436, - "velocityY": -1.6153394178448868, - "timestamp": 2.3931092274466876 - }, - { - "x": 5.247494000174595, - "y": 1.9928476485110982, - "heading": 4.731110022587156e-17, - "angularVelocity": 4.160707757547854e-16, - "velocityX": 3.4207188612266823, - "velocityY": -1.6153393898808353, - "timestamp": 2.474651775056119 - }, - { - "x": 5.526428130774659, - "y": 1.8611287594063628, - "heading": 2.424776839803803e-17, - "angularVelocity": -2.82838009158897e-16, - "velocityX": 3.420718861226756, - "velocityY": -1.6153393898806798, - "timestamp": 2.5561943226655504 - }, - { - "x": 5.805362261374722, - "y": 1.7294098703016274, - "heading": 2.194784551635826e-17, - "angularVelocity": -2.820518795507566e-17, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806796, - "timestamp": 2.6377368702749817 - }, - { - "x": 6.084296391974785, - "y": 1.5976909811968918, - "heading": -5.63799247601606e-18, - "angularVelocity": -3.3829992808814586e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806796, - "timestamp": 2.719279417884413 - }, - { - "x": 6.363230522574848, - "y": 1.4659720920921564, - "heading": -4.0160186165861685e-17, - "angularVelocity": -4.233641786017845e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806798, - "timestamp": 2.8008219654938444 - }, - { - "x": 6.642164653174911, - "y": 1.334253202987421, - "heading": -9.232752279805819e-17, - "angularVelocity": -6.39756031196107e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806796, - "timestamp": 2.882364513103276 - }, - { - "x": 6.9210987837749745, - "y": 1.2025343138826854, - "heading": -1.0876414634021774e-16, - "angularVelocity": -2.0157113095032782e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806798, - "timestamp": 2.963907060712707 - }, - { - "x": 7.200032914375007, - "y": 1.0708154247779642, - "heading": -1.0564695019881366e-16, - "angularVelocity": 3.822784834245772e-17, - "velocityX": 3.4207188612263892, - "velocityY": -1.6153393898805066, - "timestamp": 3.0454496083221385 - }, - { - "x": 7.450698808870213, - "y": 0.9524454243469528, - "heading": -6.21333471845912e-17, - "angularVelocity": 5.336307257731506e-16, - "velocityX": 3.0740503190534603, - "velocityY": -1.4516348078548387, - "timestamp": 3.12699215593157 - }, - { - "x": 7.6512315423409545, - "y": 0.8577494155613715, - "heading": -3.4011798431685756e-17, - "angularVelocity": 3.448696939400641e-16, - "velocityX": 2.4592404744483005, - "velocityY": -1.16130794979759, - "timestamp": 3.2085347035410012 - }, - { - "x": 7.8016310983382375, - "y": 0.7867274061888005, - "heading": -1.7843882684840366e-17, - "angularVelocity": 1.982758662289022e-16, - "velocityX": 1.8444304281202957, - "velocityY": -0.8709809964823354, - "timestamp": 3.2900772511504326 - }, - { - "x": 7.901897471379062, - "y": 0.7393793988184351, - "heading": -9.05403720548675e-18, - "angularVelocity": 1.0779461865175002e-16, - "velocityX": 1.2296203145512992, - "velocityY": -0.5806540114143915, - "timestamp": 3.371619798759864 - }, - { - "x": 7.952030658721924, - "y": 0.715705394744873, - "heading": 1.8215469909424705e-35, - "angularVelocity": 1.1103452913314967e-16, - "velocityX": 0.6148101673618008, - "velocityY": -0.29032701047010034, - "timestamp": 3.4531623463692953 - }, - { - "x": 7.952030658721924, - "y": 0.715705394744873, - "heading": 0, - "angularVelocity": -1.7262654336863351e-34, - "velocityX": 0, - "velocityY": -1.70530075667682e-35, - "timestamp": 3.5347048939787267 - }, - { - "x": 7.920571399136667, - "y": 0.7339164602546292, - "heading": -0.035684479584908176, - "angularVelocity": -0.530786457387229, - "velocityX": -0.46793869888314166, - "velocityY": 0.27087930269994037, - "timestamp": 3.601934340186837 - }, - { - "x": 7.857550496757403, - "y": 0.7704988919479816, - "heading": -0.1048950106648703, - "angularVelocity": -1.0294675173393406, - "velocityX": -0.937400290107708, - "velocityY": 0.544142987287306, - "timestamp": 3.669163786394947 - }, - { - "x": 7.762821414092203, - "y": 0.8256865710662646, - "heading": -0.20406484547654866, - "angularVelocity": -1.4750952209943207, - "velocityX": -1.409041543670678, - "velocityY": 0.8208855231002242, - "timestamp": 3.7363932326030573 - }, - { - "x": 7.636150871743386, - "y": 0.8998079208996147, - "heading": -0.326492004541456, - "angularVelocity": -1.8210347692874205, - "velocityX": -1.8841526963751192, - "velocityY": 1.1025131696594024, - "timestamp": 3.8036226788111676 - }, - { - "x": 7.477257379372396, - "y": 0.9932759166007383, - "heading": -0.4588279920908599, - "angularVelocity": -1.9684229904223027, - "velocityX": -2.3634508587075183, - "velocityY": 1.3902835881139206, - "timestamp": 3.870852125019278 - }, - { - "x": 7.286949361590744, - "y": 1.1061975307016165, - "heading": -0.5642233015257418, - "angularVelocity": -1.5676956360554906, - "velocityX": -2.8307241620368186, - "velocityY": 1.67964516249809, - "timestamp": 3.938081571227388 - }, - { - "x": 7.071106947748093, - "y": 1.2344586983401868, - "heading": -0.5749993730660543, - "angularVelocity": -0.16028797124038208, - "velocityX": -3.210533865986417, - "velocityY": 1.9078123482013338, - "timestamp": 4.005311017435498 - }, - { - "x": 6.8510832792762875, - "y": 1.3620156667061647, - "heading": -0.5749994357594823, - "angularVelocity": -9.32529294881991e-7, - "velocityX": -3.2727276644629284, - "velocityY": 1.8973377821843458, - "timestamp": 4.072540463643609 - }, - { - "x": 6.631059595878299, - "y": 1.4895726093259114, - "heading": -0.5749994984528743, - "angularVelocity": -9.325287600700736e-7, - "velocityX": -3.272727886481472, - "velocityY": 1.8973373992237128, - "timestamp": 4.1397699098517196 - }, - { - "x": 6.411035912480151, - "y": 1.617129551945382, - "heading": -0.5749995611462662, - "angularVelocity": -9.325287555505025e-7, - "velocityX": -3.272727886483851, - "velocityY": 1.89733739921961, - "timestamp": 4.20699935605983 - }, - { - "x": 6.191012229082002, - "y": 1.7446864945648533, - "heading": -0.5749996238396577, - "angularVelocity": -9.325287510002467e-7, - "velocityX": -3.2727278864838483, - "velocityY": 1.8973373992196134, - "timestamp": 4.274228802267941 - }, - { - "x": 5.970988545683854, - "y": 1.8722434371843246, - "heading": -0.5749996865330489, - "angularVelocity": -9.325287467182492e-7, - "velocityX": -3.2727278864838465, - "velocityY": 1.8973373992196174, - "timestamp": 4.3414582484760516 - }, - { - "x": 5.750964862285706, - "y": 1.999800379803796, - "heading": -0.5749997492264397, - "angularVelocity": -9.325287418609201e-7, - "velocityX": -3.2727278864838443, - "velocityY": 1.8973373992196212, - "timestamp": 4.408687694684162 - }, - { - "x": 5.530941178887557, - "y": 2.127357322423268, - "heading": -0.5749998119198304, - "angularVelocity": -9.325287380028671e-7, - "velocityX": -3.2727278864838425, - "velocityY": 1.8973373992196243, - "timestamp": 4.475917140892273 - }, - { - "x": 5.310917495489409, - "y": 2.2549142650427396, - "heading": -0.5749998746132208, - "angularVelocity": -9.325287337726564e-7, - "velocityX": -3.2727278864838407, - "velocityY": 1.897337399219628, - "timestamp": 4.5431465871003835 - }, - { - "x": 5.090893812091097, - "y": 2.3824712076619283, - "heading": -0.5749999373066107, - "angularVelocity": -9.325287288718162e-7, - "velocityX": -3.2727278864862823, - "velocityY": 1.8973373992154168, - "timestamp": 4.610376033308494 - }, - { - "x": 4.870870113372803, - "y": 2.510028123855591, - "heading": -0.575, - "angularVelocity": -9.325287163810678e-7, - "velocityX": -3.2727281143623554, - "velocityY": 1.8973370061506505, - "timestamp": 4.677605479516605 - }, - { - "x": 4.5943545837046464, - "y": 2.664060328782752, - "heading": -0.5750000000166958, - "angularVelocity": -1.995413922173843e-10, - "velocityX": -3.3047900712231173, - "velocityY": 1.8409240960273932, - "timestamp": 4.761276612541347 - }, - { - "x": 4.317839040217314, - "y": 2.818092508902003, - "heading": -0.5750000000333916, - "angularVelocity": -1.9954042523374304e-10, - "velocityX": -3.3047902363837314, - "velocityY": 1.8409237995343366, - "timestamp": 4.844947745566089 - }, - { - "x": 4.041323496729911, - "y": 2.972124689021126, - "heading": -0.5750000000500873, - "angularVelocity": -1.9954079180529516e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328027, - "timestamp": 4.928618878590831 - }, - { - "x": 3.764807953242507, - "y": 3.1261568691402486, - "heading": -0.5750000000667832, - "angularVelocity": -1.9954127974564644e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328034, - "timestamp": 5.0122900116155735 - }, - { - "x": 3.4882924097551036, - "y": 3.2801890492593713, - "heading": -0.5750000000834792, - "angularVelocity": -1.9954171076053385e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328027, - "timestamp": 5.095961144640316 - }, - { - "x": 3.2117768662677, - "y": 3.434221229378494, - "heading": -0.5750000001001749, - "angularVelocity": -1.995407072844536e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328032, - "timestamp": 5.179632277665058 - }, - { - "x": 2.9352613227803133, - "y": 3.5882534094976077, - "heading": -0.5750000001168709, - "angularVelocity": -1.9954301729646843e-10, - "velocityX": -3.3047902363843877, - "velocityY": 1.840923799532693, - "timestamp": 5.2633034106898 - }, - { - "x": 2.680282263927866, - "y": 3.7302887534223292, - "heading": -0.5751416355275835, - "angularVelocity": -0.00169276315011459, - "velocityX": -3.0473957939239096, - "velocityY": 1.6975429731867062, - "timestamp": 5.346974543714542 - }, - { - "x": 2.476298999494477, - "y": 3.84391703822767, - "heading": -0.5752549441981719, - "angularVelocity": -0.0013542146077421449, - "velocityX": -2.4379168425156807, - "velocityY": 1.3580344940678712, - "timestamp": 5.430645676739284 - }, - { - "x": 2.3233115454477207, - "y": 3.9291382550189407, - "heading": -0.5753399258221812, - "angularVelocity": -0.0010156624027564103, - "velocityX": -1.8284377002701389, - "velocityY": 1.0185259086436655, - "timestamp": 5.5143168097640265 - }, - { - "x": 2.2213199071101224, - "y": 3.985952400831243, - "heading": -0.5753965802881199, - "angularVelocity": -0.0006771088652765756, - "velocityX": -1.218958494412151, - "velocityY": 0.6790172877843319, - "timestamp": 5.597987942788769 - }, - { - "x": 2.1703240871429443, - "y": 4.014359474182129, - "heading": -0.5754249075385721, - "angularVelocity": -0.0003385546415838589, - "velocityX": -0.6094792567479415, - "velocityY": 0.33950864920743473, - "timestamp": 5.681659075813511 - }, - { - "x": 2.1703240871429443, - "y": 4.014359474182129, - "heading": -0.5754249075385721, - "angularVelocity": 0, - "velocityX": -2.0315826046958738e-36, - "velocityY": -3.1386682644109864e-35, - "timestamp": 5.765330208838253 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "NoTeamBottom": { - "waypoints": [ - { - "x": 0.7265444397926331, - "y": 4.389017105102539, - "heading": -1.051650239140039, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 - }, - { - "x": 2.8526127338409424, - "y": 3.84, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 1.90418541431427, - "y": 4.952258110046387, - "heading": -0.029402891635568452, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 8 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449494, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 1.645802617073059, - "y": 6.586877346038818, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.7265444397926332, - "y": 4.389017105102539, - "heading": -1.051650239140039, - "angularVelocity": -1.9490781819297303e-18, - "velocityX": -1.0487369107199747e-16, - "velocityY": 2.7065156705910276e-17, - "timestamp": 0 - }, - { - "x": 0.7771701484751878, - "y": 4.375943969063242, - "heading": -1.0516503761135536, - "angularVelocity": -0.000001419156261399036, - "velocityX": 0.5245232431272688, - "velocityY": -0.13544825132569957, - "timestamp": 0.09651756970905265 - }, - { - "x": 0.8784215587828176, - "y": 4.349797698833336, - "heading": -1.0516507145054637, - "angularVelocity": -0.000003506013550567298, - "velocityX": 1.0490464131333472, - "velocityY": -0.27089648349747875, - "timestamp": 0.1930351394181053 - }, - { - "x": 1.0185995630841687, - "y": 4.313599368591188, - "heading": -1.034957242895387, - "angularVelocity": 0.172957852755694, - "velocityX": 1.4523573762156516, - "velocityY": -0.37504394641583094, - "timestamp": 0.2895527091271579 - }, - { - "x": 1.158777567423705, - "y": 4.277401038497066, - "heading": -0.9771905498626422, - "angularVelocity": 0.5985096102904218, - "velocityX": 1.4523573766112823, - "velocityY": -0.3750439448821489, - "timestamp": 0.3860702788362106 - }, - { - "x": 1.29895557174175, - "y": 4.241202708319719, - "heading": -0.8946611027304613, - "angularVelocity": 0.8550717489153622, - "velocityX": 1.452357376388613, - "velocityY": -0.3750439457444395, - "timestamp": 0.48258784854526326 - }, - { - "x": 1.4391335760555888, - "y": 4.205004378126083, - "heading": -0.7975443374250958, - "angularVelocity": 1.0062081504757965, - "velocityX": 1.4523573763450335, - "velocityY": -0.3750439459132019, - "timestamp": 0.5791054182543159 - }, - { - "x": 1.5793115803675255, - "y": 4.168806047925082, - "heading": -0.692145515107339, - "angularVelocity": 1.0920169523069798, - "velocityX": 1.452357376325328, - "velocityY": -0.3750439459895117, - "timestamp": 0.6756229879633686 - }, - { - "x": 1.71948958467887, - "y": 4.132607717721787, - "heading": -0.5825155459732, - "angularVelocity": 1.1358550517238752, - "velocityX": 1.45235737631919, - "velocityY": -0.37504394601328, - "timestamp": 0.7721405576724213 - }, - { - "x": 1.859667588990117, - "y": 4.096409387518115, - "heading": -0.4715528544072786, - "angularVelocity": 1.1496631328411298, - "velocityX": 1.4523573763181812, - "velocityY": -0.375043946017187, - "timestamp": 0.868658127381474 - }, - { - "x": 1.999845593301567, - "y": 4.060211057315229, - "heading": -0.36180710006295524, - "angularVelocity": 1.1370546800457813, - "velocityX": 1.4523573763202846, - "velocityY": -0.3750439460090416, - "timestamp": 0.9651756970905266 - }, - { - "x": 2.1400235976133968, - "y": 4.0240127271138135, - "heading": -0.2561858402181512, - "angularVelocity": 1.0943215847973948, - "velocityX": 1.4523573763242201, - "velocityY": -0.3750439459938017, - "timestamp": 1.0616932667995793 - }, - { - "x": 2.2802016019249636, - "y": 3.987814396911379, - "heading": -0.15875132018300867, - "angularVelocity": 1.0095003461945151, - "velocityX": 1.4523573763214923, - "velocityY": -0.3750439460043652, - "timestamp": 1.1582108365086319 - }, - { - "x": 2.4203796062348975, - "y": 3.9516160667026234, - "heading": -0.07580749268653832, - "angularVelocity": 0.859365064272749, - "velocityX": 1.4523573763045803, - "velocityY": -0.37504394606985697, - "timestamp": 1.2547284062176844 - }, - { - "x": 2.560557610534093, - "y": 3.91541773645228, - "heading": -0.017491102880811727, - "angularVelocity": 0.6042049129657698, - "velocityX": 1.4523573761933126, - "velocityY": -0.3750439465007366, - "timestamp": 1.351245975926737 - }, - { - "x": 2.7007356148590005, - "y": 3.879219406301364, - "heading": 5.051589087929001e-7, - "angularVelocity": 0.1812271909917254, - "velocityX": 1.4523573764597182, - "velocityY": -0.37504394547059267, - "timestamp": 1.4477635456357896 - }, - { - "x": 2.8019870251576924, - "y": 3.8530731360366373, - "heading": 1.4580072575536665e-7, - "angularVelocity": -0.0000037232410744759824, - "velocityX": 1.0490464130407438, - "velocityY": -0.2708964838582552, - "timestamp": 1.5442811153448421 - }, - { - "x": 2.852612733840942, - "y": 3.84, - "heading": 4.9602240502975986e-18, - "angularVelocity": -0.0000015106133003901624, - "velocityX": 0.5245232431344686, - "velocityY": -0.13544825129813873, - "timestamp": 1.6407986850538947 - }, - { - "x": 2.8526127338409424, - "y": 3.84, - "heading": 2.3793426412723873e-18, - "angularVelocity": -2.1367846851313794e-18, - "velocityX": -8.310450142659232e-17, - "velocityY": 2.0891089221855998e-17, - "timestamp": 1.7373162547629473 - }, - { - "x": 2.8209562343759993, - "y": 3.856054932580381, - "heading": -0.0007560219648679484, - "angularVelocity": -0.009506842825878051, - "velocityX": -0.3980748957251135, - "velocityY": 0.20188794468215046, - "timestamp": 1.8168402333029128 - }, - { - "x": 2.7580363510582124, - "y": 3.8889174673683593, - "heading": -0.002267772169913451, - "angularVelocity": -0.019009992115595162, - "velocityX": -0.7912064319841098, - "velocityY": 0.4132405771356513, - "timestamp": 1.8963642118428783 - }, - { - "x": 2.6644940143973495, - "y": 3.9397288138759325, - "heading": -0.004533824337803499, - "angularVelocity": -0.028495206219482882, - "velocityX": -1.1762783801599213, - "velocityY": 0.6389437178628763, - "timestamp": 1.9758881903828438 - }, - { - "x": 2.541539594192653, - "y": 4.010410332743005, - "heading": -0.007548770346538153, - "angularVelocity": -0.037912414143357574, - "velocityX": -1.546130141651605, - "velocityY": 0.8888076296578984, - "timestamp": 2.055412168922809 - }, - { - "x": 2.3921429241592778, - "y": 4.104771009771783, - "heading": -0.011289431433418899, - "angularVelocity": -0.04703815321564728, - "velocityX": -1.8786367681327063, - "velocityY": 1.1865688659095006, - "timestamp": 2.1349361474627746 - }, - { - "x": 2.2285981442653044, - "y": 4.231685243413465, - "heading": -0.015605626770518823, - "angularVelocity": -0.05427539487263933, - "velocityX": -2.056546753527704, - "velocityY": 1.5959240970055453, - "timestamp": 2.21446012600274 - }, - { - "x": 2.0932777236393827, - "y": 4.3801240033728215, - "heading": -0.01970246217760219, - "angularVelocity": -0.05151698245359373, - "velocityX": -1.7016304152578978, - "velocityY": 1.8665912179526678, - "timestamp": 2.2939841045427056 - }, - { - "x": 1.9933780770975498, - "y": 4.530859168690553, - "heading": -0.02318390168855428, - "angularVelocity": -0.04377848763190915, - "velocityX": -1.2562204303149604, - "velocityY": 1.895468110187389, - "timestamp": 2.373508083082671 - }, - { - "x": 1.9287898956537786, - "y": 4.677988681014765, - "heading": -0.025975407403759726, - "angularVelocity": -0.035102691873024824, - "velocityX": -0.812184986586304, - "velocityY": 1.850127659926749, - "timestamp": 2.4530320616226367 - }, - { - "x": 1.8991532059051248, - "y": 4.818930146657707, - "heading": -0.028052234646139453, - "angularVelocity": -0.02611573616548874, - "velocityX": -0.37267614489081113, - "velocityY": 1.7723140646454278, - "timestamp": 2.532556040162602 - }, - { - "x": 1.90418541431427, - "y": 4.952258110046387, - "heading": -0.029402891635568452, - "angularVelocity": -0.016984273350335675, - "velocityX": 0.06327913292990113, - "velocityY": 1.6765756170219066, - "timestamp": 2.6120800187025677 - }, - { - "x": 1.9488424853011135, - "y": 5.08490987258151, - "heading": -0.030009855294308496, - "angularVelocity": -0.007148386818663905, - "velocityX": 0.5259392601282725, - "velocityY": 1.5622782305402025, - "timestamp": 2.6969891980932674 - }, - { - "x": 2.031885999740562, - "y": 5.204759123935114, - "heading": -0.029768448326245058, - "angularVelocity": 0.002843119787468783, - "velocityX": 0.9780275234710838, - "velocityY": 1.411499348052011, - "timestamp": 2.781898377483967 - }, - { - "x": 2.150567006153512, - "y": 5.305440928887294, - "heading": -0.028675027108268608, - "angularVelocity": 0.012877538398353833, - "velocityX": 1.397740588998668, - "velocityY": 1.1857587798476337, - "timestamp": 2.866807556874667 - }, - { - "x": 2.290536940712368, - "y": 5.371710342064258, - "heading": -0.026887862525856107, - "angularVelocity": 0.021047954947121472, - "velocityX": 1.6484664622042853, - "velocityY": 0.7804740742109001, - "timestamp": 2.9517167362653667 - }, - { - "x": 2.406483690373778, - "y": 5.405416808831692, - "heading": -0.025158746552686077, - "angularVelocity": 0.02036429966203896, - "velocityX": 1.3655384552463332, - "velocityY": 0.39697082234581244, - "timestamp": 3.0366259156560664 - }, - { - "x": 2.485516159596883, - "y": 5.4225466650859895, - "heading": -0.023904634648769633, - "angularVelocity": 0.014770039151430181, - "velocityX": 0.9307882821413964, - "velocityY": 0.20174327884476004, - "timestamp": 3.121535095046766 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": 0.0076948823555783585, - "velocityX": 0.4693772631942069, - "velocityY": 0.08249354824799049, - "timestamp": 3.206444274437466 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": -8.395893895847363e-23, - "velocityX": 1.4623773604187483e-19, - "velocityY": -1.3212521106474066e-18, - "timestamp": 3.2913534538281657 - }, - { - "x": 2.4932837152722374, - "y": 5.448396032908335, - "heading": -0.022770666861651127, - "angularVelocity": 0.005902466591950069, - "velocityX": -0.394072213481467, - "velocityY": 0.23144207684009668, - "timestamp": 3.372777318463643 - }, - { - "x": 2.429535152050195, - "y": 5.486791798588922, - "heading": -0.021801144225693077, - "angularVelocity": 0.01190710659950195, - "velocityX": -0.7829223472434627, - "velocityY": 0.47155420407122456, - "timestamp": 3.4542011830991206 - }, - { - "x": 2.334824983189393, - "y": 5.545829176600406, - "heading": -0.020330384435373747, - "angularVelocity": 0.018063006428197637, - "velocityX": -1.1631745715436788, - "velocityY": 0.7250623422971361, - "timestamp": 3.535625047734598 - }, - { - "x": 2.2105039579858143, - "y": 5.627403033507208, - "heading": -0.018338524798568293, - "angularVelocity": 0.02446284815542353, - "velocityX": -1.5268376876993603, - "velocityY": 1.0018421168289628, - "timestamp": 3.6170489123700755 - }, - { - "x": 2.060080687324347, - "y": 5.7354979281499965, - "heading": -0.01579076613135179, - "angularVelocity": 0.031290072985634236, - "velocityX": -1.8474101092460986, - "velocityY": 1.3275578987402952, - "timestamp": 3.698472777005553 - }, - { - "x": 1.90097644380825, - "y": 5.879777464179869, - "heading": -0.012665610755061086, - "angularVelocity": 0.03838131965709989, - "velocityX": -1.9540247104258965, - "velocityY": 1.77195637514615, - "timestamp": 3.7798966416410305 - }, - { - "x": 1.7772040422925621, - "y": 6.035734728989453, - "heading": -0.009508151519862748, - "angularVelocity": 0.03877805664633832, - "velocityX": -1.5200998143455622, - "velocityY": 1.9153753694667015, - "timestamp": 3.861320506276508 - }, - { - "x": 1.6904452913414765, - "y": 6.187858417258873, - "heading": -0.00660634699813739, - "angularVelocity": 0.03563825586904154, - "velocityX": -1.0655199349661346, - "velocityY": 1.8682936378720785, - "timestamp": 3.9427443709119854 - }, - { - "x": 1.6399494790279758, - "y": 6.331633207559586, - "heading": -0.004035525910181172, - "angularVelocity": 0.03157331206846292, - "velocityX": -0.6201598577955342, - "velocityY": 1.7657573850660415, - "timestamp": 4.024168235547463 - }, - { - "x": 1.6251865837463155, - "y": 6.465022310434175, - "heading": -0.0018276262317727312, - "angularVelocity": 0.027116124839970116, - "velocityX": -0.18130919415028465, - "velocityY": 1.6382064824821505, - "timestamp": 4.105592100182941 - }, - { - "x": 1.645802617073059, - "y": 6.586877346038818, - "heading": 3.400199739559167e-21, - "angularVelocity": 0.022445830100975162, - "velocityX": 0.25319399194619935, - "velocityY": 1.4965518543018048, - "timestamp": 4.187015964818419 - }, - { - "x": 1.7070719226916355, - "y": 6.702201095743444, - "heading": 0.001497238017935697, - "angularVelocity": 0.017357117852509537, - "velocityX": 0.710280226406013, - "velocityY": 1.3369203098224118, - "timestamp": 4.27327671563306 - }, - { - "x": 1.8073044586022589, - "y": 6.8024885050101265, - "heading": 0.002538425575927766, - "angularVelocity": 0.012070235282665064, - "velocityX": 1.1619715219730005, - "velocityY": 1.1626076555046687, - "timestamp": 4.359537466447702 - }, - { - "x": 1.9455905214116462, - "y": 6.885566838960804, - "heading": 0.003095480119106647, - "angularVelocity": 0.006457798453156142, - "velocityX": 1.6031168463457748, - "velocityY": 0.9631070117763845, - "timestamp": 4.445798217262344 - }, - { - "x": 2.119553491307074, - "y": 6.946933932487094, - "heading": 0.003115949285592413, - "angularVelocity": 0.00023729409137361653, - "velocityX": 2.016710592622141, - "velocityY": 0.711413857945154, - "timestamp": 4.532058968076986 - }, - { - "x": 2.317511319909413, - "y": 6.974118317411219, - "heading": 0.002507363017790665, - "angularVelocity": -0.007055193260599903, - "velocityX": 2.2948771803263415, - "velocityY": 0.3151419929388198, - "timestamp": 4.618319718891628 - }, - { - "x": 2.4838287501732563, - "y": 6.974042584379811, - "heading": 0.0016205367707247194, - "angularVelocity": -0.01028076197680645, - "velocityX": 1.9280777026996743, - "velocityY": -0.0008779546977302001, - "timestamp": 4.704580469706269 - }, - { - "x": 2.6088542632161906, - "y": 6.967704921114638, - "heading": 0.0008452961733031628, - "angularVelocity": -0.008987176555968111, - "velocityX": 1.4493905033540737, - "velocityY": -0.07347099585060349, - "timestamp": 4.790841220520911 - }, - { - "x": 2.6921160631697596, - "y": 6.961208582170421, - "heading": 0.00028995944566665723, - "angularVelocity": -0.006437884233465802, - "velocityX": 0.9652338887298002, - "velocityY": -0.07531048458152965, - "timestamp": 4.877101971335553 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 1.3621691331615836e-21, - "angularVelocity": -0.003361429652864086, - "velocityX": 0.48194856413987436, - "velocityY": -0.04622118526264293, - "timestamp": 4.963362722150195 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 6.82640346993216e-22, - "angularVelocity": 4.0562645330660325e-23, - "velocityX": -4.3269580997377834e-19, - "velocityY": -2.905097183597039e-19, - "timestamp": 5.049623472964837 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 5 - ], - "type": "StopPoint" - }, - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - 0, - 1 - ], - "type": "MaxVelocity", - "velocity": 1.5 - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "3NTMid1st": { - "waypoints": [ - { - "x": 0.6889447569847107, - "y": 6.680017471313477, - "heading": 1.057124176286416, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 - }, - { - "x": 4.411551475524902, - "y": 7.1505022048950195, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 7.907153606414795, - "y": 7.396010875701904, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 24 - }, - { - "x": 2.329073190689087, - "y": 6.98138952255249, - "heading": 0.5880023600681887, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 25 - }, - { - "x": 7.958581447601318, - "y": 5.7588982582092285, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 5.455844402313232, - "y": 6.3683881759643555, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 15 - }, - { - "x": 2.3479042053222656, - "y": 5.926850318908691, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 2.3667352199554443, - "y": 6.962558269500732, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 2.3667352199554443, - "y": 6.45412015914917, - "heading": 0.49934664103730353, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.6889447569847107, - "y": 6.680017471313477, - "heading": 1.057124176286416, - "angularVelocity": 2.8387025507521403e-19, - "velocityX": 4.1180279564081185e-19, - "velocityY": -9.327641015842875e-18, - "timestamp": 0 - }, - { - "x": 0.7066606142239513, - "y": 6.682284626893522, - "heading": 1.0367126164662654, - "angularVelocity": -0.3553197163703083, - "velocityX": 0.3083935488011813, - "velocityY": 0.03946612041250281, - "timestamp": 0.05744561553932452 - }, - { - "x": 0.7421443739769805, - "y": 6.686835073027394, - "heading": 0.996657534155969, - "angularVelocity": -0.6972696163883332, - "velocityX": 0.6176930897143716, - "velocityY": 0.07921311471990149, - "timestamp": 0.11489123107864904 - }, - { - "x": 0.7954510899599107, - "y": 6.693681954510511, - "heading": 0.9377659189851129, - "angularVelocity": -1.025171627424583, - "velocityX": 0.9279509929950843, - "velocityY": 0.11918893058827323, - "timestamp": 0.17233684661797355 - }, - { - "x": 0.866640472022295, - "y": 6.702826879784641, - "heading": 0.8609043116521772, - "angularVelocity": -1.3379890982336122, - "velocityX": 1.2392483115382664, - "velocityY": 0.15919274583921428, - "timestamp": 0.22978246215729808 - }, - { - "x": 0.9557824403052647, - "y": 6.714252443844757, - "heading": 0.7671249958790186, - "angularVelocity": -1.6324886571884958, - "velocityX": 1.5517627837401693, - "velocityY": 0.19889357878488217, - "timestamp": 0.2872280776966226 - }, - { - "x": 1.0629657750286219, - "y": 6.727919888213016, - "heading": 0.6579512096451294, - "angularVelocity": -1.9004720414067817, - "velocityX": 1.8658227214918544, - "velocityY": 0.23791971310503898, - "timestamp": 0.34467369323594715 - }, - { - "x": 1.18830476238212, - "y": 6.743775124135866, - "heading": 0.5358410167997394, - "angularVelocity": -2.1256660181802616, - "velocityX": 2.1818721268239685, - "velocityY": 0.27600428290297147, - "timestamp": 0.4021193087752717 - }, - { - "x": 1.3319325274539577, - "y": 6.761759462137426, - "heading": 0.40481020591714667, - "angularVelocity": -2.280954075475008, - "velocityX": 2.5002389429271075, - "velocityY": 0.31306719986746534, - "timestamp": 0.4595649243145962 - }, - { - "x": 1.4939586342232587, - "y": 6.781807678999511, - "heading": 0.27131178001389583, - "angularVelocity": -2.3239097475048234, - "velocityX": 2.8205130234593025, - "velocityY": 0.34899472612252225, - "timestamp": 0.5170105398539208 - }, - { - "x": 1.6743020838003386, - "y": 6.803802865918695, - "heading": 0.14646817576698382, - "angularVelocity": -2.1732486121843375, - "velocityX": 3.139377094038195, - "velocityY": 0.3828871309443344, - "timestamp": 0.5744561553932452 - }, - { - "x": 1.871509949404293, - "y": 6.8273220632242, - "heading": 0.05699752908313393, - "angularVelocity": -1.5574843413872477, - "velocityX": 3.4329489509770403, - "velocityY": 0.4094167515605277, - "timestamp": 0.6319017709325697 - }, - { - "x": 2.0850662670440805, - "y": 6.8523948454346995, - "heading": 0.008424789779770448, - "angularVelocity": -0.8455430209484467, - "velocityX": 3.7175390259957597, - "velocityY": 0.43646119856329335, - "timestamp": 0.6893473864718942 - }, - { - "x": 2.315008723320705, - "y": 6.879297231348836, - "heading": 3.6268922648533895e-7, - "angularVelocity": -0.14665047996181718, - "velocityX": 4.002785140655635, - "velocityY": 0.4683105170266588, - "timestamp": 0.7467930020112187 - }, - { - "x": 2.547959277648203, - "y": 6.909420608999938, - "heading": 3.2238006768257296e-7, - "angularVelocity": -7.016925212653802e-7, - "velocityX": 4.055149416373324, - "velocityY": 0.5243807968335108, - "timestamp": 0.8042386175505432 - }, - { - "x": 2.7809074860808103, - "y": 6.939562122456881, - "heading": 2.820828435268406e-7, - "angularVelocity": -7.014847656878403e-7, - "velocityX": 4.05510857957718, - "velocityY": 0.5246965007505043, - "timestamp": 0.8616842330898676 - }, - { - "x": 3.013855691132565, - "y": 6.969703662042265, - "heading": 2.417856204933752e-7, - "angularVelocity": -7.01484746151677e-7, - "velocityX": 4.055108520724085, - "velocityY": 0.5246969555883743, - "timestamp": 0.9191298486291921 - }, - { - "x": 3.246803890458745, - "y": 6.999845245880575, - "heading": 2.0148839914997936e-7, - "angularVelocity": -7.014847167313357e-7, - "velocityX": 4.055108421054599, - "velocityY": 0.5246977259330878, - "timestamp": 0.9765754641685166 - }, - { - "x": 3.479752088366951, - "y": 7.029986840683357, - "heading": 1.6119117842486052e-7, - "angularVelocity": -7.01484705968513e-7, - "velocityX": 4.055108396370846, - "velocityY": 0.5246979168000837, - "timestamp": 1.034021079707841 - }, - { - "x": 3.71270026445834, - "y": 7.06012860409886, - "heading": 1.208939635051467e-7, - "angularVelocity": -7.014846049093638e-7, - "velocityX": 4.0551080165887265, - "velocityY": 0.5247008519713516, - "timestamp": 1.0914666952471657 - }, - { - "x": 3.945648423411925, - "y": 7.090270499956354, - "heading": 8.059675341964947e-8, - "angularVelocity": -7.014845207564386e-7, - "velocityX": 4.055107718257797, - "velocityY": 0.5247031574909511, - "timestamp": 1.1489123107864903 - }, - { - "x": 4.178596822480232, - "y": 7.12041054039277, - "heading": 4.0299480513851105e-8, - "angularVelocity": -7.014856143177233e-7, - "velocityX": 4.05511189811939, - "velocityY": 0.5246708587495975, - "timestamp": 1.2063579263258148 - }, - { - "x": 4.411551475524902, - "y": 7.1505022048950195, - "heading": -6.644547124099774e-19, - "angularVelocity": -7.015240438601083e-7, - "velocityX": 4.055220765894668, - "velocityY": 0.5238287416669793, - "timestamp": 1.2638035418651394 - }, - { - "x": 4.733129802794184, - "y": 7.173132492258203, - "heading": 6.90202073091001e-17, - "angularVelocity": 8.838643936853847e-16, - "velocityX": 4.078826301961981, - "velocityY": 0.2870374135649262, - "timestamp": 1.3426444395799053 - }, - { - "x": 5.054711574817068, - "y": 7.1957137759487155, - "heading": -1.8089714781984044e-17, - "angularVelocity": -1.1048824198785035e-15, - "velocityX": 4.078869994432532, - "velocityY": 0.2864158621355202, - "timestamp": 1.4214853372946714 - }, - { - "x": 5.376293348799719, - "y": 7.2182950317363055, - "heading": -9.56038449707963e-17, - "angularVelocity": -9.831715772484128e-16, - "velocityX": 4.078870019289777, - "velocityY": 0.28641550822118905, - "timestamp": 1.5003262350094375 - }, - { - "x": 5.69787511780094, - "y": 7.240876358458176, - "heading": -1.30423601604514e-16, - "angularVelocity": -4.4164586162869085e-16, - "velocityX": 4.07886995610646, - "velocityY": 0.2864164079354569, - "timestamp": 1.5791671327242036 - }, - { - "x": 6.019456886422846, - "y": 7.2634576905792185, - "heading": -1.4442306040452632e-16, - "angularVelocity": -1.7756595252798448e-16, - "velocityX": 4.078869951295308, - "velocityY": 0.2864164764173412, - "timestamp": 1.6580080304389697 - }, - { - "x": 6.34103865597215, - "y": 7.286039009493073, - "heading": -1.2616330241810545e-16, - "angularVelocity": 2.316026100003538e-16, - "velocityX": 4.078869963058216, - "velocityY": 0.2864163089003463, - "timestamp": 1.7368489281537358 - }, - { - "x": 6.654261604662237, - "y": 7.308033380004462, - "heading": -7.780878230098179e-16, - "angularVelocity": -8.268862240241936e-15, - "velocityX": 3.972848582004707, - "velocityY": 0.27897158897101676, - "timestamp": 1.815689825868502 - }, - { - "x": 6.932682031084689, - "y": 7.327583933385895, - "heading": -8.681753470357217e-16, - "angularVelocity": -1.1426496494529816e-15, - "velocityX": 3.5314213116869153, - "velocityY": 0.24797476878262784, - "timestamp": 1.894530723583268 - }, - { - "x": 7.176299915122605, - "y": 7.34469066831624, - "heading": -7.853180398656596e-16, - "angularVelocity": 1.0509432267454012e-15, - "velocityX": 3.089993786210898, - "velocityY": 0.21697793183727082, - "timestamp": 1.9733716212980341 - }, - { - "x": 7.385115250070514, - "y": 7.359353584352904, - "heading": -6.358392667251897e-16, - "angularVelocity": 1.8959547377957107e-15, - "velocityX": 2.6485661756842305, - "velocityY": 0.18598108927821216, - "timestamp": 2.0522125190128 - }, - { - "x": 7.559128032575715, - "y": 7.371572681274155, - "heading": -4.621266314308384e-16, - "angularVelocity": 2.2033315294472333e-15, - "velocityX": 2.2071385226326603, - "velocityY": 0.1549842439067266, - "timestamp": 2.1310534167275663 - }, - { - "x": 7.698338260626599, - "y": 7.381347958946806, - "heading": -2.953981147731697e-16, - "angularVelocity": 2.1147465581984e-15, - "velocityX": 1.7657108440662903, - "velocityY": 0.1239873968459356, - "timestamp": 2.2098943144423324 - }, - { - "x": 7.802745932882096, - "y": 7.388679417282003, - "heading": -1.5405031499378866e-16, - "angularVelocity": 1.7928233220485274e-15, - "velocityX": 1.3242831484901139, - "velocityY": 0.09299054865815141, - "timestamp": 2.2887352121570985 - }, - { - "x": 7.872351048384304, - "y": 7.393567056216249, - "heading": -4.730665578719817e-17, - "angularVelocity": 1.3539122789856364e-15, - "velocityX": 0.8828554407641064, - "velocityY": 0.061993699664978266, - "timestamp": 2.3675761098718646 - }, - { - "x": 7.907153606414795, - "y": 7.396010875701904, - "heading": 6.668503247685733e-18, - "angularVelocity": 6.84608633328086e-16, - "velocityX": 0.4414277239257422, - "velocityY": 0.030996850067544837, - "timestamp": 2.4464170075866307 - }, - { - "x": 7.907153606414795, - "y": 7.396010875701904, - "heading": 6.8565295403527305e-18, - "angularVelocity": 2.384883844975877e-18, - "velocityX": -5.0745498315179014e-17, - "velocityY": 9.257132972080978e-18, - "timestamp": 2.525257905301397 - }, - { - "x": 7.864379482827224, - "y": 7.392836087745542, - "heading": 0.011146441893639504, - "angularVelocity": 0.12739966793783036, - "velocityX": -0.48889225758201854, - "velocityY": -0.03628664064038576, - "timestamp": 2.6127498279575754 - }, - { - "x": 7.778824354644398, - "y": 7.386483417352748, - "heading": 0.03288500177532937, - "angularVelocity": 0.24846362066035474, - "velocityX": -0.9778631625120074, - "velocityY": -0.0726086500322888, - "timestamp": 2.700241750613754 - }, - { - "x": 7.650479577823578, - "y": 7.376948909938168, - "heading": 0.06447310235121183, - "angularVelocity": 0.36104019224741246, - "velocityX": -1.4669328656221532, - "velocityY": -0.1089758588578287, - "timestamp": 2.7877336732699325 - }, - { - "x": 7.479334044381584, - "y": 7.364227520798733, - "heading": 0.10486253590537926, - "angularVelocity": 0.46163614111999585, - "velocityX": -1.956129528831516, - "velocityY": -0.14540072675540094, - "timestamp": 2.875225595926111 - }, - { - "x": 7.265373349858742, - "y": 7.348312565522, - "heading": 0.1524685401703482, - "angularVelocity": 0.5441188491427787, - "velocityX": -2.4454908296352564, - "velocityY": -0.18190199499072268, - "timestamp": 2.9627175185822896 - }, - { - "x": 7.0085798004636075, - "y": 7.329194198798068, - "heading": 0.20463142630378972, - "angularVelocity": 0.5962023070224285, - "velocityX": -2.9350543638670454, - "velocityY": -0.21851579144124697, - "timestamp": 3.050209441238468 - }, - { - "x": 6.7089429147208905, - "y": 7.306854123718901, - "heading": 0.2559598531940128, - "angularVelocity": 0.5866647495212898, - "velocityX": -3.424737697446822, - "velocityY": -0.2553387147172141, - "timestamp": 3.1377013638946467 - }, - { - "x": 6.366659484955159, - "y": 7.281232741940888, - "heading": 0.2892524000892399, - "angularVelocity": 0.38052137711110223, - "velocityX": -3.912171768253631, - "velocityY": -0.2928428248022215, - "timestamp": 3.2251932865508253 - }, - { - "x": 6.009887187340099, - "y": 7.254843656864203, - "heading": 0.2892524060420795, - "angularVelocity": 6.803873294167354e-8, - "velocityX": -4.0777741165557195, - "velocityY": -0.3016173867888123, - "timestamp": 3.312685209207004 - }, - { - "x": 5.653114904073964, - "y": 7.228454377814743, - "heading": 0.28925241199532503, - "angularVelocity": 6.804337339458498e-8, - "velocityX": -4.077773952552866, - "velocityY": -0.3016196038251856, - "timestamp": 3.4001771318631824 - }, - { - "x": 5.296342618629369, - "y": 7.202065128217197, - "heading": 0.2892524179485705, - "angularVelocity": 6.804337236729352e-8, - "velocityX": -4.077773977451856, - "velocityY": -0.30161926720079874, - "timestamp": 3.487669054519361 - }, - { - "x": 4.93957033303766, - "y": 7.175675880608572, - "heading": 0.289252423901816, - "angularVelocity": 6.804337255673262e-8, - "velocityX": -4.077773979133303, - "velocityY": -0.3016192444681868, - "timestamp": 3.5751609771755395 - }, - { - "x": 4.5827980476089545, - "y": 7.1492866307962055, - "heading": 0.28925242985506144, - "angularVelocity": 6.804337216066887e-8, - "velocityX": -4.077773977270241, - "velocityY": -0.30161926965611885, - "timestamp": 3.662652899831718 - }, - { - "x": 4.226025763989892, - "y": 7.122897356518204, - "heading": 0.289252435808307, - "angularVelocity": 6.804337356855604e-8, - "velocityX": -4.077773956586706, - "velocityY": -0.3016195492891973, - "timestamp": 3.7501448224878966 - }, - { - "x": 3.86925346440752, - "y": 7.096508298041054, - "heading": 0.28925244176109, - "angularVelocity": 6.803808613081094e-8, - "velocityX": -4.077774139041357, - "velocityY": -0.3016170827660528, - "timestamp": 3.837636745144075 - }, - { - "x": 3.5270651589106934, - "y": 7.070826464452039, - "heading": 0.32204631423630514, - "angularVelocity": 0.374821714732306, - "velocityX": -3.911084533386486, - "velocityY": -0.29353376642480367, - "timestamp": 3.9251286678002537 - }, - { - "x": 3.2274696432203713, - "y": 7.0484355191686605, - "heading": 0.3751215132796767, - "angularVelocity": 0.6066297028577579, - "velocityX": -3.424264853198595, - "velocityY": -0.2559201421526573, - "timestamp": 4.012620590456432 - }, - { - "x": 2.970717055574845, - "y": 7.029266180646158, - "heading": 0.42936926776546197, - "angularVelocity": 0.6200315736455514, - "velocityX": -2.934586186367169, - "velocityY": -0.2190983800622664, - "timestamp": 4.100112513112611 - }, - { - "x": 2.756796090717204, - "y": 7.013301299452356, - "heading": 0.4789124689781157, - "angularVelocity": 0.5662602867620836, - "velocityX": -2.445036734400004, - "velocityY": -0.18247262957679933, - "timestamp": 4.18760443576879 - }, - { - "x": 2.58568613499147, - "y": 7.00053390938676, - "heading": 0.5209305288090914, - "angularVelocity": 0.48025073121431167, - "velocityX": -1.955722888822021, - "velocityY": -0.14592650016126135, - "timestamp": 4.275096358424969 - }, - { - "x": 2.457370465153117, - "y": 6.990960629174714, - "heading": 0.5537832816752974, - "angularVelocity": 0.3754946956110353, - "velocityX": -1.466600183683258, - "velocityY": -0.10941901745223155, - "timestamp": 4.362588281081148 - }, - { - "x": 2.3718361943351365, - "y": 6.9845795744226535, - "heading": 0.576397068559217, - "angularVelocity": 0.25846713842129254, - "velocityX": -0.9776247706214999, - "velocityY": -0.07293307265789097, - "timestamp": 4.450080203737327 - }, - { - "x": 2.3290731906890865, - "y": 6.98138952255249, - "heading": 0.5880023600681887, - "angularVelocity": 0.13264414767266713, - "velocityX": -0.48876516080344745, - "velocityY": -0.03646110147446347, - "timestamp": 4.537572126393506 - }, - { - "x": 2.3290731906890865, - "y": 6.98138952255249, - "heading": 0.5880023600681887, - "angularVelocity": -4.727611122290784e-19, - "velocityX": 5.894237907108255e-17, - "velocityY": -1.7416397125878645e-17, - "timestamp": 4.625064049049685 - }, - { - "x": 2.369225863639807, - "y": 6.972681044615008, - "heading": 0.5777100333288744, - "angularVelocity": -0.12019172387184421, - "velocityX": 0.468894847806877, - "velocityY": -0.10169585576872482, - "timestamp": 4.710696623471271 - }, - { - "x": 2.4495365738113324, - "y": 6.955262677241521, - "heading": 0.5575096279601734, - "angularVelocity": -0.2358962755136905, - "velocityX": 0.9378523384820929, - "velocityY": -0.20340819473362512, - "timestamp": 4.796329197892856 - }, - { - "x": 2.5700119937448345, - "y": 6.929132707142634, - "heading": 0.52790113095987, - "angularVelocity": -0.34576207944578496, - "velocityX": 1.4068877497525563, - "velocityY": -0.30514054114786776, - "timestamp": 4.881961772314441 - }, - { - "x": 2.7306608435827027, - "y": 6.894288978936286, - "heading": 0.4895909495937422, - "angularVelocity": -0.4473786012495606, - "velocityX": 1.876024993093903, - "velocityY": -0.4068980576807838, - "timestamp": 4.967594346736027 - }, - { - "x": 2.9314950654935967, - "y": 6.850728824040358, - "heading": 0.4436739187143449, - "angularVelocity": -0.5362098616040543, - "velocityX": 2.34530169468162, - "velocityY": -0.5086867373795613, - "timestamp": 5.053226921157612 - }, - { - "x": 3.1725314152869473, - "y": 6.798449478849849, - "heading": 0.3920593457901536, - "angularVelocity": -0.6027446129329584, - "velocityX": 2.8147740672455193, - "velocityY": -0.6105076899022854, - "timestamp": 5.138859495579197 - }, - { - "x": 3.453789902204572, - "y": 6.737451911247368, - "heading": 0.3387609345614091, - "angularVelocity": -0.6224081383603658, - "velocityX": 3.2844801037153855, - "velocityY": -0.7123173396864007, - "timestamp": 5.224492070000783 - }, - { - "x": 3.7752032898636747, - "y": 6.667792428626339, - "heading": 0.2965850468038582, - "angularVelocity": -0.4925215438451132, - "velocityX": 3.753400967215154, - "velocityY": -0.8134694430425643, - "timestamp": 5.310124644422368 - }, - { - "x": 4.117354039283791, - "y": 6.5934027343019475, - "heading": 0.2965850415443264, - "angularVelocity": -6.141975552750511e-8, - "velocityX": 3.9955677116005384, - "velocityY": -0.8687079049866928, - "timestamp": 5.395757218843953 - }, - { - "x": 4.4595048423406825, - "y": 6.519013286687181, - "heading": 0.29658503628387156, - "angularVelocity": -6.143053505137436e-8, - "velocityX": 3.9955683379600107, - "velocityY": -0.868705023961243, - "timestamp": 5.481389793265539 - }, - { - "x": 4.801655639434736, - "y": 6.444623811646621, - "heading": 0.2965850310234168, - "angularVelocity": -6.143053372565425e-8, - "velocityX": 3.9955682683271863, - "velocityY": -0.868705344234141, - "timestamp": 5.567022367687124 - }, - { - "x": 5.143806436012128, - "y": 6.370234334229709, - "heading": 0.2965850257629621, - "angularVelocity": -6.143053357288208e-8, - "velocityX": 3.995568262293729, - "velocityY": -0.8687053719847133, - "timestamp": 5.652654942108709 - }, - { - "x": 5.485957232575462, - "y": 6.29584485674813, - "heading": 0.29658502050250735, - "angularVelocity": -6.1430533960204e-8, - "velocityX": 3.9955682621295425, - "velocityY": -0.868705372739879, - "timestamp": 5.738287516530295 - }, - { - "x": 5.828108029592984, - "y": 6.2214553813555735, - "heading": 0.2965850152420526, - "angularVelocity": -6.143053356332505e-8, - "velocityX": 3.995568267433484, - "velocityY": -0.868705348344694, - "timestamp": 5.82392009095188 - }, - { - "x": 6.170258832871027, - "y": 6.1470659347579835, - "heading": 0.2965850099815977, - "angularVelocity": -6.143053548401422e-8, - "velocityX": 3.995568340542574, - "velocityY": -0.8687050120828734, - "timestamp": 5.909552665373465 - }, - { - "x": 6.512409595508601, - "y": 6.072676301228697, - "heading": 0.2965850047217149, - "angularVelocity": -6.142385454674703e-8, - "velocityX": 3.9955678659513554, - "velocityY": -0.8687071950336539, - "timestamp": 5.995185239795051 - }, - { - "x": 6.833835100702535, - "y": 6.002994889630869, - "heading": 0.2538296262850911, - "angularVelocity": -0.4992887195721912, - "velocityX": 3.753542473352417, - "velocityY": -0.813725525227982, - "timestamp": 6.080817814216636 - }, - { - "x": 7.115102384879702, - "y": 5.9419710655129725, - "heading": 0.2000312974914686, - "angularVelocity": -0.6282460752465883, - "velocityX": 3.2845828363449043, - "velocityY": -0.7126239579983176, - "timestamp": 6.166450388638221 - }, - { - "x": 7.356146388960769, - "y": 5.8896622648493135, - "heading": 0.1477223466018653, - "angularVelocity": -0.6108534193083621, - "velocityX": 2.8148634524796803, - "velocityY": -0.610851664999963, - "timestamp": 6.252082963059807 - }, - { - "x": 7.556986217467668, - "y": 5.846072534731234, - "heading": 0.1009603902610464, - "angularVelocity": -0.5460767313919686, - "velocityX": 2.3453671673833587, - "velocityY": -0.5090321108820021, - "timestamp": 6.337715537481392 - }, - { - "x": 7.7176386942846635, - "y": 5.81120177694534, - "heading": 0.061773947552466436, - "angularVelocity": -0.4576114051605831, - "velocityX": 1.8760673482274681, - "velocityY": -0.4072137036803266, - "timestamp": 6.423348111902977 - }, - { - "x": 7.838116264571975, - "y": 5.785049474927425, - "heading": 0.03138354406077533, - "angularVelocity": -0.35489302636253056, - "velocityX": 1.4069128611523227, - "velocityY": -0.30540132881164084, - "timestamp": 6.508980686324563 - }, - { - "x": 7.91842817978787, - "y": 5.7676151344306845, - "heading": 0.01060107904356323, - "angularVelocity": -0.24269345114974641, - "velocityX": 0.9378664107481443, - "velocityY": -0.2035947256578809, - "timestamp": 6.594613260746148 - }, - { - "x": 7.958581447601322, - "y": 5.758898258209228, - "heading": 1.8857923302222142e-17, - "angularVelocity": -0.12379727125068163, - "velocityX": 0.46890179449433855, - "velocityY": -0.10179392924172202, - "timestamp": 6.680245835167733 - }, - { - "x": 7.958581447601321, - "y": 5.758898258209228, - "heading": 9.223143488076806e-18, - "angularVelocity": -4.402147521794544e-18, - "velocityX": -8.677336320987788e-16, - "velocityY": 6.30351391233509e-17, - "timestamp": 6.765878409589319 - }, - { - "x": 7.9330263948013044, - "y": 5.765824370875185, - "heading": -2.1346547777556126e-16, - "angularVelocity": -3.2422884449291236e-15, - "velocityX": -0.37207493315591844, - "velocityY": 0.1008424019853738, - "timestamp": 6.8345609531384435 - }, - { - "x": 7.881916289817926, - "y": 5.7796765966969605, - "heading": -6.253127415258978e-16, - "angularVelocity": -5.996389119459277e-15, - "velocityX": -0.7441498573334772, - "velocityY": 0.20168481110296488, - "timestamp": 6.903243496687568 - }, - { - "x": 7.805251133422258, - "y": 5.800454936287824, - "heading": -1.1913919936036707e-15, - "angularVelocity": -8.241966786851453e-15, - "velocityX": -1.116224770284402, - "velocityY": 0.30252722914963337, - "timestamp": 6.971926040236693 - }, - { - "x": 7.703030926606096, - "y": 5.82815939043782, - "heading": -1.851221230426738e-15, - "angularVelocity": -9.606942197740195e-15, - "velocityX": -1.4882996687950614, - "velocityY": 0.40336965869910185, - "timestamp": 7.040608583785818 - }, - { - "x": 7.575255670692555, - "y": 5.862789960203048, - "heading": -2.5464780342778514e-15, - "angularVelocity": -1.012275836510253e-14, - "velocityX": -1.8603745480414884, - "velocityY": 0.5042121036251386, - "timestamp": 7.109291127334942 - }, - { - "x": 7.4219253675353904, - "y": 5.904346647067262, - "heading": -3.1806793576894922e-15, - "angularVelocity": -9.233806434397994e-15, - "velocityX": -2.2324494002977344, - "velocityY": 0.6050545701542501, - "timestamp": 7.177973670884067 - }, - { - "x": 7.2430400199182765, - "y": 5.952829453267497, - "heading": -3.623858903243655e-15, - "angularVelocity": -6.452578990774704e-15, - "velocityX": -2.6045242120243945, - "velocityY": 0.7058970692539853, - "timestamp": 7.246656214433192 - }, - { - "x": 7.038599632488783, - "y": 6.008238382562526, - "heading": -3.650395176878746e-15, - "angularVelocity": -3.8636122947275614e-16, - "velocityX": -2.97659895608365, - "velocityY": 0.8067396230805047, - "timestamp": 7.315338757982317 - }, - { - "x": 6.808604214572368, - "y": 6.070573442583485, - "heading": -2.8238139058641448e-15, - "angularVelocity": 1.2034808505527204e-14, - "velocityX": -3.348673564366667, - "velocityY": 0.9075822880143853, - "timestamp": 7.384021301531441 - }, - { - "x": 6.553053794384004, - "y": 6.139834657117734, - "heading": 1.596126099324249e-17, - "angularVelocity": 4.1346388378013e-14, - "velocityX": -3.720747761846995, - "velocityY": 1.008425299286029, - "timestamp": 7.452703845080566 - }, - { - "x": 6.281995287149088, - "y": 6.213296859163915, - "heading": 2.79485990380299e-17, - "angularVelocity": 1.7453253668100859e-16, - "velocityX": -3.9465414824226293, - "velocityY": 1.0695905866332982, - "timestamp": 7.521386388629691 - }, - { - "x": 6.010911377533634, - "y": 6.286665266952668, - "heading": 4.354488243724968e-17, - "angularVelocity": 2.2707784403071564e-16, - "velocityX": -3.946911334487284, - "velocityY": 1.0682249665997634, - "timestamp": 7.5900689321788155 - }, - { - "x": 5.735304320584762, - "y": 6.340611159587319, - "heading": -1.2228292980098705e-17, - "angularVelocity": -8.120429446664662e-16, - "velocityX": -4.01276718518365, - "velocityY": 0.7854381891967174, - "timestamp": 7.65875147572794 - }, - { - "x": 5.45584440231323, - "y": 6.368388175964356, - "heading": 4.246681415993057e-24, - "angularVelocity": 1.780408398032683e-16, - "velocityX": -4.068863845609499, - "velocityY": 0.4044261458833793, - "timestamp": 7.727434019277065 - }, - { - "x": 5.136152723997814, - "y": 6.365732339700935, - "heading": 1.6384633695167614e-17, - "angularVelocity": 2.0955505420330122e-16, - "velocityX": -4.088772502150643, - "velocityY": -0.033967447441583046, - "timestamp": 7.805621709628452 - }, - { - "x": 4.818581346862357, - "y": 6.328877980423311, - "heading": 1.0499775957498806e-16, - "angularVelocity": 1.1333385308675126e-15, - "velocityX": -4.061654407600595, - "velocityY": -0.4713575642002044, - "timestamp": 7.88380939997984 - }, - { - "x": 4.503030122212764, - "y": 6.277523836631423, - "heading": 9.793507555323466e-17, - "angularVelocity": -9.032990448793397e-17, - "velocityX": -4.035817188510941, - "velocityY": -0.6568059953935135, - "timestamp": 7.961997090331227 - }, - { - "x": 4.187477597853484, - "y": 6.226177681156534, - "heading": 1.280731439045987e-16, - "angularVelocity": 3.8545788718687876e-16, - "velocityX": -4.035833811447281, - "velocityY": -0.6567038269737849, - "timestamp": 8.040184780682615 - }, - { - "x": 3.8719249552864756, - "y": 6.174832252105637, - "heading": 5.2578025910772326e-17, - "angularVelocity": -9.655627841710176e-16, - "velocityX": -4.035835323292801, - "velocityY": -0.6566945362034219, - "timestamp": 8.118372471034004 - }, - { - "x": 3.5671208452798924, - "y": 6.125235876580712, - "heading": 4.8122010318066635e-17, - "angularVelocity": -5.6991340073129e-17, - "velocityX": -3.898364418191803, - "velocityY": -0.6343246015194628, - "timestamp": 8.196560161385392 - }, - { - "x": 3.2961838319651493, - "y": 6.081150202022552, - "heading": 4.0696028624236024e-17, - "angularVelocity": -9.497633857848639e-17, - "velocityX": -3.465213156917534, - "velocityY": -0.563844185202012, - "timestamp": 8.27474785173678 - }, - { - "x": 3.059113934767495, - "y": 6.042575233860294, - "heading": 3.553772215118994e-17, - "angularVelocity": -6.597338124206766e-17, - "velocityX": -3.0320616471994413, - "velocityY": -0.49336369944725295, - "timestamp": 8.352935542088169 - }, - { - "x": 2.855911160163984, - "y": 6.009510973892544, - "heading": 2.649178608062371e-17, - "angularVelocity": -1.1569513470330116e-16, - "velocityX": -2.5989100546415322, - "velocityY": -0.4228831906888374, - "timestamp": 8.431123232439557 - }, - { - "x": 2.6865755113935372, - "y": 5.981957423016371, - "heading": 1.7566522066077413e-17, - "angularVelocity": -1.1415175421837036e-16, - "velocityX": -2.165758420658685, - "velocityY": -0.3524026704571279, - "timestamp": 8.509310922790945 - }, - { - "x": 2.5511069903996364, - "y": 5.959914581769268, - "heading": 1.0131326907565385e-17, - "angularVelocity": -9.509415935280826e-17, - "velocityX": -1.7326067618191963, - "velocityY": -0.28192214335099686, - "timestamp": 8.587498613142333 - }, - { - "x": 2.449505598477995, - "y": 5.943382450509244, - "heading": 7.634100326841493e-18, - "angularVelocity": -3.1938869105023585e-17, - "velocityX": -1.2994550864078938, - "velocityY": -0.2114416116660293, - "timestamp": 8.665686303493722 - }, - { - "x": 2.3817713365541486, - "y": 5.932361029491859, - "heading": 6.441956734413491e-18, - "angularVelocity": -1.5247224888248296e-17, - "velocityX": -0.8663033991592198, - "velocityY": -0.14096107671253, - "timestamp": 8.74387399384511 - }, - { - "x": 2.347904205322266, - "y": 5.926850318908691, - "heading": 9.78440936544715e-29, - "angularVelocity": -8.239093217325014e-17, - "velocityX": -0.43315170303231587, - "velocityY": -0.0704805393087875, - "timestamp": 8.822061684196498 - }, - { - "x": 2.347904205322266, - "y": 5.926850318908691, - "heading": 1.1645894687727951e-28, - "angularVelocity": 7.55747290243908e-29, - "velocityX": 1.765775264015562e-17, - "velocityY": 1.8435301762989098e-16, - "timestamp": 8.900249374547887 - }, - { - "x": 2.3488457561194225, - "y": 5.978635720040668, - "heading": 3.476477232733492e-18, - "angularVelocity": 3.618994255933981e-17, - "velocityX": 0.009801493577281458, - "velocityY": 0.5390832636166636, - "timestamp": 8.996311346096682 - }, - { - "x": 2.350728857677348, - "y": 6.0822065203033, - "heading": 1.0600655166660911e-17, - "angularVelocity": 7.416231229628075e-17, - "velocityX": 0.019602986775770496, - "velocityY": 1.0781665063997017, - "timestamp": 9.092373317645478 - }, - { - "x": 2.353553509923267, - "y": 6.237562715693952, - "heading": 2.1010600692164996e-17, - "angularVelocity": 1.0836697766594381e-16, - "velocityX": 0.02940447921667478, - "velocityY": 1.6172497075154921, - "timestamp": 9.188435289194274 - }, - { - "x": 2.357319712638855, - "y": 6.444704294204712, - "heading": 3.468807748402165e-17, - "angularVelocity": 1.4238180386272635e-16, - "velocityX": 0.039205969384825255, - "velocityY": 2.156332783629564, - "timestamp": 9.28449726074307 - }, - { - "x": 2.361085915354443, - "y": 6.651845872715471, - "heading": 1.3778053809890579e-17, - "angularVelocity": -2.1767223113507178e-16, - "velocityX": 0.039205969384825234, - "velocityY": 2.1563327836295634, - "timestamp": 9.380559232291866 - }, - { - "x": 2.363910567600362, - "y": 6.807202068106124, - "heading": -1.1169584961898105e-17, - "angularVelocity": -2.59703588940451e-16, - "velocityX": 0.029404479216674762, - "velocityY": 1.6172497075154917, - "timestamp": 9.476621203840661 - }, - { - "x": 2.365793669158288, - "y": 6.910772868368756, - "heading": -3.7105010844358794e-18, - "angularVelocity": 7.764866530502248e-17, - "velocityX": 0.01960298677577048, - "velocityY": 1.0781665063997015, - "timestamp": 9.572683175389457 - }, - { - "x": 2.3667352199554443, - "y": 6.962558269500732, - "heading": -4.000836031847083e-20, - "angularVelocity": 3.820963347732001e-17, - "velocityX": 0.00980149357728144, - "velocityY": 0.5390832636166634, - "timestamp": 9.668745146938253 - }, - { - "x": 2.3667352199554443, - "y": 6.962558269500732, - "heading": -3.5257727991835196e-20, - "angularVelocity": 4.945382777465005e-20, - "velocityX": 8.227899776891578e-20, - "velocityY": 1.1821145226159077e-19, - "timestamp": 9.764807118487049 - }, - { - "x": 2.3667489790575416, - "y": 6.937171768983989, - "heading": 0.02562390557481022, - "angularVelocity": 0.37556373996212167, - "velocityX": 0.00020166402139990016, - "velocityY": -0.3720841481710473, - "timestamp": 9.83303496598211 - }, - { - "x": 2.366781276266347, - "y": 6.886396802207365, - "heading": 0.07683165255040712, - "angularVelocity": 0.7505402684630225, - "velocityX": 0.00047337282343297855, - "velocityY": -0.7441971077909122, - "timestamp": 9.901262813477171 - }, - { - "x": 2.366837253346187, - "y": 6.810226488063326, - "heading": 0.15348515850807642, - "angularVelocity": 1.123492954445305, - "velocityX": 0.0008204432925090531, - "velocityY": -1.1164109222345555, - "timestamp": 9.969490660972232 - }, - { - "x": 2.366917007756982, - "y": 6.7086457343302905, - "heading": 0.25528804756869294, - "angularVelocity": 1.4921017267617398, - "velocityX": 0.0011689422094309653, - "velocityY": -1.4888459399278209, - "timestamp": 10.037718508467293 - }, - { - "x": 2.366858285903453, - "y": 6.606843724969224, - "heading": 0.35303920041696696, - "angularVelocity": 1.4327163531775036, - "velocityX": -0.0008606728144779493, - "velocityY": -1.4920888332060707, - "timestamp": 10.105946355962354 - }, - { - "x": 2.366800542269629, - "y": 6.530484012907421, - "heading": 0.42622373934358737, - "angularVelocity": 1.072649095839037, - "velocityX": -0.0008463352713547739, - "velocityY": -1.1191868843191977, - "timestamp": 10.174174203457415 - }, - { - "x": 2.3667570038160806, - "y": 6.479574895496538, - "heading": 0.47497446510426156, - "angularVelocity": 0.7145282688890857, - "velocityX": -0.000638133183841856, - "velocityY": -0.7461633230415036, - "timestamp": 10.242402050952476 - }, - { - "x": 2.3667352199554443, - "y": 6.45412015914917, - "heading": 0.49934664103730353, - "angularVelocity": 0.35721742408488544, - "velocityX": -0.00031928107709780734, - "velocityY": -0.3730842651779495, - "timestamp": 10.310629898447537 - }, - { - "x": 2.3667352199554443, - "y": 6.45412015914917, - "heading": 0.49934664103730353, - "angularVelocity": -2.3249470264091964e-20, - "velocityX": -2.5425798784781645e-20, - "velocityY": -4.57825100706316e-20, - "timestamp": 10.378857745942598 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 0 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - }, - { - "scope": [ - 6 - ], - "type": "StopPoint" - }, - { - "scope": [ - 8 - ], - "type": "StopPoint" - }, - { - "scope": [ - 7 - ], - "type": "StopPoint" - }, - { - "scope": [ - 8 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "3NBMid1st": { - "waypoints": [ - { - "x": 0.8085346817970276, - "y": 4.440213680267334, - "heading": -0.9944211947753461, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 21 - }, - { - "x": 4.03019905090332, - "y": 1.7862433195114136, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 7.989865779876709, - "y": 0.6650473475456238, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "x": 5.31951379776001, - "y": 0.9000114798545837, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 19 - }, - { - "x": 2.3202381134033203, - "y": 3.3388166427612305, - "heading": -0.6871242163258895, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 4.759042739868164, - "y": 1.914918065071106, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 7.989865779876709, - "y": 2.396514654159546, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 14 - }, - { - "x": 4.728747367858887, - "y": 1.293855905532837, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 - }, - { - "x": 2.3050899505615234, - "y": 3.1721901893615723, - "heading": -0.8148015740342935, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 2.4262728691101074, - "y": 4.11135721206665, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 7 - }, - { - "x": 2.2899420261383057, - "y": 5.095968246459961, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.8085346817970276, - "y": 4.440213680267334, - "heading": -0.994421194775346, - "angularVelocity": 1.5490406402833945, - "velocityX": 1.8902870667855096, - "velocityY": -2.2010802892496195, - "timestamp": 0 - }, - { - "x": 0.9215971079167389, - "y": 4.308622426592224, - "heading": -0.8609906790037924, - "angularVelocity": 2.3420643493386937, - "velocityX": 1.9845496057144765, - "velocityY": -2.309780353730473, - "timestamp": 0.05697132779858461 - }, - { - "x": 1.0571668168480761, - "y": 4.174806360601604, - "heading": -0.7788891157181965, - "angularVelocity": 1.441103208544767, - "velocityX": 2.3796129416296528, - "velocityY": -2.3488317924362083, - "timestamp": 0.11394265559716922 - }, - { - "x": 1.2076261145428426, - "y": 4.0379914622006465, - "heading": -0.737938644372034, - "angularVelocity": 0.7187908888298683, - "velocityX": 2.640965262854634, - "velocityY": -2.4014693651629555, - "timestamp": 0.17091398339575384 - }, - { - "x": 1.364794019769659, - "y": 3.9030112436724806, - "heading": -0.7095966761729068, - "angularVelocity": 0.4974777540612509, - "velocityX": 2.7587193646331154, - "velocityY": -2.369265799901533, - "timestamp": 0.22788531119433844 - }, - { - "x": 1.5254149005845012, - "y": 3.773576465253033, - "heading": -0.6782849435340634, - "angularVelocity": 0.5496051057391952, - "velocityX": 2.8193283713291417, - "velocityY": -2.271928414184925, - "timestamp": 0.2848566389929231 - }, - { - "x": 1.6863383745410894, - "y": 3.6459599816818566, - "heading": -0.6447688867134147, - "angularVelocity": 0.5882969226053631, - "velocityX": 2.8246396946462973, - "velocityY": -2.24001245016351, - "timestamp": 0.3418279667915077 - }, - { - "x": 1.8467513472707422, - "y": 3.5185431346202543, - "heading": -0.6105743641016009, - "angularVelocity": 0.6002058216495214, - "velocityX": 2.8156790253647204, - "velocityY": -2.2365082925936, - "timestamp": 0.3987992945900923 - }, - { - "x": 2.0066203974076964, - "y": 3.391336490619763, - "heading": -0.5756204436829432, - "angularVelocity": 0.6135352951967907, - "velocityX": 2.8061317212432253, - "velocityY": -2.232818663630508, - "timestamp": 0.4557706223886769 - }, - { - "x": 2.1659061752732116, - "y": 3.264353822663242, - "heading": -0.5398102091748411, - "angularVelocity": 0.6285659101136807, - "velocityX": 2.795893724447691, - "velocityY": -2.2288872817823973, - "timestamp": 0.5127419501872615 - }, - { - "x": 2.3245615366839587, - "y": 3.137612895634049, - "heading": -0.5030261268461845, - "angularVelocity": 0.6456595580622994, - "velocityX": 2.7848282204630053, - "velocityY": -2.2246440784611945, - "timestamp": 0.5697132779858461 - }, - { - "x": 2.4825289631452856, - "y": 3.0111369184355397, - "heading": -0.46512296697159733, - "angularVelocity": 0.6653023782171502, - "velocityX": 2.772753112228705, - "velocityY": -2.219993496476861, - "timestamp": 0.6266846057844307 - }, - { - "x": 2.639736737765091, - "y": 2.884956763498202, - "heading": -0.4259172061429576, - "angularVelocity": 0.6881665276829608, - "velocityX": 2.7594191796897314, - "velocityY": -2.214801020320839, - "timestamp": 0.6836559335830154 - }, - { - "x": 2.7960930159717448, - "y": 2.759114488797632, - "heading": -0.3851704679953963, - "angularVelocity": 0.7152148233514343, - "velocityX": 2.744473127946619, - "velocityY": -2.2088703135982724, - "timestamp": 0.7406272613816 - }, - { - "x": 2.9514761274080783, - "y": 2.633669229930612, - "heading": -0.3425622280740764, - "angularVelocity": 0.7478891850994993, - "velocityX": 2.7273914342609693, - "velocityY": -2.201901618135293, - "timestamp": 0.7975985891801846 - }, - { - "x": 3.105717594116411, - "y": 2.5087077543800937, - "heading": -0.29764163685561806, - "angularVelocity": 0.7884771683270175, - "velocityX": 2.707352499377148, - "velocityY": -2.193409920026875, - "timestamp": 0.8545699169787692 - }, - { - "x": 3.258569563388298, - "y": 2.384365172612816, - "heading": -0.249734366445047, - "angularVelocity": 0.8409014193936561, - "velocityX": 2.6829630829788034, - "velocityY": -2.182546669912214, - "timestamp": 0.9115412447773538 - }, - { - "x": 3.4096337882248857, - "y": 2.2608711625821383, - "heading": -0.19773775213511016, - "angularVelocity": 0.9126804011618742, - "velocityX": 2.651583360855795, - "velocityY": -2.1676519541070953, - "timestamp": 0.9685125725759384 - }, - { - "x": 3.5581727184543612, - "y": 2.1386758008181714, - "heading": -0.139571203831234, - "angularVelocity": 1.0209793338417021, - "velocityX": 2.607257649929052, - "velocityY": -2.144857184932988, - "timestamp": 1.025483900374523 - }, - { - "x": 3.702391809739322, - "y": 2.018936204532916, - "heading": -0.07007418918166924, - "angularVelocity": 1.219859486078036, - "velocityX": 2.5314328603123006, - "velocityY": -2.1017518971750273, - "timestamp": 1.0824552281731077 - }, - { - "x": 3.855921574246425, - "y": 1.8942554845290054, - "heading": -0.02632139830741084, - "angularVelocity": 0.7679791320458813, - "velocityX": 2.694860211962926, - "velocityY": -2.1884819052261575, - "timestamp": 1.1394265559716923 - }, - { - "x": 4.03019905090332, - "y": 1.7862433195114136, - "heading": 0, - "angularVelocity": 0.4620113191054108, - "velocityX": 3.059038351239285, - "velocityY": -1.8959039431107558, - "timestamp": 1.196397883770277 - }, - { - "x": 4.329561467960323, - "y": 1.669252288909222, - "heading": 2.8762828709721077e-9, - "angularVelocity": 3.3853294762370436e-8, - "velocityX": 3.5234379231905075, - "velocityY": -1.3769618709966909, - "timestamp": 1.281361042929876 - }, - { - "x": 4.6395186577016645, - "y": 1.584215240287449, - "heading": 2.8762827416187334e-9, - "angularVelocity": -1.5224642721178536e-15, - "velocityX": 3.6481363547122974, - "velocityY": -1.0008696647218007, - "timestamp": 1.3663242020894752 - }, - { - "x": 4.949475918586316, - "y": 1.4991784509810746, - "heading": 2.876282678042524e-9, - "angularVelocity": -7.482797246900931e-16, - "velocityX": 3.648137192055341, - "velocityY": -1.0008666126295613, - "timestamp": 1.4512873612490744 - }, - { - "x": 5.259433179471324, - "y": 1.4141416616759983, - "heading": 2.876282628580193e-9, - "angularVelocity": -5.82162094124467e-16, - "velocityX": 3.648137192059533, - "velocityY": -1.000866612614283, - "timestamp": 1.5362505204086736 - }, - { - "x": 5.569390440356331, - "y": 1.3291048723709222, - "heading": 2.8762825491363832e-9, - "angularVelocity": -9.350383248396003e-16, - "velocityX": 3.648137192059533, - "velocityY": -1.0008666126142827, - "timestamp": 1.6212136795682728 - }, - { - "x": 5.879347701241338, - "y": 1.244068083065846, - "heading": 2.8762824884432015e-9, - "angularVelocity": -7.143470432395581e-16, - "velocityX": 3.648137192059533, - "velocityY": -1.0008666126142824, - "timestamp": 1.706176838727872 - }, - { - "x": 6.189304962126346, - "y": 1.1590312937607696, - "heading": 2.876282331475103e-9, - "angularVelocity": -1.8474842515275816e-15, - "velocityX": 3.648137192059533, - "velocityY": -1.0008666126142827, - "timestamp": 1.7911399978874711 - }, - { - "x": 6.499262223011353, - "y": 1.0739945044556933, - "heading": 2.8762822574949493e-9, - "angularVelocity": -8.707321426249056e-16, - "velocityX": 3.648137192059533, - "velocityY": -1.0008666126142827, - "timestamp": 1.8761031570470703 - }, - { - "x": 6.809219483896361, - "y": 0.988957715150617, - "heading": 2.876282174652877e-9, - "angularVelocity": -9.750352171187047e-16, - "velocityX": 3.6481371920595334, - "velocityY": -1.0008666126142827, - "timestamp": 1.9610663162066695 - }, - { - "x": 7.119176744781355, - "y": 0.9039209258455444, - "heading": 2.876282068308637e-9, - "angularVelocity": -1.2516511966059091e-15, - "velocityX": 3.6481371920593757, - "velocityY": -1.0008666126142396, - "timestamp": 2.046029475366269 - }, - { - "x": 7.409406401112432, - "y": 0.824296405790611, - "heading": 1.9175251160573477e-9, - "angularVelocity": -1.128438445244615e-8, - "velocityX": 3.4159470905018354, - "velocityY": -0.9371652471792223, - "timestamp": 2.1309926345258683 - }, - { - "x": 7.6415901455910715, - "y": 0.7605967844204935, - "heading": 1.1505163163502184e-9, - "angularVelocity": -9.02754567148733e-9, - "velocityX": 2.732757900897883, - "velocityY": -0.7497322604313802, - "timestamp": 2.2159557936854677 - }, - { - "x": 7.815727960351833, - "y": 0.7128220666365764, - "heading": 5.752585827191569e-10, - "angularVelocity": -6.770672598819766e-9, - "velocityX": 2.049568501021147, - "velocityY": -0.5622992159951897, - "timestamp": 2.300918952845067 - }, - { - "x": 7.931819839439569, - "y": 0.680972254072654, - "heading": 1.917529656301049e-10, - "angularVelocity": -4.513787162370632e-9, - "velocityX": 1.3663790310534918, - "velocityY": -0.3748661523295523, - "timestamp": 2.3858821120046665 - }, - { - "x": 7.989865779876709, - "y": 0.6650473475456238, - "heading": 0, - "angularVelocity": -2.2568954300522894e-9, - "velocityX": 0.6831895260403813, - "velocityY": -0.18743307904919254, - "timestamp": 2.470845271164266 - }, - { - "x": 7.989865779876709, - "y": 0.6650473475456238, - "heading": 0, - "angularVelocity": 0, - "velocityX": 5.7398628141890084e-36, - "velocityY": 6.163577013070246e-34, - "timestamp": 2.5558084303238653 - }, - { - "x": 7.942973367083922, - "y": 0.667457868334859, - "heading": 1.5015515417643764e-15, - "angularVelocity": 2.0009529191708877e-14, - "velocityX": -0.6248837129789168, - "velocityY": 0.03212236460611118, - "timestamp": 2.630850253016353 - }, - { - "x": 7.84918854285797, - "y": 0.6722789098434376, - "heading": 4.5057793482934065e-15, - "angularVelocity": 4.0034046332269777e-14, - "velocityX": -1.2497674078396386, - "velocityY": 0.06424472828085026, - "timestamp": 2.7058920757088405 - }, - { - "x": 7.70851130923829, - "y": 0.6795104719665217, - "heading": 9.022031304760564e-15, - "angularVelocity": 6.018313247766123e-14, - "velocityX": -1.874651075523027, - "velocityY": 0.09636709055852906, - "timestamp": 2.780933898401328 - }, - { - "x": 7.520941669623951, - "y": 0.6891525545293808, - "heading": 1.504590019224722e-14, - "angularVelocity": 8.027348845418817e-14, - "velocityX": -2.49953469791075, - "velocityY": 0.12848945050776842, - "timestamp": 2.8559757210938157 - }, - { - "x": 7.286479630813121, - "y": 0.7012051571825528, - "heading": 2.2571401776550036e-14, - "angularVelocity": 1.0028409911019087e-13, - "velocityX": -3.124418229706744, - "velocityY": 0.16061180580010845, - "timestamp": 2.9310175437863033 - }, - { - "x": 7.005125213200534, - "y": 0.7156682788776393, - "heading": 3.160518517475634e-14, - "angularVelocity": 1.2038331525109193e-13, - "velocityX": -3.749301489724504, - "velocityY": 0.1927341471215938, - "timestamp": 3.006059366478791 - }, - { - "x": 6.721620804307546, - "y": 0.730241921622282, - "heading": 3.159325845827104e-14, - "angularVelocity": -1.5893425902210946e-16, - "velocityX": -3.777952063541342, - "velocityY": 0.19420693983358725, - "timestamp": 3.0811011891712785 - }, - { - "x": 6.438116395414555, - "y": 0.7448155643669249, - "heading": 3.1611801523052584e-14, - "angularVelocity": 2.4710307021100496e-16, - "velocityX": -3.7779520635413752, - "velocityY": 0.19420693983358922, - "timestamp": 3.156143011863766 - }, - { - "x": 6.154611986521697, - "y": 0.7593892071141327, - "heading": 3.1598377018547585e-14, - "angularVelocity": -1.7889363588638215e-16, - "velocityX": -3.777952063539618, - "velocityY": 0.19420693986776963, - "timestamp": 3.2311848345562537 - }, - { - "x": 5.8711075940921145, - "y": 0.7739631701216403, - "heading": 3.1576411220426416e-14, - "angularVelocity": -2.9271408040259687e-16, - "velocityX": -3.777951844151616, - "velocityY": 0.19421120762524555, - "timestamp": 3.3062266572487413 - }, - { - "x": 5.590080643384297, - "y": 0.8141003552309748, - "heading": 3.15610868842989e-14, - "angularVelocity": -2.0421060653488186e-16, - "velocityX": -3.744937697734655, - "velocityY": 0.5348642086401837, - "timestamp": 3.381268479941229 - }, - { - "x": 5.31951379776001, - "y": 0.9000114798545837, - "heading": 1.0126074140253907e-33, - "angularVelocity": -4.205799613054758e-13, - "velocityX": -3.60554735901122, - "velocityY": 1.1448432559489161, - "timestamp": 3.4563103026337165 - }, - { - "x": 5.087986158761807, - "y": 1.0141443752209618, - "heading": -8.06925360653917e-8, - "angularVelocity": -0.0000011825608125095831, - "velocityX": -3.393071109702901, - "velocityY": 1.6726341252821548, - "timestamp": 3.524545724619457 - }, - { - "x": 4.876193956700591, - "y": 1.1617093808932828, - "heading": -1.490061973004125e-7, - "angularVelocity": -0.0000010011466075390655, - "velocityX": -3.1038454207182142, - "velocityY": 2.162586547836665, - "timestamp": 3.5927811466051978 - }, - { - "x": 4.680065310913933, - "y": 1.329533463078152, - "heading": -2.1452736793612451e-7, - "angularVelocity": -9.602222530316319e-7, - "velocityX": -2.874293733064984, - "velocityY": 2.4594862507033355, - "timestamp": 3.6610165685909384 - }, - { - "x": 4.483936991566996, - "y": 1.4973579267580712, - "heading": -2.800485136638582e-7, - "angularVelocity": -9.602218880015987e-7, - "velocityX": -2.874288949043537, - "velocityY": 2.4594918415685947, - "timestamp": 3.729251990576679 - }, - { - "x": 4.287808672223246, - "y": 1.6651823904417138, - "heading": -3.455696595852266e-7, - "angularVelocity": -9.602218908393429e-7, - "velocityX": -2.874288948996848, - "velocityY": 2.459491841623158, - "timestamp": 3.7974874125624196 - }, - { - "x": 4.091680352879495, - "y": 1.8330068541253561, - "heading": -4.1109080579148433e-7, - "angularVelocity": -9.602218950144384e-7, - "velocityX": -2.8742889489968495, - "velocityY": 2.459491841623156, - "timestamp": 3.8657228345481602 - }, - { - "x": 3.895552033535744, - "y": 2.0008313178089985, - "heading": -4.766119523424261e-7, - "angularVelocity": -9.602219000658333e-7, - "velocityX": -2.8742889489968504, - "velocityY": 2.4594918416231537, - "timestamp": 3.933958256533901 - }, - { - "x": 3.6994237141919926, - "y": 2.1686557814926406, - "heading": -5.421330991227701e-7, - "angularVelocity": -9.60221903427755e-7, - "velocityX": -2.874288948996852, - "velocityY": 2.459491841623151, - "timestamp": 4.0021936785196415 - }, - { - "x": 3.5032953948482413, - "y": 2.3364802451762827, - "heading": -6.076542461212268e-7, - "angularVelocity": -9.602219066242283e-7, - "velocityX": -2.874288948996854, - "velocityY": 2.4594918416231493, - "timestamp": 4.0704291005053825 - }, - { - "x": 3.3071670755041227, - "y": 2.5043047088594954, - "heading": -6.731753934621747e-7, - "angularVelocity": -9.602219116434866e-7, - "velocityX": -2.874288949002235, - "velocityY": 2.45949184161686, - "timestamp": 4.138664522491123 - }, - { - "x": 3.111038718551217, - "y": 2.6721291285911115, - "heading": -7.386965721783656e-7, - "angularVelocity": -9.602223714522231e-7, - "velocityX": -2.874289500164463, - "velocityY": 2.459491197499839, - "timestamp": 4.206899944476863 - }, - { - "x": 2.91419229045963, - "y": 2.833130049750874, - "heading": -0.009424527544100834, - "angularVelocity": -0.1381069915490224, - "velocityX": -2.8848129367870357, - "velocityY": 2.3594918368557476, - "timestamp": 4.275135366462603 - }, - { - "x": 2.744128800353239, - "y": 2.9762386994865855, - "heading": -0.09849959395445267, - "angularVelocity": -1.3054080097719059, - "velocityX": -2.4923050983978627, - "velocityY": 2.0972780056319964, - "timestamp": 4.343370788448343 - }, - { - "x": 2.60131581243687, - "y": 3.0984689436943484, - "heading": -0.2549637418085224, - "angularVelocity": -2.293004766450577, - "velocityX": -2.0929450388130264, - "velocityY": 1.7913019462722148, - "timestamp": 4.411606210434083 - }, - { - "x": 2.487964457072741, - "y": 3.1954189236366886, - "heading": -0.41008490805759246, - "angularVelocity": -2.2733231763626556, - "velocityX": -1.661180543264116, - "velocityY": 1.4208160090605124, - "timestamp": 4.479841632419824 - }, - { - "x": 2.403679735462392, - "y": 3.26747969283638, - "heading": -0.5415632745482113, - "angularVelocity": -1.9268345188528966, - "velocityX": -1.2352048123621473, - "velocityY": 1.0560610179087115, - "timestamp": 4.548077054405564 - }, - { - "x": 2.3479306427779894, - "y": 3.3151481611881186, - "heading": -0.6366834880204346, - "angularVelocity": -1.3940005162143045, - "velocityX": -0.8170110341818207, - "velocityY": 0.6985883132912986, - "timestamp": 4.616312476391304 - }, - { - "x": 2.3202381134033203, - "y": 3.3388166427612305, - "heading": -0.6871242163258895, - "angularVelocity": -0.7392161847551203, - "velocityX": -0.40583803204816526, - "velocityY": 0.3468650282262215, - "timestamp": 4.684547898377044 - }, - { - "x": 2.3202381134033203, - "y": 3.3388166427612305, - "heading": -0.6871242163258895, - "angularVelocity": -2.336499408021271e-31, - "velocityX": 1.807664942161883e-33, - "velocityY": -2.7160751304258966e-32, - "timestamp": 4.752783320362784 - }, - { - "x": 2.343794886689721, - "y": 3.323479061689414, - "heading": -0.6491973045996883, - "angularVelocity": 0.6336054834749958, - "velocityX": 0.39353852048358584, - "velocityY": -0.256229021242233, - "timestamp": 4.8126421958229395 - }, - { - "x": 2.3911982166575956, - "y": 3.2927250738018596, - "heading": -0.5766774436489436, - "angularVelocity": 1.2115139215907424, - "velocityX": 0.7919181508751876, - "velocityY": -0.5137749022369447, - "timestamp": 4.872501071283095 - }, - { - "x": 2.462785855058177, - "y": 3.246405346319809, - "heading": -0.4743619817187913, - "angularVelocity": 1.7092780501407518, - "velocityX": 1.195940248630853, - "velocityY": -0.773815530712448, - "timestamp": 4.93235994674325 - }, - { - "x": 2.5589185460183237, - "y": 3.184270435840568, - "heading": -0.3491026464013455, - "angularVelocity": 2.09257748921167, - "velocityX": 1.6059889234661082, - "velocityY": -1.0380233507828165, - "timestamp": 4.992218822203405 - }, - { - "x": 2.679950432279286, - "y": 3.106027779542805, - "heading": -0.21085817155844028, - "angularVelocity": 2.309506715256066, - "velocityX": 2.021953892894741, - "velocityY": -1.3071187137460396, - "timestamp": 5.05207769766356 - }, - { - "x": 2.8260176035796434, - "y": 3.011555903011265, - "heading": -0.07818278278115957, - "angularVelocity": 2.216469784261071, - "velocityX": 2.4401923721000403, - "velocityY": -1.5782434234740206, - "timestamp": 5.111936573123716 - }, - { - "x": 2.994403761571005, - "y": 2.9064091851549483, - "heading": -0.008153033218424529, - "angularVelocity": 1.1699142194769419, - "velocityX": 2.8130524787998565, - "velocityY": -1.756576899382393, - "timestamp": 5.171795448583871 - }, - { - "x": 3.1849716588184336, - "y": 2.790203147279415, - "heading": -5.969233857045932e-7, - "angularVelocity": 0.136194277496333, - "velocityX": 3.1836197352935405, - "velocityY": -1.9413334611153064, - "timestamp": 5.231654324044026 - }, - { - "x": 3.3752288967163246, - "y": 2.6674089766097917, - "heading": -5.278378990920332e-7, - "angularVelocity": 0.0000011541394000718638, - "velocityX": 3.178429872517975, - "velocityY": -2.0513945463502785, - "timestamp": 5.291513199504181 - }, - { - "x": 3.5654860836141538, - "y": 2.5446147269206905, - "heading": -4.587524705883237e-7, - "angularVelocity": 0.0000011541384293077112, - "velocityX": 3.178429020512966, - "velocityY": -2.051395866446551, - "timestamp": 5.3513720749643365 - }, - { - "x": 3.755743270511201, - "y": 2.4218204772303773, - "heading": -3.8966704288748865e-7, - "angularVelocity": 0.0000011541384158949214, - "velocityX": 3.1784290204999057, - "velocityY": -2.0513958664667884, - "timestamp": 5.411230950424492 - }, - { - "x": 3.9460004574097183, - "y": 2.2990262275423423, - "heading": -3.205816159441191e-7, - "angularVelocity": 0.0000011541384032407341, - "velocityX": 3.1784290205244665, - "velocityY": -2.0513958664287353, - "timestamp": 5.471089825884647 - }, - { - "x": 4.136257740222623, - "y": 2.176232126464018, - "heading": -2.5149617447282606e-7, - "angularVelocity": 0.0000011541386459436468, - "velocityX": 3.178430622866425, - "velocityY": -2.051393383760805, - "timestamp": 5.530948701344802 - }, - { - "x": 4.331712975970885, - "y": 2.0618928262664933, - "heading": -1.810305943700683e-7, - "angularVelocity": 0.00000117719518719764, - "velocityX": 3.2652674185027872, - "velocityY": -1.9101478154836689, - "timestamp": 5.590807576804957 - }, - { - "x": 4.540519439656892, - "y": 1.9742795609077881, - "heading": -1.0217911975724037e-7, - "angularVelocity": 0.0000013172896083775409, - "velocityX": 3.488312503047245, - "velocityY": -1.4636637371683408, - "timestamp": 5.650666452265113 - }, - { - "x": 4.759042739868164, - "y": 1.914918065071106, - "heading": 0, - "angularVelocity": 0.0000017070003232061267, - "velocityX": 3.6506415887603976, - "velocityY": -0.991690795731634, - "timestamp": 5.710525327725268 - }, - { - "x": 5.064685674327935, - "y": 1.8884875071659226, - "heading": 3.5654904377837356e-14, - "angularVelocity": 4.3965968281974806e-13, - "velocityX": 3.768874940643874, - "velocityY": -0.32591451044713626, - "timestamp": 5.791621925323583 - }, - { - "x": 5.370151366519879, - "y": 1.9168933201476963, - "heading": 3.5699569493639844e-14, - "angularVelocity": 5.507643615792375e-16, - "velocityX": 3.7666893709273386, - "velocityY": 0.35027132855156706, - "timestamp": 5.8727185229218986 - }, - { - "x": 5.6719192342055065, - "y": 1.972141248763098, - "heading": 3.5697718803372364e-14, - "angularVelocity": -2.2820812737714697e-17, - "velocityX": 3.721091594746464, - "velocityY": 0.681260746462555, - "timestamp": 5.953815120520214 - }, - { - "x": 5.973687062735465, - "y": 2.0273893912489522, - "heading": 3.565124865229906e-14, - "angularVelocity": -5.730222037609759e-16, - "velocityX": 3.7210911119189434, - "velocityY": 0.681263383693449, - "timestamp": 6.034911718118529 - }, - { - "x": 6.275454891265202, - "y": 2.0826375337360123, - "heading": 3.5597782853381526e-14, - "angularVelocity": -6.592853522950103e-16, - "velocityX": 3.7210911119162207, - "velocityY": 0.6812633837083223, - "timestamp": 6.116008315716845 - }, - { - "x": 6.577222719794938, - "y": 2.137885676223073, - "heading": 3.5548087973236424e-14, - "angularVelocity": -6.127862526596166e-16, - "velocityX": 3.72109111191622, - "velocityY": 0.6812633837083226, - "timestamp": 6.19710491331516 - }, - { - "x": 6.878990548324675, - "y": 2.193133818710133, - "heading": 3.55770700353708e-14, - "angularVelocity": 3.573770416112574e-16, - "velocityX": 3.72109111191622, - "velocityY": 0.6812633837083226, - "timestamp": 6.278201510913475 - }, - { - "x": 7.180758376854375, - "y": 2.2483819611971865, - "heading": 3.555072564787507e-14, - "angularVelocity": -3.2485194540738507e-16, - "velocityX": 3.7210911119157672, - "velocityY": 0.6812633837082396, - "timestamp": 6.359298108511791 - }, - { - "x": 7.45046082250988, - "y": 2.2977595214867708, - "heading": 2.3695885326073477e-14, - "angularVelocity": -1.4618172245055264e-13, - "velocityX": 3.32569372381546, - "velocityY": 0.6088733899066815, - "timestamp": 6.440394706110106 - }, - { - "x": 7.666222798434458, - "y": 2.3372615732702537, - "heading": 1.421890070899971e-14, - "angularVelocity": -1.1686044664925656e-13, - "velocityX": 2.660555218275404, - "velocityY": 0.48709875572268585, - "timestamp": 6.521491303708421 - }, - { - "x": 7.828044286775192, - "y": 2.366888113279094, - "heading": 7.108906150820137e-15, - "angularVelocity": -8.767315483931054e-14, - "velocityX": 1.9954164925914981, - "velocityY": 0.36532408123439786, - "timestamp": 6.602587901306737 - }, - { - "x": 7.935925281581105, - "y": 2.3866391404237772, - "heading": 2.368258984370487e-15, - "angularVelocity": -5.845679482055086e-14, - "velocityX": 1.3302776935262535, - "velocityY": 0.24354939331133554, - "timestamp": 6.683684498905052 - }, - { - "x": 7.989865779876709, - "y": 2.396514654159546, - "heading": 4.907869380861988e-36, - "angularVelocity": -2.920293889640648e-14, - "velocityX": 0.665138857770331, - "velocityY": 0.12177469867088454, - "timestamp": 6.764781096503367 - }, - { - "x": 7.989865779876709, - "y": 2.396514654159546, - "heading": 0, - "angularVelocity": 2.2807976020901832e-34, - "velocityX": -2.386905295344097e-35, - "velocityY": -2.096158053269738e-34, - "timestamp": 6.845877694101683 - }, - { - "x": 7.941752060251425, - "y": 2.3755004012341447, - "heading": 2.369857031913319e-14, - "angularVelocity": 2.9865236303183063e-13, - "velocityX": -0.6063351445610533, - "velocityY": -0.26482425770860446, - "timestamp": 6.925229386077437 - }, - { - "x": 7.845524622758667, - "y": 2.333471896151087, - "heading": 7.109189267667888e-14, - "angularVelocity": 5.972566076099065e-13, - "velocityX": -1.2126702669699776, - "velocityY": -0.5296485057419966, - "timestamp": 7.004581078053192 - }, - { - "x": 7.701183470328115, - "y": 2.2704291401899463, - "heading": 1.4219270868952152e-13, - "angularVelocity": 8.960214236460977e-13, - "velocityX": -1.819005352458695, - "velocityY": -0.7944727376500391, - "timestamp": 7.083932770028947 - }, - { - "x": 7.508728608819127, - "y": 2.186372135909869, - "heading": 2.3699994402916615e-13, - "angularVelocity": 1.1947727008595082e-12, - "velocityX": -2.425340364107035, - "velocityY": -1.0592969373073977, - "timestamp": 7.163284462004702 - }, - { - "x": 7.268160055809756, - "y": 2.081300890988283, - "heading": 3.5550661968129605e-13, - "angularVelocity": 1.4934360276569557e-12, - "velocityX": -3.0316751542345677, - "velocityY": -1.324121040212846, - "timestamp": 7.242636153980457 - }, - { - "x": 6.993070969404171, - "y": 1.9611523827505881, - "heading": 3.5550660039058023e-13, - "angularVelocity": -2.431040264205401e-19, - "velocityX": -3.4667072567228465, - "velocityY": -1.5141266083450944, - "timestamp": 7.321987845956212 - }, - { - "x": 6.717981882998517, - "y": 1.8410038745128643, - "heading": 3.555198084077173e-13, - "angularVelocity": 1.6644909274463737e-16, - "velocityX": -3.466707256723693, - "velocityY": -1.5141266083454636, - "timestamp": 7.401339537931967 - }, - { - "x": 6.442892796592864, - "y": 1.7208553662751405, - "heading": 3.554980670621707e-13, - "angularVelocity": -2.7398717034583525e-16, - "velocityX": -3.466707256723693, - "velocityY": -1.5141266083454636, - "timestamp": 7.4806912299077215 - }, - { - "x": 6.1678037101872105, - "y": 1.6007068580374166, - "heading": 3.554659707830622e-13, - "angularVelocity": -4.0448134512759275e-16, - "velocityX": -3.4667072567236934, - "velocityY": -1.5141266083454628, - "timestamp": 7.560042921883476 - }, - { - "x": 5.892714623774693, - "y": 1.4805583498154091, - "heading": 3.5548234631329863e-13, - "angularVelocity": 2.0636649110653867e-16, - "velocityX": -3.4667072568101984, - "velocityY": -1.5141266081474032, - "timestamp": 7.639394613859231 - }, - { - "x": 5.617624458493888, - "y": 1.3604123117741571, - "heading": 3.554803531945313e-13, - "angularVelocity": -2.5117533321458206e-17, - "velocityX": -3.466720852844014, - "velocityY": -1.5140954786189205, - "timestamp": 7.718746305834986 - }, - { - "x": 5.3267635091004895, - "y": 1.2861857871057627, - "heading": 3.5547733640886213e-13, - "angularVelocity": -3.801791233480102e-17, - "velocityX": -3.6654662572572203, - "velocityY": -0.9354119971515352, - "timestamp": 7.798097997810741 - }, - { - "x": 5.027418765032504, - "y": 1.2637717125502315, - "heading": 3.55535992528238e-13, - "angularVelocity": 7.3919179182428e-16, - "velocityX": -3.7723801045029637, - "velocityY": -0.2824649859057747, - "timestamp": 7.877449689786496 - }, - { - "x": 4.728747367858887, - "y": 1.293855905532837, - "heading": 0, - "angularVelocity": -4.480509283114818e-12, - "velocityX": -3.763894502273161, - "velocityY": 0.37912478276830097, - "timestamp": 7.956801381762251 - }, - { - "x": 4.491784068028983, - "y": 1.3525348599907823, - "heading": -1.9978113544551193e-7, - "angularVelocity": -0.000003095847804257088, - "velocityX": -3.6720299433276677, - "velocityY": 0.9093006299600177, - "timestamp": 8.021333344596727 - }, - { - "x": 4.265543006179473, - "y": 1.4442399350507595, - "heading": -3.147839483974445e-7, - "angularVelocity": -0.0000017821062292327031, - "velocityX": -3.50587603277802, - "velocityY": 1.4210798964104094, - "timestamp": 8.085865307431202 - }, - { - "x": 4.054601355970229, - "y": 1.5671157428156255, - "heading": -4.0045663910570385e-7, - "angularVelocity": -0.0000013276008809465377, - "velocityX": -3.2687933381215752, - "velocityY": 1.9041077067505576, - "timestamp": 8.150397270265678 - }, - { - "x": 3.8632265680311315, - "y": 1.7186760821797984, - "heading": -4.7495861737987186e-7, - "angularVelocity": -0.000001154497321974619, - "velocityX": -2.965581388403958, - "velocityY": 2.3486088553190996, - "timestamp": 8.214929233100154 - }, - { - "x": 3.6852305250099255, - "y": 1.8857456933767656, - "heading": -5.473016621730968e-7, - "angularVelocity": -0.000001121042063741103, - "velocityX": -2.7582617233845053, - "velocityY": 2.588943584832528, - "timestamp": 8.27946119593463 - }, - { - "x": 3.507234752110206, - "y": 2.052815592360835, - "heading": -6.196446913052133e-7, - "angularVelocity": -0.0000011210418210534835, - "velocityX": -2.7582575375287743, - "velocityY": 2.588948044438113, - "timestamp": 8.343993158769106 - }, - { - "x": 3.329238979213549, - "y": 2.219885491348167, - "heading": -6.919877206511343e-7, - "angularVelocity": -0.0000011210418243666412, - "velocityX": -2.758257537481309, - "velocityY": 2.5889480444886823, - "timestamp": 8.408525121603581 - }, - { - "x": 3.1512432063159417, - "y": 2.3869553903344864, - "heading": -7.643307500404873e-7, - "angularVelocity": -0.0000011210418250396714, - "velocityX": -2.758257537496045, - "velocityY": 2.5889480444729824, - "timestamp": 8.473057084438057 - }, - { - "x": 2.9732473495807805, - "y": 2.5540252000002215, - "heading": -8.366742189084055e-7, - "angularVelocity": -0.000001121048635286036, - "velocityX": -2.7582588366592833, - "velocityY": 2.5889466603436238, - "timestamp": 8.537589047272533 - }, - { - "x": 2.805693234212926, - "y": 2.7041662861189204, - "heading": -0.04706564750086009, - "angularVelocity": -0.7293255738611609, - "velocityX": -2.5964515568452473, - "velocityY": 2.326615827629635, - "timestamp": 8.602121010107009 - }, - { - "x": 2.6613205337549295, - "y": 2.837239575411043, - "heading": -0.1660200653385467, - "angularVelocity": -1.843341076464761, - "velocityX": -2.237227787853161, - "velocityY": 2.062129887997592, - "timestamp": 8.666652972941485 - }, - { - "x": 2.5406020300757204, - "y": 2.9504211910818823, - "heading": -0.3442125528113627, - "angularVelocity": -2.7613058652791787, - "velocityX": -1.8706776979471609, - "velocityY": 1.7538845976396134, - "timestamp": 8.73118493577596 - }, - { - "x": 2.4452505510198503, - "y": 3.0399707491346923, - "heading": -0.5155036654095948, - "angularVelocity": -2.6543608016014164, - "velocityX": -1.4775852905706008, - "velocityY": 1.3876775805271009, - "timestamp": 8.795716898610436 - }, - { - "x": 2.3746567136257015, - "y": 3.106440739683581, - "heading": -0.6585119351643622, - "angularVelocity": -2.2160843010708193, - "velocityX": -1.09393600153186, - "velocityY": 1.0300320589873262, - "timestamp": 8.860248861444912 - }, - { - "x": 2.328134128221924, - "y": 3.150385948901578, - "heading": -0.7608754833484198, - "angularVelocity": -1.5862456941937377, - "velocityX": -0.7209231419646691, - "velocityY": 0.6809836132013578, - "timestamp": 8.924780824279388 - }, - { - "x": 2.305089950561523, - "y": 3.1721901893615723, - "heading": -0.8148015740342935, - "angularVelocity": -0.8356493172878373, - "velocityX": -0.3570971135576525, - "velocityY": 0.33788280260313497, - "timestamp": 8.989312787113864 - }, - { - "x": 2.305089950561523, - "y": 3.1721901893615723, - "heading": -0.8148015740342935, - "angularVelocity": -8.408063134436125e-37, - "velocityX": 3.952960951142577e-37, - "velocityY": -2.430849972147944e-37, - "timestamp": 9.05384474994834 - }, - { - "x": 2.309173596952405, - "y": 3.203562462246217, - "heading": -0.7891930042181571, - "angularVelocity": 0.41051298264787395, - "velocityX": 0.0654620649273396, - "velocityY": 0.5029068552748039, - "timestamp": 9.116226625549343 - }, - { - "x": 2.3173582215057085, - "y": 3.2662983711409606, - "heading": -0.7378275898517348, - "angularVelocity": 0.8234028533376799, - "velocityX": 0.1312019633018532, - "velocityY": 1.005675258884561, - "timestamp": 9.178608501150347 - }, - { - "x": 2.3296630094846624, - "y": 3.3603796386527667, - "heading": -0.6603623761955548, - "angularVelocity": 1.2417903903956058, - "velocityX": 0.19724940714600217, - "velocityY": 1.508150670453601, - "timestamp": 9.24099037675135 - }, - { - "x": 2.3460984624180075, - "y": 3.48577693162035, - "heading": -0.5562016902946999, - "angularVelocity": 1.669726741899689, - "velocityX": 0.2634651936159632, - "velocityY": 2.010155862732131, - "timestamp": 9.303372252352354 - }, - { - "x": 2.366651701737987, - "y": 3.642454138676695, - "heading": -0.4245464321448433, - "angularVelocity": 2.1104729038916417, - "velocityX": 0.3294745328184519, - "velocityY": 2.5115821790684434, - "timestamp": 9.365754127953357 - }, - { - "x": 2.3865812265029587, - "y": 3.7987399457879576, - "heading": -0.28286003642691626, - "angularVelocity": 2.271275019432859, - "velocityX": 0.3194762031914812, - "velocityY": 2.505307921660973, - "timestamp": 9.42813600355436 - }, - { - "x": 2.4024793520828096, - "y": 3.923778755534052, - "heading": -0.16960155668210575, - "angularVelocity": 1.8155670802400325, - "velocityX": 0.25485167649552853, - "velocityY": 2.0044092701836567, - "timestamp": 9.490517879155364 - }, - { - "x": 2.4143813115901978, - "y": 4.017565970817142, - "heading": -0.08476980252965222, - "angularVelocity": 1.359878223204452, - "velocityX": 0.19079194706349162, - "velocityY": 1.5034369258621847, - "timestamp": 9.552899754756368 - }, - { - "x": 2.4223090833363603, - "y": 4.080093755108703, - "heading": -0.02826122995679143, - "angularVelocity": 0.9058492074571726, - "velocityX": 0.12708453649052126, - "velocityY": 1.0023389596601993, - "timestamp": 9.615281630357371 - }, - { - "x": 2.4262728691101074, - "y": 4.11135721206665, - "heading": 8.520612390652084e-40, - "angularVelocity": 0.4530359128274895, - "velocityX": 0.06354066362319509, - "velocityY": 0.5011625036398218, - "timestamp": 9.677663505958375 - }, - { - "x": 2.4262728691101074, - "y": 4.11135721206665, - "heading": 3.77515756333757e-40, - "angularVelocity": -3.697495905559829e-40, - "velocityX": 5.605053727452836e-41, - "velocityY": 1.9308049528794958e-41, - "timestamp": 9.740045381559378 - }, - { - "x": 2.414911964876249, - "y": 4.1934081363146305, - "heading": 6.694836686372959e-18, - "angularVelocity": 6.716930715190013e-17, - "velocityX": -0.11398397029768016, - "velocityY": 0.8232170538420872, - "timestamp": 9.839716451133356 - }, - { - "x": 2.3921901570614073, - "y": 4.3575099800953865, - "heading": 1.2510008928780832e-17, - "angularVelocity": 5.834363238263139e-17, - "velocityX": -0.22796793404506535, - "velocityY": 1.646434060376518, - "timestamp": 9.939387520707333 - }, - { - "x": 2.3581074476242065, - "y": 4.603662729263306, - "heading": 1.8930974562426156e-17, - "angularVelocity": 6.442155844308988e-17, - "velocityX": -0.34195187814156935, - "velocityY": 2.4696509249880063, - "timestamp": 10.03905859028131 - }, - { - "x": 2.324024738187006, - "y": 4.849815478431226, - "heading": 1.2303269381274456e-17, - "angularVelocity": -6.649577665294794e-17, - "velocityX": -0.34195187814156935, - "velocityY": 2.4696509249880063, - "timestamp": 10.138729659855288 - }, - { - "x": 2.301302930372164, - "y": 5.013917322211981, - "heading": 5.069755620661162e-18, - "angularVelocity": -7.257385509688344e-17, - "velocityX": -0.22796793404506527, - "velocityY": 1.646434060376518, - "timestamp": 10.238400729429266 - }, - { - "x": 2.2899420261383057, - "y": 5.095968246459961, - "heading": 3.4889982613181514e-43, - "angularVelocity": -5.086486622779004e-17, - "velocityX": -0.11398397029768013, - "velocityY": 0.8232170538420872, - "timestamp": 10.338071799003243 - }, - { - "x": 2.2899420261383057, - "y": 5.095968246459961, - "heading": 3.871119270807216e-43, - "angularVelocity": 3.8337935220286096e-43, - "velocityX": 1.5998936301366528e-40, - "velocityY": 1.897690279064702e-41, - "timestamp": 10.437742868577221 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 9 - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - }, - { - "scope": [ - 6 - ], - "type": "StopPoint" - }, - { - "scope": [ - 8 - ], - "type": "StopPoint" - }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "3NM": { - "waypoints": [ - { - "x": 1.3615734577178955, - "y": 5.537254333496094, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 9 - }, - { - "x": 2.311396360397339, - "y": 5.537254333496094, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 4.288896560668945, - "y": 4.896035671234131, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 10 - }, - { - "x": 5.409705638885498, - "y": 4.271584510803223, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 - }, - { - "x": 7.773779392242432, - "y": 4.118379592895508, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 13 - }, - { - "x": 5.409705638885498, - "y": 4.271584510803223, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 10 - }, - { - "x": 4.288896560668945, - "y": 4.896035671234131, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 - }, - { - "x": 2.311396360397339, - "y": 5.537254333496094, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.3615734577178955, - "y": 5.537254333496094, - "heading": 2.214441679929809e-48, - "angularVelocity": 2.3240077409867487e-47, - "velocityX": 0, - "velocityY": -1.721342681226222e-38, - "timestamp": 0 - }, - { - "x": 1.4090646063019332, - "y": 5.537254333496094, - "heading": 1.9456046497276407e-25, - "angularVelocity": 2.1151263730329146e-24, - "velocityX": 0.5162908141167563, - "velocityY": 1.0163081598312787e-23, - "timestamp": 0.09198526738323455 - }, - { - "x": 1.5040469015533051, - "y": 5.537254333496094, - "heading": 7.769111318759041e-25, - "angularVelocity": 6.330912367487192e-24, - "velocityX": 1.0325816073964444, - "velocityY": 2.0326162787461842e-23, - "timestamp": 0.1839705347664691 - }, - { - "x": 1.6465203396386052, - "y": 5.537254333496094, - "heading": 1.8917582624880275e-24, - "angularVelocity": 1.2119844430819319e-23, - "velocityX": 1.5488723590019993, - "velocityY": 3.048924315511678e-23, - "timestamp": 0.27595580214970367 - }, - { - "x": 1.8364849090576172, - "y": 5.537254333496094, - "heading": 3.898859380784999e-24, - "angularVelocity": 2.1819810665276264e-23, - "velocityX": 2.0651629855851796, - "velocityY": 4.065232106205135e-23, - "timestamp": 0.3679410695329382 - }, - { - "x": 2.026449478476629, - "y": 5.537254333496094, - "heading": 2.587084214379721e-24, - "angularVelocity": -1.4260709390994972e-23, - "velocityX": 2.06516298558518, - "velocityY": 4.0652321054688834e-23, - "timestamp": 0.45992633691617274 - }, - { - "x": 2.1689229165619293, - "y": 5.537254333496094, - "heading": 1.4784843245036944e-24, - "angularVelocity": -1.2051928764389099e-23, - "velocityX": 1.5488723590019995, - "velocityY": 3.0489243147666574e-23, - "timestamp": 0.5519116042994073 - }, - { - "x": 2.2639052118133014, - "y": 5.537254333496094, - "heading": 4.9814836533956335e-25, - "angularVelocity": -1.0657532309818666e-23, - "velocityX": 1.0325816073964444, - "velocityY": 2.0326162780553615e-23, - "timestamp": 0.6438968716826419 - }, - { - "x": 2.311396360397339, - "y": 5.537254333496094, - "heading": 7.949798578706025e-32, - "angularVelocity": -5.415522507165855e-24, - "velocityX": 0.5162908141167563, - "velocityY": 1.0163081591336611e-23, - "timestamp": 0.7358821390658764 - }, - { - "x": 2.311396360397339, - "y": 5.537254333496094, - "heading": 8.439039261116126e-32, - "angularVelocity": 5.318685223491289e-32, - "velocityX": 1.5271297522513056e-32, - "velocityY": -7.173666161189126e-33, - "timestamp": 0.8278674064491109 - }, - { - "x": 2.333988702088453, - "y": 5.530687639848115, - "heading": 2.7357905411745392e-18, - "angularVelocity": 4.225566579883271e-17, - "velocityX": 0.34894980130013487, - "velocityY": -0.10142580503518746, - "timestamp": 0.8926112223416762 - }, - { - "x": 2.3791733842230687, - "y": 5.5175542496687795, - "heading": 4.548394071480566e-18, - "angularVelocity": 2.799657011171827e-17, - "velocityX": 0.6978995833302994, - "velocityY": -0.20285165460557242, - "timestamp": 0.9573550382342415 - }, - { - "x": 2.4469504052763287, - "y": 5.4978541594339525, - "heading": 7.8862048960196e-18, - "angularVelocity": 5.155416680773327e-17, - "velocityX": 1.0468493418082916, - "velocityY": -0.304277558607872, - "timestamp": 1.0220988541268068 - }, - { - "x": 2.537319763342161, - "y": 5.47158736473847, - "heading": 5.032433303714057e-18, - "angularVelocity": -4.4077881290119006e-17, - "velocityX": 1.3957990708460906, - "velocityY": -0.40570353065007425, - "timestamp": 1.086842670019372 - }, - { - "x": 2.6502814559699055, - "y": 5.438753859918548, - "heading": 7.530601737573416e-19, - "angularVelocity": -6.609699862808569e-17, - "velocityX": 1.7447487620322484, - "velocityY": -0.5071295901721661, - "timestamp": 1.1515864859119374 - }, - { - "x": 2.785835479892019, - "y": 5.399353637422471, - "heading": -9.328301380807417e-18, - "angularVelocity": -1.557115793573686e-16, - "velocityX": 2.0936984027496157, - "velocityY": -0.6085557663341342, - "timestamp": 1.2163303018045026 - }, - { - "x": 2.943981830533949, - "y": 5.353386686677836, - "heading": -2.1917419574313143e-17, - "angularVelocity": -1.9444511294798906e-16, - "velocityX": 2.4426479728108004, - "velocityY": -0.7099821057919715, - "timestamp": 1.281074117697068 - }, - { - "x": 3.1247205010338854, - "y": 5.300852991826032, - "heading": -3.425033697426937e-17, - "angularVelocity": -1.9048797438260707e-16, - "velocityX": 2.7915974368879897, - "velocityY": -0.8114086901936944, - "timestamp": 1.3458179335896332 - }, - { - "x": 3.328051479955525, - "y": 5.241752526436032, - "heading": -4.82546070777501e-17, - "angularVelocity": -2.1630285080372957e-16, - "velocityX": 3.14054672432594, - "velocityY": -0.9128356828354128, - "timestamp": 1.4105617494821985 - }, - { - "x": 3.553974744426448, - "y": 5.176085237645743, - "heading": -6.627429722777717e-17, - "angularVelocity": -2.7832298821571975e-16, - "velocityX": 3.4894956584882664, - "velocityY": -1.0142634919576448, - "timestamp": 1.4753055653747638 - }, - { - "x": 3.802490225830772, - "y": 5.103850966868795, - "heading": -8.837633885982683e-17, - "angularVelocity": -3.413769151303276e-16, - "velocityX": 3.8384435328450017, - "velocityY": -1.1156937505229125, - "timestamp": 1.540049381267329 - }, - { - "x": 4.050315104768665, - "y": 5.01076075944265, - "heading": -8.243350807926677e-17, - "angularVelocity": 9.17899369790455e-17, - "velocityX": 3.82777683892875, - "velocityY": -1.4378239240925197, - "timestamp": 1.6047931971598943 - }, - { - "x": 4.288896560668945, - "y": 4.896035671234131, - "heading": -7.62003507301664e-17, - "angularVelocity": 9.627417326532366e-17, - "velocityX": 3.6850076352559227, - "velocityY": -1.7719852719227334, - "timestamp": 1.6695370130524596 - }, - { - "x": 4.402340886082199, - "y": 4.835318762030556, - "heading": -7.303407335542255e-17, - "angularVelocity": 1.0061831085733754e-16, - "velocityX": 3.605046257294911, - "velocityY": -1.9294686224845516, - "timestamp": 1.701005215487622 - }, - { - "x": 4.513057352461556, - "y": 4.7697594586676155, - "heading": -7.06099226407047e-17, - "angularVelocity": 7.70349281854855e-17, - "velocityX": 3.5183600527186023, - "velocityY": -2.0833507569763077, - "timestamp": 1.7324734179227845 - }, - { - "x": 4.620839460727155, - "y": 4.6994801912241115, - "heading": -7.060357135873025e-17, - "angularVelocity": 2.0183173729755417e-19, - "velocityX": 3.425111697657642, - "velocityY": -2.2333422949506527, - "timestamp": 1.763941620357947 - }, - { - "x": 4.7254864149518045, - "y": 4.624612485674306, - "heading": -7.099625319518912e-17, - "angularVelocity": -1.247868661272944e-17, - "velocityX": 3.3254824275253214, - "velocityY": -2.379154186040543, - "timestamp": 1.7954098227931095 - }, - { - "x": 4.832808704383543, - "y": 4.553633020226032, - "heading": -7.165943794882052e-17, - "angularVelocity": -2.1074758083014144e-17, - "velocityX": 3.410499524188173, - "velocityY": -2.2555932641481897, - "timestamp": 1.826878025228272 - }, - { - "x": 4.943095924051047, - "y": 4.487354159347409, - "heading": -7.348529763539e-17, - "angularVelocity": -5.802237005203753e-17, - "velocityX": 3.5047194035043416, - "velocityY": -2.1062169348406337, - "timestamp": 1.8583462276634344 - }, - { - "x": 5.056142456827311, - "y": 4.425899790074916, - "heading": -7.035642800696723e-17, - "angularVelocity": 9.942956337816296e-17, - "velocityX": 3.5924051591307293, - "velocityY": -1.952903709654341, - "timestamp": 1.8898144300985968 - }, - { - "x": 5.171737422557606, - "y": 4.369384643600972, - "heading": -6.629352517662628e-17, - "angularVelocity": 1.2911137325711957e-16, - "velocityX": 3.673389541993163, - "velocityY": -1.7959445440095247, - "timestamp": 1.9212826325337593 - }, - { - "x": 5.28966515888657, - "y": 4.317914199222627, - "heading": -6.55548292961571e-17, - "angularVelocity": 2.3474358981676347e-17, - "velocityX": 3.7475205827935785, - "velocityY": -1.635633445655282, - "timestamp": 1.9527508349689218 - }, - { - "x": 5.409705638885498, - "y": 4.271584510803223, - "heading": -6.571802328863205e-17, - "angularVelocity": -5.185996652043943e-18, - "velocityX": 3.8146595836400685, - "velocityY": -1.4722699370612031, - "timestamp": 1.9842190374040842 - }, - { - "x": 5.708749554118693, - "y": 4.190657909294355, - "heading": -8.63128139069911e-17, - "angularVelocity": -2.71821079393033e-16, - "velocityX": 3.946941793718872, - "velocityY": -1.0681126398297116, - "timestamp": 2.059985017968765 - }, - { - "x": 6.01458130018582, - "y": 4.141227935433476, - "heading": -6.519812472542959e-17, - "angularVelocity": 2.786829791452436e-16, - "velocityX": 4.0365312213681594, - "velocityY": -0.652403275109303, - "timestamp": 2.1357509985334455 - }, - { - "x": 6.3238928676995245, - "y": 4.1238286836853835, - "heading": -2.2948800283958053e-18, - "angularVelocity": 8.302307213384378e-16, - "velocityX": 4.082459768993199, - "velocityY": -0.2296446455231499, - "timestamp": 2.211516979098126 - }, - { - "x": 6.613870132060174, - "y": 4.122738763203337, - "heading": -9.066080252607654e-19, - "angularVelocity": 1.8323178126471314e-17, - "velocityX": 3.8272752784233455, - "velocityY": -0.014385354383583656, - "timestamp": 2.287282959662807 - }, - { - "x": 6.871627727395008, - "y": 4.121770013181932, - "heading": -3.3199950345434843e-18, - "angularVelocity": -3.185312321183995e-17, - "velocityX": 3.4020228262582535, - "velocityY": -0.012786081739125113, - "timestamp": 2.3630489402274875 - }, - { - "x": 7.097165634009429, - "y": 4.12092238390025, - "heading": -2.8404447967139716e-18, - "angularVelocity": 6.329381483051276e-18, - "velocityX": 2.9767701141533016, - "velocityY": -0.011187465334170248, - "timestamp": 2.438814920792168 - }, - { - "x": 7.290483845335008, - "y": 4.1201958587843395, - "heading": 4.211138438186873e-19, - "angularVelocity": 4.3047776169510437e-17, - "velocityX": 2.551517315354746, - "velocityY": -0.009589067679885944, - "timestamp": 2.5145809013568488 - }, - { - "x": 7.45158235808693, - "y": 4.11959042954717, - "heading": 6.3288304367769226e-18, - "angularVelocity": 7.797317559894418e-17, - "velocityX": 2.1262644732014313, - "velocityY": -0.007990779401695184, - "timestamp": 2.5903468819215294 - }, - { - "x": 7.58046117029412, - "y": 4.119106091216504, - "heading": 7.410357641228221e-18, - "angularVelocity": 1.4274566493954026e-17, - "velocityX": 1.701011605032828, - "velocityY": -0.006392556749398596, - "timestamp": 2.66611286248621 - }, - { - "x": 7.677120280642456, - "y": 4.118742840477511, - "heading": 3.662728059919033e-18, - "angularVelocity": -4.9463211035666574e-17, - "velocityX": 1.275758719519713, - "velocityY": -0.004794377847795854, - "timestamp": 2.7418788430508907 - }, - { - "x": 7.741559688193241, - "y": 4.118500674962447, - "heading": 2.4564209438434204e-18, - "angularVelocity": -1.5921497994971635e-17, - "velocityX": 0.850505821617187, - "velocityY": -0.0031962301967359986, - "timestamp": 2.8176448236155713 - }, - { - "x": 7.773779392242432, - "y": 4.118379592895508, - "heading": 3.1269636712640265e-33, - "angularVelocity": -3.242117401081041e-17, - "velocityX": 0.42525291442234653, - "velocityY": -0.0015981059836088808, - "timestamp": 2.893410804180252 - }, - { - "x": 7.773779392242432, - "y": 4.118379592895508, - "heading": 1.1844971908267807e-33, - "angularVelocity": -2.5637705565001737e-32, - "velocityX": -4.084582127410334e-32, - "velocityY": -3.5675440722041014e-30, - "timestamp": 2.9691767847449326 - }, - { - "x": 7.74609430870524, - "y": 4.117438747396381, - "heading": -2.05497935642393e-18, - "angularVelocity": -2.925144275486426e-17, - "velocityX": -0.3940811427571139, - "velocityY": -0.013392391211521543, - "timestamp": 3.039429026056197 - }, - { - "x": 7.690721455157199, - "y": 4.115639760645022, - "heading": -5.897998326291808e-18, - "angularVelocity": -5.4703150051541466e-17, - "velocityX": -0.7882005259120873, - "velocityY": -0.02560753532956242, - "timestamp": 3.109681267367461 - }, - { - "x": 7.607657826994539, - "y": 4.113085801554182, - "heading": -8.353793315278077e-18, - "angularVelocity": -3.495681937113195e-17, - "velocityX": -1.182362677862939, - "velocityY": -0.036354129678597254, - "timestamp": 3.1799335086787255 - }, - { - "x": 7.4969001342407955, - "y": 4.109909165473227, - "heading": -1.526679962466129e-17, - "angularVelocity": -9.840264115672304e-17, - "velocityX": -1.5765716607248677, - "velocityY": -0.0452175762888664, - "timestamp": 3.2501857499899898 - }, - { - "x": 7.358444983880178, - "y": 4.106285621312532, - "heading": -2.0305706370262314e-17, - "angularVelocity": -7.172591932681999e-17, - "velocityX": -1.9708289412030204, - "velocityY": -0.051579054177674104, - "timestamp": 3.320437991301254 - }, - { - "x": 7.1922895089935714, - "y": 4.10246001625686, - "heading": -2.298438007674344e-17, - "angularVelocity": -3.8129369021947994e-17, - "velocityX": -2.365127030615676, - "velocityY": -0.05445527408475893, - "timestamp": 3.3906902326125183 - }, - { - "x": 6.998433450931076, - "y": 4.098796843405572, - "heading": -2.2611777629214343e-17, - "angularVelocity": 5.303779741165941e-18, - "velocityX": -2.7594288017599875, - "velocityY": -0.05214314565508088, - "timestamp": 3.4609424739237826 - }, - { - "x": 6.776886824035131, - "y": 4.095895839339149, - "heading": -2.465545569001984e-17, - "angularVelocity": -2.909057420246661e-17, - "velocityX": -3.1535880245349874, - "velocityY": -0.041294114070628656, - "timestamp": 3.531194715235047 - }, - { - "x": 6.52770649933481, - "y": 4.094925348650892, - "heading": -2.807407218443882e-17, - "angularVelocity": -4.866202704245043e-17, - "velocityX": -3.546937721122466, - "velocityY": -0.013814373323087034, - "timestamp": 3.601446956546311 - }, - { - "x": 6.251309534448763, - "y": 4.099112534148996, - "heading": -2.5835306002252158e-17, - "angularVelocity": 3.186753600665183e-17, - "velocityX": -3.93435084386026, - "velocityY": 0.059602162435704605, - "timestamp": 3.6716991978575755 - }, - { - "x": 5.965656271322792, - "y": 4.12940837356358, - "heading": -8.624661629595893e-18, - "angularVelocity": 2.4498356282189034e-16, - "velocityX": -4.066108892673412, - "velocityY": 0.4312437418239143, - "timestamp": 3.7419514391688398 - }, - { - "x": 5.684249331081993, - "y": 4.187077783503906, - "heading": -1.9936792288925207e-17, - "angularVelocity": -1.6102163359044556e-16, - "velocityX": -4.005664943755716, - "velocityY": 0.8208906771360378, - "timestamp": 3.812203680480104 - }, - { - "x": 5.409705638885498, - "y": 4.271584510803223, - "heading": -2.6078962645628085e-17, - "angularVelocity": -8.743024054547926e-17, - "velocityX": -3.907970579615868, - "velocityY": 1.2029043589498862, - "timestamp": 3.8824559217913683 - }, - { - "x": 5.288298232295607, - "y": 4.314786208760466, - "heading": -2.909568780009548e-17, - "angularVelocity": -9.572142426185266e-17, - "velocityX": -3.8522865954532466, - "velocityY": 1.3708004035015684, - "timestamp": 3.913971595296342 - }, - { - "x": 5.168872926450536, - "y": 4.36319841734604, - "heading": -3.3051402703085907e-17, - "angularVelocity": -1.2551579779394563e-16, - "velocityX": -3.7893940558250763, - "velocityY": 1.5361311754271532, - "timestamp": 3.945487268801316 - }, - { - "x": 5.0516532214814776, - "y": 4.416730527614229, - "heading": -3.747448089829932e-17, - "angularVelocity": -1.4034534894249094e-16, - "velocityX": -3.7194098025720335, - "velocityY": 1.698586903425662, - "timestamp": 3.9770029423062896 - }, - { - "x": 4.936858487147447, - "y": 4.475282345718925, - "heading": -4.0016033176428666e-17, - "angularVelocity": -8.064407310629592e-17, - "velocityX": -3.642464893409777, - "velocityY": 1.8578634562721388, - "timestamp": 4.008518615811264 - }, - { - "x": 4.824703549738644, - "y": 4.538744277266208, - "heading": -3.9215963888302267e-17, - "angularVelocity": 2.5386393471797817e-17, - "velocityX": -3.5587034937109068, - "velocityY": 2.013662552293777, - "timestamp": 4.040034289316237 - }, - { - "x": 4.715398282405222, - "y": 4.606997522786201, - "heading": -3.873623413321436e-17, - "angularVelocity": 1.5221942028687378e-17, - "velocityX": -3.468282767816198, - "velocityY": 2.1656921121873096, - "timestamp": 4.07154996282121 - }, - { - "x": 4.609147174630297, - "y": 4.679914251228507, - "heading": -3.880924721736098e-17, - "angularVelocity": -2.3167229516803445e-18, - "velocityX": -3.371373540792568, - "velocityY": 2.313665561702092, - "timestamp": 4.103065636326184 - }, - { - "x": 4.5055479081696435, - "y": 4.756551965520632, - "heading": -3.815016865781766e-17, - "angularVelocity": 2.0912723297480083e-17, - "velocityX": -3.287229969694346, - "velocityY": 2.4317333494406586, - "timestamp": 4.134581309831157 - }, - { - "x": 4.398731100283088, - "y": 4.828637447090138, - "heading": -3.47447014727406e-17, - "angularVelocity": 1.0805630362110442e-16, - "velocityX": -3.3893233431834044, - "velocityY": 2.287289895871401, - "timestamp": 4.16609698333613 - }, - { - "x": 4.288896560668945, - "y": 4.896035671234131, - "heading": -3.0301452538996985e-17, - "angularVelocity": 1.409853713912325e-16, - "velocityX": -3.485076706255612, - "velocityY": 2.1385620755767296, - "timestamp": 4.197612656841104 - }, - { - "x": 4.031863105840825, - "y": 5.0215689729975965, - "heading": -1.9138186658356363e-17, - "angularVelocity": 1.5957200472787174e-16, - "velocityX": -3.6741348013740933, - "velocityY": 1.7944211699950958, - "timestamp": 4.267570202963796 - }, - { - "x": 3.7639740075166586, - "y": 5.121869712419655, - "heading": 8.866585751357036e-20, - "angularVelocity": 2.7483600528454067e-16, - "velocityX": -3.829309533732605, - "velocityY": 1.4337372446734074, - "timestamp": 4.3375277490864885 - }, - { - "x": 3.499869141493399, - "y": 5.197394590829224, - "heading": 1.115946844167211e-17, - "angularVelocity": 1.5825030223112458e-16, - "velocityX": -3.775216265591548, - "velocityY": 1.0795815833378437, - "timestamp": 4.407485295209181 - }, - { - "x": 3.262174659757514, - "y": 5.26536672558899, - "heading": 1.8977549587337083e-17, - "angularVelocity": 1.117546520506224e-16, - "velocityX": -3.39769610156156, - "velocityY": 0.9716197683742961, - "timestamp": 4.477442841331873 - }, - { - "x": 3.0508906346572426, - "y": 5.3257862975836385, - "heading": 2.54298959314255e-17, - "angularVelocity": 9.223231358389955e-17, - "velocityX": -3.0201749033580736, - "velocityY": 0.8636605390458314, - "timestamp": 4.547400387454566 - }, - { - "x": 2.8660170903084694, - "y": 5.378653367108372, - "heading": 2.5485282912423973e-17, - "angularVelocity": 7.917219258078879e-19, - "velocityX": -2.6426533604328997, - "velocityY": 0.7557021716000921, - "timestamp": 4.617357933577258 - }, - { - "x": 2.7075540387690964, - "y": 5.42396796431084, - "heading": 2.7012682013486872e-17, - "angularVelocity": 2.1833229254105427e-17, - "velocityX": -2.2651316451474477, - "velocityY": 0.6477442350964383, - "timestamp": 4.687315479699951 - }, - { - "x": 2.575501487273851, - "y": 5.4617301072796485, - "heading": 2.8840678474853086e-17, - "angularVelocity": 2.6130083844828922e-17, - "velocityX": -1.887609826446018, - "velocityY": 0.539786557158247, - "timestamp": 4.757273025822643 - }, - { - "x": 2.4698594406458803, - "y": 5.491939808073871, - "heading": 2.55057361888535e-17, - "angularVelocity": -4.7670944616029463e-17, - "velocityX": -1.5100879388006847, - "velocityY": 0.4318290515971121, - "timestamp": 4.827230571945336 - }, - { - "x": 2.390627902330286, - "y": 5.514597075307139, - "heading": 1.555218460712861e-17, - "angularVelocity": -1.4227988679695002e-16, - "velocityX": -1.1325660019097468, - "velocityY": 0.3238716691624835, - "timestamp": 4.897188118068028 - }, - { - "x": 2.337806874910892, - "y": 5.529701915439674, - "heading": 5.452841277859862e-18, - "angularVelocity": -1.4436388867795342e-16, - "velocityX": -0.7550440280846282, - "velocityY": 0.2159143790727539, - "timestamp": 4.967145664190721 - }, - { - "x": 2.311396360397339, - "y": 5.537254333496094, - "heading": -9.963384565748625e-36, - "angularVelocity": -7.794500474656568e-17, - "velocityX": -0.3775220255329383, - "velocityY": 0.10795716080684535, - "timestamp": 5.037103210313413 - }, - { - "x": 2.311396360397339, - "y": 5.537254333496094, - "heading": -6.535365453130042e-35, - "angularVelocity": -7.91769987677621e-34, - "velocityX": 8.47721531223007e-33, - "velocityY": 4.36786565310015e-32, - "timestamp": 5.1070607564361055 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "4NT-Skip": { - "waypoints": [ - { - "x": 0.7644821405410767, - "y": 6.707253456115723, - "heading": 1.0390723224103011, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 18 - }, - { - "x": 2.763329029083252, - "y": 7.743913650512695, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 20 - }, - { - "x": 7.778790473937988, - "y": 7.453383922576904, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 5.19636344909668, - "y": 6.875411510467529, - "heading": 0.4089083102765838, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 17 - }, - { - "x": 7.763500213623047, - "y": 5.786660194396973, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 22 - }, - { - "x": 4.935596466064453, - "y": 6.19089937210083, - "heading": 0.18998832965561516, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.7644821405410767, - "y": 6.707253456115723, - "heading": 1.0390723224103011, - "angularVelocity": -1.7547937866246597e-32, - "velocityX": 2.2763161736314877e-32, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 0.7760278692353412, - "y": 6.71433952046308, - "heading": 1.0217643429776795, - "angularVelocity": -0.34522656494644127, - "velocityX": 0.2302921766484186, - "velocityY": 0.1413392974697392, - "timestamp": 0.05013513208436539 - }, - { - "x": 0.7991801780145195, - "y": 6.728500218028103, - "heading": 0.9877924180710587, - "angularVelocity": -0.6776071687505348, - "velocityX": 0.4617981007852623, - "velocityY": 0.28245058856519034, - "timestamp": 0.10027026416873078 - }, - { - "x": 0.8340045462176232, - "y": 6.749720753781454, - "heading": 0.9378562449760159, - "angularVelocity": -0.9960315455240489, - "velocityX": 0.69461008189831, - "velocityY": 0.4232667766316656, - "timestamp": 0.15040539625309618 - }, - { - "x": 0.8805749581377974, - "y": 6.777983973346064, - "heading": 0.872813574235644, - "angularVelocity": -1.2973471503160856, - "velocityX": 0.928897760592456, - "velocityY": 0.5637408019001573, - "timestamp": 0.20054052833746155 - }, - { - "x": 0.9389764076927801, - "y": 6.813272866000001, - "heading": 0.7938060911799183, - "angularVelocity": -1.5758905935018794, - "velocityX": 1.1648807358621667, - "velocityY": 0.7038755297294303, - "timestamp": 0.25067566042182693 - }, - { - "x": 1.0093049798201754, - "y": 6.85557383915721, - "heading": 0.70239868590727, - "angularVelocity": -1.8232205934718893, - "velocityX": 1.402780230219579, - "velocityY": 0.8437391385750465, - "timestamp": 0.3008107925061923 - }, - { - "x": 1.0916647821261949, - "y": 6.904878200303725, - "heading": 0.6007135393739693, - "angularVelocity": -2.0282213750267752, - "velocityX": 1.6427562645577014, - "velocityY": 0.983429365729964, - "timestamp": 0.3509459245905577 - }, - { - "x": 1.1861619165582793, - "y": 6.961177477000806, - "heading": 0.49155865682829086, - "angularVelocity": -2.177213423153984, - "velocityX": 1.88484861819189, - "velocityY": 1.1229506008350447, - "timestamp": 0.40108105667492305 - }, - { - "x": 1.2928950044716645, - "y": 7.024447544021754, - "heading": 0.37862860943476007, - "angularVelocity": -2.252513211763292, - "velocityX": 2.1289080825353928, - "velocityY": 1.2619906319281369, - "timestamp": 0.45121618875928843 - }, - { - "x": 1.411932213492286, - "y": 7.094613399845792, - "heading": 0.2670084248827377, - "angularVelocity": -2.2263865658953943, - "velocityX": 2.3743272246757017, - "velocityY": 1.3995346757233116, - "timestamp": 0.5013513208436539 - }, - { - "x": 1.5432310429314056, - "y": 7.171450777112641, - "heading": 0.16443069802118257, - "angularVelocity": -2.0460248651373116, - "velocityX": 2.618898644131923, - "velocityY": 1.5326054619254017, - "timestamp": 0.5514864529280192 - }, - { - "x": 1.686355782886626, - "y": 7.254093500212897, - "heading": 0.08456860071028449, - "angularVelocity": -1.5929368087932674, - "velocityX": 2.854779353415799, - "velocityY": 1.6483994289909887, - "timestamp": 0.6016215850123846 - }, - { - "x": 1.8412149510943983, - "y": 7.341984821518931, - "heading": 0.030968755693609747, - "angularVelocity": -1.0691074858739578, - "velocityX": 3.0888353489760694, - "velocityY": 1.7530884561774895, - "timestamp": 0.65175671709675 - }, - { - "x": 2.008057166165621, - "y": 7.434716608733129, - "heading": 0.002936894677818538, - "angularVelocity": -0.5591261028019302, - "velocityX": 3.3278503144355365, - "velocityY": 1.849636838657414, - "timestamp": 0.7018918491811154 - }, - { - "x": 2.187462439154994, - "y": 7.530834332052905, - "heading": 1.3401221666517954e-7, - "angularVelocity": -0.058576900937657404, - "velocityX": 3.578434234250707, - "velocityY": 1.9171730346303149, - "timestamp": 0.7520269812654807 - }, - { - "x": 2.3743734313488285, - "y": 7.615027816034377, - "heading": 9.157823134987268e-8, - "angularVelocity": -8.463922114317623e-7, - "velocityX": 3.7281440064685323, - "velocityY": 1.67933104952818, - "timestamp": 0.8021621133498461 - }, - { - "x": 2.5666324715448003, - "y": 7.68616657630836, - "heading": 4.894275703505732e-8, - "angularVelocity": -8.504111297257818e-7, - "velocityX": 3.834816668527906, - "velocityY": 1.4189403182238092, - "timestamp": 0.8522972454342115 - }, - { - "x": 2.763329029083252, - "y": 7.743913650512695, - "heading": 0, - "angularVelocity": -9.762167765450472e-7, - "velocityX": 3.923327801499733, - "velocityY": 1.1518285043541625, - "timestamp": 0.9024323775185769 - }, - { - "x": 3.0930925857719416, - "y": 7.801578883603503, - "heading": -2.025054003603945e-14, - "angularVelocity": -2.4734392492358033e-13, - "velocityX": 4.027794432293094, - "velocityY": 0.704334060174232, - "timestamp": 0.9843043703007283 - }, - { - "x": 3.427244052945336, - "y": 7.821878655200531, - "heading": -2.0171148368757895e-14, - "angularVelocity": 9.697048353617892e-16, - "velocityX": 4.081389200608798, - "velocityY": 0.24794524851792668, - "timestamp": 1.0661763630828798 - }, - { - "x": 3.7615632056259036, - "y": 7.804558480607454, - "heading": -2.0136813522881885e-14, - "angularVelocity": 4.1937229947938353e-16, - "velocityX": 4.083437343098987, - "velocityY": -0.21155188733665228, - "timestamp": 1.1480483558650312 - }, - { - "x": 4.095052943678329, - "y": 7.7753372729513455, - "heading": -2.0212632475990418e-14, - "angularVelocity": -9.260670289323022e-16, - "velocityX": 4.073306716007144, - "velocityY": -0.3569133553871345, - "timestamp": 1.2299203486471826 - }, - { - "x": 4.428549167930835, - "y": 7.746190184376778, - "heading": -2.0152082358748023e-14, - "angularVelocity": 7.395705806686352e-16, - "velocityX": 4.0733859396813115, - "velocityY": -0.35600805091092386, - "timestamp": 1.311792341429334 - }, - { - "x": 4.762045392031515, - "y": 7.71704309406487, - "heading": -2.015572108540389e-14, - "angularVelocity": -4.4444095376460173e-17, - "velocityX": 4.0733859378268935, - "velocityY": -0.3560080721311419, - "timestamp": 1.3936643342114854 - }, - { - "x": 5.095541616206554, - "y": 7.68789600460369, - "heading": -2.0229754033283868e-14, - "angularVelocity": -9.04252423377198e-16, - "velocityX": 4.0733859387351234, - "velocityY": -0.3560080617401835, - "timestamp": 1.4755363269936368 - }, - { - "x": 5.42903784040285, - "y": 7.658748915385721, - "heading": -2.0309309367944147e-14, - "angularVelocity": -9.717038996712115e-16, - "velocityX": 4.073385938994756, - "velocityY": -0.35600805876956854, - "timestamp": 1.5574083197757882 - }, - { - "x": 5.762534064600043, - "y": 7.629601826178022, - "heading": -2.0390490605011424e-14, - "angularVelocity": -9.915629790920958e-16, - "velocityX": 4.07338593900572, - "velocityY": -0.3560080586441164, - "timestamp": 1.6392803125579396 - }, - { - "x": 6.09603028879551, - "y": 7.60045473695057, - "heading": -2.0426978309746098e-14, - "angularVelocity": -4.456677246361619e-16, - "velocityX": 4.073385938984632, - "velocityY": -0.3560080588853828, - "timestamp": 1.721152305340091 - }, - { - "x": 6.4295265129813, - "y": 7.57130764761672, - "heading": -2.0431228864965123e-14, - "angularVelocity": -5.1917085129884607e-17, - "velocityX": 4.073385938866449, - "velocityY": -0.3560080601849525, - "timestamp": 1.8030242981222424 - }, - { - "x": 6.729362912053891, - "y": 7.545102378587393, - "heading": -1.7809391327471565e-14, - "angularVelocity": 3.202361940795799e-14, - "velocityX": 3.662258470615334, - "velocityY": -0.32007611075321446, - "timestamp": 1.8848962909043938 - }, - { - "x": 6.991719787015596, - "y": 7.522172765937965, - "heading": -1.4158080705052285e-14, - "angularVelocity": 4.459779832223585e-14, - "velocityX": 3.204476476586995, - "velocityY": -0.2800666243759492, - "timestamp": 1.9667682836865452 - }, - { - "x": 7.216597118349246, - "y": 7.502518811371365, - "heading": -1.0522354480483052e-14, - "angularVelocity": 4.440744754194895e-14, - "velocityX": 2.746694244172289, - "velocityY": -0.24005711719874057, - "timestamp": 2.0486402764686966 - }, - { - "x": 7.403994899549076, - "y": 7.486140515455231, - "heading": -7.2265963678814e-15, - "angularVelocity": 4.025501274271611e-14, - "velocityX": 2.2889119322949267, - "velocityY": -0.2000476030883275, - "timestamp": 2.1305122692508482 - }, - { - "x": 7.553913127362196, - "y": 7.473037878473377, - "heading": -4.440408288437967e-15, - "angularVelocity": 3.4031028009627144e-14, - "velocityX": 1.8311295806861385, - "velocityY": -0.16003808551133375, - "timestamp": 2.212384262033 - }, - { - "x": 7.666351799836867, - "y": 7.463210900596095, - "heading": -2.2615780344771733e-15, - "angularVelocity": 2.6612644691277202e-14, - "velocityX": 1.3733472052384643, - "velocityY": -0.12002856585439871, - "timestamp": 2.2942562548151515 - }, - { - "x": 7.7413109156719315, - "y": 7.456659581936907, - "heading": -7.614087443589096e-16, - "angularVelocity": 1.8323351358986854e-14, - "velocityX": 0.9155648138981872, - "velocityY": -0.08001904481083937, - "timestamp": 2.376128247597303 - }, - { - "x": 7.778790473937988, - "y": 7.453383922576904, - "heading": 1.2753058277402247e-32, - "angularVelocity": 9.299990369536053e-15, - "velocityX": 0.4577824112060448, - "velocityY": -0.04000952277683576, - "timestamp": 2.4580002403794547 - }, - { - "x": 7.778790473937988, - "y": 7.453383922576904, - "heading": -1.9205360939372803e-33, - "angularVelocity": -1.7921313580146165e-31, - "velocityX": 0, - "velocityY": -2.7149722697711243e-33, - "timestamp": 2.5398722331616064 - }, - { - "x": 7.738439660582295, - "y": 7.44435478084036, - "heading": 0.006378483370257001, - "angularVelocity": 0.07429220524317, - "velocityX": -0.4699786349727835, - "velocityY": -0.10516525827894567, - "timestamp": 2.6257289303666393 - }, - { - "x": 7.657738063777793, - "y": 7.4262963371792745, - "heading": 0.019137718520693816, - "angularVelocity": 0.14861083137133396, - "velocityX": -0.9399569216106665, - "velocityY": -0.21033238231795398, - "timestamp": 2.7115856275716723 - }, - { - "x": 7.536685745097366, - "y": 7.399208360987091, - "heading": 0.038283130587402035, - "angularVelocity": 0.2229926457686509, - "velocityX": -1.4099344910897795, - "velocityY": -0.3155021923041767, - "timestamp": 2.7974423247767053 - }, - { - "x": 7.375282797714109, - "y": 7.363090535376708, - "heading": 0.06382240791742924, - "angularVelocity": 0.29746400876611084, - "velocityX": -1.8799109753524952, - "velocityY": -0.4206756931742923, - "timestamp": 2.8832990219817383 - }, - { - "x": 7.173529348282375, - "y": 7.317942425556285, - "heading": 0.09576422280862332, - "angularVelocity": 0.3720363807486605, - "velocityX": -2.349885984432058, - "velocityY": -0.5258542582019549, - "timestamp": 2.9691557191867712 - }, - { - "x": 6.931425567675906, - "y": 7.263763399408429, - "heading": 0.1341164540627939, - "angularVelocity": 0.4467005196179663, - "velocityX": -2.819859003291312, - "velocityY": -0.6310401857000439, - "timestamp": 3.055012416391804 - }, - { - "x": 6.648971729733199, - "y": 7.200552321235508, - "heading": 0.17888339892270952, - "angularVelocity": 0.5214147098275685, - "velocityX": -3.2898288326673564, - "velocityY": -0.73623934102623, - "timestamp": 3.140869113596837 - }, - { - "x": 6.32616907238056, - "y": 7.128303578050117, - "heading": 0.23005243262398928, - "angularVelocity": 0.5959818554292148, - "velocityX": -3.759784243525683, - "velocityY": -0.8415038725849725, - "timestamp": 3.22672581080187 - }, - { - "x": 6.043717325555703, - "y": 7.065082680229889, - "heading": 0.2748002023996994, - "angularVelocity": 0.5211913715810514, - "velocityX": -3.2898044767589747, - "velocityY": -0.7363537135519165, - "timestamp": 3.312582508006903 - }, - { - "x": 5.801616036461219, - "y": 7.010892017513263, - "heading": 0.3131359109312741, - "angularVelocity": 0.44650807426269673, - "velocityX": -2.8198299838663337, - "velocityY": -0.6311757204823929, - "timestamp": 3.398439205211936 - }, - { - "x": 5.599865072469117, - "y": 6.965732394898873, - "heading": 0.345068551946627, - "angularVelocity": 0.3719295297266695, - "velocityX": -2.349857035733676, - "velocityY": -0.5259883513402064, - "timestamp": 3.484295902416969 - }, - { - "x": 5.438464367683131, - "y": 6.929604278069296, - "heading": 0.37060660034883014, - "angularVelocity": 0.29744969505659097, - "velocityX": -1.8798848551155938, - "velocityY": -0.4207955582463213, - "timestamp": 3.570152599622002 - }, - { - "x": 5.317413881627067, - "y": 6.902507972751596, - "heading": 0.38975689670647157, - "angularVelocity": 0.22304953464386104, - "velocityX": -1.4099131459364944, - "velocityY": -0.31559920425301746, - "timestamp": 3.656009296827035 - }, - { - "x": 5.2367135843204595, - "y": 6.884443678448797, - "heading": 0.40252374557523257, - "angularVelocity": 0.14869951074722523, - "velocityX": -0.9399417859492977, - "velocityY": -0.21040052658513142, - "timestamp": 3.741865994032068 - }, - { - "x": 5.19636344909668, - "y": 6.875411510467529, - "heading": 0.4089083102765838, - "angularVelocity": 0.07436303642224136, - "velocityX": -0.46997073655676014, - "velocityY": -0.10520050590460155, - "timestamp": 3.827722691237101 - }, - { - "x": 5.19636344909668, - "y": 6.875411510467529, - "heading": 0.4089083102765838, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -2.0717354478922107e-39, - "timestamp": 3.913579388442134 - }, - { - "x": 5.232018908472479, - "y": 6.860290267658786, - "heading": 0.4034159167471209, - "angularVelocity": -0.06610093533675257, - "velocityX": 0.4291133186212821, - "velocityY": -0.181984099964854, - "timestamp": 3.996670399204415 - }, - { - "x": 5.3033298450653605, - "y": 6.830047764220078, - "heading": 0.39242803594978326, - "angularVelocity": -0.13223910380357973, - "velocityX": 0.858226851962353, - "velocityY": -0.36396841440803684, - "timestamp": 4.079761409966696 - }, - { - "x": 5.410296246847904, - "y": 6.784683995320563, - "heading": 0.37593441821627777, - "angularVelocity": -0.19850062698951831, - "velocityX": 1.2873402405534362, - "velocityY": -0.5459527869903824, - "timestamp": 4.162852420728976 - }, - { - "x": 5.552918069928713, - "y": 6.72419896473783, - "heading": 0.3539176492261319, - "angularVelocity": -0.2649717314564233, - "velocityX": 1.7164531009117598, - "velocityY": -0.7279371141094613, - "timestamp": 4.2459434314912565 - }, - { - "x": 5.731195235870137, - "y": 6.64859267901311, - "heading": 0.3263531755047263, - "angularVelocity": -0.33173833689743104, - "velocityX": 2.1455650172732543, - "velocityY": -0.9099213625048691, - "timestamp": 4.329034442253537 - }, - { - "x": 5.945127628296537, - "y": 6.557865140131203, - "heading": 0.29320941514251314, - "angularVelocity": -0.39888503050030044, - "velocityX": 2.57467553305435, - "velocityY": -1.0919055870131655, - "timestamp": 4.412125453015817 - }, - { - "x": 6.194715088245195, - "y": 6.452016336978496, - "heading": 0.25444802631157254, - "angularVelocity": -0.4664931678570496, - "velocityX": 3.003784135719744, - "velocityY": -1.2738899452738088, - "timestamp": 4.495216463778098 - }, - { - "x": 6.47995740296407, - "y": 6.331046237930042, - "heading": 0.2100244721238566, - "angularVelocity": -0.5346373064928706, - "velocityX": 3.4328901779151324, - "velocityY": -1.455874684140549, - "timestamp": 4.578307474540378 - }, - { - "x": 6.765189822342828, - "y": 6.210072243447842, - "heading": 0.16347583941488503, - "angularVelocity": -0.5602126184521319, - "velocityX": 3.4327710875342943, - "velocityY": -1.4559215656709357, - "timestamp": 4.661398485302659 - }, - { - "x": 7.0147678146535055, - "y": 6.104219702621471, - "heading": 0.1226760205647685, - "angularVelocity": -0.49102566542177156, - "velocityX": 3.0036701927324945, - "velocityY": -1.2739349281621999, - "timestamp": 4.744489496064939 - }, - { - "x": 7.22869154945277, - "y": 6.013488715324614, - "heading": 0.0876591508055317, - "angularVelocity": -0.421427895003207, - "velocityX": 2.574571338544547, - "velocityY": -1.091947088674045, - "timestamp": 4.8275805068272195 - }, - { - "x": 7.406961171745674, - "y": 5.937879380234634, - "heading": 0.058451527477084785, - "angularVelocity": -0.3515136361983664, - "velocityX": 2.1454742294918456, - "velocityY": -0.9099580616042243, - "timestamp": 4.9106715175895 - }, - { - "x": 7.5495767925597805, - "y": 5.877391789669617, - "heading": 0.035072287662159383, - "angularVelocity": -0.2813690626752902, - "velocityX": 1.7163784566554727, - "velocityY": -0.7279679234865629, - "timestamp": 4.99376252835178 - }, - { - "x": 7.656538486904284, - "y": 5.832026023472794, - "heading": 0.017533973815731525, - "angularVelocity": -0.21107354075405452, - "velocityX": 1.287283586554457, - "velocityY": -0.54597682445592, - "timestamp": 5.076853539114061 - }, - { - "x": 7.7278462932659, - "y": 5.801782144275187, - "heading": 0.005843001619637481, - "angularVelocity": -0.14070080612620453, - "velocityX": 0.8581891796409112, - "velocityY": -0.36398497166117527, - "timestamp": 5.159944549876341 - }, - { - "x": 7.763500213623047, - "y": 5.786660194396973, - "heading": 0, - "angularVelocity": -0.07032050237484791, - "velocityX": 0.42909479653763305, - "velocityY": -0.18199260954325794, - "timestamp": 5.243035560638622 - }, - { - "x": 7.763500213623047, - "y": 5.786660194396973, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -4.001772889855341e-38, - "timestamp": 5.326126571400902 - }, - { - "x": 7.714022075284393, - "y": 5.791512130406288, - "heading": 0.0034123876701157535, - "angularVelocity": 0.03625557509808546, - "velocityX": -0.5256900837968682, - "velocityY": 0.0515503358242044, - "timestamp": 5.420246928363491 - }, - { - "x": 7.615065798767571, - "y": 5.801215987589317, - "heading": 0.010236903417415826, - "angularVelocity": 0.07250839210068752, - "velocityX": -1.0513801658886168, - "velocityY": 0.103100514024672, - "timestamp": 5.514367285326079 - }, - { - "x": 7.466631382770048, - "y": 5.8157717439660175, - "heading": 0.020472681304287838, - "angularVelocity": 0.10875200878106045, - "velocityX": -1.5770702618193642, - "velocityY": 0.15465045869391852, - "timestamp": 5.608487642288668 - }, - { - "x": 7.268718824587655, - "y": 5.83517937052045, - "heading": 0.03411811725863541, - "angularVelocity": 0.14497858268611724, - "velocityX": -2.1027603864811066, - "velocityY": 0.20620009507770087, - "timestamp": 5.702607999251256 - }, - { - "x": 7.021328120470755, - "y": 5.8594388313479895, - "heading": 0.05117069065282414, - "angularVelocity": 0.1811783756936537, - "velocityX": -2.6284505509815856, - "velocityY": 0.25774934998581056, - "timestamp": 5.796728356213845 - }, - { - "x": 6.724459267218483, - "y": 5.888550083771315, - "heading": 0.07162673839699378, - "angularVelocity": 0.21733924949201522, - "velocityX": -3.1541407494902667, - "velocityY": 0.3092981514604363, - "timestamp": 5.8908487131764335 - }, - { - "x": 6.378112271590919, - "y": 5.922513077815848, - "heading": 0.09548116624563086, - "angularVelocity": 0.25344599849019656, - "velocityX": -3.679830876175178, - "velocityY": 0.36084642197046873, - "timestamp": 5.984969070139022 - }, - { - "x": 5.995099579809893, - "y": 5.9600738684572026, - "heading": 0.09548116733610806, - "angularVelocity": 1.1585986600028837e-8, - "velocityX": -4.06939268125882, - "velocityY": 0.3990719101956213, - "timestamp": 6.079089427101611 - }, - { - "x": 5.61208688806035, - "y": 5.997634659420377, - "heading": 0.09548116842656575, - "angularVelocity": 1.1585779406391748e-8, - "velocityX": -4.069392680924314, - "velocityY": 0.3990719136148622, - "timestamp": 6.173209784064199 - }, - { - "x": 5.22907419631129, - "y": 6.035195450388469, - "heading": 0.09548116951702341, - "angularVelocity": 1.158577911551983e-8, - "velocityX": -4.069392680919191, - "velocityY": 0.3990719136671072, - "timestamp": 6.267330141026788 - }, - { - "x": 4.846061504562215, - "y": 6.072756241356416, - "heading": 0.09548117060748103, - "angularVelocity": 1.1585778663343773e-8, - "velocityX": -4.06939268091934, - "velocityY": 0.3990719136655779, - "timestamp": 6.3614504979893765 - }, - { - "x": 4.463048812813157, - "y": 6.110317032324537, - "heading": 0.0954811716979387, - "angularVelocity": 1.1585779124921689e-8, - "velocityX": -4.069392680919158, - "velocityY": 0.39907191366742895, - "timestamp": 6.455570854951965 - }, - { - "x": 4.080036121063514, - "y": 6.147877823286686, - "heading": 0.09548117278839631, - "angularVelocity": 1.1585778495719892e-8, - "velocityX": -4.0693926809253815, - "velocityY": 0.3990719136039721, - "timestamp": 6.549691211914554 - }, - { - "x": 3.697023429276966, - "y": 6.185438613871755, - "heading": 0.0954811738788733, - "angularVelocity": 1.1585984482237455e-8, - "velocityX": -4.069392681317477, - "velocityY": 0.39907190959759625, - "timestamp": 6.643811568877142 - }, - { - "x": 3.3506759158353345, - "y": 6.219399783259486, - "heading": 0.1191237323968371, - "angularVelocity": 0.25119495166557093, - "velocityX": -3.679836377791235, - "velocityY": 0.36082703555013734, - "timestamp": 6.737931925839731 - }, - { - "x": 3.0538065721545458, - "y": 6.248509296515882, - "heading": 0.13937914766365392, - "angularVelocity": 0.21520759079640947, - "velocityX": -3.1541459601432313, - "velocityY": 0.3092796733438538, - "timestamp": 6.832052282802319 - }, - { - "x": 2.806415429883936, - "y": 6.272767183419743, - "heading": 0.15625241023006078, - "angularVelocity": 0.17927325300214964, - "velocityX": -2.6284552062307136, - "velocityY": 0.25773262752820747, - "timestamp": 6.926172639764908 - }, - { - "x": 2.608502504943331, - "y": 6.2921734683030435, - "heading": 0.16974760497245162, - "angularVelocity": 0.14338231576995536, - "velocityX": -2.102764283174911, - "velocityY": 0.2061858402323502, - "timestamp": 7.0202929967274965 - }, - { - "x": 2.460067807083176, - "y": 6.306728169431489, - "heading": 0.17986771256728493, - "angularVelocity": 0.10752304731330184, - "velocityX": -1.5770732565236112, - "velocityY": 0.1546392470040303, - "timestamp": 7.114413353690085 - }, - { - "x": 2.361111341639132, - "y": 6.3164312990743445, - "heading": 0.1866144786841838, - "angularVelocity": 0.0716823260623689, - "velocityX": -1.051382173182552, - "velocityY": 0.10309278413290339, - "timestamp": 7.208533710652674 - }, - { - "x": 2.3116331100463867, - "y": 6.321282863616943, - "heading": 0.18998832965561516, - "angularVelocity": 0.035846134463476516, - "velocityX": -0.5256910745930495, - "velocityY": 0.05154638910397498, - "timestamp": 7.302654067615262 - }, - { - "x": 2.3116331100463867, - "y": 6.321282863616943, - "heading": 0.18998832965561516, - "angularVelocity": 0, - "velocityX": 5.252647651099752e-37, - "velocityY": 0, - "timestamp": 7.396774424577851 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "NoTeamBottomStraight": { - "waypoints": [ - { - "x": 0.727, - "y": 4.389017105102539, - "heading": -1.052, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 27 - }, - { - "x": 2.470987558364868, - "y": 2.642529249191284, - "heading": 1.5707963267948966, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 10 - }, - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 1.57, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 2.82627534866333, - "y": 5.474045753479004, - "heading": 1.57, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 2.82627534866333, - "y": 6.957221508026123, - "heading": 1.57, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.727, - "y": 4.389017105102539, - "heading": -1.052, - "angularVelocity": -1.9035720406977927e-18, - "velocityX": -7.789821171233355e-19, - "velocityY": 6.642220463091642e-19, - "timestamp": 0 - }, - { - "x": 0.7318438956432248, - "y": 4.379292392557921, - "heading": -1.0218708963854122, - "angularVelocity": 0.6376018021899323, - "velocityX": 0.10250808093224481, - "velocityY": -0.20579750143065442, - "timestamp": 0.047253793058779206 - }, - { - "x": 0.7416803389522922, - "y": 4.359743623946586, - "heading": -0.963247947139427, - "angularVelocity": 1.2405977478477537, - "velocityX": 0.20816198388205162, - "velocityY": -0.41369734249730467, - "timestamp": 0.09450758611755841 - }, - { - "x": 0.756679070865091, - "y": 4.330228659181428, - "heading": -0.8784548898183928, - "angularVelocity": 1.7944180103290261, - "velocityX": 0.31740799927196917, - "velocityY": -0.6246051979032636, - "timestamp": 0.14176137917633763 - }, - { - "x": 0.7769909237075252, - "y": 4.290540404198398, - "heading": -0.7706337125535929, - "angularVelocity": 2.2817465072207144, - "velocityX": 0.4298459769603722, - "velocityY": -0.8398956446450863, - "timestamp": 0.18901517223511682 - }, - { - "x": 0.8027070398966437, - "y": 4.240415566240092, - "heading": -0.6435269412573291, - "angularVelocity": 2.689874464429873, - "velocityX": 0.544212739856253, - "velocityY": -1.0607579775861957, - "timestamp": 0.23626896529389602 - }, - { - "x": 0.8338645488878723, - "y": 4.179590349127875, - "heading": -0.5009199000549501, - "angularVelocity": 3.0178961723768816, - "velocityX": 0.659365248255766, - "velocityY": -1.28720285028876, - "timestamp": 0.2835227583526752 - }, - { - "x": 0.8705177044270638, - "y": 4.107851215829094, - "heading": -0.34654429878237253, - "angularVelocity": 3.266946233936165, - "velocityX": 0.7756658919126009, - "velocityY": -1.518166662505675, - "timestamp": 0.3307765514114544 - }, - { - "x": 0.9127860516643015, - "y": 4.025035475201868, - "heading": -0.18511651921150357, - "angularVelocity": 3.416186704209506, - "velocityX": 0.8944963885684252, - "velocityY": -1.752573397107217, - "timestamp": 0.3780303444702336 - }, - { - "x": 0.9608190780560075, - "y": 3.9310914505496757, - "heading": -0.02385519507028553, - "angularVelocity": 3.412664120753743, - "velocityX": 1.0164903869612656, - "velocityY": -1.9880737306174552, - "timestamp": 0.4252841375290128 - }, - { - "x": 1.0146390571858634, - "y": 3.8264366161374967, - "heading": 0.1250934065508822, - "angularVelocity": 3.152098318031103, - "velocityX": 1.138955746111406, - "velocityY": -2.2147393391679286, - "timestamp": 0.472537930587792 - }, - { - "x": 1.0745072439716545, - "y": 3.7116431971477404, - "heading": 0.2562841881014644, - "angularVelocity": 2.776301605828625, - "velocityX": 1.266949866041881, - "velocityY": -2.4292953339632333, - "timestamp": 0.5197917236465712 - }, - { - "x": 1.1413904234655279, - "y": 3.5874142558818387, - "heading": 0.36909871419180623, - "angularVelocity": 2.3874173645703185, - "velocityX": 1.4154034028692064, - "velocityY": -2.6289728977179108, - "timestamp": 0.5670455167053504 - }, - { - "x": 1.2171004406920223, - "y": 3.4555713477306926, - "heading": 0.46254553428918027, - "angularVelocity": 1.9775517275647088, - "velocityX": 1.6021997881168686, - "velocityY": -2.790102119149382, - "timestamp": 0.6142993097641296 - }, - { - "x": 1.304824543975774, - "y": 3.3227207721324725, - "heading": 0.5413110165917999, - "angularVelocity": 1.6668605249242734, - "velocityX": 1.856445749754547, - "velocityY": -2.811426702465267, - "timestamp": 0.6615531028229088 - }, - { - "x": 1.4008888245081486, - "y": 3.1991200061349105, - "heading": 0.6262952838301894, - "angularVelocity": 1.798464456232692, - "velocityX": 2.0329432689748255, - "velocityY": -2.615679250209918, - "timestamp": 0.708806895881688 - }, - { - "x": 1.5007168068199321, - "y": 3.0869110822536374, - "heading": 0.7270728004213672, - "angularVelocity": 2.1326862896661085, - "velocityX": 2.1125919391826877, - "velocityY": -2.3746014154185615, - "timestamp": 0.7560606889404672 - }, - { - "x": 1.601781282376718, - "y": 2.9867599379462364, - "heading": 0.8415198083717181, - "angularVelocity": 2.4219644718888445, - "velocityX": 2.1387590077916334, - "velocityY": -2.119430797498578, - "timestamp": 0.8033144819992464 - }, - { - "x": 1.7021006490957822, - "y": 2.8990265047996244, - "heading": 0.9624877761029027, - "angularVelocity": 2.5599631246683616, - "velocityX": 2.122990774397232, - "velocityY": -1.856643191319687, - "timestamp": 0.8505682750580256 - }, - { - "x": 1.8004198198487906, - "y": 2.8236557821766897, - "heading": 1.0813753656601117, - "angularVelocity": 2.515937491183494, - "velocityX": 2.080661982641449, - "velocityY": -1.5950195263516973, - "timestamp": 0.8978220681168048 - }, - { - "x": 1.8960745245056285, - "y": 2.7604196812117996, - "heading": 1.1931387342522881, - "angularVelocity": 2.365172430775102, - "velocityX": 2.0242756922783456, - "velocityY": -1.3382227514779625, - "timestamp": 0.945075861175584 - }, - { - "x": 1.9886778939409326, - "y": 2.709098454682459, - "heading": 1.294430400917861, - "angularVelocity": 2.143566899266176, - "velocityX": 1.9597023527849797, - "velocityY": -1.0860763381577074, - "timestamp": 0.9923296542343631 - }, - { - "x": 2.0779902618648065, - "y": 2.6695027774627897, - "heading": 1.3827319365638684, - "angularVelocity": 1.868665559528068, - "velocityX": 1.8900571180133192, - "velocityY": -0.8379364841763153, - "timestamp": 1.0395834472931424 - }, - { - "x": 2.163854051486685, - "y": 2.641470045467505, - "heading": 1.4560082555180538, - "angularVelocity": 1.550697080825592, - "velocityX": 1.81707719240807, - "velocityY": -0.5932377102598947, - "timestamp": 1.0868372403519218 - }, - { - "x": 2.246158483677488, - "y": 2.624860911919073, - "heading": 1.5125976444748226, - "angularVelocity": 1.1975628895309458, - "velocityX": 1.7417529231658644, - "velocityY": -0.35148783776513787, - "timestamp": 1.134091033410701 - }, - { - "x": 2.3248213255730343, - "y": 2.6195588118421886, - "heading": 1.5511852292241632, - "angularVelocity": 0.8166029063813219, - "velocityX": 1.6646884155457526, - "velocityY": -0.11220475085013962, - "timestamp": 1.1813448264694804 - }, - { - "x": 2.3997801498313853, - "y": 2.6254709122263833, - "heading": 1.5707963267948966, - "angularVelocity": 0.41501636802656, - "velocityX": 1.5863028003933837, - "velocityY": 0.12511377397451495, - "timestamp": 1.2285986195282597 - }, - { - "x": 2.470987558364868, - "y": 2.642529249191284, - "heading": 1.5707963267948966, - "angularVelocity": 7.2320671802386e-18, - "velocityX": 1.5069141316318813, - "velocityY": 0.360994025255961, - "timestamp": 1.275852412587039 - }, - { - "x": 2.5896469460709723, - "y": 2.7162173203691675, - "heading": 1.5707635810166263, - "angularVelocity": -0.0003685297147391131, - "velocityX": 1.3354243695676342, - "velocityY": 0.8293051894141662, - "timestamp": 1.3647076037982955 - }, - { - "x": 2.691674793744484, - "y": 2.8309800714405235, - "heading": 1.570698306188048, - "angularVelocity": -0.0007346203152394417, - "velocityX": 1.1482485860723253, - "velocityY": 1.2915705825054542, - "timestamp": 1.453562795009552 - }, - { - "x": 2.7746449871658987, - "y": 2.9857496097500946, - "heading": 1.5706009997851365, - "angularVelocity": -0.0010951121885513847, - "velocityX": 0.933768666640422, - "velocityY": 1.7418176270826997, - "timestamp": 1.5424179862208085 - }, - { - "x": 2.8334018266497893, - "y": 3.177633163967886, - "heading": 1.5704732520807607, - "angularVelocity": -0.0014377067072201901, - "velocityX": 0.6612651290591912, - "velocityY": 2.159508652247243, - "timestamp": 1.631273177432065 - }, - { - "x": 2.8534735329147445, - "y": 3.39113142463186, - "heading": 1.5703246879122892, - "angularVelocity": -0.0016719807413194167, - "velocityX": 0.2258923310089341, - "velocityY": 2.4027663184739962, - "timestamp": 1.7201283686433215 - }, - { - "x": 2.8490781305616832, - "y": 3.5676824837315517, - "heading": 1.5701962202028332, - "angularVelocity": -0.0014458098362590802, - "velocityX": -0.04946702936704393, - "velocityY": 1.9869526663887853, - "timestamp": 1.808983559854578 - }, - { - "x": 2.839727568645754, - "y": 3.7001973861849375, - "heading": 1.570098429466109, - "angularVelocity": -0.0011005630103461549, - "velocityX": -0.10523371553720058, - "velocityY": 1.4913580247475502, - "timestamp": 1.8978387510658346 - }, - { - "x": 2.831251803689392, - "y": 3.7884068493950025, - "heading": 1.5700328717959458, - "angularVelocity": -0.0007378034898061426, - "velocityX": -0.09538851743856928, - "velocityY": 0.9927328050011545, - "timestamp": 1.986693942277091 - }, - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 1.57, - "angularVelocity": -0.00036994795124125425, - "velocityX": -0.056006350987752376, - "velocityY": 0.4955677288524759, - "timestamp": 2.0755491334883476 - }, - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 1.57, - "angularVelocity": 3.0178106927934417e-22, - "velocityX": -8.449689256106595e-19, - "velocityY": -6.586266614449837e-18, - "timestamp": 2.1644043246996043 - }, - { - "x": 2.82627534866333, - "y": 3.887160790032957, - "heading": 1.57, - "angularVelocity": 8.611213547003838e-17, - "velocityX": 1.0232237644061418e-17, - "velocityY": 0.5541939799877377, - "timestamp": 2.2631426316979057 - }, - { - "x": 2.82627534866333, - "y": 3.996601139183229, - "heading": 1.57, - "angularVelocity": 1.801778145303611e-16, - "velocityX": 2.3075955144475316e-17, - "velocityY": 1.1083879446318137, - "timestamp": 2.361880938696207 - }, - { - "x": 2.82627534866333, - "y": 4.1607616596261225, - "heading": 1.57, - "angularVelocity": 2.8227476617608174e-16, - "velocityX": 3.770139097941117e-17, - "velocityY": 1.6625818837031228, - "timestamp": 2.4606192456945086 - }, - { - "x": 2.82627534866333, - "y": 4.3796423463116145, - "heading": 1.57, - "angularVelocity": 3.924863437180408e-16, - "velocityX": 5.411880274246253e-17, - "velocityY": 2.2167757716289116, - "timestamp": 2.55935755269281 - }, - { - "x": 2.82627534866333, - "y": 4.653243184089661, - "heading": 1.57, - "angularVelocity": 5.109198296204701e-16, - "velocityX": 7.231247688509119e-17, - "velocityY": 2.7709695061183575, - "timestamp": 2.6580958596911115 - }, - { - "x": 2.82627534866333, - "y": 4.926844021867707, - "heading": 1.57, - "angularVelocity": 5.107227261231941e-16, - "velocityX": 1.0354556057725255e-16, - "velocityY": 2.7709695061183575, - "timestamp": 2.756834166689413 - }, - { - "x": 2.82627534866333, - "y": 5.145724708553199, - "heading": 1.57, - "angularVelocity": 3.9232798727341053e-16, - "velocityX": 7.910713017021751e-17, - "velocityY": 2.2167757716289116, - "timestamp": 2.8555724736877144 - }, - { - "x": 2.82627534866333, - "y": 5.309885228996092, - "heading": 1.57, - "angularVelocity": 2.8215477627973073e-16, - "velocityX": 5.657236997399948e-17, - "velocityY": 1.662581883703123, - "timestamp": 2.954310780686016 - }, - { - "x": 2.82627534866333, - "y": 5.419325578146364, - "heading": 1.57, - "angularVelocity": 1.8009528804964001e-16, - "velocityX": 3.589318347482986e-17, - "velocityY": 1.108387944631814, - "timestamp": 3.0530490876843173 - }, - { - "x": 2.82627534866333, - "y": 5.474045753479004, - "heading": 1.57, - "angularVelocity": 8.606670402357876e-17, - "velocityX": 1.7043045035067325e-17, - "velocityY": 0.5541939799877377, - "timestamp": 3.1517873946826187 - }, - { - "x": 2.82627534866333, - "y": 5.474045753479004, - "heading": 1.57, - "angularVelocity": -1.8688348541769264e-27, - "velocityX": -6.335524016517998e-27, - "velocityY": 3.200026210806558e-22, - "timestamp": 3.25052570168092 - }, - { - "x": 2.82627534866333, - "y": 5.523484949137364, - "heading": 1.57, - "angularVelocity": 2.815530703859563e-18, - "velocityX": -4.120948964782728e-17, - "velocityY": 0.5267733078959163, - "timestamp": 3.344378588524854 - }, - { - "x": 2.82627534866333, - "y": 5.622363339014038, - "heading": 1.57, - "angularVelocity": 5.079322160604046e-18, - "velocityX": -8.241891103838567e-17, - "velocityY": 1.053546600448171, - "timestamp": 3.438231475368788 - }, - { - "x": 2.82627534866333, - "y": 5.770680920708947, - "heading": 1.57, - "angularVelocity": 6.7861542129190236e-18, - "velocityX": -1.236282186820289e-16, - "velocityY": 1.5803198674276584, - "timestamp": 3.532084362212722 - }, - { - "x": 2.82627534866333, - "y": 5.968437689421936, - "heading": 1.57, - "angularVelocity": 7.930330289030281e-18, - "velocityX": -1.6483729882150294e-16, - "velocityY": 2.107093083261625, - "timestamp": 3.6259372490566557 - }, - { - "x": 2.82627534866333, - "y": 6.2156336307525635, - "heading": 1.57, - "angularVelocity": 8.504428120601007e-18, - "velocityX": -2.0604569642935152e-16, - "velocityY": 2.6338661456592565, - "timestamp": 3.7197901359005896 - }, - { - "x": 2.82627534866333, - "y": 6.462829572083191, - "heading": 1.57, - "angularVelocity": 8.962407688809337e-18, - "velocityX": -2.0605181411254769e-16, - "velocityY": 2.6338661456592574, - "timestamp": 3.8136430227445235 - }, - { - "x": 2.82627534866333, - "y": 6.66058634079618, - "heading": 1.57, - "angularVelocity": 8.29359895225545e-18, - "velocityX": -1.6484073568780158e-16, - "velocityY": 2.1070930832616255, - "timestamp": 3.9074959095884574 - }, - { - "x": 2.82627534866333, - "y": 6.808903922491089, - "heading": 1.57, - "angularVelocity": 7.056973102062562e-18, - "velocityX": -1.2363031580105674e-16, - "velocityY": 1.5803198674276586, - "timestamp": 4.001348796432391 - }, - { - "x": 2.82627534866333, - "y": 6.907782312367763, - "heading": 1.57, - "angularVelocity": 5.259040639378902e-18, - "velocityX": -8.242011541755058e-17, - "velocityY": 1.0535466004481713, - "timestamp": 4.095201683276326 - }, - { - "x": 2.82627534866333, - "y": 6.957221508026123, - "heading": 1.57, - "angularVelocity": 2.9050319144623277e-18, - "velocityX": -4.1210024782203125e-17, - "velocityY": 0.5267733078959164, - "timestamp": 4.18905457012026 - }, - { - "x": 2.82627534866333, - "y": 6.957221508026123, - "heading": 1.57, - "angularVelocity": 7.725181741580017e-29, - "velocityX": 2.7049094499726637e-34, - "velocityY": 9.029888043887834e-23, - "timestamp": 4.282907456964194 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 0 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - }, - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - 2 - ], - "type": "StopPoint" - }, - { - "scope": [ - 1, - 1 - ], - "type": "ZeroAngularVelocity" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "NoTeamMiddle": { - "waypoints": [ - { - "x": 1.2598519325256348, - "y": 5.564249038696289, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 11 - }, - { - "x": 2.2249319553375244, - "y": 4.102436542510986, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 9 - }, - { - "x": 1.6572377681732178, - "y": 4.968170166015625, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 8 - }, - { - "x": 2.2249319553375244, - "y": 5.564249038696289, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 9 - }, - { - "x": 1.6572377681732178, - "y": 6.3874053955078125, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 8 - }, - { - "x": 2.2249319553375244, - "y": 7.026061534881592, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 1.345005989074707, - "y": 5.5500569343566895, - "heading": -2.7546975510839134e-33, - "angularVelocity": 7.98318564036553e-35, - "velocityX": 7.982434630916693e-33, - "velocityY": 1.9344790398404236e-33, - "timestamp": 0 - }, - { - "x": 1.374336856748517, - "y": 5.501802917788148, - "heading": 9.87730102393334e-22, - "angularVelocity": 9.847392319149301e-21, - "velocityX": 0.2924205298932618, - "velocityY": -0.48107902061998664, - "timestamp": 0.10030372246612224 - }, - { - "x": 1.4329985912967438, - "y": 5.405294885966198, - "heading": 3.2109950624668562e-21, - "angularVelocity": 2.2165328394625732e-20, - "velocityX": 0.5848410518167925, - "velocityY": -0.9621580281284781, - "timestamp": 0.20060744493224447 - }, - { - "x": 1.520991191387064, - "y": 5.260532841082725, - "heading": 7.246933022052211e-21, - "angularVelocity": 4.023743732214471e-20, - "velocityX": 0.8772615604574385, - "velocityY": -1.4432370137844779, - "timestamp": 0.3009111673983667 - }, - { - "x": 1.6383146543548324, - "y": 5.067516787521501, - "heading": 1.409551317097851e-20, - "angularVelocity": 6.827869140851397e-20, - "velocityX": 1.1696820425323176, - "velocityY": -1.9243159557354987, - "timestamp": 0.40121488986448894 - }, - { - "x": 1.7849689722061157, - "y": 4.826246738433838, - "heading": 2.72851162530444e-20, - "angularVelocity": 1.3149785356894001e-19, - "velocityX": 1.462102444909918, - "velocityY": -2.405394766571619, - "timestamp": 0.5015186123306112 - }, - { - "x": 1.9316232900573989, - "y": 4.584976689346176, - "heading": 4.737044334212545e-20, - "angularVelocity": 2.0024494806875148e-19, - "velocityX": 1.4621024449099178, - "velocityY": -2.405394766571619, - "timestamp": 0.6018223347967335 - }, - { - "x": 2.0489467530251675, - "y": 4.391960635784951, - "heading": 6.089560135631662e-20, - "angularVelocity": 1.3484202907628645e-19, - "velocityX": 1.1696820425323173, - "velocityY": -1.9243159557354987, - "timestamp": 0.7021260572628558 - }, - { - "x": 2.1369393531154874, - "y": 4.247198590901478, - "heading": 6.930746734079673e-20, - "angularVelocity": 8.386394336827385e-20, - "velocityX": 0.8772615604574384, - "velocityY": -1.4432370137844779, - "timestamp": 0.8024297797289781 - }, - { - "x": 2.195601087663714, - "y": 4.1506905590795276, - "heading": 7.420098024062153e-20, - "angularVelocity": 4.8786950395290725e-20, - "velocityX": 0.5848410518167924, - "velocityY": -0.9621580281284781, - "timestamp": 0.9027335021951004 - }, - { - "x": 2.2249319553375244, - "y": 4.102436542510986, - "heading": 7.6367465447036e-20, - "angularVelocity": 2.1599249454848124e-20, - "velocityX": 0.2924205298932618, - "velocityY": -0.48107902061998664, - "timestamp": 1.0030372246612227 - }, - { - "x": 2.2249319553375244, - "y": 4.102436542510986, - "heading": 7.636746544703908e-20, - "angularVelocity": 8.194514664647686e-33, - "velocityX": 1.446672081349358e-33, - "velocityY": -1.672389052514602e-33, - "timestamp": 1.103340947127345 - }, - { - "x": 2.197352071426116, - "y": 4.119337343550709, - "heading": 2.1521498385967074e-19, - "angularVelocity": 1.8289989887525516e-18, - "velocityX": -0.36330198842015404, - "velocityY": 0.22262945860641717, - "timestamp": 1.1792554336715595 - }, - { - "x": 2.143014042988731, - "y": 4.154411991095742, - "heading": 5.261862325416936e-19, - "angularVelocity": 4.096336081347265e-18, - "velocityX": -0.7157794369818592, - "velocityY": 0.46202838406349855, - "timestamp": 1.255169920215774 - }, - { - "x": 2.0634115959086574, - "y": 4.209685549993584, - "heading": -6.256825300150602e-18, - "angularVelocity": -8.935068455939364e-17, - "velocityX": -1.0485804581410318, - "velocityY": 0.7281029143976387, - "timestamp": 1.3310844067599885 - }, - { - "x": 1.9618797741940481, - "y": 4.28873702220571, - "heading": -1.2720644596750681e-17, - "angularVelocity": -8.514605574453078e-17, - "velocityX": -1.3374498904826855, - "velocityY": 1.041322622475801, - "timestamp": 1.406998893304203 - }, - { - "x": 1.8487346991478477, - "y": 4.397978202555833, - "heading": -1.8686559268739726e-17, - "angularVelocity": -7.858729930366567e-17, - "velocityX": -1.490427982810653, - "velocityY": 1.4390030852213924, - "timestamp": 1.4829133798484175 - }, - { - "x": 1.7533063097381842, - "y": 4.534282361069246, - "heading": -2.3214525333326314e-17, - "angularVelocity": -5.964561163868586e-17, - "velocityX": -1.2570511078156814, - "velocityY": 1.7954960208289874, - "timestamp": 1.558827866392632 - }, - { - "x": 1.6889882259887987, - "y": 4.679442887556022, - "heading": -2.7498023126090817e-17, - "angularVelocity": -5.642529870698493e-17, - "velocityX": -0.8472438750135669, - "velocityY": 1.9121584442546633, - "timestamp": 1.6347423529368466 - }, - { - "x": 1.657008982479021, - "y": 4.825300663776631, - "heading": -3.2126097337513165e-17, - "angularVelocity": -6.096430842947865e-17, - "velocityX": -0.4212535046410702, - "velocityY": 1.9213431172409716, - "timestamp": 1.710656839481061 - }, - { - "x": 1.6572377681732178, - "y": 4.968170166015625, - "heading": -3.528590547494103e-17, - "angularVelocity": -4.1623256892899375e-17, - "velocityX": 0.0030137290603089984, - "velocityY": 1.8819794316303808, - "timestamp": 1.7865713260252756 - }, - { - "x": 1.6963109232900544, - "y": 5.11955499843328, - "heading": -3.8568694444513565e-17, - "angularVelocity": -3.923406280814191e-17, - "velocityX": 0.46698055196147537, - "velocityY": 1.8092670630156014, - "timestamp": 1.8702432368847668 - }, - { - "x": 1.7732918382344491, - "y": 5.260591607649996, - "heading": -4.157623585429151e-17, - "angularVelocity": -3.594445751060187e-17, - "velocityX": 0.9200329495721387, - "velocityY": 1.6855908723485022, - "timestamp": 1.953915147744258 - }, - { - "x": 1.884554817294881, - "y": 5.382423433953372, - "heading": -4.416508678675616e-17, - "angularVelocity": -3.094050334766252e-17, - "velocityX": 1.3297530547291259, - "velocityY": 1.4560660208653102, - "timestamp": 2.0375870586037492 - }, - { - "x": 2.010708721210131, - "y": 5.467891221596337, - "heading": -4.614895263674143e-17, - "angularVelocity": -2.3710060738734744e-17, - "velocityX": 1.5077210812969026, - "velocityY": 1.0214633174386198, - "timestamp": 2.1212589694632404 - }, - { - "x": 2.1156467514770063, - "y": 5.520283792104612, - "heading": -4.311417988982476e-17, - "angularVelocity": 3.626990802127755e-17, - "velocityX": 1.2541607952888236, - "velocityY": 0.6261667741311343, - "timestamp": 2.2049308803227317 - }, - { - "x": 2.188091221601493, - "y": 5.550579929073903, - "heading": -3.975437161737579e-17, - "angularVelocity": 4.0154551314033223e-17, - "velocityX": 0.8658158918605504, - "velocityY": 0.36208252755416254, - "timestamp": 2.288602791182223 - }, - { - "x": 2.2249319553375244, - "y": 5.564249038696289, - "heading": -3.992691037709511e-17, - "angularVelocity": -2.0620870119604985e-18, - "velocityX": 0.44029989703351463, - "velocityY": 0.1633655725317422, - "timestamp": 2.372274702041714 - }, - { - "x": 2.2249319553375244, - "y": 5.564249038696289, - "heading": -3.992691037709511e-17, - "angularVelocity": 1.948197557554799e-34, - "velocityX": -1.3588263959389525e-32, - "velocityY": -1.5731738564130587e-32, - "timestamp": 2.4559466129012053 - }, - { - "x": 2.198199915755316, - "y": 5.579643402434152, - "heading": -4.443401956416108e-17, - "angularVelocity": -6.079583697798712e-17, - "velocityX": -0.3605851761095616, - "velocityY": 0.20765266871766613, - "timestamp": 2.530081772506564 - }, - { - "x": 2.145519284812898, - "y": 5.611718860578594, - "heading": -4.847236948556383e-17, - "angularVelocity": -5.447280155428928e-17, - "velocityX": -0.7106025160376092, - "velocityY": 0.4326618883022348, - "timestamp": 2.604216932111923 - }, - { - "x": 2.068317300626264, - "y": 5.662510392673879, - "heading": -5.190015426747772e-17, - "angularVelocity": -4.6236964337837673e-17, - "velocityX": -1.0413680175182847, - "velocityY": 0.6851206953038543, - "timestamp": 2.6783520917172816 - }, - { - "x": 1.9697712674085048, - "y": 5.73557342108719, - "heading": -5.447049142281068e-17, - "angularVelocity": -3.467095832077673e-17, - "velocityX": -1.3292752553895588, - "velocityY": 0.9855381549354427, - "timestamp": 2.7524872513226404 - }, - { - "x": 1.8594813582153729, - "y": 5.837161331961853, - "heading": -5.868151668893362e-17, - "angularVelocity": -5.68019968220714e-17, - "velocityX": -1.487686946116183, - "velocityY": 1.3703067669300684, - "timestamp": 2.826622410927999 - }, - { - "x": 1.7641319442068013, - "y": 5.965737551430191, - "heading": -6.066243047179228e-17, - "angularVelocity": -2.6720298755315395e-17, - "velocityX": -1.2861564541864055, - "velocityY": 1.7343487240438071, - "timestamp": 2.900757570533358 - }, - { - "x": 1.6977316821706918, - "y": 6.104969019593263, - "heading": -6.137469251207977e-17, - "angularVelocity": -9.60761370564525e-18, - "velocityX": -0.8956649232236864, - "velocityY": 1.8780760560068654, - "timestamp": 2.9748927301387167 - }, - { - "x": 1.66207189361609, - "y": 6.246771743474824, - "heading": -6.43119401475664e-17, - "angularVelocity": -3.96201692050749e-17, - "velocityX": -0.4810104779490378, - "velocityY": 1.9127594064195064, - "timestamp": 3.0490278897440755 - }, - { - "x": 1.6572377681732178, - "y": 6.3874053955078125, - "heading": -6.711231297016692e-17, - "angularVelocity": -3.7773881886241834e-17, - "velocityX": -0.06520692028729415, - "velocityY": 1.8969899408272504, - "timestamp": 3.1231630493494342 - }, - { - "x": 1.692247730583365, - "y": 6.5448538320481955, - "heading": -6.973008277469343e-17, - "angularVelocity": -3.070599085824093e-17, - "velocityX": 0.4106608874835372, - "velocityY": 1.8468433049167998, - "timestamp": 3.2084157809272638 - }, - { - "x": 1.7671066950619185, - "y": 6.6935741603960714, - "heading": -7.151388543134576e-17, - "angularVelocity": -2.0923698913909792e-17, - "velocityX": 0.8780828847719975, - "velocityY": 1.7444640845566861, - "timestamp": 3.2936685125050933 - }, - { - "x": 1.8784928172798085, - "y": 6.82413163710067, - "heading": -7.201746198476356e-17, - "angularVelocity": -5.906865781623406e-18, - "velocityX": 1.3065402146815994, - "velocityY": 1.5314169327865985, - "timestamp": 3.378921244082923 - }, - { - "x": 2.006539662386896, - "y": 6.917452813996992, - "heading": -7.066227191561186e-17, - "angularVelocity": 1.5896150266967084e-17, - "velocityX": 1.501967652381787, - "velocityY": 1.0946414873654486, - "timestamp": 3.4641739756607524 - }, - { - "x": 2.1133877261411924, - "y": 6.975920959825139, - "heading": -7.201772249126913e-17, - "angularVelocity": -1.5899201837052936e-17, - "velocityX": 1.253309562952265, - "velocityY": 0.6858213777557197, - "timestamp": 3.549426707238582 - }, - { - "x": 2.1872956347822887, - "y": 7.010325255827941, - "heading": -7.084833328584506e-17, - "angularVelocity": 1.371673720081464e-17, - "velocityX": 0.8669271620185445, - "velocityY": 0.40355652383283236, - "timestamp": 3.6346794388164114 - }, - { - "x": 2.2249319553375244, - "y": 7.026061534881592, - "heading": -6.901099450925116e-17, - "angularVelocity": 2.155167137746115e-17, - "velocityX": 0.44146762055215316, - "velocityY": 0.18458386918999145, - "timestamp": 3.719932170394241 - }, - { - "x": 2.2249319553375244, - "y": 7.026061534881592, - "heading": -6.901099450925116e-17, - "angularVelocity": 9.740777663850368e-35, - "velocityX": 0, - "velocityY": -6.61490476729539e-33, - "timestamp": 3.8051849019720705 - } - ], - "constraints": [ - { - "scope": [ - "first" - ], - "type": "StopPoint" - }, - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 3 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - }, - "3NB-2ndN": { - "waypoints": [ - { - "x": 0.71, - "y": 4.38, - "heading": -1.0303768814651526, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 16 - }, - { - "x": 2.295893669128418, - "y": 4.102436542510986, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "x": 3.3319356441497803, - "y": 2.3000075817108154, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 13 - }, - { - "x": 5.758828163146973, - "y": 1.8174675703048706, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 12 - }, - { - "x": 7.845104217529297, - "y": 2.427738904953003, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": true, - "controlIntervalCount": 12 - }, - { - "x": 5.758828163146973, - "y": 1.8174675703048706, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 14 - }, - { - "x": 2.9771268367767334, - "y": 2.825124740600586, - "heading": 0, - "isInitialGuess": false, - "translationConstrained": true, - "headingConstrained": false, - "controlIntervalCount": 40 - } - ], - "trajectory": [ - { - "x": 0.71, - "y": 4.38, - "heading": -1.0303768814651526, - "angularVelocity": 4.324130717118377e-34, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 0.7889068222351219, - "y": 4.366189710689795, - "heading": -1.0303768723653812, - "angularVelocity": 8.159611106479039e-8, - "velocityX": 0.7075441203965512, - "velocityY": -0.12383452692207302, - "timestamp": 0.11152212273475985 - }, - { - "x": 0.898759131822665, - "y": 4.346963335383371, - "heading": -0.9951630127982624, - "angularVelocity": 0.3157567189683987, - "velocityX": 0.985027068116447, - "velocityY": -0.17239965340465557, - "timestamp": 0.2230442454695197 - }, - { - "x": 1.0086114414119594, - "y": 4.327736960086952, - "heading": -0.9311307451167896, - "angularVelocity": 0.5741665071580884, - "velocityX": 0.9850270681321506, - "velocityY": -0.17239965331494583, - "timestamp": 0.33456636820427954 - }, - { - "x": 1.118463751001178, - "y": 4.3085105847901, - "heading": -0.850315169265626, - "angularVelocity": 0.7246595910245763, - "velocityX": 0.9850270681314717, - "velocityY": -0.17239965331882467, - "timestamp": 0.4460884909390394 - }, - { - "x": 1.2283160605903567, - "y": 4.289284209493019, - "heading": -0.7598572882997845, - "angularVelocity": 0.811120509075883, - "velocityX": 0.9850270681311126, - "velocityY": -0.17239965332087606, - "timestamp": 0.5576106136737993 - }, - { - "x": 1.3381683701795306, - "y": 4.270057834195912, - "heading": -0.6640307295459007, - "angularVelocity": 0.8592605341793411, - "velocityX": 0.9850270681310712, - "velocityY": -0.1723996533211125, - "timestamp": 0.6691327364085591 - }, - { - "x": 1.4480206797687292, - "y": 4.250831458898944, - "heading": -0.5655034744842501, - "angularVelocity": 0.8834772208917167, - "velocityX": 0.9850270681312915, - "velocityY": -0.17239965331985382, - "timestamp": 0.7806548591433189 - }, - { - "x": 1.5578729893579737, - "y": 4.231605083602241, - "heading": -0.46613854628859924, - "angularVelocity": 0.8909884941123012, - "velocityX": 0.9850270681317038, - "velocityY": -0.17239965331749818, - "timestamp": 0.8921769818780787 - }, - { - "x": 1.667725298947277, - "y": 4.212378708305875, - "heading": -0.3675558587925076, - "angularVelocity": 0.883974274149687, - "velocityX": 0.9850270681322336, - "velocityY": -0.17239965331447085, - "timestamp": 1.0036991046128385 - }, - { - "x": 1.7775776085366437, - "y": 4.193152333009867, - "heading": -0.2716202315057031, - "angularVelocity": 0.8602385332547363, - "velocityX": 0.9850270681327978, - "velocityY": -0.17239965331124718, - "timestamp": 1.1152212273475983 - }, - { - "x": 1.8874299181260659, - "y": 4.173925957714179, - "heading": -0.1810011200646351, - "angularVelocity": 0.8125662354597859, - "velocityX": 0.9850270681332973, - "velocityY": -0.17239965330839344, - "timestamp": 1.2267433500823581 - }, - { - "x": 1.997282227715524, - "y": 4.154699582418694, - "heading": -0.09996805663035663, - "angularVelocity": 0.7266097653736787, - "velocityX": 0.9850270681336178, - "velocityY": -0.1723996533065617, - "timestamp": 1.338265472817118 - }, - { - "x": 2.107134537304987, - "y": 4.135473207123238, - "heading": -0.035641662169536334, - "angularVelocity": 0.5768038922090091, - "velocityX": 0.9850270681336627, - "velocityY": -0.17239965330630566, - "timestamp": 1.4497875955518778 - }, - { - "x": 2.2169868468951135, - "y": 4.116246831831573, - "heading": -1.0063178889168783e-8, - "angularVelocity": 0.3195926622659994, - "velocityX": 0.98502706813961, - "velocityY": -0.17239965327231074, - "timestamp": 1.5613097182866376 - }, - { - "x": 2.295893669128418, - "y": 4.102436542510986, - "heading": 5.425975400842206e-33, - "angularVelocity": 9.023482195637066e-8, - "velocityX": 0.7075441203802566, - "velocityY": -0.12383452701517264, - "timestamp": 1.6728318410213974 - }, - { - "x": 2.295893669128418, - "y": 4.102436542510986, - "heading": 0, - "angularVelocity": -4.865380308863049e-32, - "velocityX": -1.6160842456388493e-31, - "velocityY": -5.471142235353396e-32, - "timestamp": 1.7843539637561572 - }, - { - "x": 2.302624119193887, - "y": 4.07385174410917, - "heading": -8.292160863866972e-17, - "angularVelocity": -1.2280480432765743e-15, - "velocityX": 0.09967594851534668, - "velocityY": -0.4233322981545981, - "timestamp": 1.8518772747022634 - }, - { - "x": 2.3169745348343085, - "y": 4.016906308376568, - "heading": -2.581621239218751e-16, - "angularVelocity": -2.5952671245371838e-15, - "velocityX": 0.2125253551597846, - "velocityY": -0.8433448380229092, - "timestamp": 1.9194005856483696 - }, - { - "x": 2.3400599620957876, - "y": 3.931923591100493, - "heading": -5.130317206898384e-16, - "angularVelocity": -3.7745534880874335e-15, - "velocityX": 0.3418882595946266, - "velocityY": -1.2585685755830793, - "timestamp": 1.9869238965944758 - }, - { - "x": 2.373310925877869, - "y": 3.8193899863733893, - "heading": -8.488460139622293e-16, - "angularVelocity": -4.973323452730244e-15, - "velocityX": 0.49243680909789883, - "velocityY": -1.6665889623947179, - "timestamp": 2.0544472075405817 - }, - { - "x": 2.418610139619234, - "y": 3.6800752476834298, - "heading": -1.2424296277796208e-15, - "angularVelocity": -5.8288725510901505e-15, - "velocityX": 0.6708677804242058, - "velocityY": -2.063209530723095, - "timestamp": 2.121970518486688 - }, - { - "x": 2.4784973016469114, - "y": 3.515273627351516, - "heading": -1.6835259705344942e-15, - "angularVelocity": -6.532524021843532e-15, - "velocityX": 0.8869109228874443, - "velocityY": -2.440662610035145, - "timestamp": 2.189493829432794 - }, - { - "x": 2.5564401370112204, - "y": 3.3273120867732886, - "heading": -2.1550705234989717e-15, - "angularVelocity": -6.983454085778219e-15, - "velocityX": 1.1543100341523413, - "velocityY": -2.7836540884119194, - "timestamp": 2.2570171403789003 - }, - { - "x": 2.656989300962068, - "y": 3.1206064007124743, - "heading": -2.6199509362820876e-15, - "angularVelocity": -6.884759808183529e-15, - "velocityX": 1.4891029859467424, - "velocityY": -3.06124926583891, - "timestamp": 2.3245404513250065 - }, - { - "x": 2.7849336812472782, - "y": 2.9033228587477327, - "heading": -3.031826948453853e-15, - "angularVelocity": -6.0997778236109756e-15, - "velocityX": 1.894817930171848, - "velocityY": -3.21790414184601, - "timestamp": 2.3920637622711127 - }, - { - "x": 2.9422124557465033, - "y": 2.687411238717251, - "heading": -3.3386928867831784e-15, - "angularVelocity": -4.544605165609959e-15, - "velocityX": 2.3292515176692206, - "velocityY": -3.19758638913203, - "timestamp": 2.459587073217219 - }, - { - "x": 3.1260700530650354, - "y": 2.4839878188804487, - "heading": -3.4936436292754485e-15, - "angularVelocity": -2.294780490604881e-15, - "velocityX": 2.7228759185902462, - "velocityY": -3.012639886678547, - "timestamp": 2.527110384163325 - }, - { - "x": 3.3319356441497803, - "y": 2.300007581710815, - "heading": -3.488531405132631e-15, - "angularVelocity": 7.57105074510855e-17, - "velocityX": 3.048807711000621, - "velocityY": -2.7246921780307343, - "timestamp": 2.5946336951094313 - }, - { - "x": 3.489358051205153, - "y": 2.1796001473934483, - "heading": -3.4769953550620332e-15, - "angularVelocity": 2.380018423477147e-16, - "velocityX": 3.2478034227066273, - "velocityY": -2.4841424077444927, - "timestamp": 2.643104118520757 - }, - { - "x": 3.6555081557643048, - "y": 2.0715541938896345, - "heading": -3.46066402017299e-15, - "angularVelocity": 3.3693402573872426e-16, - "velocityX": 3.427865755352637, - "velocityY": -2.229110989746537, - "timestamp": 2.6915745419320825 - }, - { - "x": 3.82941737519358, - "y": 1.9764995834405001, - "heading": -3.4311786789456212e-15, - "angularVelocity": 6.083161473048826e-16, - "velocityX": 3.5879451259057435, - "velocityY": -1.961084796857044, - "timestamp": 2.740044965343408 - }, - { - "x": 4.0100719081181415, - "y": 1.894990434653268, - "heading": -3.432767846995604e-15, - "angularVelocity": -3.278643117768733e-17, - "velocityX": 3.727108620271126, - "velocityY": -1.6816265064486144, - "timestamp": 2.7885153887547336 - }, - { - "x": 4.196418636291012, - "y": 1.8275018992234235, - "heading": -3.441661064576911e-15, - "angularVelocity": -1.8347727197080018e-16, - "velocityX": 3.8445450866293402, - "velocityY": -1.3923652957839638, - "timestamp": 2.836985812166059 - }, - { - "x": 4.387371261449498, - "y": 1.7744273935556105, - "heading": -3.4348652747795826e-15, - "angularVelocity": 1.402048779260617e-16, - "velocityX": 3.9395699834934264, - "velocityY": -1.0949874569375246, - "timestamp": 2.8854562355773847 - }, - { - "x": 4.581816637165551, - "y": 1.7360763051042505, - "heading": -3.4206405163814298e-15, - "angularVelocity": 2.9347295519771835e-16, - "velocityX": 4.011629402655687, - "velocityY": -0.7912266027871694, - "timestamp": 2.9339266589887103 - }, - { - "x": 4.778621257963462, - "y": 1.712672187082093, - "heading": -3.4076727329152914e-15, - "angularVelocity": 2.675401317654148e-16, - "velocityX": 4.060303313791993, - "velocityY": -0.48285359142801165, - "timestamp": 2.982397082400036 - }, - { - "x": 4.976637868001871, - "y": 1.7043514506655806, - "heading": -3.3948654352437934e-15, - "angularVelocity": 2.642291271700789e-16, - "velocityX": 4.085308031209358, - "velocityY": -0.17166626224691683, - "timestamp": 3.0308675058113614 - }, - { - "x": 5.174712152145305, - "y": 1.7111625566741109, - "heading": -3.3878134973774393e-15, - "angularVelocity": 1.4548950411589204e-16, - "velocityX": 4.086497913635991, - "velocityY": 0.14052086879247774, - "timestamp": 3.079337929222687 - }, - { - "x": 5.371689477427423, - "y": 1.733065683889262, - "heading": -3.384734641999914e-15, - "angularVelocity": 6.352029053732012e-17, - "velocityX": 4.063866403859248, - "velocityY": 0.45188644277494805, - "timestamp": 3.1278083526340126 - }, - { - "x": 5.566421709253515, - "y": 1.7699326309440095, - "heading": -3.400508079449476e-15, - "angularVelocity": -3.2542396660491567e-16, - "velocityX": 4.017547570682143, - "velocityY": 0.7606070766456821, - "timestamp": 3.176278776045338 - }, - { - "x": 5.758828163146973, - "y": 1.8174675703048704, - "heading": -3.3977669663588387e-15, - "angularVelocity": 5.65522827687079e-17, - "velocityX": 3.9695641249210003, - "velocityY": 0.98069989935919, - "timestamp": 3.2247491994566637 - }, - { - "x": 6.048920969073673, - "y": 1.9023244295720378, - "heading": -3.3698201318973824e-15, - "angularVelocity": 3.780728833463097e-16, - "velocityX": 3.924459628012348, - "velocityY": 1.1479682072433182, - "timestamp": 3.298668371374258 - }, - { - "x": 6.339013701554556, - "y": 1.9871815395759902, - "heading": -3.3816009084007496e-15, - "angularVelocity": -1.593737615707112e-16, - "velocityX": 3.9244586344160233, - "velocityY": 1.1479715992833515, - "timestamp": 3.3725875432918526 - }, - { - "x": 6.6291064335772285, - "y": 2.072038651443169, - "heading": 5.787203315330214e-15, - "angularVelocity": 1.2403824396933447e-13, - "velocityX": 3.9244586282172382, - "velocityY": 1.147971624489548, - "timestamp": 3.446506715209447 - }, - { - "x": 6.899328128367979, - "y": 2.1510831419772893, - "heading": 4.498352972326747e-15, - "angularVelocity": -1.7435941136055766e-14, - "velocityX": 3.6556374724001697, - "velocityY": 1.0693367969847223, - "timestamp": 3.5204258871270415 - }, - { - "x": 7.135772135820312, - "y": 2.2202470783807007, - "heading": 3.371603022772973e-15, - "angularVelocity": -1.5243000099038025e-14, - "velocityX": 3.1986831199342842, - "velocityY": 0.9356697945767513, - "timestamp": 3.594345059044636 - }, - { - "x": 7.33843843743126, - "y": 2.27953045522306, - "heading": 2.4076213307840505e-15, - "angularVelocity": -1.3041023946945337e-14, - "velocityX": 2.7417285171549155, - "velocityY": 0.8020027187054662, - "timestamp": 3.6682642309622304 - }, - { - "x": 7.507327026976948, - "y": 2.3289332706826067, - "heading": 1.6027502444282843e-15, - "angularVelocity": -1.0888529545859133e-14, - "velocityX": 2.28477383017715, - "velocityY": 0.6683356181888578, - "timestamp": 3.742183402879825 - }, - { - "x": 7.64243790134214, - "y": 2.3684555238481417, - "heading": 9.570473848243299e-16, - "angularVelocity": -8.735255614723888e-15, - "velocityX": 1.8278191010555744, - "velocityY": 0.5346685053452767, - "timestamp": 3.8161025747974193 - }, - { - "x": 7.743771058659156, - "y": 2.3980972141735055, - "heading": 4.76423082170293e-16, - "angularVelocity": -6.502025012522106e-15, - "velocityX": 1.3708643466674988, - "velocityY": 0.4010013851130894, - "timestamp": 3.8900217467150138 - }, - { - "x": 7.811326497684122, - "y": 2.4178583412949837, - "heading": 1.5485559519229808e-16, - "angularVelocity": -4.350258266558453e-15, - "velocityX": 0.9139095754518946, - "velocityY": 0.2673342599604742, - "timestamp": 3.9639409186326082 - }, - { - "x": 7.845104217529297, - "y": 2.427738904953003, - "heading": -3.2792970494417754e-32, - "angularVelocity": -2.094931425289687e-15, - "velocityX": 0.45695479222670565, - "velocityY": 0.13366713129626945, - "timestamp": 4.037860090550203 - }, - { - "x": 7.845104217529297, - "y": 2.427738904953003, - "heading": -7.22600529689662e-33, - "angularVelocity": 3.458773226203961e-31, - "velocityX": -4.2135134691610547e-32, - "velocityY": -6.511603074984667e-32, - "timestamp": 4.111779262467798 - }, - { - "x": 7.817147943760311, - "y": 2.416749060100633, - "heading": -2.232777809662192e-18, - "angularVelocity": -3.2694615280544566e-17, - "velocityX": -0.40936442573103865, - "velocityY": -0.16092457686070072, - "timestamp": 4.180071160863835 - }, - { - "x": 7.761224810662351, - "y": 2.3947963380623256, - "heading": -4.082249033262829e-18, - "angularVelocity": -2.7081836286500178e-17, - "velocityX": -0.8188838560854425, - "velocityY": -0.32145426549792994, - "timestamp": 4.248363059259872 - }, - { - "x": 7.677321253648209, - "y": 2.3619154081138825, - "heading": -4.719112125486337e-18, - "angularVelocity": -9.325571155135092e-18, - "velocityX": -1.2286019130343195, - "velocityY": -0.4814762910503355, - "timestamp": 4.316654957655909 - }, - { - "x": 7.565419266048648, - "y": 2.318152489662702, - "heading": -6.783249162229931e-18, - "angularVelocity": -3.022516313863855e-17, - "velocityX": -1.638583642097525, - "velocityY": -0.6408215246480392, - "timestamp": 4.384946856051946 - }, - { - "x": 7.425493790969877, - "y": 2.263572277079932, - "heading": -6.4023636552302655e-18, - "angularVelocity": 5.57737739582102e-18, - "velocityX": -2.0489322798919862, - "velocityY": -0.7992194369283216, - "timestamp": 4.453238754447983 - }, - { - "x": 7.257507576227683, - "y": 2.1982717816979753, - "heading": -2.8414113294330474e-18, - "angularVelocity": 5.2143205281390836e-17, - "velocityX": -2.459826402364604, - "velocityY": -0.9561968098079227, - "timestamp": 4.52153065284402 - }, - { - "x": 7.061399421871421, - "y": 2.122412599297568, - "heading": 5.218813123079696e-19, - "angularVelocity": 4.924888100254347e-17, - "velocityX": -2.8716166772666143, - "velocityY": -1.1108079315719974, - "timestamp": 4.589822551240057 - }, - { - "x": 6.837050243811277, - "y": 2.0363175235605016, - "heading": 6.519999461778699e-18, - "angularVelocity": 8.783074421973704e-17, - "velocityX": -3.285150703515771, - "velocityY": -1.260692377267432, - "timestamp": 4.658114449636094 - }, - { - "x": 6.584129054535847, - "y": 1.940950563311945, - "heading": 1.4190127621525814e-17, - "angularVelocity": 1.1231410257519964e-16, - "velocityX": -3.7035313882875425, - "velocityY": -1.3964608172906436, - "timestamp": 4.726406348032131 - }, - { - "x": 6.3139999030285665, - "y": 1.8702043234601993, - "heading": 6.229392328668449e-18, - "angularVelocity": -1.1656924876640592e-16, - "velocityX": -3.95550801561808, - "velocityY": -1.0359389841734894, - "timestamp": 4.794698246428168 - }, - { - "x": 6.03783435294095, - "y": 1.8288838996250503, - "heading": -1.2479954305012523e-18, - "angularVelocity": -1.0949157857360391e-16, - "velocityX": -4.043899153104409, - "velocityY": -0.60505601404499, - "timestamp": 4.862990144824205 - }, - { - "x": 5.758828163146973, - "y": 1.8174675703048704, - "heading": -2.1428347292812524e-17, - "angularVelocity": -2.9550140406523894e-16, - "velocityX": -4.085494712359195, - "velocityY": -0.16716959973635895, - "timestamp": 4.931282043220242 - }, - { - "x": 5.442944069689868, - "y": 1.8431974590025388, - "heading": -4.640746519007176e-17, - "angularVelocity": -3.222710704686718e-16, - "velocityX": -4.075416328196858, - "velocityY": 0.33195722954357726, - "timestamp": 5.008791693282637 - }, - { - "x": 5.132549947222792, - "y": 1.907230875263747, - "heading": -5.263653684696362e-17, - "angularVelocity": -8.036512420504643e-17, - "velocityX": -4.004586812315917, - "velocityY": 0.826134761408413, - "timestamp": 5.086301343345032 - }, - { - "x": 4.832272646006637, - "y": 2.008612989199975, - "heading": -7.111877179201443e-17, - "angularVelocity": -2.384507597463398e-16, - "velocityX": -3.874063435642526, - "velocityY": 1.3079934415206356, - "timestamp": 5.163810993407427 - }, - { - "x": 4.542195548692397, - "y": 2.1362847114522143, - "heading": -8.340242391355802e-17, - "angularVelocity": -1.5847900373250254e-16, - "velocityX": -3.7424642877403507, - "velocityY": 1.6471719605306414, - "timestamp": 5.241320643469822 - }, - { - "x": 4.252118851658615, - "y": 2.263957342643499, - "heading": -9.952202813328986e-17, - "angularVelocity": -2.0796899749604922e-16, - "velocityX": -3.7424591234780267, - "velocityY": 1.6471836873075834, - "timestamp": 5.318830293532217 - }, - { - "x": 3.9687873276430823, - "y": 2.3886611935333675, - "heading": -1.0866631170259556e-16, - "angularVelocity": -1.1797607203820848e-16, - "velocityX": -3.655435468842005, - "velocityY": 1.6088816139579067, - "timestamp": 5.396339943594612 - }, - { - "x": 3.720872219657192, - "y": 2.4977770738171987, - "heading": -1.097303256449214e-16, - "angularVelocity": -1.3727509447610907e-17, - "velocityX": -3.198506350969631, - "velocityY": 1.4077715509712598, - "timestamp": 5.4738495936570075 - }, - { - "x": 3.508373546229774, - "y": 2.591304975356774, - "heading": -1.1118600147966736e-16, - "angularVelocity": -1.878057682487085e-17, - "velocityX": -2.741576994044833, - "velocityY": 1.2066613829896125, - "timestamp": 5.551359243719403 - }, - { - "x": 3.331291313540666, - "y": 2.669244895433765, - "heading": -1.1134314124155818e-16, - "angularVelocity": -2.027358801996522e-18, - "velocityX": -2.284647557389988, - "velocityY": 1.00555117993741, - "timestamp": 5.628868893781798 - }, - { - "x": 3.1896255246796548, - "y": 2.7315968326883255, - "heading": -1.0469304235431019e-16, - "angularVelocity": 8.579703039997364e-17, - "velocityX": -1.8277180808718518, - "velocityY": 0.8044409593411052, - "timestamp": 5.706378543844193 - }, - { - "x": 3.0833761815004075, - "y": 2.7783607863044333, - "heading": -1.0419235160058842e-16, - "angularVelocity": 6.459716680279016e-18, - "velocityX": -1.3707885804383855, - "velocityY": 0.6033307282168234, - "timestamp": 5.783888193906588 - }, - { - "x": 3.012543285238584, - "y": 2.809536755738053, - "heading": -1.0632657875299128e-16, - "angularVelocity": -2.7534990990417765e-17, - "velocityX": -0.9138590640629015, - "velocityY": 0.4022204900736176, - "timestamp": 5.861397843968983 - }, - { - "x": 2.9771268367767334, - "y": 2.825124740600586, - "heading": -1.1064513996869437e-16, - "angularVelocity": -5.571643241065736e-17, - "velocityX": -0.4569295363010946, - "velocityY": 0.20111024691687301, - "timestamp": 5.938907494031378 - }, - { - "x": 2.9771268367767334, - "y": 2.825124740600586, - "heading": -1.1064513996869437e-16, - "angularVelocity": 2.2349585794705205e-31, - "velocityX": 3.085088305989537e-30, - "velocityY": -2.875018364649188e-29, - "timestamp": 6.016417144093773 - } - ], - "constraints": [ - { - "scope": [ - "last" - ], - "type": "StopPoint" - }, - { - "scope": [ - 1 - ], - "type": "StopPoint" - }, - { - "scope": [ - 0 - ], - "type": "StopPoint" - }, - { - "scope": [ - "first", - 1 - ], - "type": "MaxVelocity", - "velocity": 1 - }, - { - "scope": [ - 4 - ], - "type": "StopPoint" - } - ], - "usesControlIntervalGuessing": true, - "defaultControlIntervalCount": 40, - "usesDefaultFieldObstacles": true, - "circleObstacles": [] - } - }, - "splitTrajectoriesAtStopPoints": true, - "usesObstacles": false -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-1stN.1.traj b/src/main/deploy/choreo/3NB-1stN.1.traj deleted file mode 100644 index 8ca6df3..0000000 --- a/src/main/deploy/choreo/3NB-1stN.1.traj +++ /dev/null @@ -1,139 +0,0 @@ -{ - "samples": [ - { - "x": 0.71, - "y": 4.38, - "heading": -1.0303768814651526, - "angularVelocity": 0, - "velocityX": 2.0971536544825226e-38, - "velocityY": 1.1304744522998729e-38, - "timestamp": 0 - }, - { - "x": 0.809116701461151, - "y": 4.380000000002171, - "heading": -1.0303768755698945, - "angularVelocity": 4.436264736581951e-8, - "velocityX": 0.7458671604401304, - "velocityY": 1.6337868985579388e-11, - "timestamp": 0.1328878743001141 - }, - { - "x": 0.9420045739227598, - "y": 4.380000000000036, - "heading": -1.001743993813421, - "angularVelocity": 0.21546647432864371, - "velocityX": 0.9999999861649881, - "velocityY": -1.6060202539031698e-11, - "timestamp": 0.2657757486002282 - }, - { - "x": 1.0748924463843688, - "y": 4.380000000000185, - "heading": -0.9535209860370869, - "angularVelocity": 0.36288493611860506, - "velocityX": 0.9999999861649895, - "velocityY": 1.1165887293235233e-12, - "timestamp": 0.3986636229003423 - }, - { - "x": 1.2077803188459775, - "y": 4.380000000000181, - "heading": -0.894987542957539, - "angularVelocity": 0.440472416221783, - "velocityX": 0.9999999861649895, - "velocityY": -2.8685249700374206e-14, - "timestamp": 0.5315514972004564 - }, - { - "x": 1.3406681913075864, - "y": 4.3800000000000825, - "heading": -0.8311473610319392, - "angularVelocity": 0.48040637463598235, - "velocityX": 0.9999999861649895, - "velocityY": -7.450142676686956e-13, - "timestamp": 0.6644393715005704 - }, - { - "x": 1.4735560637691953, - "y": 4.379999999999938, - "heading": -0.7647977755611476, - "angularVelocity": 0.4992899903037624, - "velocityX": 0.9999999861649895, - "velocityY": -1.0891621656864383e-12, - "timestamp": 0.7973272458006845 - }, - { - "x": 1.6064439362308043, - "y": 4.379999999999788, - "heading": -0.6976974114003008, - "angularVelocity": 0.5049397058553835, - "velocityX": 0.9999999861649895, - "velocityY": -1.1281631993388294e-12, - "timestamp": 0.9302151201007985 - }, - { - "x": 1.7393318086924132, - "y": 4.379999999999669, - "heading": -0.6313021688387096, - "angularVelocity": 0.4996335663526692, - "velocityX": 0.9999999861649895, - "velocityY": -8.982783325849748e-13, - "timestamp": 1.0631029944009127 - }, - { - "x": 1.8722196811540222, - "y": 4.379999999999615, - "heading": -0.5673736940074112, - "angularVelocity": 0.4810707912064441, - "velocityX": 0.9999999861649895, - "velocityY": -4.0282423623910287e-13, - "timestamp": 1.1959908687010268 - }, - { - "x": 2.0051075536156313, - "y": 4.379999999999665, - "heading": -0.5087129398828393, - "angularVelocity": 0.44143044979478335, - "velocityX": 0.9999999861649895, - "velocityY": 3.753353729505819e-13, - "timestamp": 1.3288787430011408 - }, - { - "x": 2.1379954260772402, - "y": 4.379999999999856, - "heading": -0.4603202485132628, - "angularVelocity": 0.3641618290942509, - "velocityX": 0.9999999861649895, - "velocityY": 1.4392523553285558e-12, - "timestamp": 1.461766617301255 - }, - { - "x": 2.2708832985388487, - "y": 4.379999999998099, - "heading": -0.4314454065710539, - "angularVelocity": 0.21728725885853115, - "velocityX": 0.9999999861649881, - "velocityY": -1.3220835293967591e-11, - "timestamp": 1.594654491601369 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": 4.944810808316635e-8, - "velocityX": 0.7458671604401304, - "velocityY": 1.4301174702823493e-11, - "timestamp": 1.727542365901483 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": 0, - "velocityX": 4.042058444555445e-32, - "velocityY": -2.454352935526138e-31, - "timestamp": 1.860430240201597 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-1stN.2.traj b/src/main/deploy/choreo/3NB-1stN.2.traj deleted file mode 100644 index f1e65bf..0000000 --- a/src/main/deploy/choreo/3NB-1stN.2.traj +++ /dev/null @@ -1,328 +0,0 @@ -{ - "samples": [ - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": 0, - "velocityX": 4.042058444555445e-32, - "velocityY": -2.454352935526138e-31, - "timestamp": 0 - }, - { - "x": 2.3630236433782694, - "y": 4.362669397059245, - "heading": -0.4294233067682944, - "angularVelocity": 0.0350427544731118, - "velocityX": -0.1208998419972325, - "velocityY": -0.30033830993037086, - "timestamp": 0.057703604128190644 - }, - { - "x": 2.34987568315914, - "y": 4.327705672048482, - "heading": -0.4253632837544961, - "angularVelocity": 0.07035995541499669, - "velocityX": -0.22785336233062292, - "velocityY": -0.6059192582336417, - "timestamp": 0.11540720825638129 - }, - { - "x": 2.331570009798934, - "y": 4.274786157183859, - "heading": -0.41924975262264014, - "angularVelocity": 0.10594712798520795, - "velocityX": -0.3172362218423166, - "velocityY": -0.917092019885982, - "timestamp": 0.1731108123845717 - }, - { - "x": 2.3094077007864797, - "y": 4.2035872375384535, - "heading": -0.41106959562082307, - "angularVelocity": 0.1417616304094878, - "velocityX": -0.3840714864768336, - "velocityY": -1.2338730088200456, - "timestamp": 0.23081441651276235 - }, - { - "x": 2.2850867554506067, - "y": 4.113831614360468, - "heading": -0.4008169264246363, - "angularVelocity": 0.17767814248291539, - "velocityX": -0.42148052454380325, - "velocityY": -1.5554595684977943, - "timestamp": 0.288518020640953 - }, - { - "x": 2.260845942760846, - "y": 4.00539414959916, - "heading": -0.38850351266806354, - "angularVelocity": 0.21339072216498506, - "velocityX": -0.4200918305908423, - "velocityY": -1.8792147630924392, - "timestamp": 0.34622162476914364 - }, - { - "x": 2.2396150278583664, - "y": 3.8785182297474, - "heading": -0.37418142990770303, - "angularVelocity": 0.24820083557341727, - "velocityX": -0.3679304823911001, - "velocityY": -2.1987520843574346, - "timestamp": 0.4039252288973343 - }, - { - "x": 2.2250552117068545, - "y": 3.734190340531852, - "heading": -0.3579887048437084, - "angularVelocity": 0.2806189545434901, - "velocityX": -0.2523207409924592, - "velocityY": -2.501193667122479, - "timestamp": 0.46162883302552493 - }, - { - "x": 2.221244664883484, - "y": 3.57457876197761, - "heading": -0.34021831223188553, - "angularVelocity": 0.3079598385604548, - "velocityX": -0.06603654799218, - "velocityY": -2.7660590870483115, - "timestamp": 0.5193324371537156 - }, - { - "x": 2.2318839537226465, - "y": 3.4031176182672533, - "heading": -0.32135306694663757, - "angularVelocity": 0.3269335697506321, - "velocityX": 0.18437823772063575, - "velocityY": -2.971411340777662, - "timestamp": 0.5770360412819062 - }, - { - "x": 2.259517906960715, - "y": 3.223882144792273, - "heading": -0.3019785070706419, - "angularVelocity": 0.3357599610732849, - "velocityX": 0.4788947528626231, - "velocityY": -3.1061400094973273, - "timestamp": 0.6347396454100969 - }, - { - "x": 2.305422788003848, - "y": 3.0407166406448676, - "heading": -0.2826352454002691, - "angularVelocity": 0.3352175650466901, - "velocityX": 0.7955288363161972, - "velocityY": -3.174247205429028, - "timestamp": 0.6924432495382875 - }, - { - "x": 2.37, - "y": 2.8567728996276855, - "heading": -0.2637444281681159, - "angularVelocity": 0.3273767300585669, - "velocityX": 1.1191192122579394, - "velocityY": -3.187734003722659, - "timestamp": 0.7501468536664782 - }, - { - "x": 2.5101147898329845, - "y": 2.582541241565645, - "heading": -0.2369267963688336, - "angularVelocity": 0.3075125984694063, - "velocityX": 1.6066692028575509, - "velocityY": -3.144561398428411, - "timestamp": 0.8373550914192494 - }, - { - "x": 2.692445936304254, - "y": 2.3145698313008403, - "heading": -0.2138681460623352, - "angularVelocity": 0.2644090845158318, - "velocityX": 2.090756001606738, - "velocityY": -3.072776347400009, - "timestamp": 0.9245633291720206 - }, - { - "x": 2.916145350701991, - "y": 2.057000183979087, - "heading": -0.1978775533130541, - "angularVelocity": 0.18336103516336708, - "velocityX": 2.565117931081324, - "velocityY": -2.9535013429847994, - "timestamp": 1.0117715669247918 - }, - { - "x": 3.1783469526388295, - "y": 1.817614435910448, - "heading": -0.19483940838664568, - "angularVelocity": 0.0348378204206141, - "velocityX": 3.0066150709301063, - "velocityY": -2.744990086231003, - "timestamp": 1.098979804677563 - }, - { - "x": 3.4685657965160517, - "y": 1.6104254712653228, - "heading": -0.1948394011506557, - "angularVelocity": 8.297369784385518e-8, - "velocityX": 3.327883366933304, - "velocityY": -2.375795796181351, - "timestamp": 1.1861880424303342 - }, - { - "x": 3.7814630349981173, - "y": 1.4394004168841172, - "heading": -0.19483939812430692, - "angularVelocity": 3.4702556370809717e-8, - "velocityX": 3.5879321328443092, - "velocityY": -1.9611112297253483, - "timestamp": 1.2733962801831054 - }, - { - "x": 4.112554783061403, - "y": 1.3069900395824194, - "heading": -0.19483939551160215, - "angularVelocity": 2.995938037806074e-8, - "velocityX": 3.7965650561793693, - "velocityY": -1.518324194067512, - "timestamp": 1.3606045179358766 - }, - { - "x": 4.45709640896014, - "y": 1.2150916006881398, - "heading": -0.1948393931126124, - "angularVelocity": 2.7508751650457417e-8, - "velocityX": 3.950792204692831, - "velocityY": -1.0537816296109497, - "timestamp": 1.4478127556886478 - }, - { - "x": 4.810149751561953, - "y": 1.1650157280273563, - "heading": -0.1948393907882322, - "angularVelocity": 2.6653218265092305e-8, - "velocityX": 4.048394414336398, - "velocityY": -0.5742103492351371, - "timestamp": 1.535020993441419 - }, - { - "x": 5.163509032960075, - "y": 1.1171463873442062, - "heading": -0.19483938846606072, - "angularVelocity": 2.6627891462865042e-8, - "velocityX": 4.051902555350812, - "velocityY": -0.5489084737505261, - "timestamp": 1.6222292311941902 - }, - { - "x": 5.516868318784233, - "y": 1.0692770793329736, - "heading": -0.19483938614388904, - "angularVelocity": 2.6627893716137373e-8, - "velocityX": 4.0519026061033125, - "velocityY": -0.5489080991079575, - "timestamp": 1.7094374689469614 - }, - { - "x": 5.870227604608456, - "y": 1.0214077713222123, - "heading": -0.1948393838217175, - "angularVelocity": 2.6627892144106447e-8, - "velocityX": 4.051902606104045, - "velocityY": -0.5489080991025519, - "timestamp": 1.7966457066997326 - }, - { - "x": 6.223586890335042, - "y": 0.9735384625909697, - "heading": -0.1948393814995219, - "angularVelocity": 2.6628167647885842e-8, - "velocityX": 4.051902604984475, - "velocityY": -0.5489081073641706, - "timestamp": 1.8838539444525038 - }, - { - "x": 6.56185786613434, - "y": 0.9276752702257555, - "heading": -0.15388858338732217, - "angularVelocity": 0.46957488383510493, - "velocityX": 3.878887872476839, - "velocityY": -0.5259043588914193, - "timestamp": 1.971062182205275 - }, - { - "x": 6.857837819484865, - "y": 0.8875569901395891, - "heading": -0.11639807755802914, - "angularVelocity": 0.4298963812981445, - "velocityX": 3.393944895316064, - "velocityY": -0.4600285606054367, - "timestamp": 2.0582704199580464 - }, - { - "x": 7.1115317110378875, - "y": 0.8531747968258696, - "heading": -0.08363192967569993, - "angularVelocity": 0.37572308220727396, - "velocityX": 2.9090588009842437, - "velocityY": -0.39425396269720775, - "timestamp": 2.145478657710817 - }, - { - "x": 7.322941572891862, - "y": 0.8245255713030246, - "heading": -0.05600648536782893, - "angularVelocity": 0.31677562830923217, - "velocityX": 2.4241960083324776, - "velocityY": -0.3285151295476042, - "timestamp": 2.232686895463589 - }, - { - "x": 7.492068478804665, - "y": 0.8016076744881854, - "heading": -0.03372719469686328, - "angularVelocity": 0.2554723182702995, - "velocityX": 1.9393455282546543, - "velocityY": -0.26279509144433477, - "timestamp": 2.3198951332163604 - }, - { - "x": 7.618913088814633, - "y": 0.7844200882405312, - "heading": -0.016915967524029574, - "angularVelocity": 0.1927710914247968, - "velocityX": 1.4545026167088106, - "velocityY": -0.19708672816499287, - "timestamp": 2.407103370969132 - }, - { - "x": 7.703475852711533, - "y": 0.7729621267495633, - "heading": -0.005653945175052056, - "angularVelocity": 0.12913943268645117, - "velocityX": 0.9696648628152448, - "velocityY": -0.1313862289420392, - "timestamp": 2.4943116087219037 - }, - { - "x": 7.745757102966309, - "y": 0.7672333121299744, - "heading": 2.3206028060930825e-30, - "angularVelocity": 0.06483269609323732, - "velocityX": 0.4848309213017403, - "velocityY": -0.06569120953750239, - "timestamp": 2.5815198464746754 - }, - { - "x": 7.745757102966309, - "y": 0.7672333121299744, - "heading": -8.234811939268404e-31, - "angularVelocity": -3.633238130815655e-29, - "velocityX": -2.7472728518460047e-31, - "velocityY": -1.5587964590889528e-30, - "timestamp": 2.668728084227447 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-1stN.3.traj b/src/main/deploy/choreo/3NB-1stN.3.traj deleted file mode 100644 index 1768ce0..0000000 --- a/src/main/deploy/choreo/3NB-1stN.3.traj +++ /dev/null @@ -1,328 +0,0 @@ -{ - "samples": [ - { - "x": 7.745757102966309, - "y": 0.7672333121299744, - "heading": -8.234811939268404e-31, - "angularVelocity": -3.633238130815655e-29, - "velocityX": -2.7472728518460047e-31, - "velocityY": -1.5587964590889528e-30, - "timestamp": 0 - }, - { - "x": 7.70654223065801, - "y": 0.7719092516777752, - "heading": -0.004953190228808496, - "angularVelocity": -0.059037568964601335, - "velocityX": -0.467405979054445, - "velocityY": 0.05573299041131227, - "timestamp": 0.08389895308491813 - }, - { - "x": 7.628112492041216, - "y": 0.7812611877206381, - "heading": -0.014860324472800461, - "angularVelocity": -0.11808412238427937, - "velocityX": -0.9348118865966086, - "velocityY": 0.11146665958271756, - "timestamp": 0.16779790616983625 - }, - { - "x": 7.510467900615875, - "y": 0.7952892053137158, - "heading": -0.029723096869892895, - "angularVelocity": -0.1771508684029673, - "velocityX": -1.4022176332315919, - "velocityY": 0.1672013425349818, - "timestamp": 0.2516968592547544 - }, - { - "x": 7.353608476239863, - "y": 0.813993418196902, - "heading": -0.049543726815246006, - "angularVelocity": -0.2362440676141995, - "velocityX": -1.8696231431785302, - "velocityY": 0.2229373811643965, - "timestamp": 0.3355958123396725 - }, - { - "x": 7.157534243577833, - "y": 0.8373739693753024, - "heading": -0.07432438646100914, - "angularVelocity": -0.29536315692381165, - "velocityX": -2.3370283591450236, - "velocityY": 0.2786751242859491, - "timestamp": 0.41949476542459063 - }, - { - "x": 6.922245230336082, - "y": 0.8654310314879592, - "heading": -0.10406646621205466, - "angularVelocity": -0.3544988186079438, - "velocityX": -2.8044332448773686, - "velocityY": 0.33441492510948284, - "timestamp": 0.5033937185095088 - }, - { - "x": 6.647741466165584, - "y": 0.8981648066152749, - "heading": -0.13876967015547761, - "angularVelocity": -0.413630953276612, - "velocityX": -3.2718377771968092, - "velocityY": 0.3901571345495152, - "timestamp": 0.5872926715944269 - }, - { - "x": 6.334022988624143, - "y": 0.9355755244610184, - "heading": -0.1784308890646727, - "angularVelocity": -0.47272602876288267, - "velocityX": -3.739241861861053, - "velocityY": 0.44590208185170727, - "timestamp": 0.671191624679345 - }, - { - "x": 5.993381166833567, - "y": 0.9761991039310791, - "heading": -0.17843089111825428, - "angularVelocity": -2.4476843635546923e-8, - "velocityX": -4.0601438905417195, - "velocityY": 0.4841965003895071, - "timestamp": 0.7550905777642631 - }, - { - "x": 5.6527393449598975, - "y": 1.0168226827053695, - "heading": -0.17843089317177263, - "angularVelocity": -2.4476090712713582e-8, - "velocityX": -4.060143891532119, - "velocityY": 0.4841964920965512, - "timestamp": 0.8389895308491813 - }, - { - "x": 5.312097523086226, - "y": 1.0574462614796487, - "heading": -0.17843089522529096, - "angularVelocity": -2.447609001089921e-8, - "velocityX": -4.060143891532136, - "velocityY": 0.48419649209641696, - "timestamp": 0.9228884839340994 - }, - { - "x": 4.9714557012148255, - "y": 1.0980698402729543, - "heading": -0.17843089727880931, - "angularVelocity": -2.44760903452213e-8, - "velocityX": -4.060143891505091, - "velocityY": 0.48419649232319817, - "timestamp": 1.0067874370190175 - }, - { - "x": 4.63081401567638, - "y": 1.1386945622457554, - "heading": -0.1784308993323284, - "angularVelocity": -2.4476099139063654e-8, - "velocityX": -4.060142266538957, - "velocityY": 0.4842101179937616, - "timestamp": 1.0906863901039356 - }, - { - "x": 4.294909882891934, - "y": 1.2083762621040492, - "heading": -0.17843090142312937, - "angularVelocity": -2.4920465555084847e-8, - "velocityX": -4.003674902170267, - "velocityY": 0.8305431390516352, - "timestamp": 1.1745853431888538 - }, - { - "x": 3.9692448455861586, - "y": 1.3162159960510575, - "heading": -0.1784309036504149, - "angularVelocity": -2.654723854820396e-8, - "velocityX": -3.881634100680077, - "velocityY": 1.2853525578305942, - "timestamp": 1.258484296273772 - }, - { - "x": 3.6581383145057944, - "y": 1.4607837972243456, - "heading": -0.17843090615389368, - "angularVelocity": -2.983921383921478e-8, - "velocityX": -3.7081098111615183, - "velocityY": 1.723118058779164, - "timestamp": 1.34238324935869 - }, - { - "x": 3.3657165804558278, - "y": 1.6401623049726775, - "heading": -0.17843090915540483, - "angularVelocity": -3.5775311105492386e-8, - "velocityX": -3.4854038494853836, - "velocityY": 2.1380303466573016, - "timestamp": 1.4262822024436081 - }, - { - "x": 3.0958581558807583, - "y": 1.851972375004246, - "heading": -0.1784309902750725, - "angularVelocity": -9.668734194205462e-7, - "velocityX": -3.2164695106735524, - "velocityY": 2.524585376139148, - "timestamp": 1.5101811555285263 - }, - { - "x": 2.8575420142720582, - "y": 2.0873477427865708, - "heading": -0.1922018309233486, - "angularVelocity": -0.1641360248480786, - "velocityX": -2.8405138901732085, - "velocityY": 2.805462513269867, - "timestamp": 1.5940801086134444 - }, - { - "x": 2.6562685264660293, - "y": 2.336347374122755, - "heading": -0.21363610908830247, - "angularVelocity": -0.25547730188310785, - "velocityX": -2.398998800405901, - "velocityY": 2.96785147109238, - "timestamp": 1.6779790616983625 - }, - { - "x": 2.4935734358591812, - "y": 2.5938140264471015, - "heading": -0.23928053059906224, - "angularVelocity": -0.3056584208482637, - "velocityX": -1.9391790317356759, - "velocityY": 3.068770739770169, - "timestamp": 1.7618780147832807 - }, - { - "x": 2.37, - "y": 2.8567728996276855, - "heading": -0.2670876942616293, - "angularVelocity": -0.3314363605278995, - "velocityX": -1.4728841220951399, - "velocityY": 3.1342330686108957, - "timestamp": 1.8457769678681988 - }, - { - "x": 2.299791533091049, - "y": 3.0546011768493755, - "heading": -0.2882641191045993, - "angularVelocity": -0.33891320852752377, - "velocityX": -1.1236352199369757, - "velocityY": 3.1660970474403998, - "timestamp": 1.9082602948974898 - }, - { - "x": 2.2514737936228975, - "y": 3.2514391795997852, - "heading": -0.30953916781592244, - "angularVelocity": -0.34049161148783, - "velocityX": -0.7732901201868099, - "velocityY": 3.150248428002838, - "timestamp": 1.9707436219267809 - }, - { - "x": 2.2245033630694606, - "y": 3.4433305709133575, - "heading": -0.33040483151292044, - "angularVelocity": -0.3339397034222824, - "velocityX": -0.431642036935605, - "velocityY": 3.0710815258543787, - "timestamp": 2.033226948956072 - }, - { - "x": 2.217116881452816, - "y": 3.625393519094175, - "heading": -0.35022202500571886, - "angularVelocity": -0.3171597037960086, - "velocityX": -0.11821524185455658, - "velocityY": 2.913784473984096, - "timestamp": 2.095710275985363 - }, - { - "x": 2.225788569605819, - "y": 3.7925510322181357, - "heading": -0.36834815899940476, - "angularVelocity": -0.29009553196789717, - "velocityX": 0.13878403352206445, - "velocityY": 2.6752338755201226, - "timestamp": 2.158193603014654 - }, - { - "x": 2.2455610808640105, - "y": 3.940821796699816, - "heading": -0.38433795999185266, - "angularVelocity": -0.2559050830464319, - "velocityX": 0.31644459727508745, - "velocityY": 2.372965261791757, - "timestamp": 2.220676930043945 - }, - { - "x": 2.271296811862093, - "y": 4.068013754579051, - "heading": -0.39799791167793525, - "angularVelocity": -0.21861754704065436, - "velocityX": 0.41188157259965286, - "velocityY": 2.0356143618858633, - "timestamp": 2.283160257073236 - }, - { - "x": 2.298666674813771, - "y": 4.1733611816283265, - "heading": -0.40928906927102027, - "angularVelocity": -0.18070672817713285, - "velocityX": 0.4380346606519097, - "velocityY": 1.6860086051418306, - "timestamp": 2.345643584102527 - }, - { - "x": 2.3243703381234817, - "y": 4.2568665137587525, - "heading": -0.41823553880068076, - "angularVelocity": -0.14318170870553068, - "velocityX": 0.41136835267529415, - "velocityY": 1.3364418333755004, - "timestamp": 2.408126911131818 - }, - { - "x": 2.345964878201746, - "y": 4.318854956416003, - "heading": -0.42488052820760597, - "angularVelocity": -0.10634820075778077, - "velocityX": 0.3456048374015221, - "velocityY": 0.992079737178395, - "timestamp": 2.4706102381611093 - }, - { - "x": 2.3616333943908905, - "y": 4.359754200754428, - "heading": -0.42926967074734346, - "angularVelocity": -0.0702450197262999, - "velocityX": 0.25076315449399394, - "velocityY": 0.6545625254438318, - "timestamp": 2.5330935651904003 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": -0.03482095714327977, - "velocityX": 0.1339014102944245, - "velocityY": 0.32401922573810255, - "timestamp": 2.5955768922196913 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": -1.1758721771275293e-31, - "velocityX": 1.7010814766774368e-33, - "velocityY": -2.1446217942541315e-33, - "timestamp": 2.6580602192489824 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-1stN.traj b/src/main/deploy/choreo/3NB-1stN.traj deleted file mode 100644 index 606374b..0000000 --- a/src/main/deploy/choreo/3NB-1stN.traj +++ /dev/null @@ -1,769 +0,0 @@ -{ - "samples": [ - { - "x": 0.71, - "y": 4.38, - "heading": -1.0303768814651526, - "angularVelocity": 0, - "velocityX": 2.0971536544825226e-38, - "velocityY": 1.1304744522998729e-38, - "timestamp": 0 - }, - { - "x": 0.809116701461151, - "y": 4.380000000002171, - "heading": -1.0303768755698945, - "angularVelocity": 4.436264736581951e-8, - "velocityX": 0.7458671604401304, - "velocityY": 1.6337868985579388e-11, - "timestamp": 0.1328878743001141 - }, - { - "x": 0.9420045739227598, - "y": 4.380000000000036, - "heading": -1.001743993813421, - "angularVelocity": 0.21546647432864371, - "velocityX": 0.9999999861649881, - "velocityY": -1.6060202539031698e-11, - "timestamp": 0.2657757486002282 - }, - { - "x": 1.0748924463843688, - "y": 4.380000000000185, - "heading": -0.9535209860370869, - "angularVelocity": 0.36288493611860506, - "velocityX": 0.9999999861649895, - "velocityY": 1.1165887293235233e-12, - "timestamp": 0.3986636229003423 - }, - { - "x": 1.2077803188459775, - "y": 4.380000000000181, - "heading": -0.894987542957539, - "angularVelocity": 0.440472416221783, - "velocityX": 0.9999999861649895, - "velocityY": -2.8685249700374206e-14, - "timestamp": 0.5315514972004564 - }, - { - "x": 1.3406681913075864, - "y": 4.3800000000000825, - "heading": -0.8311473610319392, - "angularVelocity": 0.48040637463598235, - "velocityX": 0.9999999861649895, - "velocityY": -7.450142676686956e-13, - "timestamp": 0.6644393715005704 - }, - { - "x": 1.4735560637691953, - "y": 4.379999999999938, - "heading": -0.7647977755611476, - "angularVelocity": 0.4992899903037624, - "velocityX": 0.9999999861649895, - "velocityY": -1.0891621656864383e-12, - "timestamp": 0.7973272458006845 - }, - { - "x": 1.6064439362308043, - "y": 4.379999999999788, - "heading": -0.6976974114003008, - "angularVelocity": 0.5049397058553835, - "velocityX": 0.9999999861649895, - "velocityY": -1.1281631993388294e-12, - "timestamp": 0.9302151201007985 - }, - { - "x": 1.7393318086924132, - "y": 4.379999999999669, - "heading": -0.6313021688387096, - "angularVelocity": 0.4996335663526692, - "velocityX": 0.9999999861649895, - "velocityY": -8.982783325849748e-13, - "timestamp": 1.0631029944009127 - }, - { - "x": 1.8722196811540222, - "y": 4.379999999999615, - "heading": -0.5673736940074112, - "angularVelocity": 0.4810707912064441, - "velocityX": 0.9999999861649895, - "velocityY": -4.0282423623910287e-13, - "timestamp": 1.1959908687010268 - }, - { - "x": 2.0051075536156313, - "y": 4.379999999999665, - "heading": -0.5087129398828393, - "angularVelocity": 0.44143044979478335, - "velocityX": 0.9999999861649895, - "velocityY": 3.753353729505819e-13, - "timestamp": 1.3288787430011408 - }, - { - "x": 2.1379954260772402, - "y": 4.379999999999856, - "heading": -0.4603202485132628, - "angularVelocity": 0.3641618290942509, - "velocityX": 0.9999999861649895, - "velocityY": 1.4392523553285558e-12, - "timestamp": 1.461766617301255 - }, - { - "x": 2.2708832985388487, - "y": 4.379999999998099, - "heading": -0.4314454065710539, - "angularVelocity": 0.21728725885853115, - "velocityX": 0.9999999861649881, - "velocityY": -1.3220835293967591e-11, - "timestamp": 1.594654491601369 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": 4.944810808316635e-8, - "velocityX": 0.7458671604401304, - "velocityY": 1.4301174702823493e-11, - "timestamp": 1.727542365901483 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": 0, - "velocityX": 4.042058444555445e-32, - "velocityY": -2.454352935526138e-31, - "timestamp": 1.860430240201597 - }, - { - "x": 2.3630236433782694, - "y": 4.362669397059245, - "heading": -0.4294233067682944, - "angularVelocity": 0.0350427544731118, - "velocityX": -0.1208998419972325, - "velocityY": -0.30033830993037086, - "timestamp": 1.9181338443297877 - }, - { - "x": 2.34987568315914, - "y": 4.327705672048482, - "heading": -0.4253632837544961, - "angularVelocity": 0.07035995541499669, - "velocityX": -0.22785336233062292, - "velocityY": -0.6059192582336417, - "timestamp": 1.9758374484579784 - }, - { - "x": 2.331570009798934, - "y": 4.274786157183859, - "heading": -0.41924975262264014, - "angularVelocity": 0.10594712798520795, - "velocityX": -0.3172362218423166, - "velocityY": -0.917092019885982, - "timestamp": 2.033541052586169 - }, - { - "x": 2.3094077007864797, - "y": 4.2035872375384535, - "heading": -0.41106959562082307, - "angularVelocity": 0.1417616304094878, - "velocityX": -0.3840714864768336, - "velocityY": -1.2338730088200456, - "timestamp": 2.0912446567143594 - }, - { - "x": 2.2850867554506067, - "y": 4.113831614360468, - "heading": -0.4008169264246363, - "angularVelocity": 0.17767814248291539, - "velocityX": -0.42148052454380325, - "velocityY": -1.5554595684977943, - "timestamp": 2.14894826084255 - }, - { - "x": 2.260845942760846, - "y": 4.00539414959916, - "heading": -0.38850351266806354, - "angularVelocity": 0.21339072216498506, - "velocityX": -0.4200918305908423, - "velocityY": -1.8792147630924392, - "timestamp": 2.2066518649707407 - }, - { - "x": 2.2396150278583664, - "y": 3.8785182297474, - "heading": -0.37418142990770303, - "angularVelocity": 0.24820083557341727, - "velocityX": -0.3679304823911001, - "velocityY": -2.1987520843574346, - "timestamp": 2.2643554690989314 - }, - { - "x": 2.2250552117068545, - "y": 3.734190340531852, - "heading": -0.3579887048437084, - "angularVelocity": 0.2806189545434901, - "velocityX": -0.2523207409924592, - "velocityY": -2.501193667122479, - "timestamp": 2.322059073227122 - }, - { - "x": 2.221244664883484, - "y": 3.57457876197761, - "heading": -0.34021831223188553, - "angularVelocity": 0.3079598385604548, - "velocityX": -0.06603654799218, - "velocityY": -2.7660590870483115, - "timestamp": 2.3797626773553127 - }, - { - "x": 2.2318839537226465, - "y": 3.4031176182672533, - "heading": -0.32135306694663757, - "angularVelocity": 0.3269335697506321, - "velocityX": 0.18437823772063575, - "velocityY": -2.971411340777662, - "timestamp": 2.4374662814835033 - }, - { - "x": 2.259517906960715, - "y": 3.223882144792273, - "heading": -0.3019785070706419, - "angularVelocity": 0.3357599610732849, - "velocityX": 0.4788947528626231, - "velocityY": -3.1061400094973273, - "timestamp": 2.495169885611694 - }, - { - "x": 2.305422788003848, - "y": 3.0407166406448676, - "heading": -0.2826352454002691, - "angularVelocity": 0.3352175650466901, - "velocityX": 0.7955288363161972, - "velocityY": -3.174247205429028, - "timestamp": 2.5528734897398846 - }, - { - "x": 2.37, - "y": 2.8567728996276855, - "heading": -0.2637444281681159, - "angularVelocity": 0.3273767300585669, - "velocityX": 1.1191192122579394, - "velocityY": -3.187734003722659, - "timestamp": 2.6105770938680752 - }, - { - "x": 2.5101147898329845, - "y": 2.582541241565645, - "heading": -0.2369267963688336, - "angularVelocity": 0.3075125984694063, - "velocityX": 1.6066692028575509, - "velocityY": -3.144561398428411, - "timestamp": 2.6977853316208464 - }, - { - "x": 2.692445936304254, - "y": 2.3145698313008403, - "heading": -0.2138681460623352, - "angularVelocity": 0.2644090845158318, - "velocityX": 2.090756001606738, - "velocityY": -3.072776347400009, - "timestamp": 2.7849935693736176 - }, - { - "x": 2.916145350701991, - "y": 2.057000183979087, - "heading": -0.1978775533130541, - "angularVelocity": 0.18336103516336708, - "velocityX": 2.565117931081324, - "velocityY": -2.9535013429847994, - "timestamp": 2.872201807126389 - }, - { - "x": 3.1783469526388295, - "y": 1.817614435910448, - "heading": -0.19483940838664568, - "angularVelocity": 0.0348378204206141, - "velocityX": 3.0066150709301063, - "velocityY": -2.744990086231003, - "timestamp": 2.95941004487916 - }, - { - "x": 3.4685657965160517, - "y": 1.6104254712653228, - "heading": -0.1948394011506557, - "angularVelocity": 8.297369784385518e-8, - "velocityX": 3.327883366933304, - "velocityY": -2.375795796181351, - "timestamp": 3.0466182826319312 - }, - { - "x": 3.7814630349981173, - "y": 1.4394004168841172, - "heading": -0.19483939812430692, - "angularVelocity": 3.4702556370809717e-8, - "velocityX": 3.5879321328443092, - "velocityY": -1.9611112297253483, - "timestamp": 3.1338265203847024 - }, - { - "x": 4.112554783061403, - "y": 1.3069900395824194, - "heading": -0.19483939551160215, - "angularVelocity": 2.995938037806074e-8, - "velocityX": 3.7965650561793693, - "velocityY": -1.518324194067512, - "timestamp": 3.2210347581374736 - }, - { - "x": 4.45709640896014, - "y": 1.2150916006881398, - "heading": -0.1948393931126124, - "angularVelocity": 2.7508751650457417e-8, - "velocityX": 3.950792204692831, - "velocityY": -1.0537816296109497, - "timestamp": 3.308242995890245 - }, - { - "x": 4.810149751561953, - "y": 1.1650157280273563, - "heading": -0.1948393907882322, - "angularVelocity": 2.6653218265092305e-8, - "velocityX": 4.048394414336398, - "velocityY": -0.5742103492351371, - "timestamp": 3.395451233643016 - }, - { - "x": 5.163509032960075, - "y": 1.1171463873442062, - "heading": -0.19483938846606072, - "angularVelocity": 2.6627891462865042e-8, - "velocityX": 4.051902555350812, - "velocityY": -0.5489084737505261, - "timestamp": 3.4826594713957872 - }, - { - "x": 5.516868318784233, - "y": 1.0692770793329736, - "heading": -0.19483938614388904, - "angularVelocity": 2.6627893716137373e-8, - "velocityX": 4.0519026061033125, - "velocityY": -0.5489080991079575, - "timestamp": 3.5698677091485584 - }, - { - "x": 5.870227604608456, - "y": 1.0214077713222123, - "heading": -0.1948393838217175, - "angularVelocity": 2.6627892144106447e-8, - "velocityX": 4.051902606104045, - "velocityY": -0.5489080991025519, - "timestamp": 3.6570759469013296 - }, - { - "x": 6.223586890335042, - "y": 0.9735384625909697, - "heading": -0.1948393814995219, - "angularVelocity": 2.6628167647885842e-8, - "velocityX": 4.051902604984475, - "velocityY": -0.5489081073641706, - "timestamp": 3.744284184654101 - }, - { - "x": 6.56185786613434, - "y": 0.9276752702257555, - "heading": -0.15388858338732217, - "angularVelocity": 0.46957488383510493, - "velocityX": 3.878887872476839, - "velocityY": -0.5259043588914193, - "timestamp": 3.831492422406872 - }, - { - "x": 6.857837819484865, - "y": 0.8875569901395891, - "heading": -0.11639807755802914, - "angularVelocity": 0.4298963812981445, - "velocityX": 3.393944895316064, - "velocityY": -0.4600285606054367, - "timestamp": 3.9187006601596432 - }, - { - "x": 7.1115317110378875, - "y": 0.8531747968258696, - "heading": -0.08363192967569993, - "angularVelocity": 0.37572308220727396, - "velocityX": 2.9090588009842437, - "velocityY": -0.39425396269720775, - "timestamp": 4.0059088979124144 - }, - { - "x": 7.322941572891862, - "y": 0.8245255713030246, - "heading": -0.05600648536782893, - "angularVelocity": 0.31677562830923217, - "velocityX": 2.4241960083324776, - "velocityY": -0.3285151295476042, - "timestamp": 4.093117135665186 - }, - { - "x": 7.492068478804665, - "y": 0.8016076744881854, - "heading": -0.03372719469686328, - "angularVelocity": 0.2554723182702995, - "velocityX": 1.9393455282546543, - "velocityY": -0.26279509144433477, - "timestamp": 4.180325373417958 - }, - { - "x": 7.618913088814633, - "y": 0.7844200882405312, - "heading": -0.016915967524029574, - "angularVelocity": 0.1927710914247968, - "velocityX": 1.4545026167088106, - "velocityY": -0.19708672816499287, - "timestamp": 4.267533611170729 - }, - { - "x": 7.703475852711533, - "y": 0.7729621267495633, - "heading": -0.005653945175052056, - "angularVelocity": 0.12913943268645117, - "velocityX": 0.9696648628152448, - "velocityY": -0.1313862289420392, - "timestamp": 4.354741848923501 - }, - { - "x": 7.745757102966309, - "y": 0.7672333121299744, - "heading": 2.3206028060930825e-30, - "angularVelocity": 0.06483269609323732, - "velocityX": 0.4848309213017403, - "velocityY": -0.06569120953750239, - "timestamp": 4.441950086676273 - }, - { - "x": 7.745757102966309, - "y": 0.7672333121299744, - "heading": -8.234811939268404e-31, - "angularVelocity": -3.633238130815655e-29, - "velocityX": -2.7472728518460047e-31, - "velocityY": -1.5587964590889528e-30, - "timestamp": 4.529158324429044 - }, - { - "x": 7.70654223065801, - "y": 0.7719092516777752, - "heading": -0.004953190228808496, - "angularVelocity": -0.059037568964601335, - "velocityX": -0.467405979054445, - "velocityY": 0.05573299041131227, - "timestamp": 4.613057277513962 - }, - { - "x": 7.628112492041216, - "y": 0.7812611877206381, - "heading": -0.014860324472800461, - "angularVelocity": -0.11808412238427937, - "velocityX": -0.9348118865966086, - "velocityY": 0.11146665958271756, - "timestamp": 4.696956230598881 - }, - { - "x": 7.510467900615875, - "y": 0.7952892053137158, - "heading": -0.029723096869892895, - "angularVelocity": -0.1771508684029673, - "velocityX": -1.4022176332315919, - "velocityY": 0.1672013425349818, - "timestamp": 4.780855183683799 - }, - { - "x": 7.353608476239863, - "y": 0.813993418196902, - "heading": -0.049543726815246006, - "angularVelocity": -0.2362440676141995, - "velocityX": -1.8696231431785302, - "velocityY": 0.2229373811643965, - "timestamp": 4.864754136768717 - }, - { - "x": 7.157534243577833, - "y": 0.8373739693753024, - "heading": -0.07432438646100914, - "angularVelocity": -0.29536315692381165, - "velocityX": -2.3370283591450236, - "velocityY": 0.2786751242859491, - "timestamp": 4.948653089853635 - }, - { - "x": 6.922245230336082, - "y": 0.8654310314879592, - "heading": -0.10406646621205466, - "angularVelocity": -0.3544988186079438, - "velocityX": -2.8044332448773686, - "velocityY": 0.33441492510948284, - "timestamp": 5.032552042938553 - }, - { - "x": 6.647741466165584, - "y": 0.8981648066152749, - "heading": -0.13876967015547761, - "angularVelocity": -0.413630953276612, - "velocityX": -3.2718377771968092, - "velocityY": 0.3901571345495152, - "timestamp": 5.116450996023471 - }, - { - "x": 6.334022988624143, - "y": 0.9355755244610184, - "heading": -0.1784308890646727, - "angularVelocity": -0.47272602876288267, - "velocityX": -3.739241861861053, - "velocityY": 0.44590208185170727, - "timestamp": 5.200349949108389 - }, - { - "x": 5.993381166833567, - "y": 0.9761991039310791, - "heading": -0.17843089111825428, - "angularVelocity": -2.4476843635546923e-8, - "velocityX": -4.0601438905417195, - "velocityY": 0.4841965003895071, - "timestamp": 5.2842489021933075 - }, - { - "x": 5.6527393449598975, - "y": 1.0168226827053695, - "heading": -0.17843089317177263, - "angularVelocity": -2.4476090712713582e-8, - "velocityX": -4.060143891532119, - "velocityY": 0.4841964920965512, - "timestamp": 5.368147855278226 - }, - { - "x": 5.312097523086226, - "y": 1.0574462614796487, - "heading": -0.17843089522529096, - "angularVelocity": -2.447609001089921e-8, - "velocityX": -4.060143891532136, - "velocityY": 0.48419649209641696, - "timestamp": 5.452046808363144 - }, - { - "x": 4.9714557012148255, - "y": 1.0980698402729543, - "heading": -0.17843089727880931, - "angularVelocity": -2.44760903452213e-8, - "velocityX": -4.060143891505091, - "velocityY": 0.48419649232319817, - "timestamp": 5.535945761448062 - }, - { - "x": 4.63081401567638, - "y": 1.1386945622457554, - "heading": -0.1784308993323284, - "angularVelocity": -2.4476099139063654e-8, - "velocityX": -4.060142266538957, - "velocityY": 0.4842101179937616, - "timestamp": 5.61984471453298 - }, - { - "x": 4.294909882891934, - "y": 1.2083762621040492, - "heading": -0.17843090142312937, - "angularVelocity": -2.4920465555084847e-8, - "velocityX": -4.003674902170267, - "velocityY": 0.8305431390516352, - "timestamp": 5.703743667617898 - }, - { - "x": 3.9692448455861586, - "y": 1.3162159960510575, - "heading": -0.1784309036504149, - "angularVelocity": -2.654723854820396e-8, - "velocityX": -3.881634100680077, - "velocityY": 1.2853525578305942, - "timestamp": 5.787642620702816 - }, - { - "x": 3.6581383145057944, - "y": 1.4607837972243456, - "heading": -0.17843090615389368, - "angularVelocity": -2.983921383921478e-8, - "velocityX": -3.7081098111615183, - "velocityY": 1.723118058779164, - "timestamp": 5.871541573787734 - }, - { - "x": 3.3657165804558278, - "y": 1.6401623049726775, - "heading": -0.17843090915540483, - "angularVelocity": -3.5775311105492386e-8, - "velocityX": -3.4854038494853836, - "velocityY": 2.1380303466573016, - "timestamp": 5.9554405268726525 - }, - { - "x": 3.0958581558807583, - "y": 1.851972375004246, - "heading": -0.1784309902750725, - "angularVelocity": -9.668734194205462e-7, - "velocityX": -3.2164695106735524, - "velocityY": 2.524585376139148, - "timestamp": 6.039339479957571 - }, - { - "x": 2.8575420142720582, - "y": 2.0873477427865708, - "heading": -0.1922018309233486, - "angularVelocity": -0.1641360248480786, - "velocityX": -2.8405138901732085, - "velocityY": 2.805462513269867, - "timestamp": 6.123238433042489 - }, - { - "x": 2.6562685264660293, - "y": 2.336347374122755, - "heading": -0.21363610908830247, - "angularVelocity": -0.25547730188310785, - "velocityX": -2.398998800405901, - "velocityY": 2.96785147109238, - "timestamp": 6.207137386127407 - }, - { - "x": 2.4935734358591812, - "y": 2.5938140264471015, - "heading": -0.23928053059906224, - "angularVelocity": -0.3056584208482637, - "velocityX": -1.9391790317356759, - "velocityY": 3.068770739770169, - "timestamp": 6.291036339212325 - }, - { - "x": 2.37, - "y": 2.8567728996276855, - "heading": -0.2670876942616293, - "angularVelocity": -0.3314363605278995, - "velocityX": -1.4728841220951399, - "velocityY": 3.1342330686108957, - "timestamp": 6.374935292297243 - }, - { - "x": 2.299791533091049, - "y": 3.0546011768493755, - "heading": -0.2882641191045993, - "angularVelocity": -0.33891320852752377, - "velocityX": -1.1236352199369757, - "velocityY": 3.1660970474403998, - "timestamp": 6.437418619326534 - }, - { - "x": 2.2514737936228975, - "y": 3.2514391795997852, - "heading": -0.30953916781592244, - "angularVelocity": -0.34049161148783, - "velocityX": -0.7732901201868099, - "velocityY": 3.150248428002838, - "timestamp": 6.499901946355825 - }, - { - "x": 2.2245033630694606, - "y": 3.4433305709133575, - "heading": -0.33040483151292044, - "angularVelocity": -0.3339397034222824, - "velocityX": -0.431642036935605, - "velocityY": 3.0710815258543787, - "timestamp": 6.562385273385116 - }, - { - "x": 2.217116881452816, - "y": 3.625393519094175, - "heading": -0.35022202500571886, - "angularVelocity": -0.3171597037960086, - "velocityX": -0.11821524185455658, - "velocityY": 2.913784473984096, - "timestamp": 6.624868600414407 - }, - { - "x": 2.225788569605819, - "y": 3.7925510322181357, - "heading": -0.36834815899940476, - "angularVelocity": -0.29009553196789717, - "velocityX": 0.13878403352206445, - "velocityY": 2.6752338755201226, - "timestamp": 6.687351927443698 - }, - { - "x": 2.2455610808640105, - "y": 3.940821796699816, - "heading": -0.38433795999185266, - "angularVelocity": -0.2559050830464319, - "velocityX": 0.31644459727508745, - "velocityY": 2.372965261791757, - "timestamp": 6.749835254472989 - }, - { - "x": 2.271296811862093, - "y": 4.068013754579051, - "heading": -0.39799791167793525, - "angularVelocity": -0.21861754704065436, - "velocityX": 0.41188157259965286, - "velocityY": 2.0356143618858633, - "timestamp": 6.81231858150228 - }, - { - "x": 2.298666674813771, - "y": 4.1733611816283265, - "heading": -0.40928906927102027, - "angularVelocity": -0.18070672817713285, - "velocityX": 0.4380346606519097, - "velocityY": 1.6860086051418306, - "timestamp": 6.8748019085315715 - }, - { - "x": 2.3243703381234817, - "y": 4.2568665137587525, - "heading": -0.41823553880068076, - "angularVelocity": -0.14318170870553068, - "velocityX": 0.41136835267529415, - "velocityY": 1.3364418333755004, - "timestamp": 6.9372852355608625 - }, - { - "x": 2.345964878201746, - "y": 4.318854956416003, - "heading": -0.42488052820760597, - "angularVelocity": -0.10634820075778077, - "velocityX": 0.3456048374015221, - "velocityY": 0.992079737178395, - "timestamp": 6.999768562590154 - }, - { - "x": 2.3616333943908905, - "y": 4.359754200754428, - "heading": -0.42926967074734346, - "angularVelocity": -0.0702450197262999, - "velocityX": 0.25076315449399394, - "velocityY": 0.6545625254438318, - "timestamp": 7.062251889619445 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": -0.03482095714327977, - "velocityX": 0.1339014102944245, - "velocityY": 0.32401922573810255, - "timestamp": 7.124735216648736 - }, - { - "x": 2.37, - "y": 4.38, - "heading": -0.4314454, - "angularVelocity": -1.1758721771275293e-31, - "velocityX": 1.7010814766774368e-33, - "velocityY": -2.1446217942541315e-33, - "timestamp": 7.187218543678027 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-2ndN.1.traj b/src/main/deploy/choreo/3NB-2ndN.1.traj deleted file mode 100644 index e51ddb7..0000000 --- a/src/main/deploy/choreo/3NB-2ndN.1.traj +++ /dev/null @@ -1,157 +0,0 @@ -{ - "samples": [ - { - "x": 0.71, - "y": 4.38, - "heading": -1.0303768814651526, - "angularVelocity": 4.324130717118377e-34, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 0.7889068222351219, - "y": 4.366189710689795, - "heading": -1.0303768723653812, - "angularVelocity": 8.159611106479039e-8, - "velocityX": 0.7075441203965512, - "velocityY": -0.12383452692207302, - "timestamp": 0.11152212273475985 - }, - { - "x": 0.898759131822665, - "y": 4.346963335383371, - "heading": -0.9951630127982624, - "angularVelocity": 0.3157567189683987, - "velocityX": 0.985027068116447, - "velocityY": -0.17239965340465557, - "timestamp": 0.2230442454695197 - }, - { - "x": 1.0086114414119594, - "y": 4.327736960086952, - "heading": -0.9311307451167896, - "angularVelocity": 0.5741665071580884, - "velocityX": 0.9850270681321506, - "velocityY": -0.17239965331494583, - "timestamp": 0.33456636820427954 - }, - { - "x": 1.118463751001178, - "y": 4.3085105847901, - "heading": -0.850315169265626, - "angularVelocity": 0.7246595910245763, - "velocityX": 0.9850270681314717, - "velocityY": -0.17239965331882467, - "timestamp": 0.4460884909390394 - }, - { - "x": 1.2283160605903567, - "y": 4.289284209493019, - "heading": -0.7598572882997845, - "angularVelocity": 0.811120509075883, - "velocityX": 0.9850270681311126, - "velocityY": -0.17239965332087606, - "timestamp": 0.5576106136737993 - }, - { - "x": 1.3381683701795306, - "y": 4.270057834195912, - "heading": -0.6640307295459007, - "angularVelocity": 0.8592605341793411, - "velocityX": 0.9850270681310712, - "velocityY": -0.1723996533211125, - "timestamp": 0.6691327364085591 - }, - { - "x": 1.4480206797687292, - "y": 4.250831458898944, - "heading": -0.5655034744842501, - "angularVelocity": 0.8834772208917167, - "velocityX": 0.9850270681312915, - "velocityY": -0.17239965331985382, - "timestamp": 0.7806548591433189 - }, - { - "x": 1.5578729893579737, - "y": 4.231605083602241, - "heading": -0.46613854628859924, - "angularVelocity": 0.8909884941123012, - "velocityX": 0.9850270681317038, - "velocityY": -0.17239965331749818, - "timestamp": 0.8921769818780787 - }, - { - "x": 1.667725298947277, - "y": 4.212378708305875, - "heading": -0.3675558587925076, - "angularVelocity": 0.883974274149687, - "velocityX": 0.9850270681322336, - "velocityY": -0.17239965331447085, - "timestamp": 1.0036991046128385 - }, - { - "x": 1.7775776085366437, - "y": 4.193152333009867, - "heading": -0.2716202315057031, - "angularVelocity": 0.8602385332547363, - "velocityX": 0.9850270681327978, - "velocityY": -0.17239965331124718, - "timestamp": 1.1152212273475983 - }, - { - "x": 1.8874299181260659, - "y": 4.173925957714179, - "heading": -0.1810011200646351, - "angularVelocity": 0.8125662354597859, - "velocityX": 0.9850270681332973, - "velocityY": -0.17239965330839344, - "timestamp": 1.2267433500823581 - }, - { - "x": 1.997282227715524, - "y": 4.154699582418694, - "heading": -0.09996805663035663, - "angularVelocity": 0.7266097653736787, - "velocityX": 0.9850270681336178, - "velocityY": -0.1723996533065617, - "timestamp": 1.338265472817118 - }, - { - "x": 2.107134537304987, - "y": 4.135473207123238, - "heading": -0.035641662169536334, - "angularVelocity": 0.5768038922090091, - "velocityX": 0.9850270681336627, - "velocityY": -0.17239965330630566, - "timestamp": 1.4497875955518778 - }, - { - "x": 2.2169868468951135, - "y": 4.116246831831573, - "heading": -1.0063178889168783e-8, - "angularVelocity": 0.3195926622659994, - "velocityX": 0.98502706813961, - "velocityY": -0.17239965327231074, - "timestamp": 1.5613097182866376 - }, - { - "x": 2.295893669128418, - "y": 4.102436542510986, - "heading": 5.425975400842206e-33, - "angularVelocity": 9.023482195637066e-8, - "velocityX": 0.7075441203802566, - "velocityY": -0.12383452701517264, - "timestamp": 1.6728318410213974 - }, - { - "x": 2.295893669128418, - "y": 4.102436542510986, - "heading": 0, - "angularVelocity": -4.865380308863049e-32, - "velocityX": -1.6160842456388493e-31, - "velocityY": -5.471142235353396e-32, - "timestamp": 1.7843539637561572 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-2ndN.2.traj b/src/main/deploy/choreo/3NB-2ndN.2.traj deleted file mode 100644 index 4015e59..0000000 --- a/src/main/deploy/choreo/3NB-2ndN.2.traj +++ /dev/null @@ -1,346 +0,0 @@ -{ - "samples": [ - { - "x": 2.295893669128418, - "y": 4.102436542510986, - "heading": 0, - "angularVelocity": -4.865380308863049e-32, - "velocityX": -1.6160842456388493e-31, - "velocityY": -5.471142235353396e-32, - "timestamp": 0 - }, - { - "x": 2.302624119193887, - "y": 4.07385174410917, - "heading": -8.292160863866972e-17, - "angularVelocity": -1.2280480432765743e-15, - "velocityX": 0.09967594851534668, - "velocityY": -0.4233322981545981, - "timestamp": 0.0675233109461062 - }, - { - "x": 2.3169745348343085, - "y": 4.016906308376568, - "heading": -2.581621239218751e-16, - "angularVelocity": -2.5952671245371838e-15, - "velocityX": 0.2125253551597846, - "velocityY": -0.8433448380229092, - "timestamp": 0.1350466218922124 - }, - { - "x": 2.3400599620957876, - "y": 3.931923591100493, - "heading": -5.130317206898384e-16, - "angularVelocity": -3.7745534880874335e-15, - "velocityX": 0.3418882595946266, - "velocityY": -1.2585685755830793, - "timestamp": 0.2025699328383186 - }, - { - "x": 2.373310925877869, - "y": 3.8193899863733893, - "heading": -8.488460139622293e-16, - "angularVelocity": -4.973323452730244e-15, - "velocityX": 0.49243680909789883, - "velocityY": -1.6665889623947179, - "timestamp": 0.27009324378442456 - }, - { - "x": 2.418610139619234, - "y": 3.6800752476834298, - "heading": -1.2424296277796208e-15, - "angularVelocity": -5.8288725510901505e-15, - "velocityX": 0.6708677804242058, - "velocityY": -2.063209530723095, - "timestamp": 0.33761655473053076 - }, - { - "x": 2.4784973016469114, - "y": 3.515273627351516, - "heading": -1.6835259705344942e-15, - "angularVelocity": -6.532524021843532e-15, - "velocityX": 0.8869109228874443, - "velocityY": -2.440662610035145, - "timestamp": 0.40513986567663696 - }, - { - "x": 2.5564401370112204, - "y": 3.3273120867732886, - "heading": -2.1550705234989717e-15, - "angularVelocity": -6.983454085778219e-15, - "velocityX": 1.1543100341523413, - "velocityY": -2.7836540884119194, - "timestamp": 0.47266317662274315 - }, - { - "x": 2.656989300962068, - "y": 3.1206064007124743, - "heading": -2.6199509362820876e-15, - "angularVelocity": -6.884759808183529e-15, - "velocityX": 1.4891029859467424, - "velocityY": -3.06124926583891, - "timestamp": 0.5401864875688493 - }, - { - "x": 2.7849336812472782, - "y": 2.9033228587477327, - "heading": -3.031826948453853e-15, - "angularVelocity": -6.0997778236109756e-15, - "velocityX": 1.894817930171848, - "velocityY": -3.21790414184601, - "timestamp": 0.6077097985149555 - }, - { - "x": 2.9422124557465033, - "y": 2.687411238717251, - "heading": -3.3386928867831784e-15, - "angularVelocity": -4.544605165609959e-15, - "velocityX": 2.3292515176692206, - "velocityY": -3.19758638913203, - "timestamp": 0.6752331094610617 - }, - { - "x": 3.1260700530650354, - "y": 2.4839878188804487, - "heading": -3.4936436292754485e-15, - "angularVelocity": -2.294780490604881e-15, - "velocityX": 2.7228759185902462, - "velocityY": -3.012639886678547, - "timestamp": 0.7427564204071679 - }, - { - "x": 3.3319356441497803, - "y": 2.300007581710815, - "heading": -3.488531405132631e-15, - "angularVelocity": 7.57105074510855e-17, - "velocityX": 3.048807711000621, - "velocityY": -2.7246921780307343, - "timestamp": 0.8102797313532741 - }, - { - "x": 3.489358051205153, - "y": 2.1796001473934483, - "heading": -3.4769953550620332e-15, - "angularVelocity": 2.380018423477147e-16, - "velocityX": 3.2478034227066273, - "velocityY": -2.4841424077444927, - "timestamp": 0.8587501547645997 - }, - { - "x": 3.6555081557643048, - "y": 2.0715541938896345, - "heading": -3.46066402017299e-15, - "angularVelocity": 3.3693402573872426e-16, - "velocityX": 3.427865755352637, - "velocityY": -2.229110989746537, - "timestamp": 0.9072205781759253 - }, - { - "x": 3.82941737519358, - "y": 1.9764995834405001, - "heading": -3.4311786789456212e-15, - "angularVelocity": 6.083161473048826e-16, - "velocityX": 3.5879451259057435, - "velocityY": -1.961084796857044, - "timestamp": 0.9556910015872508 - }, - { - "x": 4.0100719081181415, - "y": 1.894990434653268, - "heading": -3.432767846995604e-15, - "angularVelocity": -3.278643117768733e-17, - "velocityX": 3.727108620271126, - "velocityY": -1.6816265064486144, - "timestamp": 1.0041614249985764 - }, - { - "x": 4.196418636291012, - "y": 1.8275018992234235, - "heading": -3.441661064576911e-15, - "angularVelocity": -1.8347727197080018e-16, - "velocityX": 3.8445450866293402, - "velocityY": -1.3923652957839638, - "timestamp": 1.052631848409902 - }, - { - "x": 4.387371261449498, - "y": 1.7744273935556105, - "heading": -3.4348652747795826e-15, - "angularVelocity": 1.402048779260617e-16, - "velocityX": 3.9395699834934264, - "velocityY": -1.0949874569375246, - "timestamp": 1.1011022718212276 - }, - { - "x": 4.581816637165551, - "y": 1.7360763051042505, - "heading": -3.4206405163814298e-15, - "angularVelocity": 2.9347295519771835e-16, - "velocityX": 4.011629402655687, - "velocityY": -0.7912266027871694, - "timestamp": 1.1495726952325531 - }, - { - "x": 4.778621257963462, - "y": 1.712672187082093, - "heading": -3.4076727329152914e-15, - "angularVelocity": 2.675401317654148e-16, - "velocityX": 4.060303313791993, - "velocityY": -0.48285359142801165, - "timestamp": 1.1980431186438787 - }, - { - "x": 4.976637868001871, - "y": 1.7043514506655806, - "heading": -3.3948654352437934e-15, - "angularVelocity": 2.642291271700789e-16, - "velocityX": 4.085308031209358, - "velocityY": -0.17166626224691683, - "timestamp": 1.2465135420552043 - }, - { - "x": 5.174712152145305, - "y": 1.7111625566741109, - "heading": -3.3878134973774393e-15, - "angularVelocity": 1.4548950411589204e-16, - "velocityX": 4.086497913635991, - "velocityY": 0.14052086879247774, - "timestamp": 1.2949839654665298 - }, - { - "x": 5.371689477427423, - "y": 1.733065683889262, - "heading": -3.384734641999914e-15, - "angularVelocity": 6.352029053732012e-17, - "velocityX": 4.063866403859248, - "velocityY": 0.45188644277494805, - "timestamp": 1.3434543888778554 - }, - { - "x": 5.566421709253515, - "y": 1.7699326309440095, - "heading": -3.400508079449476e-15, - "angularVelocity": -3.2542396660491567e-16, - "velocityX": 4.017547570682143, - "velocityY": 0.7606070766456821, - "timestamp": 1.391924812289181 - }, - { - "x": 5.758828163146973, - "y": 1.8174675703048704, - "heading": -3.3977669663588387e-15, - "angularVelocity": 5.65522827687079e-17, - "velocityX": 3.9695641249210003, - "velocityY": 0.98069989935919, - "timestamp": 1.4403952357005065 - }, - { - "x": 6.048920969073673, - "y": 1.9023244295720378, - "heading": -3.3698201318973824e-15, - "angularVelocity": 3.780728833463097e-16, - "velocityX": 3.924459628012348, - "velocityY": 1.1479682072433182, - "timestamp": 1.514314407618101 - }, - { - "x": 6.339013701554556, - "y": 1.9871815395759902, - "heading": -3.3816009084007496e-15, - "angularVelocity": -1.593737615707112e-16, - "velocityX": 3.9244586344160233, - "velocityY": 1.1479715992833515, - "timestamp": 1.5882335795356954 - }, - { - "x": 6.6291064335772285, - "y": 2.072038651443169, - "heading": 5.787203315330214e-15, - "angularVelocity": 1.2403824396933447e-13, - "velocityX": 3.9244586282172382, - "velocityY": 1.147971624489548, - "timestamp": 1.66215275145329 - }, - { - "x": 6.899328128367979, - "y": 2.1510831419772893, - "heading": 4.498352972326747e-15, - "angularVelocity": -1.7435941136055766e-14, - "velocityX": 3.6556374724001697, - "velocityY": 1.0693367969847223, - "timestamp": 1.7360719233708843 - }, - { - "x": 7.135772135820312, - "y": 2.2202470783807007, - "heading": 3.371603022772973e-15, - "angularVelocity": -1.5243000099038025e-14, - "velocityX": 3.1986831199342842, - "velocityY": 0.9356697945767513, - "timestamp": 1.8099910952884788 - }, - { - "x": 7.33843843743126, - "y": 2.27953045522306, - "heading": 2.4076213307840505e-15, - "angularVelocity": -1.3041023946945337e-14, - "velocityX": 2.7417285171549155, - "velocityY": 0.8020027187054662, - "timestamp": 1.8839102672060732 - }, - { - "x": 7.507327026976948, - "y": 2.3289332706826067, - "heading": 1.6027502444282843e-15, - "angularVelocity": -1.0888529545859133e-14, - "velocityX": 2.28477383017715, - "velocityY": 0.6683356181888578, - "timestamp": 1.9578294391236677 - }, - { - "x": 7.64243790134214, - "y": 2.3684555238481417, - "heading": 9.570473848243299e-16, - "angularVelocity": -8.735255614723888e-15, - "velocityX": 1.8278191010555744, - "velocityY": 0.5346685053452767, - "timestamp": 2.0317486110412624 - }, - { - "x": 7.743771058659156, - "y": 2.3980972141735055, - "heading": 4.76423082170293e-16, - "angularVelocity": -6.502025012522106e-15, - "velocityX": 1.3708643466674988, - "velocityY": 0.4010013851130894, - "timestamp": 2.1056677829588564 - }, - { - "x": 7.811326497684122, - "y": 2.4178583412949837, - "heading": 1.5485559519229808e-16, - "angularVelocity": -4.350258266558453e-15, - "velocityX": 0.9139095754518946, - "velocityY": 0.2673342599604742, - "timestamp": 2.1795869548764513 - }, - { - "x": 7.845104217529297, - "y": 2.427738904953003, - "heading": -3.2792970494417754e-32, - "angularVelocity": -2.094931425289687e-15, - "velocityX": 0.45695479222670565, - "velocityY": 0.13366713129626945, - "timestamp": 2.253506126794046 - }, - { - "x": 7.845104217529297, - "y": 2.427738904953003, - "heading": -7.22600529689662e-33, - "angularVelocity": 3.458773226203961e-31, - "velocityX": -4.2135134691610547e-32, - "velocityY": -6.511603074984667e-32, - "timestamp": 2.327425298711641 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-2ndN.3.traj b/src/main/deploy/choreo/3NB-2ndN.3.traj deleted file mode 100644 index 35cf506..0000000 --- a/src/main/deploy/choreo/3NB-2ndN.3.traj +++ /dev/null @@ -1,247 +0,0 @@ -{ - "samples": [ - { - "x": 7.845104217529297, - "y": 2.427738904953003, - "heading": -7.22600529689662e-33, - "angularVelocity": 3.458773226203961e-31, - "velocityX": -4.2135134691610547e-32, - "velocityY": -6.511603074984667e-32, - "timestamp": 0 - }, - { - "x": 7.817147943760311, - "y": 2.416749060100633, - "heading": -2.232777809662192e-18, - "angularVelocity": -3.2694615280544566e-17, - "velocityX": -0.40936442573103865, - "velocityY": -0.16092457686070072, - "timestamp": 0.068291898396037 - }, - { - "x": 7.761224810662351, - "y": 2.3947963380623256, - "heading": -4.082249033262829e-18, - "angularVelocity": -2.7081836286500178e-17, - "velocityX": -0.8188838560854425, - "velocityY": -0.32145426549792994, - "timestamp": 0.136583796792074 - }, - { - "x": 7.677321253648209, - "y": 2.3619154081138825, - "heading": -4.719112125486337e-18, - "angularVelocity": -9.325571155135092e-18, - "velocityX": -1.2286019130343195, - "velocityY": -0.4814762910503355, - "timestamp": 0.204875695188111 - }, - { - "x": 7.565419266048648, - "y": 2.318152489662702, - "heading": -6.783249162229931e-18, - "angularVelocity": -3.022516313863855e-17, - "velocityX": -1.638583642097525, - "velocityY": -0.6408215246480392, - "timestamp": 0.273167593584148 - }, - { - "x": 7.425493790969877, - "y": 2.263572277079932, - "heading": -6.4023636552302655e-18, - "angularVelocity": 5.57737739582102e-18, - "velocityX": -2.0489322798919862, - "velocityY": -0.7992194369283216, - "timestamp": 0.341459491980185 - }, - { - "x": 7.257507576227683, - "y": 2.1982717816979753, - "heading": -2.8414113294330474e-18, - "angularVelocity": 5.2143205281390836e-17, - "velocityX": -2.459826402364604, - "velocityY": -0.9561968098079227, - "timestamp": 0.409751390376222 - }, - { - "x": 7.061399421871421, - "y": 2.122412599297568, - "heading": 5.218813123079696e-19, - "angularVelocity": 4.924888100254347e-17, - "velocityX": -2.8716166772666143, - "velocityY": -1.1108079315719974, - "timestamp": 0.478043288772259 - }, - { - "x": 6.837050243811277, - "y": 2.0363175235605016, - "heading": 6.519999461778699e-18, - "angularVelocity": 8.783074421973704e-17, - "velocityX": -3.285150703515771, - "velocityY": -1.260692377267432, - "timestamp": 0.546335187168296 - }, - { - "x": 6.584129054535847, - "y": 1.940950563311945, - "heading": 1.4190127621525814e-17, - "angularVelocity": 1.1231410257519964e-16, - "velocityX": -3.7035313882875425, - "velocityY": -1.3964608172906436, - "timestamp": 0.614627085564333 - }, - { - "x": 6.3139999030285665, - "y": 1.8702043234601993, - "heading": 6.229392328668449e-18, - "angularVelocity": -1.1656924876640592e-16, - "velocityX": -3.95550801561808, - "velocityY": -1.0359389841734894, - "timestamp": 0.68291898396037 - }, - { - "x": 6.03783435294095, - "y": 1.8288838996250503, - "heading": -1.2479954305012523e-18, - "angularVelocity": -1.0949157857360391e-16, - "velocityX": -4.043899153104409, - "velocityY": -0.60505601404499, - "timestamp": 0.751210882356407 - }, - { - "x": 5.758828163146973, - "y": 1.8174675703048704, - "heading": -2.1428347292812524e-17, - "angularVelocity": -2.9550140406523894e-16, - "velocityX": -4.085494712359195, - "velocityY": -0.16716959973635895, - "timestamp": 0.819502780752444 - }, - { - "x": 5.442944069689868, - "y": 1.8431974590025388, - "heading": -4.640746519007176e-17, - "angularVelocity": -3.222710704686718e-16, - "velocityX": -4.075416328196858, - "velocityY": 0.33195722954357726, - "timestamp": 0.8970124308148391 - }, - { - "x": 5.132549947222792, - "y": 1.907230875263747, - "heading": -5.263653684696362e-17, - "angularVelocity": -8.036512420504643e-17, - "velocityX": -4.004586812315917, - "velocityY": 0.826134761408413, - "timestamp": 0.9745220808772341 - }, - { - "x": 4.832272646006637, - "y": 2.008612989199975, - "heading": -7.111877179201443e-17, - "angularVelocity": -2.384507597463398e-16, - "velocityX": -3.874063435642526, - "velocityY": 1.3079934415206356, - "timestamp": 1.0520317309396292 - }, - { - "x": 4.542195548692397, - "y": 2.1362847114522143, - "heading": -8.340242391355802e-17, - "angularVelocity": -1.5847900373250254e-16, - "velocityX": -3.7424642877403507, - "velocityY": 1.6471719605306414, - "timestamp": 1.1295413810020243 - }, - { - "x": 4.252118851658615, - "y": 2.263957342643499, - "heading": -9.952202813328986e-17, - "angularVelocity": -2.0796899749604922e-16, - "velocityX": -3.7424591234780267, - "velocityY": 1.6471836873075834, - "timestamp": 1.2070510310644194 - }, - { - "x": 3.9687873276430823, - "y": 2.3886611935333675, - "heading": -1.0866631170259556e-16, - "angularVelocity": -1.1797607203820848e-16, - "velocityX": -3.655435468842005, - "velocityY": 1.6088816139579067, - "timestamp": 1.2845606811268144 - }, - { - "x": 3.720872219657192, - "y": 2.4977770738171987, - "heading": -1.097303256449214e-16, - "angularVelocity": -1.3727509447610907e-17, - "velocityX": -3.198506350969631, - "velocityY": 1.4077715509712598, - "timestamp": 1.3620703311892095 - }, - { - "x": 3.508373546229774, - "y": 2.591304975356774, - "heading": -1.1118600147966736e-16, - "angularVelocity": -1.878057682487085e-17, - "velocityX": -2.741576994044833, - "velocityY": 1.2066613829896125, - "timestamp": 1.4395799812516046 - }, - { - "x": 3.331291313540666, - "y": 2.669244895433765, - "heading": -1.1134314124155818e-16, - "angularVelocity": -2.027358801996522e-18, - "velocityX": -2.284647557389988, - "velocityY": 1.00555117993741, - "timestamp": 1.5170896313139997 - }, - { - "x": 3.1896255246796548, - "y": 2.7315968326883255, - "heading": -1.0469304235431019e-16, - "angularVelocity": 8.579703039997364e-17, - "velocityX": -1.8277180808718518, - "velocityY": 0.8044409593411052, - "timestamp": 1.5945992813763947 - }, - { - "x": 3.0833761815004075, - "y": 2.7783607863044333, - "heading": -1.0419235160058842e-16, - "angularVelocity": 6.459716680279016e-18, - "velocityX": -1.3707885804383855, - "velocityY": 0.6033307282168234, - "timestamp": 1.6721089314387898 - }, - { - "x": 3.012543285238584, - "y": 2.809536755738053, - "heading": -1.0632657875299128e-16, - "angularVelocity": -2.7534990990417765e-17, - "velocityX": -0.9138590640629015, - "velocityY": 0.4022204900736176, - "timestamp": 1.7496185815011849 - }, - { - "x": 2.9771268367767334, - "y": 2.825124740600586, - "heading": -1.1064513996869437e-16, - "angularVelocity": -5.571643241065736e-17, - "velocityX": -0.4569295363010946, - "velocityY": 0.20111024691687301, - "timestamp": 1.82712823156358 - }, - { - "x": 2.9771268367767334, - "y": 2.825124740600586, - "heading": -1.1064513996869437e-16, - "angularVelocity": 2.2349585794705205e-31, - "velocityX": 3.085088305989537e-30, - "velocityY": -2.875018364649188e-29, - "timestamp": 1.904637881625975 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-2ndN.traj b/src/main/deploy/choreo/3NB-2ndN.traj deleted file mode 100644 index da35169..0000000 --- a/src/main/deploy/choreo/3NB-2ndN.traj +++ /dev/null @@ -1,724 +0,0 @@ -{ - "samples": [ - { - "x": 0.71, - "y": 4.38, - "heading": -1.0303768814651526, - "angularVelocity": 4.324130717118377e-34, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 0.7889068222351219, - "y": 4.366189710689795, - "heading": -1.0303768723653812, - "angularVelocity": 8.159611106479039e-8, - "velocityX": 0.7075441203965512, - "velocityY": -0.12383452692207302, - "timestamp": 0.11152212273475985 - }, - { - "x": 0.898759131822665, - "y": 4.346963335383371, - "heading": -0.9951630127982624, - "angularVelocity": 0.3157567189683987, - "velocityX": 0.985027068116447, - "velocityY": -0.17239965340465557, - "timestamp": 0.2230442454695197 - }, - { - "x": 1.0086114414119594, - "y": 4.327736960086952, - "heading": -0.9311307451167896, - "angularVelocity": 0.5741665071580884, - "velocityX": 0.9850270681321506, - "velocityY": -0.17239965331494583, - "timestamp": 0.33456636820427954 - }, - { - "x": 1.118463751001178, - "y": 4.3085105847901, - "heading": -0.850315169265626, - "angularVelocity": 0.7246595910245763, - "velocityX": 0.9850270681314717, - "velocityY": -0.17239965331882467, - "timestamp": 0.4460884909390394 - }, - { - "x": 1.2283160605903567, - "y": 4.289284209493019, - "heading": -0.7598572882997845, - "angularVelocity": 0.811120509075883, - "velocityX": 0.9850270681311126, - "velocityY": -0.17239965332087606, - "timestamp": 0.5576106136737993 - }, - { - "x": 1.3381683701795306, - "y": 4.270057834195912, - "heading": -0.6640307295459007, - "angularVelocity": 0.8592605341793411, - "velocityX": 0.9850270681310712, - "velocityY": -0.1723996533211125, - "timestamp": 0.6691327364085591 - }, - { - "x": 1.4480206797687292, - "y": 4.250831458898944, - "heading": -0.5655034744842501, - "angularVelocity": 0.8834772208917167, - "velocityX": 0.9850270681312915, - "velocityY": -0.17239965331985382, - "timestamp": 0.7806548591433189 - }, - { - "x": 1.5578729893579737, - "y": 4.231605083602241, - "heading": -0.46613854628859924, - "angularVelocity": 0.8909884941123012, - "velocityX": 0.9850270681317038, - "velocityY": -0.17239965331749818, - "timestamp": 0.8921769818780787 - }, - { - "x": 1.667725298947277, - "y": 4.212378708305875, - "heading": -0.3675558587925076, - "angularVelocity": 0.883974274149687, - "velocityX": 0.9850270681322336, - "velocityY": -0.17239965331447085, - "timestamp": 1.0036991046128385 - }, - { - "x": 1.7775776085366437, - "y": 4.193152333009867, - "heading": -0.2716202315057031, - "angularVelocity": 0.8602385332547363, - "velocityX": 0.9850270681327978, - "velocityY": -0.17239965331124718, - "timestamp": 1.1152212273475983 - }, - { - "x": 1.8874299181260659, - "y": 4.173925957714179, - "heading": -0.1810011200646351, - "angularVelocity": 0.8125662354597859, - "velocityX": 0.9850270681332973, - "velocityY": -0.17239965330839344, - "timestamp": 1.2267433500823581 - }, - { - "x": 1.997282227715524, - "y": 4.154699582418694, - "heading": -0.09996805663035663, - "angularVelocity": 0.7266097653736787, - "velocityX": 0.9850270681336178, - "velocityY": -0.1723996533065617, - "timestamp": 1.338265472817118 - }, - { - "x": 2.107134537304987, - "y": 4.135473207123238, - "heading": -0.035641662169536334, - "angularVelocity": 0.5768038922090091, - "velocityX": 0.9850270681336627, - "velocityY": -0.17239965330630566, - "timestamp": 1.4497875955518778 - }, - { - "x": 2.2169868468951135, - "y": 4.116246831831573, - "heading": -1.0063178889168783e-8, - "angularVelocity": 0.3195926622659994, - "velocityX": 0.98502706813961, - "velocityY": -0.17239965327231074, - "timestamp": 1.5613097182866376 - }, - { - "x": 2.295893669128418, - "y": 4.102436542510986, - "heading": 5.425975400842206e-33, - "angularVelocity": 9.023482195637066e-8, - "velocityX": 0.7075441203802566, - "velocityY": -0.12383452701517264, - "timestamp": 1.6728318410213974 - }, - { - "x": 2.295893669128418, - "y": 4.102436542510986, - "heading": 0, - "angularVelocity": -4.865380308863049e-32, - "velocityX": -1.6160842456388493e-31, - "velocityY": -5.471142235353396e-32, - "timestamp": 1.7843539637561572 - }, - { - "x": 2.302624119193887, - "y": 4.07385174410917, - "heading": -8.292160863866972e-17, - "angularVelocity": -1.2280480432765743e-15, - "velocityX": 0.09967594851534668, - "velocityY": -0.4233322981545981, - "timestamp": 1.8518772747022634 - }, - { - "x": 2.3169745348343085, - "y": 4.016906308376568, - "heading": -2.581621239218751e-16, - "angularVelocity": -2.5952671245371838e-15, - "velocityX": 0.2125253551597846, - "velocityY": -0.8433448380229092, - "timestamp": 1.9194005856483696 - }, - { - "x": 2.3400599620957876, - "y": 3.931923591100493, - "heading": -5.130317206898384e-16, - "angularVelocity": -3.7745534880874335e-15, - "velocityX": 0.3418882595946266, - "velocityY": -1.2585685755830793, - "timestamp": 1.9869238965944758 - }, - { - "x": 2.373310925877869, - "y": 3.8193899863733893, - "heading": -8.488460139622293e-16, - "angularVelocity": -4.973323452730244e-15, - "velocityX": 0.49243680909789883, - "velocityY": -1.6665889623947179, - "timestamp": 2.0544472075405817 - }, - { - "x": 2.418610139619234, - "y": 3.6800752476834298, - "heading": -1.2424296277796208e-15, - "angularVelocity": -5.8288725510901505e-15, - "velocityX": 0.6708677804242058, - "velocityY": -2.063209530723095, - "timestamp": 2.121970518486688 - }, - { - "x": 2.4784973016469114, - "y": 3.515273627351516, - "heading": -1.6835259705344942e-15, - "angularVelocity": -6.532524021843532e-15, - "velocityX": 0.8869109228874443, - "velocityY": -2.440662610035145, - "timestamp": 2.189493829432794 - }, - { - "x": 2.5564401370112204, - "y": 3.3273120867732886, - "heading": -2.1550705234989717e-15, - "angularVelocity": -6.983454085778219e-15, - "velocityX": 1.1543100341523413, - "velocityY": -2.7836540884119194, - "timestamp": 2.2570171403789003 - }, - { - "x": 2.656989300962068, - "y": 3.1206064007124743, - "heading": -2.6199509362820876e-15, - "angularVelocity": -6.884759808183529e-15, - "velocityX": 1.4891029859467424, - "velocityY": -3.06124926583891, - "timestamp": 2.3245404513250065 - }, - { - "x": 2.7849336812472782, - "y": 2.9033228587477327, - "heading": -3.031826948453853e-15, - "angularVelocity": -6.0997778236109756e-15, - "velocityX": 1.894817930171848, - "velocityY": -3.21790414184601, - "timestamp": 2.3920637622711127 - }, - { - "x": 2.9422124557465033, - "y": 2.687411238717251, - "heading": -3.3386928867831784e-15, - "angularVelocity": -4.544605165609959e-15, - "velocityX": 2.3292515176692206, - "velocityY": -3.19758638913203, - "timestamp": 2.459587073217219 - }, - { - "x": 3.1260700530650354, - "y": 2.4839878188804487, - "heading": -3.4936436292754485e-15, - "angularVelocity": -2.294780490604881e-15, - "velocityX": 2.7228759185902462, - "velocityY": -3.012639886678547, - "timestamp": 2.527110384163325 - }, - { - "x": 3.3319356441497803, - "y": 2.300007581710815, - "heading": -3.488531405132631e-15, - "angularVelocity": 7.57105074510855e-17, - "velocityX": 3.048807711000621, - "velocityY": -2.7246921780307343, - "timestamp": 2.5946336951094313 - }, - { - "x": 3.489358051205153, - "y": 2.1796001473934483, - "heading": -3.4769953550620332e-15, - "angularVelocity": 2.380018423477147e-16, - "velocityX": 3.2478034227066273, - "velocityY": -2.4841424077444927, - "timestamp": 2.643104118520757 - }, - { - "x": 3.6555081557643048, - "y": 2.0715541938896345, - "heading": -3.46066402017299e-15, - "angularVelocity": 3.3693402573872426e-16, - "velocityX": 3.427865755352637, - "velocityY": -2.229110989746537, - "timestamp": 2.6915745419320825 - }, - { - "x": 3.82941737519358, - "y": 1.9764995834405001, - "heading": -3.4311786789456212e-15, - "angularVelocity": 6.083161473048826e-16, - "velocityX": 3.5879451259057435, - "velocityY": -1.961084796857044, - "timestamp": 2.740044965343408 - }, - { - "x": 4.0100719081181415, - "y": 1.894990434653268, - "heading": -3.432767846995604e-15, - "angularVelocity": -3.278643117768733e-17, - "velocityX": 3.727108620271126, - "velocityY": -1.6816265064486144, - "timestamp": 2.7885153887547336 - }, - { - "x": 4.196418636291012, - "y": 1.8275018992234235, - "heading": -3.441661064576911e-15, - "angularVelocity": -1.8347727197080018e-16, - "velocityX": 3.8445450866293402, - "velocityY": -1.3923652957839638, - "timestamp": 2.836985812166059 - }, - { - "x": 4.387371261449498, - "y": 1.7744273935556105, - "heading": -3.4348652747795826e-15, - "angularVelocity": 1.402048779260617e-16, - "velocityX": 3.9395699834934264, - "velocityY": -1.0949874569375246, - "timestamp": 2.8854562355773847 - }, - { - "x": 4.581816637165551, - "y": 1.7360763051042505, - "heading": -3.4206405163814298e-15, - "angularVelocity": 2.9347295519771835e-16, - "velocityX": 4.011629402655687, - "velocityY": -0.7912266027871694, - "timestamp": 2.9339266589887103 - }, - { - "x": 4.778621257963462, - "y": 1.712672187082093, - "heading": -3.4076727329152914e-15, - "angularVelocity": 2.675401317654148e-16, - "velocityX": 4.060303313791993, - "velocityY": -0.48285359142801165, - "timestamp": 2.982397082400036 - }, - { - "x": 4.976637868001871, - "y": 1.7043514506655806, - "heading": -3.3948654352437934e-15, - "angularVelocity": 2.642291271700789e-16, - "velocityX": 4.085308031209358, - "velocityY": -0.17166626224691683, - "timestamp": 3.0308675058113614 - }, - { - "x": 5.174712152145305, - "y": 1.7111625566741109, - "heading": -3.3878134973774393e-15, - "angularVelocity": 1.4548950411589204e-16, - "velocityX": 4.086497913635991, - "velocityY": 0.14052086879247774, - "timestamp": 3.079337929222687 - }, - { - "x": 5.371689477427423, - "y": 1.733065683889262, - "heading": -3.384734641999914e-15, - "angularVelocity": 6.352029053732012e-17, - "velocityX": 4.063866403859248, - "velocityY": 0.45188644277494805, - "timestamp": 3.1278083526340126 - }, - { - "x": 5.566421709253515, - "y": 1.7699326309440095, - "heading": -3.400508079449476e-15, - "angularVelocity": -3.2542396660491567e-16, - "velocityX": 4.017547570682143, - "velocityY": 0.7606070766456821, - "timestamp": 3.176278776045338 - }, - { - "x": 5.758828163146973, - "y": 1.8174675703048704, - "heading": -3.3977669663588387e-15, - "angularVelocity": 5.65522827687079e-17, - "velocityX": 3.9695641249210003, - "velocityY": 0.98069989935919, - "timestamp": 3.2247491994566637 - }, - { - "x": 6.048920969073673, - "y": 1.9023244295720378, - "heading": -3.3698201318973824e-15, - "angularVelocity": 3.780728833463097e-16, - "velocityX": 3.924459628012348, - "velocityY": 1.1479682072433182, - "timestamp": 3.298668371374258 - }, - { - "x": 6.339013701554556, - "y": 1.9871815395759902, - "heading": -3.3816009084007496e-15, - "angularVelocity": -1.593737615707112e-16, - "velocityX": 3.9244586344160233, - "velocityY": 1.1479715992833515, - "timestamp": 3.3725875432918526 - }, - { - "x": 6.6291064335772285, - "y": 2.072038651443169, - "heading": 5.787203315330214e-15, - "angularVelocity": 1.2403824396933447e-13, - "velocityX": 3.9244586282172382, - "velocityY": 1.147971624489548, - "timestamp": 3.446506715209447 - }, - { - "x": 6.899328128367979, - "y": 2.1510831419772893, - "heading": 4.498352972326747e-15, - "angularVelocity": -1.7435941136055766e-14, - "velocityX": 3.6556374724001697, - "velocityY": 1.0693367969847223, - "timestamp": 3.5204258871270415 - }, - { - "x": 7.135772135820312, - "y": 2.2202470783807007, - "heading": 3.371603022772973e-15, - "angularVelocity": -1.5243000099038025e-14, - "velocityX": 3.1986831199342842, - "velocityY": 0.9356697945767513, - "timestamp": 3.594345059044636 - }, - { - "x": 7.33843843743126, - "y": 2.27953045522306, - "heading": 2.4076213307840505e-15, - "angularVelocity": -1.3041023946945337e-14, - "velocityX": 2.7417285171549155, - "velocityY": 0.8020027187054662, - "timestamp": 3.6682642309622304 - }, - { - "x": 7.507327026976948, - "y": 2.3289332706826067, - "heading": 1.6027502444282843e-15, - "angularVelocity": -1.0888529545859133e-14, - "velocityX": 2.28477383017715, - "velocityY": 0.6683356181888578, - "timestamp": 3.742183402879825 - }, - { - "x": 7.64243790134214, - "y": 2.3684555238481417, - "heading": 9.570473848243299e-16, - "angularVelocity": -8.735255614723888e-15, - "velocityX": 1.8278191010555744, - "velocityY": 0.5346685053452767, - "timestamp": 3.8161025747974193 - }, - { - "x": 7.743771058659156, - "y": 2.3980972141735055, - "heading": 4.76423082170293e-16, - "angularVelocity": -6.502025012522106e-15, - "velocityX": 1.3708643466674988, - "velocityY": 0.4010013851130894, - "timestamp": 3.8900217467150138 - }, - { - "x": 7.811326497684122, - "y": 2.4178583412949837, - "heading": 1.5485559519229808e-16, - "angularVelocity": -4.350258266558453e-15, - "velocityX": 0.9139095754518946, - "velocityY": 0.2673342599604742, - "timestamp": 3.9639409186326082 - }, - { - "x": 7.845104217529297, - "y": 2.427738904953003, - "heading": -3.2792970494417754e-32, - "angularVelocity": -2.094931425289687e-15, - "velocityX": 0.45695479222670565, - "velocityY": 0.13366713129626945, - "timestamp": 4.037860090550203 - }, - { - "x": 7.845104217529297, - "y": 2.427738904953003, - "heading": -7.22600529689662e-33, - "angularVelocity": 3.458773226203961e-31, - "velocityX": -4.2135134691610547e-32, - "velocityY": -6.511603074984667e-32, - "timestamp": 4.111779262467798 - }, - { - "x": 7.817147943760311, - "y": 2.416749060100633, - "heading": -2.232777809662192e-18, - "angularVelocity": -3.2694615280544566e-17, - "velocityX": -0.40936442573103865, - "velocityY": -0.16092457686070072, - "timestamp": 4.180071160863835 - }, - { - "x": 7.761224810662351, - "y": 2.3947963380623256, - "heading": -4.082249033262829e-18, - "angularVelocity": -2.7081836286500178e-17, - "velocityX": -0.8188838560854425, - "velocityY": -0.32145426549792994, - "timestamp": 4.248363059259872 - }, - { - "x": 7.677321253648209, - "y": 2.3619154081138825, - "heading": -4.719112125486337e-18, - "angularVelocity": -9.325571155135092e-18, - "velocityX": -1.2286019130343195, - "velocityY": -0.4814762910503355, - "timestamp": 4.316654957655909 - }, - { - "x": 7.565419266048648, - "y": 2.318152489662702, - "heading": -6.783249162229931e-18, - "angularVelocity": -3.022516313863855e-17, - "velocityX": -1.638583642097525, - "velocityY": -0.6408215246480392, - "timestamp": 4.384946856051946 - }, - { - "x": 7.425493790969877, - "y": 2.263572277079932, - "heading": -6.4023636552302655e-18, - "angularVelocity": 5.57737739582102e-18, - "velocityX": -2.0489322798919862, - "velocityY": -0.7992194369283216, - "timestamp": 4.453238754447983 - }, - { - "x": 7.257507576227683, - "y": 2.1982717816979753, - "heading": -2.8414113294330474e-18, - "angularVelocity": 5.2143205281390836e-17, - "velocityX": -2.459826402364604, - "velocityY": -0.9561968098079227, - "timestamp": 4.52153065284402 - }, - { - "x": 7.061399421871421, - "y": 2.122412599297568, - "heading": 5.218813123079696e-19, - "angularVelocity": 4.924888100254347e-17, - "velocityX": -2.8716166772666143, - "velocityY": -1.1108079315719974, - "timestamp": 4.589822551240057 - }, - { - "x": 6.837050243811277, - "y": 2.0363175235605016, - "heading": 6.519999461778699e-18, - "angularVelocity": 8.783074421973704e-17, - "velocityX": -3.285150703515771, - "velocityY": -1.260692377267432, - "timestamp": 4.658114449636094 - }, - { - "x": 6.584129054535847, - "y": 1.940950563311945, - "heading": 1.4190127621525814e-17, - "angularVelocity": 1.1231410257519964e-16, - "velocityX": -3.7035313882875425, - "velocityY": -1.3964608172906436, - "timestamp": 4.726406348032131 - }, - { - "x": 6.3139999030285665, - "y": 1.8702043234601993, - "heading": 6.229392328668449e-18, - "angularVelocity": -1.1656924876640592e-16, - "velocityX": -3.95550801561808, - "velocityY": -1.0359389841734894, - "timestamp": 4.794698246428168 - }, - { - "x": 6.03783435294095, - "y": 1.8288838996250503, - "heading": -1.2479954305012523e-18, - "angularVelocity": -1.0949157857360391e-16, - "velocityX": -4.043899153104409, - "velocityY": -0.60505601404499, - "timestamp": 4.862990144824205 - }, - { - "x": 5.758828163146973, - "y": 1.8174675703048704, - "heading": -2.1428347292812524e-17, - "angularVelocity": -2.9550140406523894e-16, - "velocityX": -4.085494712359195, - "velocityY": -0.16716959973635895, - "timestamp": 4.931282043220242 - }, - { - "x": 5.442944069689868, - "y": 1.8431974590025388, - "heading": -4.640746519007176e-17, - "angularVelocity": -3.222710704686718e-16, - "velocityX": -4.075416328196858, - "velocityY": 0.33195722954357726, - "timestamp": 5.008791693282637 - }, - { - "x": 5.132549947222792, - "y": 1.907230875263747, - "heading": -5.263653684696362e-17, - "angularVelocity": -8.036512420504643e-17, - "velocityX": -4.004586812315917, - "velocityY": 0.826134761408413, - "timestamp": 5.086301343345032 - }, - { - "x": 4.832272646006637, - "y": 2.008612989199975, - "heading": -7.111877179201443e-17, - "angularVelocity": -2.384507597463398e-16, - "velocityX": -3.874063435642526, - "velocityY": 1.3079934415206356, - "timestamp": 5.163810993407427 - }, - { - "x": 4.542195548692397, - "y": 2.1362847114522143, - "heading": -8.340242391355802e-17, - "angularVelocity": -1.5847900373250254e-16, - "velocityX": -3.7424642877403507, - "velocityY": 1.6471719605306414, - "timestamp": 5.241320643469822 - }, - { - "x": 4.252118851658615, - "y": 2.263957342643499, - "heading": -9.952202813328986e-17, - "angularVelocity": -2.0796899749604922e-16, - "velocityX": -3.7424591234780267, - "velocityY": 1.6471836873075834, - "timestamp": 5.318830293532217 - }, - { - "x": 3.9687873276430823, - "y": 2.3886611935333675, - "heading": -1.0866631170259556e-16, - "angularVelocity": -1.1797607203820848e-16, - "velocityX": -3.655435468842005, - "velocityY": 1.6088816139579067, - "timestamp": 5.396339943594612 - }, - { - "x": 3.720872219657192, - "y": 2.4977770738171987, - "heading": -1.097303256449214e-16, - "angularVelocity": -1.3727509447610907e-17, - "velocityX": -3.198506350969631, - "velocityY": 1.4077715509712598, - "timestamp": 5.4738495936570075 - }, - { - "x": 3.508373546229774, - "y": 2.591304975356774, - "heading": -1.1118600147966736e-16, - "angularVelocity": -1.878057682487085e-17, - "velocityX": -2.741576994044833, - "velocityY": 1.2066613829896125, - "timestamp": 5.551359243719403 - }, - { - "x": 3.331291313540666, - "y": 2.669244895433765, - "heading": -1.1134314124155818e-16, - "angularVelocity": -2.027358801996522e-18, - "velocityX": -2.284647557389988, - "velocityY": 1.00555117993741, - "timestamp": 5.628868893781798 - }, - { - "x": 3.1896255246796548, - "y": 2.7315968326883255, - "heading": -1.0469304235431019e-16, - "angularVelocity": 8.579703039997364e-17, - "velocityX": -1.8277180808718518, - "velocityY": 0.8044409593411052, - "timestamp": 5.706378543844193 - }, - { - "x": 3.0833761815004075, - "y": 2.7783607863044333, - "heading": -1.0419235160058842e-16, - "angularVelocity": 6.459716680279016e-18, - "velocityX": -1.3707885804383855, - "velocityY": 0.6033307282168234, - "timestamp": 5.783888193906588 - }, - { - "x": 3.012543285238584, - "y": 2.809536755738053, - "heading": -1.0632657875299128e-16, - "angularVelocity": -2.7534990990417765e-17, - "velocityX": -0.9138590640629015, - "velocityY": 0.4022204900736176, - "timestamp": 5.861397843968983 - }, - { - "x": 2.9771268367767334, - "y": 2.825124740600586, - "heading": -1.1064513996869437e-16, - "angularVelocity": -5.571643241065736e-17, - "velocityX": -0.4569295363010946, - "velocityY": 0.20111024691687301, - "timestamp": 5.938907494031378 - }, - { - "x": 2.9771268367767334, - "y": 2.825124740600586, - "heading": -1.1064513996869437e-16, - "angularVelocity": 2.2349585794705205e-31, - "velocityX": 3.085088305989537e-30, - "velocityY": -2.875018364649188e-29, - "timestamp": 6.016417144093773 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-Close.1.traj b/src/main/deploy/choreo/3NB-Close.1.traj new file mode 100644 index 0000000..4ca2fc4 --- /dev/null +++ b/src/main/deploy/choreo/3NB-Close.1.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 0.756615161895752, + "y": 4.431221961975098, + "heading": -1.034985029325618, + "angularVelocity": -1.2153572582704763e-25, + "velocityX": -8.099282150741186e-25, + "velocityY": 3.9386843631996907e-25, + "timestamp": 0 + }, + { + "x": 0.7825301409073798, + "y": 4.426111948589589, + "heading": -1.0184480247142116, + "angularVelocity": 0.25729744903186014, + "velocityX": 0.4032083287203581, + "velocityY": -0.079506140289953, + "timestamp": 0.06427193380125104 + }, + { + "x": 0.8343681989936829, + "y": 4.4158863120476, + "heading": -0.9855108542950051, + "angularVelocity": 0.5124658380601789, + "velocityX": 0.8065426854371999, + "velocityY": -0.1590995623938954, + "timestamp": 0.12854386760250208 + }, + { + "x": 0.912137967019861, + "y": 4.4005383558351046, + "heading": -0.9362991408222285, + "angularVelocity": 0.765679676372499, + "velocityX": 1.210011328843265, + "velocityY": -0.2387971748283722, + "timestamp": 0.19281580140375312 + }, + { + "x": 1.0158503745896044, + "y": 4.380061388038885, + "heading": -0.8709611243682479, + "angularVelocity": 1.0165870635855798, + "velocityX": 1.6136500247597099, + "velocityY": -0.31859890600991186, + "timestamp": 0.25708773520500416 + }, + { + "x": 1.145523011747702, + "y": 4.354449737528758, + "heading": -0.7897691851783357, + "angularVelocity": 1.2632565163043505, + "velocityX": 2.0175624022623317, + "velocityY": -0.39848887368670943, + "timestamp": 0.32135966900625523 + }, + { + "x": 1.3011913004178595, + "y": 4.32369771075132, + "heading": -0.6934250833799015, + "angularVelocity": 1.499007359827703, + "velocityX": 2.422025905608079, + "velocityY": -0.4784674267392117, + "timestamp": 0.3856316028075063 + }, + { + "x": 1.4829411037935518, + "y": 4.287786779701849, + "heading": -0.58418273386785, + "angularVelocity": 1.6996897876118522, + "velocityX": 2.827825345005485, + "velocityY": -0.558734254994066, + "timestamp": 0.44990353660875737 + }, + { + "x": 1.6909603916353093, + "y": 4.2465633189263965, + "heading": -0.47548948460818546, + "angularVelocity": 1.6911463967425988, + "velocityX": 3.236549385382092, + "velocityY": -0.6413913249121974, + "timestamp": 0.5141754704100084 + }, + { + "x": 1.8728106274219034, + "y": 4.211006125740984, + "heading": -0.36224313486482546, + "angularVelocity": 1.761987590003954, + "velocityX": 2.829387961920238, + "velocityY": -0.5532304861927155, + "timestamp": 0.5784474042112595 + }, + { + "x": 2.0285640030407976, + "y": 4.180677932000477, + "heading": -0.2611966487318228, + "angularVelocity": 1.572171244224113, + "velocityX": 2.4233497641526145, + "velocityY": -0.47187305479700664, + "timestamp": 0.6427193380125106 + }, + { + "x": 2.1583029244219367, + "y": 4.155482855052808, + "heading": -0.17529966695510088, + "angularVelocity": 1.3364617601571205, + "velocityX": 2.018593711250895, + "velocityY": -0.39200745111512536, + "timestamp": 0.7069912718137616 + }, + { + "x": 2.262065112690402, + "y": 4.135371482146998, + "heading": -0.10573833466683616, + "angularVelocity": 1.082297173496758, + "velocityX": 1.6144245572154519, + "velocityY": -0.3129106550302491, + "timestamp": 0.7712632056150127 + }, + { + "x": 2.339870608422664, + "y": 4.120312466180706, + "heading": -0.05311299136785256, + "angularVelocity": 0.8187919700956509, + "velocityX": 1.2105672123210285, + "velocityY": -0.2343015850878145, + "timestamp": 0.8355351394162638 + }, + { + "x": 2.3917318142501975, + "y": 4.110284912416539, + "heading": -0.017780765488657927, + "angularVelocity": 0.5497302444400217, + "velocityX": 0.8069028386154424, + "velocityY": -0.15601761408291742, + "timestamp": 0.8998070732175149 + }, + { + "x": 2.4176580905914307, + "y": 4.1052751541137695, + "heading": 8.497464730716836e-25, + "angularVelocity": 0.27664898871164556, + "velocityX": 0.4033841026381037, + "velocityY": -0.0779462824047118, + "timestamp": 0.9640790070187659 + }, + { + "x": 2.4176580905914307, + "y": 4.1052751541137695, + "heading": 4.243816717185121e-25, + "angularVelocity": -1.1560345853556316e-25, + "velocityX": 1.998362528445031e-20, + "velocityY": -7.254658147615057e-21, + "timestamp": 1.0283509408200169 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-Close.2.traj b/src/main/deploy/choreo/3NB-Close.2.traj new file mode 100644 index 0000000..80328da --- /dev/null +++ b/src/main/deploy/choreo/3NB-Close.2.traj @@ -0,0 +1,346 @@ +{ + "samples": [ + { + "x": 2.4176580905914307, + "y": 4.1052751541137695, + "heading": 4.243816717185121e-25, + "angularVelocity": -1.1560345853556316e-25, + "velocityX": 1.998362528445031e-20, + "velocityY": -7.254658147615057e-21, + "timestamp": 0 + }, + { + "x": 2.4083001000392614, + "y": 4.07855200651975, + "heading": 5.678182580423427e-18, + "angularVelocity": 8.595410324058872e-17, + "velocityX": -0.14165760527850227, + "velocityY": -0.40452456887715366, + "timestamp": 0.06606062931652201 + }, + { + "x": 2.391723371861385, + "y": 4.0244502470643715, + "heading": 1.2092714840241192e-17, + "angularVelocity": 9.710068345931569e-17, + "velocityX": -0.2509320354556493, + "velocityY": -0.818971299776089, + "timestamp": 0.13212125863304403 + }, + { + "x": 2.370959981958984, + "y": 3.9423454455218585, + "heading": 2.0506186000436857e-17, + "angularVelocity": 1.2735983962200107e-16, + "velocityX": -0.31430808512156017, + "velocityY": -1.242870411499066, + "timestamp": 0.19818188794956604 + }, + { + "x": 2.3504243304381753, + "y": 3.831927280299648, + "heading": 2.7876588197896166e-17, + "angularVelocity": 1.1157026914706097e-16, + "velocityX": -0.3108606704670367, + "velocityY": -1.6714670502630964, + "timestamp": 0.26424251726608805 + }, + { + "x": 2.3364844874655226, + "y": 3.6939737986961876, + "heading": 3.612951501320603e-17, + "angularVelocity": 1.2492958211265208e-16, + "velocityX": -0.21101589731853923, + "velocityY": -2.088285912967496, + "timestamp": 0.33030314658261006 + }, + { + "x": 2.3373423735310555, + "y": 3.531880609628321, + "heading": 3.956479825579656e-17, + "angularVelocity": 5.2001975734663045e-17, + "velocityX": 0.012986344126731107, + "velocityY": -2.4537033743837897, + "timestamp": 0.3963637758991321 + }, + { + "x": 2.3607492318969396, + "y": 3.3526631075737106, + "heading": 4.165260110772871e-17, + "angularVelocity": 3.1604343976295837e-17, + "velocityX": 0.35432387805046023, + "velocityY": -2.7129245347620055, + "timestamp": 0.4624244052156541 + }, + { + "x": 2.4111324989473553, + "y": 3.1648449262631324, + "heading": 4.251470247939207e-17, + "angularVelocity": 1.3050153801581261e-17, + "velocityX": 0.7626822143793112, + "velocityY": -2.8431182574823626, + "timestamp": 0.5284850345321761 + }, + { + "x": 2.489790439605713, + "y": 2.975529670715332, + "heading": 4.331854475647563e-17, + "angularVelocity": 1.2168250381293626e-17, + "velocityX": 1.1906931779513854, + "velocityY": -2.865780382453187, + "timestamp": 0.5945456638486981 + }, + { + "x": 2.566881258362406, + "y": 2.832040035804707, + "heading": 4.346891950915645e-17, + "angularVelocity": 2.9604733656461275e-18, + "velocityX": 1.5177103424948193, + "velocityY": -2.8249239852022456, + "timestamp": 0.6453398217312694 + }, + { + "x": 2.6605214587645087, + "y": 2.691067428150466, + "heading": 4.410412763377102e-17, + "angularVelocity": 1.2505535086604585e-17, + "velocityX": 1.8435230409486294, + "velocityY": -2.775370505800075, + "timestamp": 0.6961339796138406 + }, + { + "x": 2.770613075448226, + "y": 2.5531880922477597, + "heading": 4.417340863018206e-17, + "angularVelocity": 1.3639559691841993e-18, + "velocityX": 2.167407065557281, + "velocityY": -2.7144723261573476, + "timestamp": 0.7469281374964118 + }, + { + "x": 2.8969902149108995, + "y": 2.419181627819772, + "heading": 4.438492564713358e-17, + "angularVelocity": 4.164199663614473e-18, + "velocityX": 2.488025094437815, + "velocityY": -2.63822593019046, + "timestamp": 0.7977222953789831 + }, + { + "x": 3.039350168869116, + "y": 2.290151805534909, + "heading": 4.425906230668265e-17, + "angularVelocity": -2.477909823337866e-18, + "velocityX": 2.802683613484297, + "velocityY": -2.5402492661294147, + "timestamp": 0.8485164532615543 + }, + { + "x": 3.197082247467654, + "y": 2.1677488242794944, + "heading": 4.264246918702522e-17, + "angularVelocity": -3.182635938410446e-17, + "velocityX": 3.1053192960338345, + "velocityY": -2.409784635831394, + "timestamp": 0.8993106111441256 + }, + { + "x": 3.368783708739533, + "y": 2.054569369929602, + "heading": 4.142429145389646e-17, + "angularVelocity": -2.398263475091366e-17, + "velocityX": 3.3803387718097024, + "velocityY": -2.2281982627125525, + "timestamp": 0.9501047690266968 + }, + { + "x": 3.5508140800737693, + "y": 1.9545630137345602, + "heading": 3.816577991230304e-17, + "angularVelocity": -6.41513055323665e-17, + "velocityX": 3.5836871585717445, + "velocityY": -1.9688554818891133, + "timestamp": 1.000898926909268 + }, + { + "x": 3.7403070217648606, + "y": 1.8695408729970555, + "heading": 4.394413528240289e-17, + "angularVelocity": 1.137602356449689e-16, + "velocityX": 3.7306050457450586, + "velocityY": -1.673856685134212, + "timestamp": 1.0516930847918393 + }, + { + "x": 3.936031548275179, + "y": 1.8000552607614357, + "heading": 3.256197745041857e-17, + "angularVelocity": -2.240839952163188e-16, + "velocityX": 3.853288147089766, + "velocityY": -1.3679843338728146, + "timestamp": 1.1024872426744106 + }, + { + "x": 4.13671621691862, + "y": 1.7465575488326603, + "heading": 2.5319682073900963e-17, + "angularVelocity": -1.4258126679157706e-16, + "velocityX": 3.9509399704468953, + "velocityY": -1.0532256889159286, + "timestamp": 1.1532814005569818 + }, + { + "x": 4.341057366167047, + "y": 1.7093952475260255, + "heading": -7.734065432963739e-18, + "angularVelocity": -6.507391575084453e-16, + "velocityX": 4.022926213696339, + "velocityY": -0.7316255029278887, + "timestamp": 1.204075558439553 + }, + { + "x": 4.547727584838867, + "y": 1.6888097524642944, + "heading": -3.3452466298014664e-29, + "angularVelocity": 1.5226289312285738e-16, + "velocityX": 4.068779310203581, + "velocityY": -0.40527288806168965, + "timestamp": 1.2548697163221243 + }, + { + "x": 4.872720718401865, + "y": 1.6975768387772854, + "heading": -1.982534191916707e-17, + "angularVelocity": -2.493425929833114e-16, + "velocityX": 4.0874266358219495, + "velocityY": 0.11026332070896885, + "timestamp": 1.3343801669109567 + }, + { + "x": 5.1940235017101015, + "y": 1.74719458876398, + "heading": 1.0615577094344061e-17, + "angularVelocity": 3.828543139690045e-16, + "velocityX": 4.041013236986538, + "velocityY": 0.6240406087406034, + "timestamp": 1.4138906174997892 + }, + { + "x": 5.509692276544731, + "y": 1.8249803490868903, + "heading": -3.172576507949871e-18, + "angularVelocity": -1.734130985315765e-16, + "velocityX": 3.970154520530006, + "velocityY": 0.9783086342342504, + "timestamp": 1.4934010680886216 + }, + { + "x": 5.8253663131661115, + "y": 1.902744753011146, + "heading": 2.832120477993219e-17, + "angularVelocity": 3.960961239018336e-16, + "velocityX": 3.970220697827599, + "velocityY": 0.9780400355972578, + "timestamp": 1.572911518677454 + }, + { + "x": 6.141040349651611, + "y": 1.980509157486996, + "heading": 4.3091388724994025e-17, + "angularVelocity": 1.8576405787818657e-16, + "velocityX": 3.9702206961186195, + "velocityY": 0.9780400425346409, + "timestamp": 1.6524219692662865 + }, + { + "x": 6.456714386011232, + "y": 2.0582735624738286, + "heading": 3.5113592050441506e-17, + "angularVelocity": -1.0033645408255158e-16, + "velocityX": 3.9702206945354614, + "velocityY": 0.9780400489612482, + "timestamp": 1.731932419855119 + }, + { + "x": 6.772388422403809, + "y": 2.1360379673249357, + "heading": 3.856790983929373e-17, + "angularVelocity": 4.3444827230051585e-17, + "velocityX": 3.9702206949499432, + "velocityY": 0.9780400472542335, + "timestamp": 1.8114428704439514 + }, + { + "x": 7.051175893570039, + "y": 2.20471558817087, + "heading": 2.5601706470255025e-17, + "angularVelocity": -1.630754608117844e-16, + "velocityX": 3.5062997266599183, + "velocityY": 0.8637559004800854, + "timestamp": 1.8909533210327838 + }, + { + "x": 7.290136606671851, + "y": 2.263582126121373, + "heading": 1.755827518657081e-17, + "angularVelocity": -1.0116193813703912e-16, + "velocityX": 3.0054000616539693, + "velocityY": 0.7403622733182037, + "timestamp": 1.9704637716216162 + }, + { + "x": 7.489270543021993, + "y": 2.312637576572798, + "heading": 1.0387054455853603e-17, + "angularVelocity": -9.019217823090925e-17, + "velocityX": 2.5045001616191533, + "velocityY": 0.6169685882564364, + "timestamp": 2.049974222210449 + }, + { + "x": 7.648577696391329, + "y": 2.35188193799059, + "heading": 7.528064188393706e-18, + "angularVelocity": -3.595741498540887e-17, + "velocityX": 2.003600183240717, + "velocityY": 0.49357488389462933, + "timestamp": 2.129484672799281 + }, + { + "x": 7.768058063665282, + "y": 2.381315209607472, + "heading": 4.376696429652505e-18, + "angularVelocity": -3.9634635892779204e-17, + "velocityX": 1.502700165690348, + "velocityY": 0.37018116988278865, + "timestamp": 2.2089951233881138 + }, + { + "x": 7.847711642975101, + "y": 2.400937390963075, + "heading": 3.6531453006199625e-20, + "angularVelocity": -5.4586094589255846e-17, + "velocityX": 1.0018001246367811, + "velocityY": 0.24678745008092345, + "timestamp": 2.2885055739769458 + }, + { + "x": 7.887538433074951, + "y": 2.4107484817504883, + "heading": -1.0863094495902712e-28, + "angularVelocity": -4.594547368911346e-19, + "velocityX": 0.5009000679143996, + "velocityY": 0.12339372641904009, + "timestamp": 2.3680160245657786 + }, + { + "x": 7.887538433074951, + "y": 2.4107484817504883, + "heading": -7.163298591279974e-29, + "angularVelocity": 1.88053587914437e-30, + "velocityX": -1.0646456405257399e-17, + "velocityY": 2.0398963035439954e-17, + "timestamp": 2.4475264751546106 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-Close.3.traj b/src/main/deploy/choreo/3NB-Close.3.traj new file mode 100644 index 0000000..8298479 --- /dev/null +++ b/src/main/deploy/choreo/3NB-Close.3.traj @@ -0,0 +1,265 @@ +{ + "samples": [ + { + "x": 7.887538433074951, + "y": 2.4107484817504883, + "heading": -7.163298591279974e-29, + "angularVelocity": 1.88053587914437e-30, + "velocityX": -1.0646456405257399e-17, + "velocityY": 2.0398963035439954e-17, + "timestamp": 0 + }, + { + "x": 7.850990393264518, + "y": 2.398572533679078, + "heading": -1.177521215275177e-18, + "angularVelocity": -1.5281620709395144e-17, + "velocityX": -0.47431269588407104, + "velocityY": -0.1580168672423615, + "timestamp": 0.07705473652209927 + }, + { + "x": 7.77789431437577, + "y": 2.3742206377658586, + "heading": -3.5352413601672825e-18, + "angularVelocity": -3.059799124525414e-17, + "velocityX": -0.9486253822668896, + "velocityY": -0.3160337315050108, + "timestamp": 0.15410947304419853 + }, + { + "x": 7.668250197384856, + "y": 2.3376927943169763, + "heading": -7.072611394418981e-18, + "angularVelocity": -4.590723742808682e-17, + "velocityX": -1.4229380559814202, + "velocityY": -0.47405059179455206, + "timestamp": 0.2311642095662978 + }, + { + "x": 7.522058043658382, + "y": 2.2889890037610603, + "heading": -2.3232935434204652e-17, + "angularVelocity": -2.0972525521788622e-16, + "velocityX": -1.8972507119604403, + "velocityY": -0.6320674465214426, + "timestamp": 0.30821894608839706 + }, + { + "x": 7.339317855246242, + "y": 2.2281092667411033, + "heading": -3.444177035375071e-17, + "angularVelocity": -1.4546587686435312e-16, + "velocityX": -2.3715633413363952, + "velocityY": -0.7900842929037087, + "timestamp": 0.3852736826104963 + }, + { + "x": 7.1200296355648796, + "y": 2.155053584328892, + "heading": -4.681767867288413e-17, + "angularVelocity": -1.606119068108914e-16, + "velocityX": -2.8458759263744398, + "velocityY": -0.9481011253765523, + "timestamp": 0.4623284191325956 + }, + { + "x": 6.864193391447034, + "y": 2.069821958668493, + "heading": -6.091862473343017e-17, + "angularVelocity": -1.8299908088539646e-16, + "velocityX": -3.320188422738658, + "velocityY": -1.1061179300241564, + "timestamp": 0.5393831556546944 + }, + { + "x": 6.571809143389688, + "y": 1.9724143961960199, + "heading": -6.906000748724748e-17, + "angularVelocity": -1.0565713593171376e-16, + "velocityX": -3.794500653097392, + "velocityY": -1.2641346511452527, + "timestamp": 0.6164378921767932 + }, + { + "x": 6.272890863117038, + "y": 1.8728299317270023, + "heading": -7.894686935840572e-17, + "angularVelocity": -1.2830959286049874e-16, + "velocityX": -3.8792979350065835, + "velocityY": -1.2923860227649977, + "timestamp": 0.693492628698892 + }, + { + "x": 5.973997771041301, + "y": 1.7731698928043986, + "heading": -9.67999949293296e-17, + "angularVelocity": -2.316940706917987e-16, + "velocityX": -3.87897104793804, + "velocityY": -1.2933668119685562, + "timestamp": 0.7705473652209909 + }, + { + "x": 5.669879935237952, + "y": 1.690819752200994, + "heading": -1.1050901644346994e-16, + "angularVelocity": -1.779127686741746e-16, + "velocityX": -3.9467766620175, + "velocityY": -1.0687226291385734, + "timestamp": 0.8476021017430897 + }, + { + "x": 5.357985582093433, + "y": 1.6461978364052787, + "heading": -1.2113052448910788e-16, + "angularVelocity": -1.3784367483440315e-16, + "velocityX": -4.0476986519195854, + "velocityY": -0.5790937430940615, + "timestamp": 0.9246568382651885 + }, + { + "x": 5.042976936505313, + "y": 1.639972541371015, + "heading": -5.4820283875488555e-17, + "angularVelocity": 8.605602148108709e-16, + "velocityX": -4.0881152776088845, + "velocityY": -0.0807905563660157, + "timestamp": 1.0017115747872873 + }, + { + "x": 4.729563151516996, + "y": 1.6722370314267299, + "heading": 4.449704756389741e-18, + "angularVelocity": 7.691933203211217e-16, + "velocityX": -4.06741751557905, + "velocityY": 0.41872169722451913, + "timestamp": 1.0787663113093862 + }, + { + "x": 4.42242956161499, + "y": 1.7425090074539185, + "heading": -1.5670272990102803e-28, + "angularVelocity": -5.774732297268101e-17, + "velocityX": -3.9859144780012232, + "velocityY": 0.9119747753214509, + "timestamp": 1.155821047831485 + }, + { + "x": 4.128704472216037, + "y": 1.8484554786573326, + "heading": 1.5665974518926906e-17, + "angularVelocity": 2.0514694884473714e-16, + "velocityX": -3.846349029561837, + "velocityY": 1.3873758878840463, + "timestamp": 1.2321856957460344 + }, + { + "x": 3.8499499444697896, + "y": 1.9891501856691707, + "heading": -4.951052254344938e-18, + "angularVelocity": -2.699812981022409e-16, + "velocityX": -3.650308557150352, + "velocityY": 1.842406281624709, + "timestamp": 1.3085503436605839 + }, + { + "x": 3.590258853943372, + "y": 2.1625273105920915, + "heading": -4.443248682637518e-18, + "angularVelocity": 6.64972059401854e-18, + "velocityX": -3.4006716146588456, + "velocityY": 2.2703846564828614, + "timestamp": 1.3849149915751333 + }, + { + "x": 3.35344412752997, + "y": 2.3660411333508096, + "heading": -1.2020557304661416e-17, + "angularVelocity": -9.92253461355458e-17, + "velocityX": -3.1011041480660335, + "velocityY": 2.665026662421532, + "timestamp": 1.4612796394896828 + }, + { + "x": 3.149014297898049, + "y": 2.58911892320566, + "heading": -3.113544050291928e-17, + "angularVelocity": -2.5031060068627764e-16, + "velocityX": -2.6770218316292542, + "velocityY": 2.9212180759932416, + "timestamp": 1.5376442874042322 + }, + { + "x": 2.9718701800847187, + "y": 2.785985113880848, + "heading": -3.7052285700782595e-17, + "angularVelocity": -7.748146793988773e-17, + "velocityX": -2.319713671849138, + "velocityY": 2.5779754906417227, + "timestamp": 1.6140089353187816 + }, + { + "x": 2.8207247945797818, + "y": 2.955362663178613, + "heading": -3.796704916740075e-17, + "angularVelocity": -1.1978890626359567e-17, + "velocityX": -1.9792585919345465, + "velocityY": 2.2180099551732835, + "timestamp": 1.690373583233331 + }, + { + "x": 2.6951291405768862, + "y": 3.096833739929104, + "heading": -3.0938817386489246e-17, + "angularVelocity": 9.203514802265678e-17, + "velocityX": -1.6446832065987498, + "velocityY": 1.8525728935305812, + "timestamp": 1.7667382311478805 + }, + { + "x": 2.594855061076495, + "y": 3.2101911526058116, + "heading": -1.8551698396752348e-17, + "angularVelocity": 1.6221011125667273e-16, + "velocityX": -1.313095551918033, + "velocityY": 1.4844226454571197, + "timestamp": 1.84310287906243 + }, + { + "x": 2.519764506253801, + "y": 3.2953111702184255, + "heading": -8.201545305515358e-18, + "angularVelocity": 1.3553591742048043e-16, + "velocityX": -0.9833156686155428, + "velocityY": 1.1146521320686176, + "timestamp": 1.9194675269769794 + }, + { + "x": 2.469764964962585, + "y": 3.352111556180341, + "heading": -9.057115232772252e-20, + "angularVelocity": 1.0621372327118296e-16, + "velocityX": -0.654747224751943, + "velocityY": 0.7438047252634438, + "timestamp": 1.9958321748915289 + }, + { + "x": 2.4447901248931885, + "y": 3.3805336952209473, + "heading": 3.7188233361746653e-28, + "angularVelocity": 1.186034116335578e-18, + "velocityX": -0.32704714486917663, + "velocityY": 0.37218974770122537, + "timestamp": 2.0721968228060783 + }, + { + "x": 2.4447901248931885, + "y": 3.3805336952209473, + "heading": 1.8360534693167372e-28, + "angularVelocity": -6.116044573310473e-29, + "velocityX": -9.224611572050135e-18, + "velocityY": -5.199995140976138e-18, + "timestamp": 2.1485614707206278 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-Close.traj b/src/main/deploy/choreo/3NB-Close.traj new file mode 100644 index 0000000..4e30042 --- /dev/null +++ b/src/main/deploy/choreo/3NB-Close.traj @@ -0,0 +1,742 @@ +{ + "samples": [ + { + "x": 0.756615161895752, + "y": 4.431221961975098, + "heading": -1.034985029325618, + "angularVelocity": -1.2153572582704763e-25, + "velocityX": -8.099282150741186e-25, + "velocityY": 3.9386843631996907e-25, + "timestamp": 0 + }, + { + "x": 0.7825301409073798, + "y": 4.426111948589589, + "heading": -1.0184480247142116, + "angularVelocity": 0.25729744903186014, + "velocityX": 0.4032083287203581, + "velocityY": -0.079506140289953, + "timestamp": 0.06427193380125104 + }, + { + "x": 0.8343681989936829, + "y": 4.4158863120476, + "heading": -0.9855108542950051, + "angularVelocity": 0.5124658380601789, + "velocityX": 0.8065426854371999, + "velocityY": -0.1590995623938954, + "timestamp": 0.12854386760250208 + }, + { + "x": 0.912137967019861, + "y": 4.4005383558351046, + "heading": -0.9362991408222285, + "angularVelocity": 0.765679676372499, + "velocityX": 1.210011328843265, + "velocityY": -0.2387971748283722, + "timestamp": 0.19281580140375312 + }, + { + "x": 1.0158503745896044, + "y": 4.380061388038885, + "heading": -0.8709611243682479, + "angularVelocity": 1.0165870635855798, + "velocityX": 1.6136500247597099, + "velocityY": -0.31859890600991186, + "timestamp": 0.25708773520500416 + }, + { + "x": 1.145523011747702, + "y": 4.354449737528758, + "heading": -0.7897691851783357, + "angularVelocity": 1.2632565163043505, + "velocityX": 2.0175624022623317, + "velocityY": -0.39848887368670943, + "timestamp": 0.32135966900625523 + }, + { + "x": 1.3011913004178595, + "y": 4.32369771075132, + "heading": -0.6934250833799015, + "angularVelocity": 1.499007359827703, + "velocityX": 2.422025905608079, + "velocityY": -0.4784674267392117, + "timestamp": 0.3856316028075063 + }, + { + "x": 1.4829411037935518, + "y": 4.287786779701849, + "heading": -0.58418273386785, + "angularVelocity": 1.6996897876118522, + "velocityX": 2.827825345005485, + "velocityY": -0.558734254994066, + "timestamp": 0.44990353660875737 + }, + { + "x": 1.6909603916353093, + "y": 4.2465633189263965, + "heading": -0.47548948460818546, + "angularVelocity": 1.6911463967425988, + "velocityX": 3.236549385382092, + "velocityY": -0.6413913249121974, + "timestamp": 0.5141754704100084 + }, + { + "x": 1.8728106274219034, + "y": 4.211006125740984, + "heading": -0.36224313486482546, + "angularVelocity": 1.761987590003954, + "velocityX": 2.829387961920238, + "velocityY": -0.5532304861927155, + "timestamp": 0.5784474042112595 + }, + { + "x": 2.0285640030407976, + "y": 4.180677932000477, + "heading": -0.2611966487318228, + "angularVelocity": 1.572171244224113, + "velocityX": 2.4233497641526145, + "velocityY": -0.47187305479700664, + "timestamp": 0.6427193380125106 + }, + { + "x": 2.1583029244219367, + "y": 4.155482855052808, + "heading": -0.17529966695510088, + "angularVelocity": 1.3364617601571205, + "velocityX": 2.018593711250895, + "velocityY": -0.39200745111512536, + "timestamp": 0.7069912718137616 + }, + { + "x": 2.262065112690402, + "y": 4.135371482146998, + "heading": -0.10573833466683616, + "angularVelocity": 1.082297173496758, + "velocityX": 1.6144245572154519, + "velocityY": -0.3129106550302491, + "timestamp": 0.7712632056150127 + }, + { + "x": 2.339870608422664, + "y": 4.120312466180706, + "heading": -0.05311299136785256, + "angularVelocity": 0.8187919700956509, + "velocityX": 1.2105672123210285, + "velocityY": -0.2343015850878145, + "timestamp": 0.8355351394162638 + }, + { + "x": 2.3917318142501975, + "y": 4.110284912416539, + "heading": -0.017780765488657927, + "angularVelocity": 0.5497302444400217, + "velocityX": 0.8069028386154424, + "velocityY": -0.15601761408291742, + "timestamp": 0.8998070732175149 + }, + { + "x": 2.4176580905914307, + "y": 4.1052751541137695, + "heading": 8.497464730716836e-25, + "angularVelocity": 0.27664898871164556, + "velocityX": 0.4033841026381037, + "velocityY": -0.0779462824047118, + "timestamp": 0.9640790070187659 + }, + { + "x": 2.4176580905914307, + "y": 4.1052751541137695, + "heading": 4.243816717185121e-25, + "angularVelocity": -1.1560345853556316e-25, + "velocityX": 1.998362528445031e-20, + "velocityY": -7.254658147615057e-21, + "timestamp": 1.0283509408200169 + }, + { + "x": 2.4083001000392614, + "y": 4.07855200651975, + "heading": 5.678182580423427e-18, + "angularVelocity": 8.595410324058872e-17, + "velocityX": -0.14165760527850227, + "velocityY": -0.40452456887715366, + "timestamp": 1.094411570136539 + }, + { + "x": 2.391723371861385, + "y": 4.0244502470643715, + "heading": 1.2092714840241192e-17, + "angularVelocity": 9.710068345931569e-17, + "velocityX": -0.2509320354556493, + "velocityY": -0.818971299776089, + "timestamp": 1.160472199453061 + }, + { + "x": 2.370959981958984, + "y": 3.9423454455218585, + "heading": 2.0506186000436857e-17, + "angularVelocity": 1.2735983962200107e-16, + "velocityX": -0.31430808512156017, + "velocityY": -1.242870411499066, + "timestamp": 1.226532828769583 + }, + { + "x": 2.3504243304381753, + "y": 3.831927280299648, + "heading": 2.7876588197896166e-17, + "angularVelocity": 1.1157026914706097e-16, + "velocityX": -0.3108606704670367, + "velocityY": -1.6714670502630964, + "timestamp": 1.292593458086105 + }, + { + "x": 2.3364844874655226, + "y": 3.6939737986961876, + "heading": 3.612951501320603e-17, + "angularVelocity": 1.2492958211265208e-16, + "velocityX": -0.21101589731853923, + "velocityY": -2.088285912967496, + "timestamp": 1.358654087402627 + }, + { + "x": 2.3373423735310555, + "y": 3.531880609628321, + "heading": 3.956479825579656e-17, + "angularVelocity": 5.2001975734663045e-17, + "velocityX": 0.012986344126731107, + "velocityY": -2.4537033743837897, + "timestamp": 1.424714716719149 + }, + { + "x": 2.3607492318969396, + "y": 3.3526631075737106, + "heading": 4.165260110772871e-17, + "angularVelocity": 3.1604343976295837e-17, + "velocityX": 0.35432387805046023, + "velocityY": -2.7129245347620055, + "timestamp": 1.490775346035671 + }, + { + "x": 2.4111324989473553, + "y": 3.1648449262631324, + "heading": 4.251470247939207e-17, + "angularVelocity": 1.3050153801581261e-17, + "velocityX": 0.7626822143793112, + "velocityY": -2.8431182574823626, + "timestamp": 1.556835975352193 + }, + { + "x": 2.489790439605713, + "y": 2.975529670715332, + "heading": 4.331854475647563e-17, + "angularVelocity": 1.2168250381293626e-17, + "velocityX": 1.1906931779513854, + "velocityY": -2.865780382453187, + "timestamp": 1.622896604668715 + }, + { + "x": 2.566881258362406, + "y": 2.832040035804707, + "heading": 4.346891950915645e-17, + "angularVelocity": 2.9604733656461275e-18, + "velocityX": 1.5177103424948193, + "velocityY": -2.8249239852022456, + "timestamp": 1.6736907625512862 + }, + { + "x": 2.6605214587645087, + "y": 2.691067428150466, + "heading": 4.410412763377102e-17, + "angularVelocity": 1.2505535086604585e-17, + "velocityX": 1.8435230409486294, + "velocityY": -2.775370505800075, + "timestamp": 1.7244849204338575 + }, + { + "x": 2.770613075448226, + "y": 2.5531880922477597, + "heading": 4.417340863018206e-17, + "angularVelocity": 1.3639559691841993e-18, + "velocityX": 2.167407065557281, + "velocityY": -2.7144723261573476, + "timestamp": 1.7752790783164287 + }, + { + "x": 2.8969902149108995, + "y": 2.419181627819772, + "heading": 4.438492564713358e-17, + "angularVelocity": 4.164199663614473e-18, + "velocityX": 2.488025094437815, + "velocityY": -2.63822593019046, + "timestamp": 1.826073236199 + }, + { + "x": 3.039350168869116, + "y": 2.290151805534909, + "heading": 4.425906230668265e-17, + "angularVelocity": -2.477909823337866e-18, + "velocityX": 2.802683613484297, + "velocityY": -2.5402492661294147, + "timestamp": 1.8768673940815712 + }, + { + "x": 3.197082247467654, + "y": 2.1677488242794944, + "heading": 4.264246918702522e-17, + "angularVelocity": -3.182635938410446e-17, + "velocityX": 3.1053192960338345, + "velocityY": -2.409784635831394, + "timestamp": 1.9276615519641425 + }, + { + "x": 3.368783708739533, + "y": 2.054569369929602, + "heading": 4.142429145389646e-17, + "angularVelocity": -2.398263475091366e-17, + "velocityX": 3.3803387718097024, + "velocityY": -2.2281982627125525, + "timestamp": 1.9784557098467137 + }, + { + "x": 3.5508140800737693, + "y": 1.9545630137345602, + "heading": 3.816577991230304e-17, + "angularVelocity": -6.41513055323665e-17, + "velocityX": 3.5836871585717445, + "velocityY": -1.9688554818891133, + "timestamp": 2.029249867729285 + }, + { + "x": 3.7403070217648606, + "y": 1.8695408729970555, + "heading": 4.394413528240289e-17, + "angularVelocity": 1.137602356449689e-16, + "velocityX": 3.7306050457450586, + "velocityY": -1.673856685134212, + "timestamp": 2.080044025611856 + }, + { + "x": 3.936031548275179, + "y": 1.8000552607614357, + "heading": 3.256197745041857e-17, + "angularVelocity": -2.240839952163188e-16, + "velocityX": 3.853288147089766, + "velocityY": -1.3679843338728146, + "timestamp": 2.1308381834944274 + }, + { + "x": 4.13671621691862, + "y": 1.7465575488326603, + "heading": 2.5319682073900963e-17, + "angularVelocity": -1.4258126679157706e-16, + "velocityX": 3.9509399704468953, + "velocityY": -1.0532256889159286, + "timestamp": 2.1816323413769987 + }, + { + "x": 4.341057366167047, + "y": 1.7093952475260255, + "heading": -7.734065432963739e-18, + "angularVelocity": -6.507391575084453e-16, + "velocityX": 4.022926213696339, + "velocityY": -0.7316255029278887, + "timestamp": 2.23242649925957 + }, + { + "x": 4.547727584838867, + "y": 1.6888097524642944, + "heading": -3.3452466298014664e-29, + "angularVelocity": 1.5226289312285738e-16, + "velocityX": 4.068779310203581, + "velocityY": -0.40527288806168965, + "timestamp": 2.283220657142141 + }, + { + "x": 4.872720718401865, + "y": 1.6975768387772854, + "heading": -1.982534191916707e-17, + "angularVelocity": -2.493425929833114e-16, + "velocityX": 4.0874266358219495, + "velocityY": 0.11026332070896885, + "timestamp": 2.3627311077309736 + }, + { + "x": 5.1940235017101015, + "y": 1.74719458876398, + "heading": 1.0615577094344061e-17, + "angularVelocity": 3.828543139690045e-16, + "velocityX": 4.041013236986538, + "velocityY": 0.6240406087406034, + "timestamp": 2.442241558319806 + }, + { + "x": 5.509692276544731, + "y": 1.8249803490868903, + "heading": -3.172576507949871e-18, + "angularVelocity": -1.734130985315765e-16, + "velocityX": 3.970154520530006, + "velocityY": 0.9783086342342504, + "timestamp": 2.5217520089086385 + }, + { + "x": 5.8253663131661115, + "y": 1.902744753011146, + "heading": 2.832120477993219e-17, + "angularVelocity": 3.960961239018336e-16, + "velocityX": 3.970220697827599, + "velocityY": 0.9780400355972578, + "timestamp": 2.601262459497471 + }, + { + "x": 6.141040349651611, + "y": 1.980509157486996, + "heading": 4.3091388724994025e-17, + "angularVelocity": 1.8576405787818657e-16, + "velocityX": 3.9702206961186195, + "velocityY": 0.9780400425346409, + "timestamp": 2.6807729100863034 + }, + { + "x": 6.456714386011232, + "y": 2.0582735624738286, + "heading": 3.5113592050441506e-17, + "angularVelocity": -1.0033645408255158e-16, + "velocityX": 3.9702206945354614, + "velocityY": 0.9780400489612482, + "timestamp": 2.760283360675136 + }, + { + "x": 6.772388422403809, + "y": 2.1360379673249357, + "heading": 3.856790983929373e-17, + "angularVelocity": 4.3444827230051585e-17, + "velocityX": 3.9702206949499432, + "velocityY": 0.9780400472542335, + "timestamp": 2.8397938112639682 + }, + { + "x": 7.051175893570039, + "y": 2.20471558817087, + "heading": 2.5601706470255025e-17, + "angularVelocity": -1.630754608117844e-16, + "velocityX": 3.5062997266599183, + "velocityY": 0.8637559004800854, + "timestamp": 2.9193042618528007 + }, + { + "x": 7.290136606671851, + "y": 2.263582126121373, + "heading": 1.755827518657081e-17, + "angularVelocity": -1.0116193813703912e-16, + "velocityX": 3.0054000616539693, + "velocityY": 0.7403622733182037, + "timestamp": 2.998814712441633 + }, + { + "x": 7.489270543021993, + "y": 2.312637576572798, + "heading": 1.0387054455853603e-17, + "angularVelocity": -9.019217823090925e-17, + "velocityX": 2.5045001616191533, + "velocityY": 0.6169685882564364, + "timestamp": 3.0783251630304655 + }, + { + "x": 7.648577696391329, + "y": 2.35188193799059, + "heading": 7.528064188393706e-18, + "angularVelocity": -3.595741498540887e-17, + "velocityX": 2.003600183240717, + "velocityY": 0.49357488389462933, + "timestamp": 3.157835613619298 + }, + { + "x": 7.768058063665282, + "y": 2.381315209607472, + "heading": 4.376696429652505e-18, + "angularVelocity": -3.9634635892779204e-17, + "velocityX": 1.502700165690348, + "velocityY": 0.37018116988278865, + "timestamp": 3.2373460642081304 + }, + { + "x": 7.847711642975101, + "y": 2.400937390963075, + "heading": 3.6531453006199625e-20, + "angularVelocity": -5.4586094589255846e-17, + "velocityX": 1.0018001246367811, + "velocityY": 0.24678745008092345, + "timestamp": 3.316856514796963 + }, + { + "x": 7.887538433074951, + "y": 2.4107484817504883, + "heading": -1.0863094495902712e-28, + "angularVelocity": -4.594547368911346e-19, + "velocityX": 0.5009000679143996, + "velocityY": 0.12339372641904009, + "timestamp": 3.3963669653857953 + }, + { + "x": 7.887538433074951, + "y": 2.4107484817504883, + "heading": -7.163298591279974e-29, + "angularVelocity": 1.88053587914437e-30, + "velocityX": -1.0646456405257399e-17, + "velocityY": 2.0398963035439954e-17, + "timestamp": 3.4758774159746277 + }, + { + "x": 7.850990393264518, + "y": 2.398572533679078, + "heading": -1.177521215275177e-18, + "angularVelocity": -1.5281620709395144e-17, + "velocityX": -0.47431269588407104, + "velocityY": -0.1580168672423615, + "timestamp": 3.552932152496727 + }, + { + "x": 7.77789431437577, + "y": 2.3742206377658586, + "heading": -3.5352413601672825e-18, + "angularVelocity": -3.059799124525414e-17, + "velocityX": -0.9486253822668896, + "velocityY": -0.3160337315050108, + "timestamp": 3.6299868890188263 + }, + { + "x": 7.668250197384856, + "y": 2.3376927943169763, + "heading": -7.072611394418981e-18, + "angularVelocity": -4.590723742808682e-17, + "velocityX": -1.4229380559814202, + "velocityY": -0.47405059179455206, + "timestamp": 3.7070416255409255 + }, + { + "x": 7.522058043658382, + "y": 2.2889890037610603, + "heading": -2.3232935434204652e-17, + "angularVelocity": -2.0972525521788622e-16, + "velocityX": -1.8972507119604403, + "velocityY": -0.6320674465214426, + "timestamp": 3.784096362063025 + }, + { + "x": 7.339317855246242, + "y": 2.2281092667411033, + "heading": -3.444177035375071e-17, + "angularVelocity": -1.4546587686435312e-16, + "velocityX": -2.3715633413363952, + "velocityY": -0.7900842929037087, + "timestamp": 3.861151098585124 + }, + { + "x": 7.1200296355648796, + "y": 2.155053584328892, + "heading": -4.681767867288413e-17, + "angularVelocity": -1.606119068108914e-16, + "velocityX": -2.8458759263744398, + "velocityY": -0.9481011253765523, + "timestamp": 3.9382058351072233 + }, + { + "x": 6.864193391447034, + "y": 2.069821958668493, + "heading": -6.091862473343017e-17, + "angularVelocity": -1.8299908088539646e-16, + "velocityX": -3.320188422738658, + "velocityY": -1.1061179300241564, + "timestamp": 4.015260571629322 + }, + { + "x": 6.571809143389688, + "y": 1.9724143961960199, + "heading": -6.906000748724748e-17, + "angularVelocity": -1.0565713593171376e-16, + "velocityX": -3.794500653097392, + "velocityY": -1.2641346511452527, + "timestamp": 4.092315308151421 + }, + { + "x": 6.272890863117038, + "y": 1.8728299317270023, + "heading": -7.894686935840572e-17, + "angularVelocity": -1.2830959286049874e-16, + "velocityX": -3.8792979350065835, + "velocityY": -1.2923860227649977, + "timestamp": 4.16937004467352 + }, + { + "x": 5.973997771041301, + "y": 1.7731698928043986, + "heading": -9.67999949293296e-17, + "angularVelocity": -2.316940706917987e-16, + "velocityX": -3.87897104793804, + "velocityY": -1.2933668119685562, + "timestamp": 4.246424781195619 + }, + { + "x": 5.669879935237952, + "y": 1.690819752200994, + "heading": -1.1050901644346994e-16, + "angularVelocity": -1.779127686741746e-16, + "velocityX": -3.9467766620175, + "velocityY": -1.0687226291385734, + "timestamp": 4.323479517717717 + }, + { + "x": 5.357985582093433, + "y": 1.6461978364052787, + "heading": -1.2113052448910788e-16, + "angularVelocity": -1.3784367483440315e-16, + "velocityX": -4.0476986519195854, + "velocityY": -0.5790937430940615, + "timestamp": 4.400534254239816 + }, + { + "x": 5.042976936505313, + "y": 1.639972541371015, + "heading": -5.4820283875488555e-17, + "angularVelocity": 8.605602148108709e-16, + "velocityX": -4.0881152776088845, + "velocityY": -0.0807905563660157, + "timestamp": 4.477588990761915 + }, + { + "x": 4.729563151516996, + "y": 1.6722370314267299, + "heading": 4.449704756389741e-18, + "angularVelocity": 7.691933203211217e-16, + "velocityX": -4.06741751557905, + "velocityY": 0.41872169722451913, + "timestamp": 4.554643727284014 + }, + { + "x": 4.42242956161499, + "y": 1.7425090074539185, + "heading": -1.5670272990102803e-28, + "angularVelocity": -5.774732297268101e-17, + "velocityX": -3.9859144780012232, + "velocityY": 0.9119747753214509, + "timestamp": 4.631698463806113 + }, + { + "x": 4.128704472216037, + "y": 1.8484554786573326, + "heading": 1.5665974518926906e-17, + "angularVelocity": 2.0514694884473714e-16, + "velocityX": -3.846349029561837, + "velocityY": 1.3873758878840463, + "timestamp": 4.708063111720662 + }, + { + "x": 3.8499499444697896, + "y": 1.9891501856691707, + "heading": -4.951052254344938e-18, + "angularVelocity": -2.699812981022409e-16, + "velocityX": -3.650308557150352, + "velocityY": 1.842406281624709, + "timestamp": 4.784427759635212 + }, + { + "x": 3.590258853943372, + "y": 2.1625273105920915, + "heading": -4.443248682637518e-18, + "angularVelocity": 6.64972059401854e-18, + "velocityX": -3.4006716146588456, + "velocityY": 2.2703846564828614, + "timestamp": 4.860792407549761 + }, + { + "x": 3.35344412752997, + "y": 2.3660411333508096, + "heading": -1.2020557304661416e-17, + "angularVelocity": -9.92253461355458e-17, + "velocityX": -3.1011041480660335, + "velocityY": 2.665026662421532, + "timestamp": 4.9371570554643105 + }, + { + "x": 3.149014297898049, + "y": 2.58911892320566, + "heading": -3.113544050291928e-17, + "angularVelocity": -2.5031060068627764e-16, + "velocityX": -2.6770218316292542, + "velocityY": 2.9212180759932416, + "timestamp": 5.01352170337886 + }, + { + "x": 2.9718701800847187, + "y": 2.785985113880848, + "heading": -3.7052285700782595e-17, + "angularVelocity": -7.748146793988773e-17, + "velocityX": -2.319713671849138, + "velocityY": 2.5779754906417227, + "timestamp": 5.089886351293409 + }, + { + "x": 2.8207247945797818, + "y": 2.955362663178613, + "heading": -3.796704916740075e-17, + "angularVelocity": -1.1978890626359567e-17, + "velocityX": -1.9792585919345465, + "velocityY": 2.2180099551732835, + "timestamp": 5.166250999207959 + }, + { + "x": 2.6951291405768862, + "y": 3.096833739929104, + "heading": -3.0938817386489246e-17, + "angularVelocity": 9.203514802265678e-17, + "velocityX": -1.6446832065987498, + "velocityY": 1.8525728935305812, + "timestamp": 5.242615647122508 + }, + { + "x": 2.594855061076495, + "y": 3.2101911526058116, + "heading": -1.8551698396752348e-17, + "angularVelocity": 1.6221011125667273e-16, + "velocityX": -1.313095551918033, + "velocityY": 1.4844226454571197, + "timestamp": 5.318980295037058 + }, + { + "x": 2.519764506253801, + "y": 3.2953111702184255, + "heading": -8.201545305515358e-18, + "angularVelocity": 1.3553591742048043e-16, + "velocityX": -0.9833156686155428, + "velocityY": 1.1146521320686176, + "timestamp": 5.395344942951607 + }, + { + "x": 2.469764964962585, + "y": 3.352111556180341, + "heading": -9.057115232772252e-20, + "angularVelocity": 1.0621372327118296e-16, + "velocityX": -0.654747224751943, + "velocityY": 0.7438047252634438, + "timestamp": 5.471709590866157 + }, + { + "x": 2.4447901248931885, + "y": 3.3805336952209473, + "heading": 3.7188233361746653e-28, + "angularVelocity": 1.186034116335578e-18, + "velocityX": -0.32704714486917663, + "velocityY": 0.37218974770122537, + "timestamp": 5.548074238780706 + }, + { + "x": 2.4447901248931885, + "y": 3.3805336952209473, + "heading": 1.8360534693167372e-28, + "angularVelocity": -6.116044573310473e-29, + "velocityX": -9.224611572050135e-18, + "velocityY": -5.199995140976138e-18, + "timestamp": 5.6244388866952555 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-Skip.1.traj b/src/main/deploy/choreo/3NB-Skip.1.traj new file mode 100644 index 0000000..715c2e4 --- /dev/null +++ b/src/main/deploy/choreo/3NB-Skip.1.traj @@ -0,0 +1,373 @@ +{ + "samples": [ + { + "x": 0.7100000000000002, + "y": 4.38, + "heading": -1.0303768814651526, + "angularVelocity": 1.5560405504853757e-19, + "velocityX": 3.6579942357642265e-17, + "velocityY": 3.5772569104609715e-17, + "timestamp": 0 + }, + { + "x": 0.7196231633442646, + "y": 4.37055163049096, + "heading": -1.0120510397413214, + "angularVelocity": 0.3659111239569298, + "velocityX": 0.19214519957043122, + "velocityY": -0.1886551001974958, + "timestamp": 0.05008276743722991 + }, + { + "x": 0.7389303360988976, + "y": 4.351635791048551, + "heading": -0.9762703828685111, + "angularVelocity": 0.7144305058152697, + "velocityX": 0.3855053093628394, + "velocityY": -0.3776915775693403, + "timestamp": 0.10016553487445982 + }, + { + "x": 0.7679871206287426, + "y": 4.323231755138663, + "heading": -0.9240353862773414, + "angularVelocity": 1.0429734470371541, + "velocityX": 0.5801752981455696, + "velocityY": -0.5671419005645384, + "timestamp": 0.15024830231168973 + }, + { + "x": 0.806863824332317, + "y": 4.285312709598439, + "heading": -0.8565791228845684, + "angularVelocity": 1.3468956857728758, + "velocityX": 0.7762491110800954, + "velocityY": -0.7571276005812321, + "timestamp": 0.20033106974891965 + }, + { + "x": 0.855633414600585, + "y": 4.2378416688864, + "heading": -0.7754660421005081, + "angularVelocity": 1.6195806448938828, + "velocityX": 0.9737798600965913, + "velocityY": -0.9478517889716715, + "timestamp": 0.25041383718614957 + }, + { + "x": 0.914367608814468, + "y": 4.180769559172441, + "heading": -0.6826698176675856, + "angularVelocity": 1.8528573635477696, + "velocityX": 1.1727425863098562, + "velocityY": -1.1395558319631454, + "timestamp": 0.30049660462337946 + }, + { + "x": 0.9831324058657855, + "y": 4.114037722550235, + "heading": -0.5806325593431202, + "angularVelocity": 2.0373726043064977, + "velocityX": 1.3730231089468503, + "velocityY": -1.3324310943047128, + "timestamp": 0.35057937206060935 + }, + { + "x": 1.0619841550917224, + "y": 4.037586277197585, + "heading": -0.47236160213245165, + "angularVelocity": 2.161840544182557, + "velocityX": 1.5744287558544843, + "velocityY": -1.5265020138605254, + "timestamp": 0.40066213949783924 + }, + { + "x": 1.1509626755897528, + "y": 3.9513700864804493, + "heading": -0.3616923537124617, + "angularVelocity": 2.209727099419903, + "velocityX": 1.7766294686003532, + "velocityY": -1.721474174229327, + "timestamp": 0.45074490693506913 + }, + { + "x": 1.2500676182895643, + "y": 3.855395460973611, + "heading": -0.2538729536839052, + "angularVelocity": 2.1528243255265638, + "velocityX": 1.9788232114775315, + "velocityY": -1.9163203316812512, + "timestamp": 0.500827674372299 + }, + { + "x": 1.3591936256250599, + "y": 3.749835630519891, + "heading": -0.15652201954824588, + "angularVelocity": 1.9438010141446913, + "velocityX": 2.1789132853386093, + "velocityY": -2.1077076179149334, + "timestamp": 0.550910441809529 + }, + { + "x": 1.4780432446812957, + "y": 3.6354509708518212, + "heading": -0.08074916244880044, + "angularVelocity": 1.5129526776732942, + "velocityX": 2.3730641323924204, + "velocityY": -2.283912521635818, + "timestamp": 0.6009932092467589 + }, + { + "x": 1.6064830934074645, + "y": 3.512581098747126, + "heading": -0.02955142499135005, + "angularVelocity": 1.0222625481233227, + "velocityX": 2.564551747008494, + "velocityY": -2.453336314904971, + "timestamp": 0.6510759766839889 + }, + { + "x": 1.7444408206294664, + "y": 3.3811278178903486, + "heading": -0.002660437876514541, + "angularVelocity": 0.5369309343486035, + "velocityX": 2.7545947295123865, + "velocityY": -2.6247207888730513, + "timestamp": 0.7011587441212188 + }, + { + "x": 1.8918929861971419, + "y": 3.2410564135466466, + "heading": -1.0357533070062969e-7, + "angularVelocity": 0.053118755957712996, + "velocityX": 2.944169683762683, + "velocityY": -2.796798410137877, + "timestamp": 0.7512415115584488 + }, + { + "x": 2.042423702860939, + "y": 3.102214682631318, + "heading": -7.051190961599685e-8, + "angularVelocity": 6.601756010788189e-7, + "velocityX": 3.005638952608048, + "velocityY": -2.772245585058687, + "timestamp": 0.8013242789956787 + }, + { + "x": 2.2019910957141193, + "y": 2.9738606026990846, + "heading": -3.641731647880258e-8, + "angularVelocity": 6.807649595630949e-7, + "velocityX": 3.1860737938089994, + "velocityY": -2.5628392059822205, + "timestamp": 0.8514070464329087 + }, + { + "x": 2.3699999999999997, + "y": 2.856772899627685, + "heading": 1.2045924074798219e-17, + "angularVelocity": 7.271426553754078e-7, + "velocityX": 3.3546250114165237, + "velocityY": -2.337884048003882, + "timestamp": 0.9014898138701386 + }, + { + "x": 2.6672211579606206, + "y": 2.6962515532804225, + "heading": 4.4196760900488604e-15, + "angularVelocity": 5.335264470419185e-14, + "velocityX": 3.597746231094708, + "velocityY": -1.9430483105374854, + "timestamp": 0.9841029663464744 + }, + { + "x": 2.9807027390289593, + "y": 2.5704071318299984, + "heading": 4.257815888008083e-15, + "angularVelocity": -1.9592546692313966e-15, + "velocityX": 3.7945723128987225, + "velocityY": -1.5232976551446145, + "timestamp": 1.0667161188228103 + }, + { + "x": 3.2966550111442214, + "y": 2.4509011416193687, + "heading": 4.252784018072507e-15, + "angularVelocity": -6.090882409757076e-17, + "velocityX": 3.8244790647055846, + "velocityY": -1.446573416325604, + "timestamp": 1.149329271299146 + }, + { + "x": 3.6125876520861304, + "y": 2.331343263293774, + "heading": 4.270047996286602e-15, + "angularVelocity": 2.089737255596764e-16, + "velocityX": 3.8242414370090176, + "velocityY": -1.447201501718981, + "timestamp": 1.231942423775482 + }, + { + "x": 3.928520259560843, + "y": 2.2117852965308926, + "heading": 4.2804411742991834e-15, + "angularVelocity": 1.2580536751887178e-16, + "velocityX": 3.8242410319016575, + "velocityY": -1.4472025722178512, + "timestamp": 1.3145555762518177 + }, + { + "x": 4.244452867036739, + "y": 2.0922273297711427, + "heading": 4.296224621909443e-15, + "angularVelocity": 1.9105247937253344e-16, + "velocityX": 3.824241031915982, + "velocityY": -1.447202572179951, + "timestamp": 1.3971687287281536 + }, + { + "x": 4.560385474513634, + "y": 1.9726693630140306, + "heading": 4.316587253622556e-15, + "angularVelocity": 2.4648171752648295e-16, + "velocityX": 3.824241031928069, + "velocityY": -1.4472025721480182, + "timestamp": 1.4797818812044894 + }, + { + "x": 4.876318081990444, + "y": 1.8531113962566956, + "heading": 4.307806953716989e-15, + "angularVelocity": -1.0628210693875022e-16, + "velocityX": 3.8242410319270483, + "velocityY": -1.447202572150715, + "timestamp": 1.5623950336808252 + }, + { + "x": 5.1922506894672535, + "y": 1.7335534294993578, + "heading": 4.330504238512068e-15, + "angularVelocity": 2.747417828097679e-16, + "velocityX": 3.824241031927034, + "velocityY": -1.4472025721507527, + "timestamp": 1.645008186157161 + }, + { + "x": 5.508183296944064, + "y": 1.6139954627420232, + "heading": 4.320958307300627e-15, + "angularVelocity": -1.1554977538132229e-16, + "velocityX": 3.8242410319270497, + "velocityY": -1.4472025721507116, + "timestamp": 1.7276213386334969 + }, + { + "x": 5.824115904420868, + "y": 1.4944374959846716, + "heading": 4.32750725592863e-15, + "angularVelocity": 7.927246971250498e-17, + "velocityX": 3.8242410319269715, + "velocityY": -1.4472025721509183, + "timestamp": 1.8102344911098327 + }, + { + "x": 6.14004851189783, + "y": 1.374879529227738, + "heading": 4.3338322343448905e-15, + "angularVelocity": 7.656139762494459e-17, + "velocityX": 3.8242410319288864, + "velocityY": -1.4472025721458581, + "timestamp": 1.8928476435861685 + }, + { + "x": 6.455981119863945, + "y": 1.2553215637651396, + "heading": 4.319457948267045e-15, + "angularVelocity": -1.7399512846812765e-16, + "velocityX": 3.824241037849887, + "velocityY": -1.4472025564784359, + "timestamp": 1.9754607960625044 + }, + { + "x": 6.742597970324816, + "y": 1.1468575210026002, + "heading": 4.7282996807897215e-15, + "angularVelocity": 4.948869815524412e-15, + "velocityX": 3.469385223412319, + "velocityY": -1.312914947695717, + "timestamp": 2.05807394853884 + }, + { + "x": 6.993387738830674, + "y": 1.0519514743449843, + "heading": 4.134595926485456e-15, + "angularVelocity": -7.186552464221235e-15, + "velocityX": 3.0357123652644447, + "velocityY": -1.1488006910851218, + "timestamp": 2.140687101015176 + }, + { + "x": 7.208350406940146, + "y": 0.9706034307897187, + "heading": 3.2355739078561107e-15, + "angularVelocity": -1.0882311014677393e-14, + "velocityX": 2.6020392838909574, + "velocityY": -0.9846863497733962, + "timestamp": 2.223300253491512 + }, + { + "x": 7.387485968506043, + "y": 0.9028133926693046, + "heading": 2.30776491712744e-15, + "angularVelocity": -1.1230766104875401e-14, + "velocityX": 2.168366128108148, + "velocityY": -0.8205719802276565, + "timestamp": 2.3059134059678477 + }, + { + "x": 7.530794420454761, + "y": 0.8485813611499973, + "heading": 1.4584800676415084e-15, + "angularVelocity": -1.0280261998735142e-14, + "velocityX": 1.7346929351205342, + "velocityY": -0.6564575965648514, + "timestamp": 2.3885265584441835 + }, + { + "x": 7.63827576094213, + "y": 0.8079073369315515, + "heading": 7.577076994785405e-16, + "angularVelocity": -8.482576307571573e-15, + "velocityX": 1.301019719809994, + "velocityY": -0.4923432044317867, + "timestamp": 2.4711397109205193 + }, + { + "x": 7.709929988738705, + "y": 0.7807913204804714, + "heading": 2.614341795751167e-16, + "angularVelocity": -6.0071974782633566e-15, + "velocityX": 0.8673464896174847, + "velocityY": -0.32822880665187276, + "timestamp": 2.553752863396855 + }, + { + "x": 7.745757102966309, + "y": 0.7672333121299746, + "heading": -9.721098396623117e-28, + "angularVelocity": -3.1645588171616285e-15, + "velocityX": 0.4336732487949893, + "velocityY": -0.16411440483848933, + "timestamp": 2.636366015873191 + }, + { + "x": 7.745757102966309, + "y": 0.7672333121299745, + "heading": -4.7866198406740125e-28, + "angularVelocity": 1.5898414573642954e-28, + "velocityX": -1.603276029328449e-17, + "velocityY": -6.86367715897337e-17, + "timestamp": 2.718979168349527 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-Skip.2.traj b/src/main/deploy/choreo/3NB-Skip.2.traj new file mode 100644 index 0000000..3a89b6d --- /dev/null +++ b/src/main/deploy/choreo/3NB-Skip.2.traj @@ -0,0 +1,229 @@ +{ + "samples": [ + { + "x": 7.745757102966309, + "y": 0.7672333121299745, + "heading": -4.7866198406740125e-28, + "angularVelocity": 1.5898414573642954e-28, + "velocityX": -1.603276029328449e-17, + "velocityY": -6.86367715897337e-17, + "timestamp": 0 + }, + { + "x": 7.697012773089809, + "y": 0.7878434827717467, + "heading": 9.784997624243692e-20, + "angularVelocity": 1.0076940613114793e-18, + "velocityX": -0.5019865449737438, + "velocityY": 0.2122509095518505, + "timestamp": 0.09710286135069479 + }, + { + "x": 7.599524114232943, + "y": 0.8290638236763864, + "heading": 2.93708175506948e-19, + "angularVelocity": 2.0170177947228716e-18, + "velocityX": -1.003973080718789, + "velocityY": 0.424501815201605, + "timestamp": 0.19420572270138958 + }, + { + "x": 7.453291127650296, + "y": 0.8908943343134272, + "heading": 7.722005480992539e-18, + "angularVelocity": 7.64992627627732e-17, + "velocityX": -1.5059596035436553, + "velocityY": 0.6367527153884249, + "timestamp": 0.2913085840520844 + }, + { + "x": 7.258313815223747, + "y": 0.973335013887169, + "heading": 1.3656857911178107e-17, + "angularVelocity": 6.111923322990157e-17, + "velocityX": -2.007946106988254, + "velocityY": 0.8490036073808429, + "timestamp": 0.38841144540277917 + }, + { + "x": 7.014592180089764, + "y": 1.0763858610714456, + "heading": 1.9690795477324688e-17, + "angularVelocity": 6.213964740290064e-17, + "velocityX": -2.5099325781324064, + "velocityY": 1.0612544857159245, + "timestamp": 0.48551430675347396 + }, + { + "x": 6.722126228521277, + "y": 1.200046873213924, + "heading": 2.5824842503133833e-17, + "angularVelocity": 6.317061042778013e-17, + "velocityX": -3.0119189846756718, + "velocityY": 1.273505336736336, + "timestamp": 0.5826171681041687 + }, + { + "x": 6.38091597933707, + "y": 1.34431804235761, + "heading": 3.0656082796974605e-17, + "angularVelocity": 4.9753840686431446e-17, + "velocityX": -3.5139051974163613, + "velocityY": 1.4857561058127722, + "timestamp": 0.6797200294548635 + }, + { + "x": 6.015216878005737, + "y": 1.4989436347306673, + "heading": 1.124404487622351e-17, + "angularVelocity": -1.999121102172735e-16, + "velocityX": -3.7661001565193835, + "velocityY": 1.5923896600185086, + "timestamp": 0.7768228908055583 + }, + { + "x": 5.649517776674369, + "y": 1.65356922710374, + "heading": -1.3050160652934763e-17, + "angularVelocity": -2.501904186058887e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 0.8739257521562531 + }, + { + "x": 5.283818675343, + "y": 1.808194819476813, + "heading": -3.0278972898890853e-17, + "angularVelocity": -1.7742847127577924e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 0.9710286135069479 + }, + { + "x": 4.918119574011632, + "y": 1.9628204118498858, + "heading": -2.1847399359503068e-17, + "angularVelocity": 8.68313602926161e-17, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 1.0681314748576427 + }, + { + "x": 4.5524204726802635, + "y": 2.1174460042229586, + "heading": -4.272133931768823e-18, + "angularVelocity": 1.8099637006841738e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 1.1652343362083375 + }, + { + "x": 4.186721371348895, + "y": 2.2720715965960316, + "heading": -1.2968663792925179e-17, + "angularVelocity": -8.955997526941852e-17, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 1.2623371975590323 + }, + { + "x": 3.8210222700175263, + "y": 2.4266971889691047, + "heading": -1.2904793423104192e-17, + "angularVelocity": 6.57759914907778e-19, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 1.3594400589097275 + }, + { + "x": 3.455323168686157, + "y": 2.5813227813421777, + "heading": -2.5257394960019227e-18, + "angularVelocity": 1.0688720994138185e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 1.4565429202604228 + }, + { + "x": 3.089624067354825, + "y": 2.7359483737152357, + "heading": 1.1768908098193852e-18, + "angularVelocity": 3.8131011324654024e-17, + "velocityX": -3.766100156519383, + "velocityY": 1.5923896600185086, + "timestamp": 1.553645781611118 + }, + { + "x": 2.748413818170618, + "y": 2.8802195428589217, + "heading": 8.82496336912766e-19, + "angularVelocity": -3.03177958726717e-18, + "velocityX": -3.5139051974163618, + "velocityY": 1.485756105812772, + "timestamp": 1.6507486429618132 + }, + { + "x": 2.4559478666021306, + "y": 3.0038805550014, + "heading": 4.913896997517123e-19, + "angularVelocity": -4.02775604881706e-18, + "velocityX": -3.011918984675672, + "velocityY": 1.2735053367363358, + "timestamp": 1.7478515043125085 + }, + { + "x": 2.212226231468147, + "y": 3.1069314021856766, + "heading": 3.397185296681489e-21, + "angularVelocity": -5.025521469385519e-18, + "velocityX": -2.509932578132407, + "velocityY": 1.0612544857159247, + "timestamp": 1.8449543656632037 + }, + { + "x": 2.017248919041599, + "y": 3.1893720817594184, + "heading": -5.814891269033512e-19, + "angularVelocity": -6.023368457591792e-18, + "velocityX": -2.007946106988254, + "velocityY": 0.849003607380843, + "timestamp": 1.942057227013899 + }, + { + "x": 1.8710159324589521, + "y": 3.251202592396459, + "heading": -2.9073580616535128e-19, + "angularVelocity": 2.994281699756195e-18, + "velocityX": -1.5059596035436553, + "velocityY": 0.6367527153884249, + "timestamp": 2.039160088364594 + }, + { + "x": 1.7735272736020857, + "y": 3.292422933301099, + "heading": -9.69089983169116e-20, + "angularVelocity": 1.9960977992890128e-18, + "velocityX": -1.0039730807187888, + "velocityY": 0.424501815201605, + "timestamp": 2.1362629497152894 + }, + { + "x": 1.724782943725586, + "y": 3.313033103942871, + "heading": -8.658446510793699e-29, + "angularVelocity": 9.980035282243734e-19, + "velocityX": -0.5019865449737438, + "velocityY": 0.21225090955185055, + "timestamp": 2.2333658110659846 + }, + { + "x": 1.724782943725586, + "y": 3.313033103942871, + "heading": -8.802662084224178e-29, + "angularVelocity": -3.186156327432381e-29, + "velocityX": -3.740246502381023e-28, + "velocityY": -1.8632531177340128e-29, + "timestamp": 2.33046867241668 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-Skip.3.traj b/src/main/deploy/choreo/3NB-Skip.3.traj new file mode 100644 index 0000000..c4b515a --- /dev/null +++ b/src/main/deploy/choreo/3NB-Skip.3.traj @@ -0,0 +1,94 @@ +{ + "samples": [ + { + "x": 1.724782943725586, + "y": 3.313033103942871, + "heading": -8.802662084224178e-29, + "angularVelocity": -3.186156327432381e-29, + "velocityX": -3.740246502381023e-28, + "velocityY": -1.8632531177340128e-29, + "timestamp": 0 + }, + { + "x": 1.7551582357609965, + "y": 3.3546585112037994, + "heading": -2.6221860421386343e-18, + "angularVelocity": -2.7366661467732622e-17, + "velocityX": 0.3170142472018904, + "velocityY": 0.43442700507741666, + "timestamp": 0.09581680414529181 + }, + { + "x": 1.8159088186549184, + "y": 3.437909324112868, + "heading": 2.022452950709123e-18, + "angularVelocity": 4.847415893658119e-17, + "velocityX": 0.634028482120975, + "velocityY": 0.868853993322837, + "timestamp": 0.19163360829058362 + }, + { + "x": 1.9070346900535533, + "y": 3.562785539444501, + "heading": 9.965469770186681e-18, + "angularVelocity": 8.289795188242313e-17, + "velocityX": 0.9510426924744488, + "velocityY": 1.303280947904267, + "timestamp": 0.28745041243587544 + }, + { + "x": 2.028535842895508, + "y": 3.729287147521972, + "heading": 1.7089484611640945e-17, + "angularVelocity": 7.435037001169609e-17, + "velocityX": 1.268056829131106, + "velocityY": 1.737707801493746, + "timestamp": 0.38326721658116725 + }, + { + "x": 2.1500369957374623, + "y": 3.8957887555994435, + "heading": 1.4813846609443293e-17, + "angularVelocity": -2.3749884192136335e-17, + "velocityX": 1.2680568291311063, + "velocityY": 1.737707801493746, + "timestamp": 0.47908402072645906 + }, + { + "x": 2.2411628671360972, + "y": 4.020664970931077, + "heading": 1.3332074215599954e-17, + "angularVelocity": -1.5464640122103033e-17, + "velocityX": 0.951042692474449, + "velocityY": 1.3032809479042669, + "timestamp": 0.5749008248717509 + }, + { + "x": 2.3019134500300193, + "y": 4.103915783840145, + "heading": 8.539272261435287e-18, + "angularVelocity": -5.002047393284737e-17, + "velocityX": 0.634028482120975, + "velocityY": 0.868853993322837, + "timestamp": 0.6707176290170427 + }, + { + "x": 2.3322887420654297, + "y": 4.145541191101074, + "heading": 1.747304188459868e-28, + "angularVelocity": -8.912082110592716e-17, + "velocityX": 0.3170142472018904, + "velocityY": 0.43442700507741666, + "timestamp": 0.7665344331623345 + }, + { + "x": 2.3322887420654297, + "y": 4.145541191101074, + "heading": 8.637491834911917e-29, + "angularVelocity": -2.0670507700607165e-29, + "velocityX": -3.920785157230939e-28, + "velocityY": 7.068097925324523e-28, + "timestamp": 0.8623512373076263 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NB-Skip.traj b/src/main/deploy/choreo/3NB-Skip.traj new file mode 100644 index 0000000..111c5ae --- /dev/null +++ b/src/main/deploy/choreo/3NB-Skip.traj @@ -0,0 +1,670 @@ +{ + "samples": [ + { + "x": 0.7100000000000002, + "y": 4.38, + "heading": -1.0303768814651526, + "angularVelocity": 1.5560405504853757e-19, + "velocityX": 3.6579942357642265e-17, + "velocityY": 3.5772569104609715e-17, + "timestamp": 0 + }, + { + "x": 0.7196231633442646, + "y": 4.37055163049096, + "heading": -1.0120510397413214, + "angularVelocity": 0.3659111239569298, + "velocityX": 0.19214519957043122, + "velocityY": -0.1886551001974958, + "timestamp": 0.05008276743722991 + }, + { + "x": 0.7389303360988976, + "y": 4.351635791048551, + "heading": -0.9762703828685111, + "angularVelocity": 0.7144305058152697, + "velocityX": 0.3855053093628394, + "velocityY": -0.3776915775693403, + "timestamp": 0.10016553487445982 + }, + { + "x": 0.7679871206287426, + "y": 4.323231755138663, + "heading": -0.9240353862773414, + "angularVelocity": 1.0429734470371541, + "velocityX": 0.5801752981455696, + "velocityY": -0.5671419005645384, + "timestamp": 0.15024830231168973 + }, + { + "x": 0.806863824332317, + "y": 4.285312709598439, + "heading": -0.8565791228845684, + "angularVelocity": 1.3468956857728758, + "velocityX": 0.7762491110800954, + "velocityY": -0.7571276005812321, + "timestamp": 0.20033106974891965 + }, + { + "x": 0.855633414600585, + "y": 4.2378416688864, + "heading": -0.7754660421005081, + "angularVelocity": 1.6195806448938828, + "velocityX": 0.9737798600965913, + "velocityY": -0.9478517889716715, + "timestamp": 0.25041383718614957 + }, + { + "x": 0.914367608814468, + "y": 4.180769559172441, + "heading": -0.6826698176675856, + "angularVelocity": 1.8528573635477696, + "velocityX": 1.1727425863098562, + "velocityY": -1.1395558319631454, + "timestamp": 0.30049660462337946 + }, + { + "x": 0.9831324058657855, + "y": 4.114037722550235, + "heading": -0.5806325593431202, + "angularVelocity": 2.0373726043064977, + "velocityX": 1.3730231089468503, + "velocityY": -1.3324310943047128, + "timestamp": 0.35057937206060935 + }, + { + "x": 1.0619841550917224, + "y": 4.037586277197585, + "heading": -0.47236160213245165, + "angularVelocity": 2.161840544182557, + "velocityX": 1.5744287558544843, + "velocityY": -1.5265020138605254, + "timestamp": 0.40066213949783924 + }, + { + "x": 1.1509626755897528, + "y": 3.9513700864804493, + "heading": -0.3616923537124617, + "angularVelocity": 2.209727099419903, + "velocityX": 1.7766294686003532, + "velocityY": -1.721474174229327, + "timestamp": 0.45074490693506913 + }, + { + "x": 1.2500676182895643, + "y": 3.855395460973611, + "heading": -0.2538729536839052, + "angularVelocity": 2.1528243255265638, + "velocityX": 1.9788232114775315, + "velocityY": -1.9163203316812512, + "timestamp": 0.500827674372299 + }, + { + "x": 1.3591936256250599, + "y": 3.749835630519891, + "heading": -0.15652201954824588, + "angularVelocity": 1.9438010141446913, + "velocityX": 2.1789132853386093, + "velocityY": -2.1077076179149334, + "timestamp": 0.550910441809529 + }, + { + "x": 1.4780432446812957, + "y": 3.6354509708518212, + "heading": -0.08074916244880044, + "angularVelocity": 1.5129526776732942, + "velocityX": 2.3730641323924204, + "velocityY": -2.283912521635818, + "timestamp": 0.6009932092467589 + }, + { + "x": 1.6064830934074645, + "y": 3.512581098747126, + "heading": -0.02955142499135005, + "angularVelocity": 1.0222625481233227, + "velocityX": 2.564551747008494, + "velocityY": -2.453336314904971, + "timestamp": 0.6510759766839889 + }, + { + "x": 1.7444408206294664, + "y": 3.3811278178903486, + "heading": -0.002660437876514541, + "angularVelocity": 0.5369309343486035, + "velocityX": 2.7545947295123865, + "velocityY": -2.6247207888730513, + "timestamp": 0.7011587441212188 + }, + { + "x": 1.8918929861971419, + "y": 3.2410564135466466, + "heading": -1.0357533070062969e-7, + "angularVelocity": 0.053118755957712996, + "velocityX": 2.944169683762683, + "velocityY": -2.796798410137877, + "timestamp": 0.7512415115584488 + }, + { + "x": 2.042423702860939, + "y": 3.102214682631318, + "heading": -7.051190961599685e-8, + "angularVelocity": 6.601756010788189e-7, + "velocityX": 3.005638952608048, + "velocityY": -2.772245585058687, + "timestamp": 0.8013242789956787 + }, + { + "x": 2.2019910957141193, + "y": 2.9738606026990846, + "heading": -3.641731647880258e-8, + "angularVelocity": 6.807649595630949e-7, + "velocityX": 3.1860737938089994, + "velocityY": -2.5628392059822205, + "timestamp": 0.8514070464329087 + }, + { + "x": 2.3699999999999997, + "y": 2.856772899627685, + "heading": 1.2045924074798219e-17, + "angularVelocity": 7.271426553754078e-7, + "velocityX": 3.3546250114165237, + "velocityY": -2.337884048003882, + "timestamp": 0.9014898138701386 + }, + { + "x": 2.6672211579606206, + "y": 2.6962515532804225, + "heading": 4.4196760900488604e-15, + "angularVelocity": 5.335264470419185e-14, + "velocityX": 3.597746231094708, + "velocityY": -1.9430483105374854, + "timestamp": 0.9841029663464744 + }, + { + "x": 2.9807027390289593, + "y": 2.5704071318299984, + "heading": 4.257815888008083e-15, + "angularVelocity": -1.9592546692313966e-15, + "velocityX": 3.7945723128987225, + "velocityY": -1.5232976551446145, + "timestamp": 1.0667161188228103 + }, + { + "x": 3.2966550111442214, + "y": 2.4509011416193687, + "heading": 4.252784018072507e-15, + "angularVelocity": -6.090882409757076e-17, + "velocityX": 3.8244790647055846, + "velocityY": -1.446573416325604, + "timestamp": 1.149329271299146 + }, + { + "x": 3.6125876520861304, + "y": 2.331343263293774, + "heading": 4.270047996286602e-15, + "angularVelocity": 2.089737255596764e-16, + "velocityX": 3.8242414370090176, + "velocityY": -1.447201501718981, + "timestamp": 1.231942423775482 + }, + { + "x": 3.928520259560843, + "y": 2.2117852965308926, + "heading": 4.2804411742991834e-15, + "angularVelocity": 1.2580536751887178e-16, + "velocityX": 3.8242410319016575, + "velocityY": -1.4472025722178512, + "timestamp": 1.3145555762518177 + }, + { + "x": 4.244452867036739, + "y": 2.0922273297711427, + "heading": 4.296224621909443e-15, + "angularVelocity": 1.9105247937253344e-16, + "velocityX": 3.824241031915982, + "velocityY": -1.447202572179951, + "timestamp": 1.3971687287281536 + }, + { + "x": 4.560385474513634, + "y": 1.9726693630140306, + "heading": 4.316587253622556e-15, + "angularVelocity": 2.4648171752648295e-16, + "velocityX": 3.824241031928069, + "velocityY": -1.4472025721480182, + "timestamp": 1.4797818812044894 + }, + { + "x": 4.876318081990444, + "y": 1.8531113962566956, + "heading": 4.307806953716989e-15, + "angularVelocity": -1.0628210693875022e-16, + "velocityX": 3.8242410319270483, + "velocityY": -1.447202572150715, + "timestamp": 1.5623950336808252 + }, + { + "x": 5.1922506894672535, + "y": 1.7335534294993578, + "heading": 4.330504238512068e-15, + "angularVelocity": 2.747417828097679e-16, + "velocityX": 3.824241031927034, + "velocityY": -1.4472025721507527, + "timestamp": 1.645008186157161 + }, + { + "x": 5.508183296944064, + "y": 1.6139954627420232, + "heading": 4.320958307300627e-15, + "angularVelocity": -1.1554977538132229e-16, + "velocityX": 3.8242410319270497, + "velocityY": -1.4472025721507116, + "timestamp": 1.7276213386334969 + }, + { + "x": 5.824115904420868, + "y": 1.4944374959846716, + "heading": 4.32750725592863e-15, + "angularVelocity": 7.927246971250498e-17, + "velocityX": 3.8242410319269715, + "velocityY": -1.4472025721509183, + "timestamp": 1.8102344911098327 + }, + { + "x": 6.14004851189783, + "y": 1.374879529227738, + "heading": 4.3338322343448905e-15, + "angularVelocity": 7.656139762494459e-17, + "velocityX": 3.8242410319288864, + "velocityY": -1.4472025721458581, + "timestamp": 1.8928476435861685 + }, + { + "x": 6.455981119863945, + "y": 1.2553215637651396, + "heading": 4.319457948267045e-15, + "angularVelocity": -1.7399512846812765e-16, + "velocityX": 3.824241037849887, + "velocityY": -1.4472025564784359, + "timestamp": 1.9754607960625044 + }, + { + "x": 6.742597970324816, + "y": 1.1468575210026002, + "heading": 4.7282996807897215e-15, + "angularVelocity": 4.948869815524412e-15, + "velocityX": 3.469385223412319, + "velocityY": -1.312914947695717, + "timestamp": 2.05807394853884 + }, + { + "x": 6.993387738830674, + "y": 1.0519514743449843, + "heading": 4.134595926485456e-15, + "angularVelocity": -7.186552464221235e-15, + "velocityX": 3.0357123652644447, + "velocityY": -1.1488006910851218, + "timestamp": 2.140687101015176 + }, + { + "x": 7.208350406940146, + "y": 0.9706034307897187, + "heading": 3.2355739078561107e-15, + "angularVelocity": -1.0882311014677393e-14, + "velocityX": 2.6020392838909574, + "velocityY": -0.9846863497733962, + "timestamp": 2.223300253491512 + }, + { + "x": 7.387485968506043, + "y": 0.9028133926693046, + "heading": 2.30776491712744e-15, + "angularVelocity": -1.1230766104875401e-14, + "velocityX": 2.168366128108148, + "velocityY": -0.8205719802276565, + "timestamp": 2.3059134059678477 + }, + { + "x": 7.530794420454761, + "y": 0.8485813611499973, + "heading": 1.4584800676415084e-15, + "angularVelocity": -1.0280261998735142e-14, + "velocityX": 1.7346929351205342, + "velocityY": -0.6564575965648514, + "timestamp": 2.3885265584441835 + }, + { + "x": 7.63827576094213, + "y": 0.8079073369315515, + "heading": 7.577076994785405e-16, + "angularVelocity": -8.482576307571573e-15, + "velocityX": 1.301019719809994, + "velocityY": -0.4923432044317867, + "timestamp": 2.4711397109205193 + }, + { + "x": 7.709929988738705, + "y": 0.7807913204804714, + "heading": 2.614341795751167e-16, + "angularVelocity": -6.0071974782633566e-15, + "velocityX": 0.8673464896174847, + "velocityY": -0.32822880665187276, + "timestamp": 2.553752863396855 + }, + { + "x": 7.745757102966309, + "y": 0.7672333121299746, + "heading": -9.721098396623117e-28, + "angularVelocity": -3.1645588171616285e-15, + "velocityX": 0.4336732487949893, + "velocityY": -0.16411440483848933, + "timestamp": 2.636366015873191 + }, + { + "x": 7.745757102966309, + "y": 0.7672333121299745, + "heading": -4.7866198406740125e-28, + "angularVelocity": 1.5898414573642954e-28, + "velocityX": -1.603276029328449e-17, + "velocityY": -6.86367715897337e-17, + "timestamp": 2.718979168349527 + }, + { + "x": 7.697012773089809, + "y": 0.7878434827717467, + "heading": 9.784997624243692e-20, + "angularVelocity": 1.0076940613114793e-18, + "velocityX": -0.5019865449737438, + "velocityY": 0.2122509095518505, + "timestamp": 2.8160820297002216 + }, + { + "x": 7.599524114232943, + "y": 0.8290638236763864, + "heading": 2.93708175506948e-19, + "angularVelocity": 2.0170177947228716e-18, + "velocityX": -1.003973080718789, + "velocityY": 0.424501815201605, + "timestamp": 2.9131848910509164 + }, + { + "x": 7.453291127650296, + "y": 0.8908943343134272, + "heading": 7.722005480992539e-18, + "angularVelocity": 7.64992627627732e-17, + "velocityX": -1.5059596035436553, + "velocityY": 0.6367527153884249, + "timestamp": 3.010287752401611 + }, + { + "x": 7.258313815223747, + "y": 0.973335013887169, + "heading": 1.3656857911178107e-17, + "angularVelocity": 6.111923322990157e-17, + "velocityX": -2.007946106988254, + "velocityY": 0.8490036073808429, + "timestamp": 3.107390613752306 + }, + { + "x": 7.014592180089764, + "y": 1.0763858610714456, + "heading": 1.9690795477324688e-17, + "angularVelocity": 6.213964740290064e-17, + "velocityX": -2.5099325781324064, + "velocityY": 1.0612544857159245, + "timestamp": 3.2044934751030008 + }, + { + "x": 6.722126228521277, + "y": 1.200046873213924, + "heading": 2.5824842503133833e-17, + "angularVelocity": 6.317061042778013e-17, + "velocityX": -3.0119189846756718, + "velocityY": 1.273505336736336, + "timestamp": 3.3015963364536955 + }, + { + "x": 6.38091597933707, + "y": 1.34431804235761, + "heading": 3.0656082796974605e-17, + "angularVelocity": 4.9753840686431446e-17, + "velocityX": -3.5139051974163613, + "velocityY": 1.4857561058127722, + "timestamp": 3.3986991978043903 + }, + { + "x": 6.015216878005737, + "y": 1.4989436347306673, + "heading": 1.124404487622351e-17, + "angularVelocity": -1.999121102172735e-16, + "velocityX": -3.7661001565193835, + "velocityY": 1.5923896600185086, + "timestamp": 3.495802059155085 + }, + { + "x": 5.649517776674369, + "y": 1.65356922710374, + "heading": -1.3050160652934763e-17, + "angularVelocity": -2.501904186058887e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.59290492050578 + }, + { + "x": 5.283818675343, + "y": 1.808194819476813, + "heading": -3.0278972898890853e-17, + "angularVelocity": -1.7742847127577924e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.6900077818564747 + }, + { + "x": 4.918119574011632, + "y": 1.9628204118498858, + "heading": -2.1847399359503068e-17, + "angularVelocity": 8.68313602926161e-17, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.7871106432071695 + }, + { + "x": 4.5524204726802635, + "y": 2.1174460042229586, + "heading": -4.272133931768823e-18, + "angularVelocity": 1.8099637006841738e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.8842135045578643 + }, + { + "x": 4.186721371348895, + "y": 2.2720715965960316, + "heading": -1.2968663792925179e-17, + "angularVelocity": -8.955997526941852e-17, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 3.981316365908559 + }, + { + "x": 3.8210222700175263, + "y": 2.4266971889691047, + "heading": -1.2904793423104192e-17, + "angularVelocity": 6.57759914907778e-19, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 4.078419227259254 + }, + { + "x": 3.455323168686157, + "y": 2.5813227813421777, + "heading": -2.5257394960019227e-18, + "angularVelocity": 1.0688720994138185e-16, + "velocityX": -3.7661001565197627, + "velocityY": 1.592389660018669, + "timestamp": 4.1755220886099496 + }, + { + "x": 3.089624067354825, + "y": 2.7359483737152357, + "heading": 1.1768908098193852e-18, + "angularVelocity": 3.8131011324654024e-17, + "velocityX": -3.766100156519383, + "velocityY": 1.5923896600185086, + "timestamp": 4.272624949960645 + }, + { + "x": 2.748413818170618, + "y": 2.8802195428589217, + "heading": 8.82496336912766e-19, + "angularVelocity": -3.03177958726717e-18, + "velocityX": -3.5139051974163618, + "velocityY": 1.485756105812772, + "timestamp": 4.36972781131134 + }, + { + "x": 2.4559478666021306, + "y": 3.0038805550014, + "heading": 4.913896997517123e-19, + "angularVelocity": -4.02775604881706e-18, + "velocityX": -3.011918984675672, + "velocityY": 1.2735053367363358, + "timestamp": 4.466830672662035 + }, + { + "x": 2.212226231468147, + "y": 3.1069314021856766, + "heading": 3.397185296681489e-21, + "angularVelocity": -5.025521469385519e-18, + "velocityX": -2.509932578132407, + "velocityY": 1.0612544857159247, + "timestamp": 4.5639335340127305 + }, + { + "x": 2.017248919041599, + "y": 3.1893720817594184, + "heading": -5.814891269033512e-19, + "angularVelocity": -6.023368457591792e-18, + "velocityX": -2.007946106988254, + "velocityY": 0.849003607380843, + "timestamp": 4.661036395363426 + }, + { + "x": 1.8710159324589521, + "y": 3.251202592396459, + "heading": -2.9073580616535128e-19, + "angularVelocity": 2.994281699756195e-18, + "velocityX": -1.5059596035436553, + "velocityY": 0.6367527153884249, + "timestamp": 4.758139256714121 + }, + { + "x": 1.7735272736020857, + "y": 3.292422933301099, + "heading": -9.69089983169116e-20, + "angularVelocity": 1.9960977992890128e-18, + "velocityX": -1.0039730807187888, + "velocityY": 0.424501815201605, + "timestamp": 4.855242118064816 + }, + { + "x": 1.724782943725586, + "y": 3.313033103942871, + "heading": -8.658446510793699e-29, + "angularVelocity": 9.980035282243734e-19, + "velocityX": -0.5019865449737438, + "velocityY": 0.21225090955185055, + "timestamp": 4.952344979415511 + }, + { + "x": 1.724782943725586, + "y": 3.313033103942871, + "heading": -8.802662084224178e-29, + "angularVelocity": -3.186156327432381e-29, + "velocityX": -3.740246502381023e-28, + "velocityY": -1.8632531177340128e-29, + "timestamp": 5.049447840766207 + }, + { + "x": 1.7551582357609965, + "y": 3.3546585112037994, + "heading": -2.6221860421386343e-18, + "angularVelocity": -2.7366661467732622e-17, + "velocityX": 0.3170142472018904, + "velocityY": 0.43442700507741666, + "timestamp": 5.1452646449114985 + }, + { + "x": 1.8159088186549184, + "y": 3.437909324112868, + "heading": 2.022452950709123e-18, + "angularVelocity": 4.847415893658119e-17, + "velocityX": 0.634028482120975, + "velocityY": 0.868853993322837, + "timestamp": 5.24108144905679 + }, + { + "x": 1.9070346900535533, + "y": 3.562785539444501, + "heading": 9.965469770186681e-18, + "angularVelocity": 8.289795188242313e-17, + "velocityX": 0.9510426924744488, + "velocityY": 1.303280947904267, + "timestamp": 5.336898253202082 + }, + { + "x": 2.028535842895508, + "y": 3.729287147521972, + "heading": 1.7089484611640945e-17, + "angularVelocity": 7.435037001169609e-17, + "velocityX": 1.268056829131106, + "velocityY": 1.737707801493746, + "timestamp": 5.432715057347374 + }, + { + "x": 2.1500369957374623, + "y": 3.8957887555994435, + "heading": 1.4813846609443293e-17, + "angularVelocity": -2.3749884192136335e-17, + "velocityX": 1.2680568291311063, + "velocityY": 1.737707801493746, + "timestamp": 5.528531861492666 + }, + { + "x": 2.2411628671360972, + "y": 4.020664970931077, + "heading": 1.3332074215599954e-17, + "angularVelocity": -1.5464640122103033e-17, + "velocityX": 0.951042692474449, + "velocityY": 1.3032809479042669, + "timestamp": 5.6243486656379575 + }, + { + "x": 2.3019134500300193, + "y": 4.103915783840145, + "heading": 8.539272261435287e-18, + "angularVelocity": -5.002047393284737e-17, + "velocityX": 0.634028482120975, + "velocityY": 0.868853993322837, + "timestamp": 5.720165469783249 + }, + { + "x": 2.3322887420654297, + "y": 4.145541191101074, + "heading": 1.747304188459868e-28, + "angularVelocity": -8.912082110592716e-17, + "velocityX": 0.3170142472018904, + "velocityY": 0.43442700507741666, + "timestamp": 5.815982273928541 + }, + { + "x": 2.3322887420654297, + "y": 4.145541191101074, + "heading": 8.637491834911917e-29, + "angularVelocity": -2.0670507700607165e-29, + "velocityX": -3.920785157230939e-28, + "velocityY": 7.068097925324523e-28, + "timestamp": 5.911799078073833 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBClose.1.traj b/src/main/deploy/choreo/3NBClose.1.traj deleted file mode 100644 index 79a563a..0000000 --- a/src/main/deploy/choreo/3NBClose.1.traj +++ /dev/null @@ -1,112 +0,0 @@ -{ - "samples": [ - { - "x": 2.5509378910064697, - "y": 4.086857318878174, - "heading": -2.52291805755639e-30, - "angularVelocity": -7.496319427174816e-32, - "velocityX": 0, - "velocityY": 7.845413232696036e-33, - "timestamp": 0 - }, - { - "x": 2.540265709715395, - "y": 4.06437744223644, - "heading": -0.027677023929901675, - "angularVelocity": -0.49532482401432165, - "velocityX": -0.1909958358687293, - "velocityY": -0.4023135207612014, - "timestamp": 0.05587651292256135 - }, - { - "x": 2.5189263879945742, - "y": 4.0194411386787365, - "heading": -0.08346743892564222, - "angularVelocity": -0.998459139228317, - "velocityX": -0.3819014574226356, - "velocityY": -0.804207370992889, - "timestamp": 0.1117530258451227 - }, - { - "x": 2.4869145481158594, - "y": 3.952095228464412, - "heading": -0.16807523339881444, - "angularVelocity": -1.5141924584740862, - "velocityX": -0.5729033220644937, - "velocityY": -1.2052632974369473, - "timestamp": 0.16762953876768405 - }, - { - "x": 2.444192615028637, - "y": 3.8624080728656693, - "heading": -0.2822872457171867, - "angularVelocity": -2.044007514868679, - "velocityX": -0.7645776526253585, - "velocityY": -1.6050957890489632, - "timestamp": 0.2235060516902454 - }, - { - "x": 2.390671759835886, - "y": 3.750452426932401, - "heading": -0.42657055754485035, - "angularVelocity": -2.58218174830674, - "velocityX": -0.9578417190586789, - "velocityY": -2.0036262121157438, - "timestamp": 0.27938256461280675 - }, - { - "x": 2.3354143484351475, - "y": 3.6390964679555458, - "heading": -0.5674310004450425, - "angularVelocity": -2.520924007827922, - "velocityX": -0.9889201832855812, - "velocityY": -1.9928938502512255, - "timestamp": 0.3352590775353681 - }, - { - "x": 2.29114926051941, - "y": 3.54997658752366, - "heading": -0.6791457851227963, - "angularVelocity": -1.9993156128511118, - "velocityX": -0.7921948883439481, - "velocityY": -1.5949434882487357, - "timestamp": 0.39113559045792945 - }, - { - "x": 2.257927443594339, - "y": 3.4831161723554422, - "heading": -0.7624378374394924, - "angularVelocity": -1.4906451380051187, - "velocityX": -0.5945578059087696, - "velocityY": -1.1965745833294734, - "timestamp": 0.4470121033804908 - }, - { - "x": 2.2357738106690572, - "y": 3.4385377084317534, - "heading": -0.8178321387402372, - "angularVelocity": -0.9913700480471095, - "velocityX": -0.3964748651366984, - "velocityY": -0.7978032556446372, - "timestamp": 0.5028886163030521 - }, - { - "x": 2.2246973514556885, - "y": 3.4162516593933105, - "heading": -0.8455662927866701, - "angularVelocity": -0.49634725926561274, - "velocityX": -0.19823103901848363, - "velocityY": -0.39884466429264903, - "timestamp": 0.5587651292256135 - }, - { - "x": 2.2246973514556885, - "y": 3.4162516593933105, - "heading": -0.8455662927866701, - "angularVelocity": 1.7401147103630586e-35, - "velocityX": 0, - "velocityY": -2.9784720237642915e-35, - "timestamp": 0.6146416421481748 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBClose.2.traj b/src/main/deploy/choreo/3NBClose.2.traj deleted file mode 100644 index 0405034..0000000 --- a/src/main/deploy/choreo/3NBClose.2.traj +++ /dev/null @@ -1,292 +0,0 @@ -{ - "samples": [ - { - "x": 2.2246973514556885, - "y": 3.4162516593933105, - "heading": -0.8455662927866701, - "angularVelocity": 1.7401147103630586e-35, - "velocityX": 0, - "velocityY": -2.9784720237642915e-35, - "timestamp": 0 - }, - { - "x": 2.2478567142138566, - "y": 3.4045992699268264, - "heading": -0.801702995310435, - "angularVelocity": 0.7505801290697112, - "velocityX": 0.3962984656503782, - "velocityY": -0.1993933993326178, - "timestamp": 0.05843919360162153 - }, - { - "x": 2.294609413503049, - "y": 3.381442972938176, - "heading": -0.7165359103981246, - "angularVelocity": 1.4573624251712332, - "velocityX": 0.8000230052437824, - "velocityY": -0.39624600480468514, - "timestamp": 0.11687838720324306 - }, - { - "x": 2.365522904611265, - "y": 3.3469050328439485, - "heading": -0.5944096615423881, - "angularVelocity": 2.089800377607326, - "velocityX": 1.2134577282436745, - "velocityY": -0.591006445600051, - "timestamp": 0.1753175808048646 - }, - { - "x": 2.4613273363775683, - "y": 3.300956971286685, - "heading": -0.44341803786852796, - "angularVelocity": 2.5837390006297127, - "velocityX": 1.6393866147332483, - "velocityY": -0.7862542024534139, - "timestamp": 0.23375677440648612 - }, - { - "x": 2.582796668027654, - "y": 3.243418728875599, - "heading": -0.27720516334390327, - "angularVelocity": 2.8442020548348648, - "velocityX": 2.078559339441595, - "velocityY": -0.9845831002276041, - "timestamp": 0.29219596800810765 - }, - { - "x": 2.7301911975501905, - "y": 3.1745171553945775, - "heading": -0.12210660840345536, - "angularVelocity": 2.6540160016195737, - "velocityX": 2.522186232194111, - "velocityY": -1.1790301890666381, - "timestamp": 0.3506351616097292 - }, - { - "x": 2.8997764163925672, - "y": 3.0981960670393747, - "heading": -0.030345077601412643, - "angularVelocity": 1.5702052876974757, - "velocityX": 2.901908948272534, - "velocityY": -1.3059914699624646, - "timestamp": 0.4090743552113507 - }, - { - "x": 3.0915054018011863, - "y": 3.0138284358769285, - "heading": -7.683299452582235e-7, - "angularVelocity": 0.5192458588378841, - "velocityX": 3.280828731409792, - "velocityY": -1.4436823296635168, - "timestamp": 0.46751354881297225 - }, - { - "x": 3.2912704992742374, - "y": 2.9191356764509986, - "heading": -6.722882817433478e-7, - "angularVelocity": 0.000001643446077808863, - "velocityX": 3.418341102289066, - "velocityY": -1.6203638960429139, - "timestamp": 0.5259527424145938 - }, - { - "x": 3.4910355335063255, - "y": 2.8244427836108152, - "heading": -5.762470893344414e-7, - "angularVelocity": 0.0000016434380163356612, - "velocityX": 3.418340020122122, - "velocityY": -1.6203661790014092, - "timestamp": 0.5843919360162153 - }, - { - "x": 3.690800567737388, - "y": 2.72974989076847, - "heading": -4.802059000608906e-7, - "angularVelocity": 0.000001643437962684092, - "velocityX": 3.4183400201045884, - "velocityY": -1.6203661790384074, - "timestamp": 0.6428311296178368 - }, - { - "x": 3.8905656019684516, - "y": 2.635056997926124, - "heading": -3.8416471392980063e-7, - "angularVelocity": 0.0000016434379089109065, - "velocityX": 3.4183400201045884, - "velocityY": -1.6203661790384178, - "timestamp": 0.7012703232194586 - }, - { - "x": 4.090330636199514, - "y": 2.5403641050837775, - "heading": -2.881235309010655e-7, - "angularVelocity": 0.0000016434378558240006, - "velocityX": 3.4183400201045897, - "velocityY": -1.6203661790384272, - "timestamp": 0.7597095168210803 - }, - { - "x": 4.290095670430578, - "y": 2.44567121224143, - "heading": -1.9208235097393964e-7, - "angularVelocity": 0.0000016434378027498543, - "velocityX": 3.418340020104591, - "velocityY": -1.6203661790384367, - "timestamp": 0.8181487104227021 - }, - { - "x": 4.48986070466166, - "y": 2.3509783193991223, - "heading": -9.604117422336942e-8, - "angularVelocity": 0.0000016434377483932399, - "velocityX": 3.4183400201049134, - "velocityY": -1.6203661790377692, - "timestamp": 0.8765879040243238 - }, - { - "x": 4.6896257400512695, - "y": 2.2562854290008545, - "heading": 0, - "angularVelocity": 0.0000016434377051482952, - "velocityX": 3.418340039929407, - "velocityY": -1.620366137215836, - "timestamp": 0.9350270976259456 - }, - { - "x": 4.968559869574538, - "y": 2.1245665376158462, - "heading": 1.3383629188384767e-17, - "angularVelocity": 1.6413040812646372e-16, - "velocityX": 3.420718848021436, - "velocityY": -1.6153394178448868, - "timestamp": 1.016569645235377 - }, - { - "x": 5.247494000174595, - "y": 1.9928476485110982, - "heading": 4.731110022587156e-17, - "angularVelocity": 4.160707757547854e-16, - "velocityX": 3.4207188612266823, - "velocityY": -1.6153393898808353, - "timestamp": 1.0981121928448083 - }, - { - "x": 5.526428130774659, - "y": 1.8611287594063628, - "heading": 2.424776839803803e-17, - "angularVelocity": -2.82838009158897e-16, - "velocityX": 3.420718861226756, - "velocityY": -1.6153393898806798, - "timestamp": 1.1796547404542397 - }, - { - "x": 5.805362261374722, - "y": 1.7294098703016274, - "heading": 2.194784551635826e-17, - "angularVelocity": -2.820518795507566e-17, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806796, - "timestamp": 1.261197288063671 - }, - { - "x": 6.084296391974785, - "y": 1.5976909811968918, - "heading": -5.63799247601606e-18, - "angularVelocity": -3.3829992808814586e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806796, - "timestamp": 1.3427398356731024 - }, - { - "x": 6.363230522574848, - "y": 1.4659720920921564, - "heading": -4.0160186165861685e-17, - "angularVelocity": -4.233641786017845e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806798, - "timestamp": 1.4242823832825338 - }, - { - "x": 6.642164653174911, - "y": 1.334253202987421, - "heading": -9.232752279805819e-17, - "angularVelocity": -6.39756031196107e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806796, - "timestamp": 1.5058249308919651 - }, - { - "x": 6.9210987837749745, - "y": 1.2025343138826854, - "heading": -1.0876414634021774e-16, - "angularVelocity": -2.0157113095032782e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806798, - "timestamp": 1.5873674785013965 - }, - { - "x": 7.200032914375007, - "y": 1.0708154247779642, - "heading": -1.0564695019881366e-16, - "angularVelocity": 3.822784834245772e-17, - "velocityX": 3.4207188612263892, - "velocityY": -1.6153393898805066, - "timestamp": 1.6689100261108278 - }, - { - "x": 7.450698808870213, - "y": 0.9524454243469528, - "heading": -6.21333471845912e-17, - "angularVelocity": 5.336307257731506e-16, - "velocityX": 3.0740503190534603, - "velocityY": -1.4516348078548387, - "timestamp": 1.7504525737202592 - }, - { - "x": 7.6512315423409545, - "y": 0.8577494155613715, - "heading": -3.4011798431685756e-17, - "angularVelocity": 3.448696939400641e-16, - "velocityX": 2.4592404744483005, - "velocityY": -1.16130794979759, - "timestamp": 1.8319951213296906 - }, - { - "x": 7.8016310983382375, - "y": 0.7867274061888005, - "heading": -1.7843882684840366e-17, - "angularVelocity": 1.982758662289022e-16, - "velocityX": 1.8444304281202957, - "velocityY": -0.8709809964823354, - "timestamp": 1.913537668939122 - }, - { - "x": 7.901897471379062, - "y": 0.7393793988184351, - "heading": -9.05403720548675e-18, - "angularVelocity": 1.0779461865175002e-16, - "velocityX": 1.2296203145512992, - "velocityY": -0.5806540114143915, - "timestamp": 1.9950802165485533 - }, - { - "x": 7.952030658721924, - "y": 0.715705394744873, - "heading": 1.8215469909424705e-35, - "angularVelocity": 1.1103452913314967e-16, - "velocityX": 0.6148101673618008, - "velocityY": -0.29032701047010034, - "timestamp": 2.0766227641579844 - }, - { - "x": 7.952030658721924, - "y": 0.715705394744873, - "heading": 0, - "angularVelocity": -1.7262654336863351e-34, - "velocityX": 0, - "velocityY": -1.70530075667682e-35, - "timestamp": 2.1581653117674158 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBClose.3.traj b/src/main/deploy/choreo/3NBClose.3.traj deleted file mode 100644 index 28b3a6b..0000000 --- a/src/main/deploy/choreo/3NBClose.3.traj +++ /dev/null @@ -1,283 +0,0 @@ -{ - "samples": [ - { - "x": 7.952030658721924, - "y": 0.715705394744873, - "heading": 0, - "angularVelocity": -1.7262654336863351e-34, - "velocityX": 0, - "velocityY": -1.70530075667682e-35, - "timestamp": 0 - }, - { - "x": 7.920571399136667, - "y": 0.7339164602546292, - "heading": -0.035684479584908176, - "angularVelocity": -0.530786457387229, - "velocityX": -0.46793869888314166, - "velocityY": 0.27087930269994037, - "timestamp": 0.06722944620811022 - }, - { - "x": 7.857550496757403, - "y": 0.7704988919479816, - "heading": -0.1048950106648703, - "angularVelocity": -1.0294675173393406, - "velocityX": -0.937400290107708, - "velocityY": 0.544142987287306, - "timestamp": 0.13445889241622044 - }, - { - "x": 7.762821414092203, - "y": 0.8256865710662646, - "heading": -0.20406484547654866, - "angularVelocity": -1.4750952209943207, - "velocityX": -1.409041543670678, - "velocityY": 0.8208855231002242, - "timestamp": 0.20168833862433067 - }, - { - "x": 7.636150871743386, - "y": 0.8998079208996147, - "heading": -0.326492004541456, - "angularVelocity": -1.8210347692874205, - "velocityX": -1.8841526963751192, - "velocityY": 1.1025131696594024, - "timestamp": 0.2689177848324409 - }, - { - "x": 7.477257379372396, - "y": 0.9932759166007383, - "heading": -0.4588279920908599, - "angularVelocity": -1.9684229904223027, - "velocityX": -2.3634508587075183, - "velocityY": 1.3902835881139206, - "timestamp": 0.3361472310405511 - }, - { - "x": 7.286949361590744, - "y": 1.1061975307016165, - "heading": -0.5642233015257418, - "angularVelocity": -1.5676956360554906, - "velocityX": -2.8307241620368186, - "velocityY": 1.67964516249809, - "timestamp": 0.40337667724866133 - }, - { - "x": 7.071106947748093, - "y": 1.2344586983401868, - "heading": -0.5749993730660543, - "angularVelocity": -0.16028797124038208, - "velocityX": -3.210533865986417, - "velocityY": 1.9078123482013338, - "timestamp": 0.47060612345677155 - }, - { - "x": 6.8510832792762875, - "y": 1.3620156667061647, - "heading": -0.5749994357594823, - "angularVelocity": -9.32529294881991e-7, - "velocityX": -3.2727276644629284, - "velocityY": 1.8973377821843458, - "timestamp": 0.5378355696648822 - }, - { - "x": 6.631059595878299, - "y": 1.4895726093259114, - "heading": -0.5749994984528743, - "angularVelocity": -9.325287600700736e-7, - "velocityX": -3.272727886481472, - "velocityY": 1.8973373992237128, - "timestamp": 0.6050650158729929 - }, - { - "x": 6.411035912480151, - "y": 1.617129551945382, - "heading": -0.5749995611462662, - "angularVelocity": -9.325287555505025e-7, - "velocityX": -3.272727886483851, - "velocityY": 1.89733739921961, - "timestamp": 0.6722944620811035 - }, - { - "x": 6.191012229082002, - "y": 1.7446864945648533, - "heading": -0.5749996238396577, - "angularVelocity": -9.325287510002467e-7, - "velocityX": -3.2727278864838483, - "velocityY": 1.8973373992196134, - "timestamp": 0.7395239082892142 - }, - { - "x": 5.970988545683854, - "y": 1.8722434371843246, - "heading": -0.5749996865330489, - "angularVelocity": -9.325287467182492e-7, - "velocityX": -3.2727278864838465, - "velocityY": 1.8973373992196174, - "timestamp": 0.8067533544973249 - }, - { - "x": 5.750964862285706, - "y": 1.999800379803796, - "heading": -0.5749997492264397, - "angularVelocity": -9.325287418609201e-7, - "velocityX": -3.2727278864838443, - "velocityY": 1.8973373992196212, - "timestamp": 0.8739828007054355 - }, - { - "x": 5.530941178887557, - "y": 2.127357322423268, - "heading": -0.5749998119198304, - "angularVelocity": -9.325287380028671e-7, - "velocityX": -3.2727278864838425, - "velocityY": 1.8973373992196243, - "timestamp": 0.9412122469135462 - }, - { - "x": 5.310917495489409, - "y": 2.2549142650427396, - "heading": -0.5749998746132208, - "angularVelocity": -9.325287337726564e-7, - "velocityX": -3.2727278864838407, - "velocityY": 1.897337399219628, - "timestamp": 1.0084416931216569 - }, - { - "x": 5.090893812091097, - "y": 2.3824712076619283, - "heading": -0.5749999373066107, - "angularVelocity": -9.325287288718162e-7, - "velocityX": -3.2727278864862823, - "velocityY": 1.8973373992154168, - "timestamp": 1.0756711393297675 - }, - { - "x": 4.870870113372803, - "y": 2.510028123855591, - "heading": -0.575, - "angularVelocity": -9.325287163810678e-7, - "velocityX": -3.2727281143623554, - "velocityY": 1.8973370061506505, - "timestamp": 1.1429005855378782 - }, - { - "x": 4.5943545837046464, - "y": 2.664060328782752, - "heading": -0.5750000000166958, - "angularVelocity": -1.995413922173843e-10, - "velocityX": -3.3047900712231173, - "velocityY": 1.8409240960273932, - "timestamp": 1.2265717185626204 - }, - { - "x": 4.317839040217314, - "y": 2.818092508902003, - "heading": -0.5750000000333916, - "angularVelocity": -1.9954042523374304e-10, - "velocityX": -3.3047902363837314, - "velocityY": 1.8409237995343366, - "timestamp": 1.3102428515873625 - }, - { - "x": 4.041323496729911, - "y": 2.972124689021126, - "heading": -0.5750000000500873, - "angularVelocity": -1.9954079180529516e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328027, - "timestamp": 1.3939139846121047 - }, - { - "x": 3.764807953242507, - "y": 3.1261568691402486, - "heading": -0.5750000000667832, - "angularVelocity": -1.9954127974564644e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328034, - "timestamp": 1.4775851176368469 - }, - { - "x": 3.4882924097551036, - "y": 3.2801890492593713, - "heading": -0.5750000000834792, - "angularVelocity": -1.9954171076053385e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328027, - "timestamp": 1.561256250661589 - }, - { - "x": 3.2117768662677, - "y": 3.434221229378494, - "heading": -0.5750000001001749, - "angularVelocity": -1.995407072844536e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328032, - "timestamp": 1.6449273836863312 - }, - { - "x": 2.9352613227803133, - "y": 3.5882534094976077, - "heading": -0.5750000001168709, - "angularVelocity": -1.9954301729646843e-10, - "velocityX": -3.3047902363843877, - "velocityY": 1.840923799532693, - "timestamp": 1.7285985167110733 - }, - { - "x": 2.680282263927866, - "y": 3.7302887534223292, - "heading": -0.5751416355275835, - "angularVelocity": -0.00169276315011459, - "velocityX": -3.0473957939239096, - "velocityY": 1.6975429731867062, - "timestamp": 1.8122696497358155 - }, - { - "x": 2.476298999494477, - "y": 3.84391703822767, - "heading": -0.5752549441981719, - "angularVelocity": -0.0013542146077421449, - "velocityX": -2.4379168425156807, - "velocityY": 1.3580344940678712, - "timestamp": 1.8959407827605577 - }, - { - "x": 2.3233115454477207, - "y": 3.9291382550189407, - "heading": -0.5753399258221812, - "angularVelocity": -0.0010156624027564103, - "velocityX": -1.8284377002701389, - "velocityY": 1.0185259086436655, - "timestamp": 1.9796119157852998 - }, - { - "x": 2.2213199071101224, - "y": 3.985952400831243, - "heading": -0.5753965802881199, - "angularVelocity": -0.0006771088652765756, - "velocityX": -1.218958494412151, - "velocityY": 0.6790172877843319, - "timestamp": 2.063283048810042 - }, - { - "x": 2.1703240871429443, - "y": 4.014359474182129, - "heading": -0.5754249075385721, - "angularVelocity": -0.0003385546415838589, - "velocityX": -0.6094792567479415, - "velocityY": 0.33950864920743473, - "timestamp": 2.146954181834784 - }, - { - "x": 2.1703240871429443, - "y": 4.014359474182129, - "heading": -0.5754249075385721, - "angularVelocity": 0, - "velocityX": -2.0315826046958738e-36, - "velocityY": -3.1386682644109864e-35, - "timestamp": 2.2306253148595263 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBClose.traj b/src/main/deploy/choreo/3NBClose.traj deleted file mode 100644 index 6a04815..0000000 --- a/src/main/deploy/choreo/3NBClose.traj +++ /dev/null @@ -1,796 +0,0 @@ -{ - "samples": [ - { - "x": 0.756615161895752, - "y": 4.431221961975098, - "heading": -1.034985029325618, - "angularVelocity": 8.110335832445358e-7, - "velocityX": 3.46491583266257, - "velocityY": -0.6635831503447084, - "timestamp": 0 - }, - { - "x": 0.9453330335621133, - "y": 4.395079669962969, - "heading": -1.0349849176957346, - "angularVelocity": 0.0000021977330086686356, - "velocityX": 3.7154163650328855, - "velocityY": -0.7115577450399997, - "timestamp": 0.05079319600420905 - }, - { - "x": 1.1340509052288028, - "y": 4.358937377950767, - "heading": -1.0349848060666427, - "angularVelocity": 0.0000021977174247335837, - "velocityX": 3.715416365039343, - "velocityY": -0.7115577450414385, - "timestamp": 0.1015863920084181 - }, - { - "x": 1.322768776895351, - "y": 4.322795085937826, - "heading": -1.0349846944375516, - "angularVelocity": 0.0000021977174097972457, - "velocityX": 3.7154163650365644, - "velocityY": -0.7115577450559716, - "timestamp": 0.15237958801262713 - }, - { - "x": 1.5114866425477105, - "y": 4.28665276252143, - "heading": -1.0349845828083795, - "angularVelocity": 0.000002197719005657343, - "velocityX": 3.7154162466311615, - "velocityY": -0.7115583633170585, - "timestamp": 0.2031727840168362 - }, - { - "x": 1.69813453980795, - "y": 4.2495459266525195, - "heading": -1.030459064976107, - "angularVelocity": 0.08909693006711296, - "velocityX": 3.6746633790237286, - "velocityY": -0.7305473722469792, - "timestamp": 0.25396598002104526 - }, - { - "x": 1.8669872465749084, - "y": 4.212655978268133, - "heading": -0.9831886401407888, - "angularVelocity": 0.9306448216292826, - "velocityX": 3.3243174293062006, - "velocityY": -0.7262773616633765, - "timestamp": 0.3047591760252543 - }, - { - "x": 2.0190124095336994, - "y": 4.18140148614854, - "heading": -0.892440005402738, - "angularVelocity": 1.7866297432933886, - "velocityX": 2.993022194275648, - "velocityY": -0.615328323049506, - "timestamp": 0.3555523720294634 - }, - { - "x": 2.1541013042131363, - "y": 4.155546420509619, - "heading": -0.7564993339594785, - "angularVelocity": 2.6763559322393244, - "velocityX": 2.659586427052991, - "velocityY": -0.5090261624171132, - "timestamp": 0.40634556803367244 - }, - { - "x": 2.2707485094165962, - "y": 4.134387232003345, - "heading": -0.5827726511536211, - "angularVelocity": 3.4202746917414184, - "velocityX": 2.2965124146508487, - "velocityY": -0.41657525359346464, - "timestamp": 0.4571387640378815 - }, - { - "x": 2.3663024178277605, - "y": 4.117419152577522, - "heading": -0.4109047131229225, - "angularVelocity": 3.3836803263267097, - "velocityX": 1.8812344157915586, - "velocityY": -0.3340620547763231, - "timestamp": 0.5079319600420905 - }, - { - "x": 2.4413363437910904, - "y": 4.104502135595215, - "heading": -0.2574297741497486, - "angularVelocity": 3.0215649151208352, - "velocityX": 1.4772436441509222, - "velocityY": -0.2543060488108931, - "timestamp": 0.5587251560462996 - }, - { - "x": 2.4966779864989124, - "y": 4.095364710580153, - "heading": -0.13334577720883498, - "angularVelocity": 2.4429255629165603, - "velocityX": 1.0895483462634659, - "velocityY": -0.17989466570101328, - "timestamp": 0.6095183520505086 - }, - { - "x": 2.5330251703027096, - "y": 4.089609545054607, - "heading": -0.04586327123160215, - "angularVelocity": 1.7223272575717314, - "velocityX": 0.7155915883061368, - "velocityY": -0.11330583578692442, - "timestamp": 0.6603115480547177 - }, - { - "x": 2.5509378910064697, - "y": 4.086857318878174, - "heading": -2.5191104221446134e-30, - "angularVelocity": 0.9029412370074453, - "velocityX": 0.3526598464541567, - "velocityY": -0.05418493800242671, - "timestamp": 0.7111047440589268 - }, - { - "x": 2.5509378910064697, - "y": 4.086857318878174, - "heading": -2.52291805755639e-30, - "angularVelocity": -7.496319427174816e-32, - "velocityX": 0, - "velocityY": 7.845413232696036e-33, - "timestamp": 0.7618979400631358 - }, - { - "x": 2.540265709715395, - "y": 4.06437744223644, - "heading": -0.027677023929901675, - "angularVelocity": -0.49532482401432165, - "velocityX": -0.1909958358687293, - "velocityY": -0.4023135207612014, - "timestamp": 0.8177744529856972 - }, - { - "x": 2.5189263879945742, - "y": 4.0194411386787365, - "heading": -0.08346743892564222, - "angularVelocity": -0.998459139228317, - "velocityX": -0.3819014574226356, - "velocityY": -0.804207370992889, - "timestamp": 0.8736509659082585 - }, - { - "x": 2.4869145481158594, - "y": 3.952095228464412, - "heading": -0.16807523339881444, - "angularVelocity": -1.5141924584740862, - "velocityX": -0.5729033220644937, - "velocityY": -1.2052632974369473, - "timestamp": 0.9295274788308199 - }, - { - "x": 2.444192615028637, - "y": 3.8624080728656693, - "heading": -0.2822872457171867, - "angularVelocity": -2.044007514868679, - "velocityX": -0.7645776526253585, - "velocityY": -1.6050957890489632, - "timestamp": 0.9854039917533812 - }, - { - "x": 2.390671759835886, - "y": 3.750452426932401, - "heading": -0.42657055754485035, - "angularVelocity": -2.58218174830674, - "velocityX": -0.9578417190586789, - "velocityY": -2.0036262121157438, - "timestamp": 1.0412805046759426 - }, - { - "x": 2.3354143484351475, - "y": 3.6390964679555458, - "heading": -0.5674310004450425, - "angularVelocity": -2.520924007827922, - "velocityX": -0.9889201832855812, - "velocityY": -1.9928938502512255, - "timestamp": 1.097157017598504 - }, - { - "x": 2.29114926051941, - "y": 3.54997658752366, - "heading": -0.6791457851227963, - "angularVelocity": -1.9993156128511118, - "velocityX": -0.7921948883439481, - "velocityY": -1.5949434882487357, - "timestamp": 1.1530335305210653 - }, - { - "x": 2.257927443594339, - "y": 3.4831161723554422, - "heading": -0.7624378374394924, - "angularVelocity": -1.4906451380051187, - "velocityX": -0.5945578059087696, - "velocityY": -1.1965745833294734, - "timestamp": 1.2089100434436266 - }, - { - "x": 2.2357738106690572, - "y": 3.4385377084317534, - "heading": -0.8178321387402372, - "angularVelocity": -0.9913700480471095, - "velocityX": -0.3964748651366984, - "velocityY": -0.7978032556446372, - "timestamp": 1.264786556366188 - }, - { - "x": 2.2246973514556885, - "y": 3.4162516593933105, - "heading": -0.8455662927866701, - "angularVelocity": -0.49634725926561274, - "velocityX": -0.19823103901848363, - "velocityY": -0.39884466429264903, - "timestamp": 1.3206630692887493 - }, - { - "x": 2.2246973514556885, - "y": 3.4162516593933105, - "heading": -0.8455662927866701, - "angularVelocity": 1.7401147103630586e-35, - "velocityX": 0, - "velocityY": -2.9784720237642915e-35, - "timestamp": 1.3765395822113107 - }, - { - "x": 2.2478567142138566, - "y": 3.4045992699268264, - "heading": -0.801702995310435, - "angularVelocity": 0.7505801290697112, - "velocityX": 0.3962984656503782, - "velocityY": -0.1993933993326178, - "timestamp": 1.4349787758129322 - }, - { - "x": 2.294609413503049, - "y": 3.381442972938176, - "heading": -0.7165359103981246, - "angularVelocity": 1.4573624251712332, - "velocityX": 0.8000230052437824, - "velocityY": -0.39624600480468514, - "timestamp": 1.4934179694145537 - }, - { - "x": 2.365522904611265, - "y": 3.3469050328439485, - "heading": -0.5944096615423881, - "angularVelocity": 2.089800377607326, - "velocityX": 1.2134577282436745, - "velocityY": -0.591006445600051, - "timestamp": 1.5518571630161753 - }, - { - "x": 2.4613273363775683, - "y": 3.300956971286685, - "heading": -0.44341803786852796, - "angularVelocity": 2.5837390006297127, - "velocityX": 1.6393866147332483, - "velocityY": -0.7862542024534139, - "timestamp": 1.6102963566177968 - }, - { - "x": 2.582796668027654, - "y": 3.243418728875599, - "heading": -0.27720516334390327, - "angularVelocity": 2.8442020548348648, - "velocityX": 2.078559339441595, - "velocityY": -0.9845831002276041, - "timestamp": 1.6687355502194183 - }, - { - "x": 2.7301911975501905, - "y": 3.1745171553945775, - "heading": -0.12210660840345536, - "angularVelocity": 2.6540160016195737, - "velocityX": 2.522186232194111, - "velocityY": -1.1790301890666381, - "timestamp": 1.7271747438210399 - }, - { - "x": 2.8997764163925672, - "y": 3.0981960670393747, - "heading": -0.030345077601412643, - "angularVelocity": 1.5702052876974757, - "velocityX": 2.901908948272534, - "velocityY": -1.3059914699624646, - "timestamp": 1.7856139374226614 - }, - { - "x": 3.0915054018011863, - "y": 3.0138284358769285, - "heading": -7.683299452582235e-7, - "angularVelocity": 0.5192458588378841, - "velocityX": 3.280828731409792, - "velocityY": -1.4436823296635168, - "timestamp": 1.844053131024283 - }, - { - "x": 3.2912704992742374, - "y": 2.9191356764509986, - "heading": -6.722882817433478e-7, - "angularVelocity": 0.000001643446077808863, - "velocityX": 3.418341102289066, - "velocityY": -1.6203638960429139, - "timestamp": 1.9024923246259045 - }, - { - "x": 3.4910355335063255, - "y": 2.8244427836108152, - "heading": -5.762470893344414e-7, - "angularVelocity": 0.0000016434380163356612, - "velocityX": 3.418340020122122, - "velocityY": -1.6203661790014092, - "timestamp": 1.960931518227526 - }, - { - "x": 3.690800567737388, - "y": 2.72974989076847, - "heading": -4.802059000608906e-7, - "angularVelocity": 0.000001643437962684092, - "velocityX": 3.4183400201045884, - "velocityY": -1.6203661790384074, - "timestamp": 2.0193707118291475 - }, - { - "x": 3.8905656019684516, - "y": 2.635056997926124, - "heading": -3.8416471392980063e-7, - "angularVelocity": 0.0000016434379089109065, - "velocityX": 3.4183400201045884, - "velocityY": -1.6203661790384178, - "timestamp": 2.0778099054307693 - }, - { - "x": 4.090330636199514, - "y": 2.5403641050837775, - "heading": -2.881235309010655e-7, - "angularVelocity": 0.0000016434378558240006, - "velocityX": 3.4183400201045897, - "velocityY": -1.6203661790384272, - "timestamp": 2.136249099032391 - }, - { - "x": 4.290095670430578, - "y": 2.44567121224143, - "heading": -1.9208235097393964e-7, - "angularVelocity": 0.0000016434378027498543, - "velocityX": 3.418340020104591, - "velocityY": -1.6203661790384367, - "timestamp": 2.1946882926340128 - }, - { - "x": 4.48986070466166, - "y": 2.3509783193991223, - "heading": -9.604117422336942e-8, - "angularVelocity": 0.0000016434377483932399, - "velocityX": 3.4183400201049134, - "velocityY": -1.6203661790377692, - "timestamp": 2.2531274862356345 - }, - { - "x": 4.6896257400512695, - "y": 2.2562854290008545, - "heading": 0, - "angularVelocity": 0.0000016434377051482952, - "velocityX": 3.418340039929407, - "velocityY": -1.620366137215836, - "timestamp": 2.3115666798372563 - }, - { - "x": 4.968559869574538, - "y": 2.1245665376158462, - "heading": 1.3383629188384767e-17, - "angularVelocity": 1.6413040812646372e-16, - "velocityX": 3.420718848021436, - "velocityY": -1.6153394178448868, - "timestamp": 2.3931092274466876 - }, - { - "x": 5.247494000174595, - "y": 1.9928476485110982, - "heading": 4.731110022587156e-17, - "angularVelocity": 4.160707757547854e-16, - "velocityX": 3.4207188612266823, - "velocityY": -1.6153393898808353, - "timestamp": 2.474651775056119 - }, - { - "x": 5.526428130774659, - "y": 1.8611287594063628, - "heading": 2.424776839803803e-17, - "angularVelocity": -2.82838009158897e-16, - "velocityX": 3.420718861226756, - "velocityY": -1.6153393898806798, - "timestamp": 2.5561943226655504 - }, - { - "x": 5.805362261374722, - "y": 1.7294098703016274, - "heading": 2.194784551635826e-17, - "angularVelocity": -2.820518795507566e-17, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806796, - "timestamp": 2.6377368702749817 - }, - { - "x": 6.084296391974785, - "y": 1.5976909811968918, - "heading": -5.63799247601606e-18, - "angularVelocity": -3.3829992808814586e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806796, - "timestamp": 2.719279417884413 - }, - { - "x": 6.363230522574848, - "y": 1.4659720920921564, - "heading": -4.0160186165861685e-17, - "angularVelocity": -4.233641786017845e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806798, - "timestamp": 2.8008219654938444 - }, - { - "x": 6.642164653174911, - "y": 1.334253202987421, - "heading": -9.232752279805819e-17, - "angularVelocity": -6.39756031196107e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806796, - "timestamp": 2.882364513103276 - }, - { - "x": 6.9210987837749745, - "y": 1.2025343138826854, - "heading": -1.0876414634021774e-16, - "angularVelocity": -2.0157113095032782e-16, - "velocityX": 3.4207188612267556, - "velocityY": -1.6153393898806798, - "timestamp": 2.963907060712707 - }, - { - "x": 7.200032914375007, - "y": 1.0708154247779642, - "heading": -1.0564695019881366e-16, - "angularVelocity": 3.822784834245772e-17, - "velocityX": 3.4207188612263892, - "velocityY": -1.6153393898805066, - "timestamp": 3.0454496083221385 - }, - { - "x": 7.450698808870213, - "y": 0.9524454243469528, - "heading": -6.21333471845912e-17, - "angularVelocity": 5.336307257731506e-16, - "velocityX": 3.0740503190534603, - "velocityY": -1.4516348078548387, - "timestamp": 3.12699215593157 - }, - { - "x": 7.6512315423409545, - "y": 0.8577494155613715, - "heading": -3.4011798431685756e-17, - "angularVelocity": 3.448696939400641e-16, - "velocityX": 2.4592404744483005, - "velocityY": -1.16130794979759, - "timestamp": 3.2085347035410012 - }, - { - "x": 7.8016310983382375, - "y": 0.7867274061888005, - "heading": -1.7843882684840366e-17, - "angularVelocity": 1.982758662289022e-16, - "velocityX": 1.8444304281202957, - "velocityY": -0.8709809964823354, - "timestamp": 3.2900772511504326 - }, - { - "x": 7.901897471379062, - "y": 0.7393793988184351, - "heading": -9.05403720548675e-18, - "angularVelocity": 1.0779461865175002e-16, - "velocityX": 1.2296203145512992, - "velocityY": -0.5806540114143915, - "timestamp": 3.371619798759864 - }, - { - "x": 7.952030658721924, - "y": 0.715705394744873, - "heading": 1.8215469909424705e-35, - "angularVelocity": 1.1103452913314967e-16, - "velocityX": 0.6148101673618008, - "velocityY": -0.29032701047010034, - "timestamp": 3.4531623463692953 - }, - { - "x": 7.952030658721924, - "y": 0.715705394744873, - "heading": 0, - "angularVelocity": -1.7262654336863351e-34, - "velocityX": 0, - "velocityY": -1.70530075667682e-35, - "timestamp": 3.5347048939787267 - }, - { - "x": 7.920571399136667, - "y": 0.7339164602546292, - "heading": -0.035684479584908176, - "angularVelocity": -0.530786457387229, - "velocityX": -0.46793869888314166, - "velocityY": 0.27087930269994037, - "timestamp": 3.601934340186837 - }, - { - "x": 7.857550496757403, - "y": 0.7704988919479816, - "heading": -0.1048950106648703, - "angularVelocity": -1.0294675173393406, - "velocityX": -0.937400290107708, - "velocityY": 0.544142987287306, - "timestamp": 3.669163786394947 - }, - { - "x": 7.762821414092203, - "y": 0.8256865710662646, - "heading": -0.20406484547654866, - "angularVelocity": -1.4750952209943207, - "velocityX": -1.409041543670678, - "velocityY": 0.8208855231002242, - "timestamp": 3.7363932326030573 - }, - { - "x": 7.636150871743386, - "y": 0.8998079208996147, - "heading": -0.326492004541456, - "angularVelocity": -1.8210347692874205, - "velocityX": -1.8841526963751192, - "velocityY": 1.1025131696594024, - "timestamp": 3.8036226788111676 - }, - { - "x": 7.477257379372396, - "y": 0.9932759166007383, - "heading": -0.4588279920908599, - "angularVelocity": -1.9684229904223027, - "velocityX": -2.3634508587075183, - "velocityY": 1.3902835881139206, - "timestamp": 3.870852125019278 - }, - { - "x": 7.286949361590744, - "y": 1.1061975307016165, - "heading": -0.5642233015257418, - "angularVelocity": -1.5676956360554906, - "velocityX": -2.8307241620368186, - "velocityY": 1.67964516249809, - "timestamp": 3.938081571227388 - }, - { - "x": 7.071106947748093, - "y": 1.2344586983401868, - "heading": -0.5749993730660543, - "angularVelocity": -0.16028797124038208, - "velocityX": -3.210533865986417, - "velocityY": 1.9078123482013338, - "timestamp": 4.005311017435498 - }, - { - "x": 6.8510832792762875, - "y": 1.3620156667061647, - "heading": -0.5749994357594823, - "angularVelocity": -9.32529294881991e-7, - "velocityX": -3.2727276644629284, - "velocityY": 1.8973377821843458, - "timestamp": 4.072540463643609 - }, - { - "x": 6.631059595878299, - "y": 1.4895726093259114, - "heading": -0.5749994984528743, - "angularVelocity": -9.325287600700736e-7, - "velocityX": -3.272727886481472, - "velocityY": 1.8973373992237128, - "timestamp": 4.1397699098517196 - }, - { - "x": 6.411035912480151, - "y": 1.617129551945382, - "heading": -0.5749995611462662, - "angularVelocity": -9.325287555505025e-7, - "velocityX": -3.272727886483851, - "velocityY": 1.89733739921961, - "timestamp": 4.20699935605983 - }, - { - "x": 6.191012229082002, - "y": 1.7446864945648533, - "heading": -0.5749996238396577, - "angularVelocity": -9.325287510002467e-7, - "velocityX": -3.2727278864838483, - "velocityY": 1.8973373992196134, - "timestamp": 4.274228802267941 - }, - { - "x": 5.970988545683854, - "y": 1.8722434371843246, - "heading": -0.5749996865330489, - "angularVelocity": -9.325287467182492e-7, - "velocityX": -3.2727278864838465, - "velocityY": 1.8973373992196174, - "timestamp": 4.3414582484760516 - }, - { - "x": 5.750964862285706, - "y": 1.999800379803796, - "heading": -0.5749997492264397, - "angularVelocity": -9.325287418609201e-7, - "velocityX": -3.2727278864838443, - "velocityY": 1.8973373992196212, - "timestamp": 4.408687694684162 - }, - { - "x": 5.530941178887557, - "y": 2.127357322423268, - "heading": -0.5749998119198304, - "angularVelocity": -9.325287380028671e-7, - "velocityX": -3.2727278864838425, - "velocityY": 1.8973373992196243, - "timestamp": 4.475917140892273 - }, - { - "x": 5.310917495489409, - "y": 2.2549142650427396, - "heading": -0.5749998746132208, - "angularVelocity": -9.325287337726564e-7, - "velocityX": -3.2727278864838407, - "velocityY": 1.897337399219628, - "timestamp": 4.5431465871003835 - }, - { - "x": 5.090893812091097, - "y": 2.3824712076619283, - "heading": -0.5749999373066107, - "angularVelocity": -9.325287288718162e-7, - "velocityX": -3.2727278864862823, - "velocityY": 1.8973373992154168, - "timestamp": 4.610376033308494 - }, - { - "x": 4.870870113372803, - "y": 2.510028123855591, - "heading": -0.575, - "angularVelocity": -9.325287163810678e-7, - "velocityX": -3.2727281143623554, - "velocityY": 1.8973370061506505, - "timestamp": 4.677605479516605 - }, - { - "x": 4.5943545837046464, - "y": 2.664060328782752, - "heading": -0.5750000000166958, - "angularVelocity": -1.995413922173843e-10, - "velocityX": -3.3047900712231173, - "velocityY": 1.8409240960273932, - "timestamp": 4.761276612541347 - }, - { - "x": 4.317839040217314, - "y": 2.818092508902003, - "heading": -0.5750000000333916, - "angularVelocity": -1.9954042523374304e-10, - "velocityX": -3.3047902363837314, - "velocityY": 1.8409237995343366, - "timestamp": 4.844947745566089 - }, - { - "x": 4.041323496729911, - "y": 2.972124689021126, - "heading": -0.5750000000500873, - "angularVelocity": -1.9954079180529516e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328027, - "timestamp": 4.928618878590831 - }, - { - "x": 3.764807953242507, - "y": 3.1261568691402486, - "heading": -0.5750000000667832, - "angularVelocity": -1.9954127974564644e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328034, - "timestamp": 5.0122900116155735 - }, - { - "x": 3.4882924097551036, - "y": 3.2801890492593713, - "heading": -0.5750000000834792, - "angularVelocity": -1.9954171076053385e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328027, - "timestamp": 5.095961144640316 - }, - { - "x": 3.2117768662677, - "y": 3.434221229378494, - "heading": -0.5750000001001749, - "angularVelocity": -1.995407072844536e-10, - "velocityX": -3.304790236384586, - "velocityY": 1.8409237995328032, - "timestamp": 5.179632277665058 - }, - { - "x": 2.9352613227803133, - "y": 3.5882534094976077, - "heading": -0.5750000001168709, - "angularVelocity": -1.9954301729646843e-10, - "velocityX": -3.3047902363843877, - "velocityY": 1.840923799532693, - "timestamp": 5.2633034106898 - }, - { - "x": 2.680282263927866, - "y": 3.7302887534223292, - "heading": -0.5751416355275835, - "angularVelocity": -0.00169276315011459, - "velocityX": -3.0473957939239096, - "velocityY": 1.6975429731867062, - "timestamp": 5.346974543714542 - }, - { - "x": 2.476298999494477, - "y": 3.84391703822767, - "heading": -0.5752549441981719, - "angularVelocity": -0.0013542146077421449, - "velocityX": -2.4379168425156807, - "velocityY": 1.3580344940678712, - "timestamp": 5.430645676739284 - }, - { - "x": 2.3233115454477207, - "y": 3.9291382550189407, - "heading": -0.5753399258221812, - "angularVelocity": -0.0010156624027564103, - "velocityX": -1.8284377002701389, - "velocityY": 1.0185259086436655, - "timestamp": 5.5143168097640265 - }, - { - "x": 2.2213199071101224, - "y": 3.985952400831243, - "heading": -0.5753965802881199, - "angularVelocity": -0.0006771088652765756, - "velocityX": -1.218958494412151, - "velocityY": 0.6790172877843319, - "timestamp": 5.597987942788769 - }, - { - "x": 2.1703240871429443, - "y": 4.014359474182129, - "heading": -0.5754249075385721, - "angularVelocity": -0.0003385546415838589, - "velocityX": -0.6094792567479415, - "velocityY": 0.33950864920743473, - "timestamp": 5.681659075813511 - }, - { - "x": 2.1703240871429443, - "y": 4.014359474182129, - "heading": -0.5754249075385721, - "angularVelocity": 0, - "velocityX": -2.0315826046958738e-36, - "velocityY": -3.1386682644109864e-35, - "timestamp": 5.765330208838253 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBMid1st.1.traj b/src/main/deploy/choreo/3NBMid1st.1.traj deleted file mode 100644 index ef0413f..0000000 --- a/src/main/deploy/choreo/3NBMid1st.1.traj +++ /dev/null @@ -1,292 +0,0 @@ -{ - "samples": [ - { - "x": 7.989865779876709, - "y": 0.6650473475456238, - "heading": 0, - "angularVelocity": 0, - "velocityX": 5.7398628141890084e-36, - "velocityY": 6.163577013070246e-34, - "timestamp": 0 - }, - { - "x": 7.942973367083922, - "y": 0.667457868334859, - "heading": 1.5015515417643764e-15, - "angularVelocity": 2.0009529191708877e-14, - "velocityX": -0.6248837129789168, - "velocityY": 0.03212236460611118, - "timestamp": 0.0750418226924876 - }, - { - "x": 7.84918854285797, - "y": 0.6722789098434376, - "heading": 4.5057793482934065e-15, - "angularVelocity": 4.0034046332269777e-14, - "velocityX": -1.2497674078396386, - "velocityY": 0.06424472828085026, - "timestamp": 0.1500836453849752 - }, - { - "x": 7.70851130923829, - "y": 0.6795104719665217, - "heading": 9.022031304760564e-15, - "angularVelocity": 6.018313247766123e-14, - "velocityX": -1.874651075523027, - "velocityY": 0.09636709055852906, - "timestamp": 0.2251254680774628 - }, - { - "x": 7.520941669623951, - "y": 0.6891525545293808, - "heading": 1.504590019224722e-14, - "angularVelocity": 8.027348845418817e-14, - "velocityX": -2.49953469791075, - "velocityY": 0.12848945050776842, - "timestamp": 0.3001672907699504 - }, - { - "x": 7.286479630813121, - "y": 0.7012051571825528, - "heading": 2.2571401776550036e-14, - "angularVelocity": 1.0028409911019087e-13, - "velocityX": -3.124418229706744, - "velocityY": 0.16061180580010845, - "timestamp": 0.375209113462438 - }, - { - "x": 7.005125213200534, - "y": 0.7156682788776393, - "heading": 3.160518517475634e-14, - "angularVelocity": 1.2038331525109193e-13, - "velocityX": -3.749301489724504, - "velocityY": 0.1927341471215938, - "timestamp": 0.4502509361549256 - }, - { - "x": 6.721620804307546, - "y": 0.730241921622282, - "heading": 3.159325845827104e-14, - "angularVelocity": -1.5893425902210946e-16, - "velocityX": -3.777952063541342, - "velocityY": 0.19420693983358725, - "timestamp": 0.5252927588474132 - }, - { - "x": 6.438116395414555, - "y": 0.7448155643669249, - "heading": 3.1611801523052584e-14, - "angularVelocity": 2.4710307021100496e-16, - "velocityX": -3.7779520635413752, - "velocityY": 0.19420693983358922, - "timestamp": 0.6003345815399008 - }, - { - "x": 6.154611986521697, - "y": 0.7593892071141327, - "heading": 3.1598377018547585e-14, - "angularVelocity": -1.7889363588638215e-16, - "velocityX": -3.777952063539618, - "velocityY": 0.19420693986776963, - "timestamp": 0.6753764042323884 - }, - { - "x": 5.8711075940921145, - "y": 0.7739631701216403, - "heading": 3.1576411220426416e-14, - "angularVelocity": -2.9271408040259687e-16, - "velocityX": -3.777951844151616, - "velocityY": 0.19421120762524555, - "timestamp": 0.750418226924876 - }, - { - "x": 5.590080643384297, - "y": 0.8141003552309748, - "heading": 3.15610868842989e-14, - "angularVelocity": -2.0421060653488186e-16, - "velocityX": -3.744937697734655, - "velocityY": 0.5348642086401837, - "timestamp": 0.8254600496173636 - }, - { - "x": 5.31951379776001, - "y": 0.9000114798545837, - "heading": 1.0126074140253907e-33, - "angularVelocity": -4.205799613054758e-13, - "velocityX": -3.60554735901122, - "velocityY": 1.1448432559489161, - "timestamp": 0.9005018723098512 - }, - { - "x": 5.087986158761807, - "y": 1.0141443752209618, - "heading": -8.06925360653917e-8, - "angularVelocity": -0.0000011825608125095831, - "velocityX": -3.393071109702901, - "velocityY": 1.6726341252821548, - "timestamp": 0.9687372942955919 - }, - { - "x": 4.876193956700591, - "y": 1.1617093808932828, - "heading": -1.490061973004125e-7, - "angularVelocity": -0.0000010011466075390655, - "velocityX": -3.1038454207182142, - "velocityY": 2.162586547836665, - "timestamp": 1.0369727162813325 - }, - { - "x": 4.680065310913933, - "y": 1.329533463078152, - "heading": -2.1452736793612451e-7, - "angularVelocity": -9.602222530316319e-7, - "velocityX": -2.874293733064984, - "velocityY": 2.4594862507033355, - "timestamp": 1.105208138267073 - }, - { - "x": 4.483936991566996, - "y": 1.4973579267580712, - "heading": -2.800485136638582e-7, - "angularVelocity": -9.602218880015987e-7, - "velocityX": -2.874288949043537, - "velocityY": 2.4594918415685947, - "timestamp": 1.1734435602528137 - }, - { - "x": 4.287808672223246, - "y": 1.6651823904417138, - "heading": -3.455696595852266e-7, - "angularVelocity": -9.602218908393429e-7, - "velocityX": -2.874288948996848, - "velocityY": 2.459491841623158, - "timestamp": 1.2416789822385543 - }, - { - "x": 4.091680352879495, - "y": 1.8330068541253561, - "heading": -4.1109080579148433e-7, - "angularVelocity": -9.602218950144384e-7, - "velocityX": -2.8742889489968495, - "velocityY": 2.459491841623156, - "timestamp": 1.309914404224295 - }, - { - "x": 3.895552033535744, - "y": 2.0008313178089985, - "heading": -4.766119523424261e-7, - "angularVelocity": -9.602219000658333e-7, - "velocityX": -2.8742889489968504, - "velocityY": 2.4594918416231537, - "timestamp": 1.3781498262100356 - }, - { - "x": 3.6994237141919926, - "y": 2.1686557814926406, - "heading": -5.421330991227701e-7, - "angularVelocity": -9.60221903427755e-7, - "velocityX": -2.874288948996852, - "velocityY": 2.459491841623151, - "timestamp": 1.4463852481957762 - }, - { - "x": 3.5032953948482413, - "y": 2.3364802451762827, - "heading": -6.076542461212268e-7, - "angularVelocity": -9.602219066242283e-7, - "velocityX": -2.874288948996854, - "velocityY": 2.4594918416231493, - "timestamp": 1.5146206701815172 - }, - { - "x": 3.3071670755041227, - "y": 2.5043047088594954, - "heading": -6.731753934621747e-7, - "angularVelocity": -9.602219116434866e-7, - "velocityX": -2.874288949002235, - "velocityY": 2.45949184161686, - "timestamp": 1.5828560921672574 - }, - { - "x": 3.111038718551217, - "y": 2.6721291285911115, - "heading": -7.386965721783656e-7, - "angularVelocity": -9.602223714522231e-7, - "velocityX": -2.874289500164463, - "velocityY": 2.459491197499839, - "timestamp": 1.6510915141529976 - }, - { - "x": 2.91419229045963, - "y": 2.833130049750874, - "heading": -0.009424527544100834, - "angularVelocity": -0.1381069915490224, - "velocityX": -2.8848129367870357, - "velocityY": 2.3594918368557476, - "timestamp": 1.7193269361387378 - }, - { - "x": 2.744128800353239, - "y": 2.9762386994865855, - "heading": -0.09849959395445267, - "angularVelocity": -1.3054080097719059, - "velocityX": -2.4923050983978627, - "velocityY": 2.0972780056319964, - "timestamp": 1.787562358124478 - }, - { - "x": 2.60131581243687, - "y": 3.0984689436943484, - "heading": -0.2549637418085224, - "angularVelocity": -2.293004766450577, - "velocityX": -2.0929450388130264, - "velocityY": 1.7913019462722148, - "timestamp": 1.8557977801102181 - }, - { - "x": 2.487964457072741, - "y": 3.1954189236366886, - "heading": -0.41008490805759246, - "angularVelocity": -2.2733231763626556, - "velocityX": -1.661180543264116, - "velocityY": 1.4208160090605124, - "timestamp": 1.9240332020959583 - }, - { - "x": 2.403679735462392, - "y": 3.26747969283638, - "heading": -0.5415632745482113, - "angularVelocity": -1.9268345188528966, - "velocityX": -1.2352048123621473, - "velocityY": 1.0560610179087115, - "timestamp": 1.9922686240816985 - }, - { - "x": 2.3479306427779894, - "y": 3.3151481611881186, - "heading": -0.6366834880204346, - "angularVelocity": -1.3940005162143045, - "velocityX": -0.8170110341818207, - "velocityY": 0.6985883132912986, - "timestamp": 2.0605040460674386 - }, - { - "x": 2.3202381134033203, - "y": 3.3388166427612305, - "heading": -0.6871242163258895, - "angularVelocity": -0.7392161847551203, - "velocityX": -0.40583803204816526, - "velocityY": 0.3468650282262215, - "timestamp": 2.128739468053179 - }, - { - "x": 2.3202381134033203, - "y": 3.3388166427612305, - "heading": -0.6871242163258895, - "angularVelocity": -2.336499408021271e-31, - "velocityX": 1.807664942161883e-33, - "velocityY": -2.7160751304258966e-32, - "timestamp": 2.196974890038919 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBMid1st.2.traj b/src/main/deploy/choreo/3NBMid1st.2.traj deleted file mode 100644 index 3d9ee9a..0000000 --- a/src/main/deploy/choreo/3NBMid1st.2.traj +++ /dev/null @@ -1,283 +0,0 @@ -{ - "samples": [ - { - "x": 2.3202381134033203, - "y": 3.3388166427612305, - "heading": -0.6871242163258895, - "angularVelocity": -2.336499408021271e-31, - "velocityX": 1.807664942161883e-33, - "velocityY": -2.7160751304258966e-32, - "timestamp": 0 - }, - { - "x": 2.343794886689721, - "y": 3.323479061689414, - "heading": -0.6491973045996883, - "angularVelocity": 0.6336054834749958, - "velocityX": 0.39353852048358584, - "velocityY": -0.256229021242233, - "timestamp": 0.059858875460155225 - }, - { - "x": 2.3911982166575956, - "y": 3.2927250738018596, - "heading": -0.5766774436489436, - "angularVelocity": 1.2115139215907424, - "velocityX": 0.7919181508751876, - "velocityY": -0.5137749022369447, - "timestamp": 0.11971775092031045 - }, - { - "x": 2.462785855058177, - "y": 3.246405346319809, - "heading": -0.4743619817187913, - "angularVelocity": 1.7092780501407518, - "velocityX": 1.195940248630853, - "velocityY": -0.773815530712448, - "timestamp": 0.17957662638046568 - }, - { - "x": 2.5589185460183237, - "y": 3.184270435840568, - "heading": -0.3491026464013455, - "angularVelocity": 2.09257748921167, - "velocityX": 1.6059889234661082, - "velocityY": -1.0380233507828165, - "timestamp": 0.2394355018406209 - }, - { - "x": 2.679950432279286, - "y": 3.106027779542805, - "heading": -0.21085817155844028, - "angularVelocity": 2.309506715256066, - "velocityX": 2.021953892894741, - "velocityY": -1.3071187137460396, - "timestamp": 0.2992943773007761 - }, - { - "x": 2.8260176035796434, - "y": 3.011555903011265, - "heading": -0.07818278278115957, - "angularVelocity": 2.216469784261071, - "velocityX": 2.4401923721000403, - "velocityY": -1.5782434234740206, - "timestamp": 0.35915325276093135 - }, - { - "x": 2.994403761571005, - "y": 2.9064091851549483, - "heading": -0.008153033218424529, - "angularVelocity": 1.1699142194769419, - "velocityX": 2.8130524787998565, - "velocityY": -1.756576899382393, - "timestamp": 0.4190121282210866 - }, - { - "x": 3.1849716588184336, - "y": 2.790203147279415, - "heading": -5.969233857045932e-7, - "angularVelocity": 0.136194277496333, - "velocityX": 3.1836197352935405, - "velocityY": -1.9413334611153064, - "timestamp": 0.4788710036812418 - }, - { - "x": 3.3752288967163246, - "y": 2.6674089766097917, - "heading": -5.278378990920332e-7, - "angularVelocity": 0.0000011541394000718638, - "velocityX": 3.178429872517975, - "velocityY": -2.0513945463502785, - "timestamp": 0.538729879141397 - }, - { - "x": 3.5654860836141538, - "y": 2.5446147269206905, - "heading": -4.587524705883237e-7, - "angularVelocity": 0.0000011541384293077112, - "velocityX": 3.178429020512966, - "velocityY": -2.051395866446551, - "timestamp": 0.5985887546015523 - }, - { - "x": 3.755743270511201, - "y": 2.4218204772303773, - "heading": -3.8966704288748865e-7, - "angularVelocity": 0.0000011541384158949214, - "velocityX": 3.1784290204999057, - "velocityY": -2.0513958664667884, - "timestamp": 0.6584476300617075 - }, - { - "x": 3.9460004574097183, - "y": 2.2990262275423423, - "heading": -3.205816159441191e-7, - "angularVelocity": 0.0000011541384032407341, - "velocityX": 3.1784290205244665, - "velocityY": -2.0513958664287353, - "timestamp": 0.7183065055218627 - }, - { - "x": 4.136257740222623, - "y": 2.176232126464018, - "heading": -2.5149617447282606e-7, - "angularVelocity": 0.0000011541386459436468, - "velocityX": 3.178430622866425, - "velocityY": -2.051393383760805, - "timestamp": 0.7781653809820179 - }, - { - "x": 4.331712975970885, - "y": 2.0618928262664933, - "heading": -1.810305943700683e-7, - "angularVelocity": 0.00000117719518719764, - "velocityX": 3.2652674185027872, - "velocityY": -1.9101478154836689, - "timestamp": 0.8380242564421732 - }, - { - "x": 4.540519439656892, - "y": 1.9742795609077881, - "heading": -1.0217911975724037e-7, - "angularVelocity": 0.0000013172896083775409, - "velocityX": 3.488312503047245, - "velocityY": -1.4636637371683408, - "timestamp": 0.8978831319023284 - }, - { - "x": 4.759042739868164, - "y": 1.914918065071106, - "heading": 0, - "angularVelocity": 0.0000017070003232061267, - "velocityX": 3.6506415887603976, - "velocityY": -0.991690795731634, - "timestamp": 0.9577420073624836 - }, - { - "x": 5.064685674327935, - "y": 1.8884875071659226, - "heading": 3.5654904377837356e-14, - "angularVelocity": 4.3965968281974806e-13, - "velocityX": 3.768874940643874, - "velocityY": -0.32591451044713626, - "timestamp": 1.038838604960799 - }, - { - "x": 5.370151366519879, - "y": 1.9168933201476963, - "heading": 3.5699569493639844e-14, - "angularVelocity": 5.507643615792375e-16, - "velocityX": 3.7666893709273386, - "velocityY": 0.35027132855156706, - "timestamp": 1.1199352025591143 - }, - { - "x": 5.6719192342055065, - "y": 1.972141248763098, - "heading": 3.5697718803372364e-14, - "angularVelocity": -2.2820812737714697e-17, - "velocityX": 3.721091594746464, - "velocityY": 0.681260746462555, - "timestamp": 1.2010318001574296 - }, - { - "x": 5.973687062735465, - "y": 2.0273893912489522, - "heading": 3.565124865229906e-14, - "angularVelocity": -5.730222037609759e-16, - "velocityX": 3.7210911119189434, - "velocityY": 0.681263383693449, - "timestamp": 1.282128397755745 - }, - { - "x": 6.275454891265202, - "y": 2.0826375337360123, - "heading": 3.5597782853381526e-14, - "angularVelocity": -6.592853522950103e-16, - "velocityX": 3.7210911119162207, - "velocityY": 0.6812633837083223, - "timestamp": 1.3632249953540603 - }, - { - "x": 6.577222719794938, - "y": 2.137885676223073, - "heading": 3.5548087973236424e-14, - "angularVelocity": -6.127862526596166e-16, - "velocityX": 3.72109111191622, - "velocityY": 0.6812633837083226, - "timestamp": 1.4443215929523756 - }, - { - "x": 6.878990548324675, - "y": 2.193133818710133, - "heading": 3.55770700353708e-14, - "angularVelocity": 3.573770416112574e-16, - "velocityX": 3.72109111191622, - "velocityY": 0.6812633837083226, - "timestamp": 1.525418190550691 - }, - { - "x": 7.180758376854375, - "y": 2.2483819611971865, - "heading": 3.555072564787507e-14, - "angularVelocity": -3.2485194540738507e-16, - "velocityX": 3.7210911119157672, - "velocityY": 0.6812633837082396, - "timestamp": 1.6065147881490063 - }, - { - "x": 7.45046082250988, - "y": 2.2977595214867708, - "heading": 2.3695885326073477e-14, - "angularVelocity": -1.4618172245055264e-13, - "velocityX": 3.32569372381546, - "velocityY": 0.6088733899066815, - "timestamp": 1.6876113857473216 - }, - { - "x": 7.666222798434458, - "y": 2.3372615732702537, - "heading": 1.421890070899971e-14, - "angularVelocity": -1.1686044664925656e-13, - "velocityX": 2.660555218275404, - "velocityY": 0.48709875572268585, - "timestamp": 1.768707983345637 - }, - { - "x": 7.828044286775192, - "y": 2.366888113279094, - "heading": 7.108906150820137e-15, - "angularVelocity": -8.767315483931054e-14, - "velocityX": 1.9954164925914981, - "velocityY": 0.36532408123439786, - "timestamp": 1.8498045809439523 - }, - { - "x": 7.935925281581105, - "y": 2.3866391404237772, - "heading": 2.368258984370487e-15, - "angularVelocity": -5.845679482055086e-14, - "velocityX": 1.3302776935262535, - "velocityY": 0.24354939331133554, - "timestamp": 1.9309011785422676 - }, - { - "x": 7.989865779876709, - "y": 2.396514654159546, - "heading": 4.907869380861988e-36, - "angularVelocity": -2.920293889640648e-14, - "velocityX": 0.665138857770331, - "velocityY": 0.12177469867088454, - "timestamp": 2.011997776140583 - }, - { - "x": 7.989865779876709, - "y": 2.396514654159546, - "heading": 0, - "angularVelocity": 2.2807976020901832e-34, - "velocityX": -2.386905295344097e-35, - "velocityY": -2.096158053269738e-34, - "timestamp": 2.0930943737388983 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBMid1st.3.traj b/src/main/deploy/choreo/3NBMid1st.3.traj deleted file mode 100644 index 238f456..0000000 --- a/src/main/deploy/choreo/3NBMid1st.3.traj +++ /dev/null @@ -1,292 +0,0 @@ -{ - "samples": [ - { - "x": 7.989865779876709, - "y": 2.396514654159546, - "heading": 0, - "angularVelocity": 2.2807976020901832e-34, - "velocityX": -2.386905295344097e-35, - "velocityY": -2.096158053269738e-34, - "timestamp": 0 - }, - { - "x": 7.941752060251425, - "y": 2.3755004012341447, - "heading": 2.369857031913319e-14, - "angularVelocity": 2.9865236303183063e-13, - "velocityX": -0.6063351445610533, - "velocityY": -0.26482425770860446, - "timestamp": 0.07935169197575487 - }, - { - "x": 7.845524622758667, - "y": 2.333471896151087, - "heading": 7.109189267667888e-14, - "angularVelocity": 5.972566076099065e-13, - "velocityX": -1.2126702669699776, - "velocityY": -0.5296485057419966, - "timestamp": 0.15870338395150974 - }, - { - "x": 7.701183470328115, - "y": 2.2704291401899463, - "heading": 1.4219270868952152e-13, - "angularVelocity": 8.960214236460977e-13, - "velocityX": -1.819005352458695, - "velocityY": -0.7944727376500391, - "timestamp": 0.2380550759272646 - }, - { - "x": 7.508728608819127, - "y": 2.186372135909869, - "heading": 2.3699994402916615e-13, - "angularVelocity": 1.1947727008595082e-12, - "velocityX": -2.425340364107035, - "velocityY": -1.0592969373073977, - "timestamp": 0.31740676790301947 - }, - { - "x": 7.268160055809756, - "y": 2.081300890988283, - "heading": 3.5550661968129605e-13, - "angularVelocity": 1.4934360276569557e-12, - "velocityX": -3.0316751542345677, - "velocityY": -1.324121040212846, - "timestamp": 0.39675845987877434 - }, - { - "x": 6.993070969404171, - "y": 1.9611523827505881, - "heading": 3.5550660039058023e-13, - "angularVelocity": -2.431040264205401e-19, - "velocityX": -3.4667072567228465, - "velocityY": -1.5141266083450944, - "timestamp": 0.4761101518545292 - }, - { - "x": 6.717981882998517, - "y": 1.8410038745128643, - "heading": 3.555198084077173e-13, - "angularVelocity": 1.6644909274463737e-16, - "velocityX": -3.466707256723693, - "velocityY": -1.5141266083454636, - "timestamp": 0.5554618438302841 - }, - { - "x": 6.442892796592864, - "y": 1.7208553662751405, - "heading": 3.554980670621707e-13, - "angularVelocity": -2.7398717034583525e-16, - "velocityX": -3.466707256723693, - "velocityY": -1.5141266083454636, - "timestamp": 0.6348135358060389 - }, - { - "x": 6.1678037101872105, - "y": 1.6007068580374166, - "heading": 3.554659707830622e-13, - "angularVelocity": -4.0448134512759275e-16, - "velocityX": -3.4667072567236934, - "velocityY": -1.5141266083454628, - "timestamp": 0.7141652277817938 - }, - { - "x": 5.892714623774693, - "y": 1.4805583498154091, - "heading": 3.5548234631329863e-13, - "angularVelocity": 2.0636649110653867e-16, - "velocityX": -3.4667072568101984, - "velocityY": -1.5141266081474032, - "timestamp": 0.7935169197575487 - }, - { - "x": 5.617624458493888, - "y": 1.3604123117741571, - "heading": 3.554803531945313e-13, - "angularVelocity": -2.5117533321458206e-17, - "velocityX": -3.466720852844014, - "velocityY": -1.5140954786189205, - "timestamp": 0.8728686117333035 - }, - { - "x": 5.3267635091004895, - "y": 1.2861857871057627, - "heading": 3.5547733640886213e-13, - "angularVelocity": -3.801791233480102e-17, - "velocityX": -3.6654662572572203, - "velocityY": -0.9354119971515352, - "timestamp": 0.9522203037090584 - }, - { - "x": 5.027418765032504, - "y": 1.2637717125502315, - "heading": 3.55535992528238e-13, - "angularVelocity": 7.3919179182428e-16, - "velocityX": -3.7723801045029637, - "velocityY": -0.2824649859057747, - "timestamp": 1.0315719956848133 - }, - { - "x": 4.728747367858887, - "y": 1.293855905532837, - "heading": 0, - "angularVelocity": -4.480509283114818e-12, - "velocityX": -3.763894502273161, - "velocityY": 0.37912478276830097, - "timestamp": 1.1109236876605681 - }, - { - "x": 4.491784068028983, - "y": 1.3525348599907823, - "heading": -1.9978113544551193e-7, - "angularVelocity": -0.000003095847804257088, - "velocityX": -3.6720299433276677, - "velocityY": 0.9093006299600177, - "timestamp": 1.175455650495044 - }, - { - "x": 4.265543006179473, - "y": 1.4442399350507595, - "heading": -3.147839483974445e-7, - "angularVelocity": -0.0000017821062292327031, - "velocityX": -3.50587603277802, - "velocityY": 1.4210798964104094, - "timestamp": 1.2399876133295198 - }, - { - "x": 4.054601355970229, - "y": 1.5671157428156255, - "heading": -4.0045663910570385e-7, - "angularVelocity": -0.0000013276008809465377, - "velocityX": -3.2687933381215752, - "velocityY": 1.9041077067505576, - "timestamp": 1.3045195761639956 - }, - { - "x": 3.8632265680311315, - "y": 1.7186760821797984, - "heading": -4.7495861737987186e-7, - "angularVelocity": -0.000001154497321974619, - "velocityX": -2.965581388403958, - "velocityY": 2.3486088553190996, - "timestamp": 1.3690515389984714 - }, - { - "x": 3.6852305250099255, - "y": 1.8857456933767656, - "heading": -5.473016621730968e-7, - "angularVelocity": -0.000001121042063741103, - "velocityX": -2.7582617233845053, - "velocityY": 2.588943584832528, - "timestamp": 1.4335835018329472 - }, - { - "x": 3.507234752110206, - "y": 2.052815592360835, - "heading": -6.196446913052133e-7, - "angularVelocity": -0.0000011210418210534835, - "velocityX": -2.7582575375287743, - "velocityY": 2.588948044438113, - "timestamp": 1.498115464667423 - }, - { - "x": 3.329238979213549, - "y": 2.219885491348167, - "heading": -6.919877206511343e-7, - "angularVelocity": -0.0000011210418243666412, - "velocityX": -2.758257537481309, - "velocityY": 2.5889480444886823, - "timestamp": 1.562647427501899 - }, - { - "x": 3.1512432063159417, - "y": 2.3869553903344864, - "heading": -7.643307500404873e-7, - "angularVelocity": -0.0000011210418250396714, - "velocityX": -2.758257537496045, - "velocityY": 2.5889480444729824, - "timestamp": 1.6271793903363747 - }, - { - "x": 2.9732473495807805, - "y": 2.5540252000002215, - "heading": -8.366742189084055e-7, - "angularVelocity": -0.000001121048635286036, - "velocityX": -2.7582588366592833, - "velocityY": 2.5889466603436238, - "timestamp": 1.6917113531708505 - }, - { - "x": 2.805693234212926, - "y": 2.7041662861189204, - "heading": -0.04706564750086009, - "angularVelocity": -0.7293255738611609, - "velocityX": -2.5964515568452473, - "velocityY": 2.326615827629635, - "timestamp": 1.7562433160053263 - }, - { - "x": 2.6613205337549295, - "y": 2.837239575411043, - "heading": -0.1660200653385467, - "angularVelocity": -1.843341076464761, - "velocityX": -2.237227787853161, - "velocityY": 2.062129887997592, - "timestamp": 1.8207752788398022 - }, - { - "x": 2.5406020300757204, - "y": 2.9504211910818823, - "heading": -0.3442125528113627, - "angularVelocity": -2.7613058652791787, - "velocityX": -1.8706776979471609, - "velocityY": 1.7538845976396134, - "timestamp": 1.885307241674278 - }, - { - "x": 2.4452505510198503, - "y": 3.0399707491346923, - "heading": -0.5155036654095948, - "angularVelocity": -2.6543608016014164, - "velocityX": -1.4775852905706008, - "velocityY": 1.3876775805271009, - "timestamp": 1.9498392045087538 - }, - { - "x": 2.3746567136257015, - "y": 3.106440739683581, - "heading": -0.6585119351643622, - "angularVelocity": -2.2160843010708193, - "velocityX": -1.09393600153186, - "velocityY": 1.0300320589873262, - "timestamp": 2.0143711673432296 - }, - { - "x": 2.328134128221924, - "y": 3.150385948901578, - "heading": -0.7608754833484198, - "angularVelocity": -1.5862456941937377, - "velocityX": -0.7209231419646691, - "velocityY": 0.6809836132013578, - "timestamp": 2.0789031301777054 - }, - { - "x": 2.305089950561523, - "y": 3.1721901893615723, - "heading": -0.8148015740342935, - "angularVelocity": -0.8356493172878373, - "velocityX": -0.3570971135576525, - "velocityY": 0.33788280260313497, - "timestamp": 2.1434350930121813 - }, - { - "x": 2.305089950561523, - "y": 3.1721901893615723, - "heading": -0.8148015740342935, - "angularVelocity": -8.408063134436125e-37, - "velocityX": 3.952960951142577e-37, - "velocityY": -2.430849972147944e-37, - "timestamp": 2.207967055846657 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBMid1st.4.traj b/src/main/deploy/choreo/3NBMid1st.4.traj deleted file mode 100644 index 7a1a925..0000000 --- a/src/main/deploy/choreo/3NBMid1st.4.traj +++ /dev/null @@ -1,112 +0,0 @@ -{ - "samples": [ - { - "x": 2.305089950561523, - "y": 3.1721901893615723, - "heading": -0.8148015740342935, - "angularVelocity": -8.408063134436125e-37, - "velocityX": 3.952960951142577e-37, - "velocityY": -2.430849972147944e-37, - "timestamp": 0 - }, - { - "x": 2.309173596952405, - "y": 3.203562462246217, - "heading": -0.7891930042181571, - "angularVelocity": 0.41051298264787395, - "velocityX": 0.0654620649273396, - "velocityY": 0.5029068552748039, - "timestamp": 0.0623818756010035 - }, - { - "x": 2.3173582215057085, - "y": 3.2662983711409606, - "heading": -0.7378275898517348, - "angularVelocity": 0.8234028533376799, - "velocityX": 0.1312019633018532, - "velocityY": 1.005675258884561, - "timestamp": 0.124763751202007 - }, - { - "x": 2.3296630094846624, - "y": 3.3603796386527667, - "heading": -0.6603623761955548, - "angularVelocity": 1.2417903903956058, - "velocityX": 0.19724940714600217, - "velocityY": 1.508150670453601, - "timestamp": 0.1871456268030105 - }, - { - "x": 2.3460984624180075, - "y": 3.48577693162035, - "heading": -0.5562016902946999, - "angularVelocity": 1.669726741899689, - "velocityX": 0.2634651936159632, - "velocityY": 2.010155862732131, - "timestamp": 0.249527502404014 - }, - { - "x": 2.366651701737987, - "y": 3.642454138676695, - "heading": -0.4245464321448433, - "angularVelocity": 2.1104729038916417, - "velocityX": 0.3294745328184519, - "velocityY": 2.5115821790684434, - "timestamp": 0.3119093780050175 - }, - { - "x": 2.3865812265029587, - "y": 3.7987399457879576, - "heading": -0.28286003642691626, - "angularVelocity": 2.271275019432859, - "velocityX": 0.3194762031914812, - "velocityY": 2.505307921660973, - "timestamp": 0.374291253606021 - }, - { - "x": 2.4024793520828096, - "y": 3.923778755534052, - "heading": -0.16960155668210575, - "angularVelocity": 1.8155670802400325, - "velocityX": 0.25485167649552853, - "velocityY": 2.0044092701836567, - "timestamp": 0.4366731292070245 - }, - { - "x": 2.4143813115901978, - "y": 4.017565970817142, - "heading": -0.08476980252965222, - "angularVelocity": 1.359878223204452, - "velocityX": 0.19079194706349162, - "velocityY": 1.5034369258621847, - "timestamp": 0.499055004808028 - }, - { - "x": 2.4223090833363603, - "y": 4.080093755108703, - "heading": -0.02826122995679143, - "angularVelocity": 0.9058492074571726, - "velocityX": 0.12708453649052126, - "velocityY": 1.0023389596601993, - "timestamp": 0.5614368804090315 - }, - { - "x": 2.4262728691101074, - "y": 4.11135721206665, - "heading": 8.520612390652084e-40, - "angularVelocity": 0.4530359128274895, - "velocityX": 0.06354066362319509, - "velocityY": 0.5011625036398218, - "timestamp": 0.623818756010035 - }, - { - "x": 2.4262728691101074, - "y": 4.11135721206665, - "heading": 3.77515756333757e-40, - "angularVelocity": -3.697495905559829e-40, - "velocityX": 5.605053727452836e-41, - "velocityY": 1.9308049528794958e-41, - "timestamp": 0.6862006316110385 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBMid1st.5.traj b/src/main/deploy/choreo/3NBMid1st.5.traj deleted file mode 100644 index 6a37aa0..0000000 --- a/src/main/deploy/choreo/3NBMid1st.5.traj +++ /dev/null @@ -1,76 +0,0 @@ -{ - "samples": [ - { - "x": 2.4262728691101074, - "y": 4.11135721206665, - "heading": 3.77515756333757e-40, - "angularVelocity": -3.697495905559829e-40, - "velocityX": 5.605053727452836e-41, - "velocityY": 1.9308049528794958e-41, - "timestamp": 0 - }, - { - "x": 2.414911964876249, - "y": 4.1934081363146305, - "heading": 6.694836686372959e-18, - "angularVelocity": 6.716930715190013e-17, - "velocityX": -0.11398397029768016, - "velocityY": 0.8232170538420872, - "timestamp": 0.09967106957397753 - }, - { - "x": 2.3921901570614073, - "y": 4.3575099800953865, - "heading": 1.2510008928780832e-17, - "angularVelocity": 5.834363238263139e-17, - "velocityX": -0.22796793404506535, - "velocityY": 1.646434060376518, - "timestamp": 0.19934213914795507 - }, - { - "x": 2.3581074476242065, - "y": 4.603662729263306, - "heading": 1.8930974562426156e-17, - "angularVelocity": 6.442155844308988e-17, - "velocityX": -0.34195187814156935, - "velocityY": 2.4696509249880063, - "timestamp": 0.2990132087219326 - }, - { - "x": 2.324024738187006, - "y": 4.849815478431226, - "heading": 1.2303269381274456e-17, - "angularVelocity": -6.649577665294794e-17, - "velocityX": -0.34195187814156935, - "velocityY": 2.4696509249880063, - "timestamp": 0.39868427829591013 - }, - { - "x": 2.301302930372164, - "y": 5.013917322211981, - "heading": 5.069755620661162e-18, - "angularVelocity": -7.257385509688344e-17, - "velocityX": -0.22796793404506527, - "velocityY": 1.646434060376518, - "timestamp": 0.49835534786988767 - }, - { - "x": 2.2899420261383057, - "y": 5.095968246459961, - "heading": 3.4889982613181514e-43, - "angularVelocity": -5.086486622779004e-17, - "velocityX": -0.11398397029768013, - "velocityY": 0.8232170538420872, - "timestamp": 0.5980264174438652 - }, - { - "x": 2.2899420261383057, - "y": 5.095968246459961, - "heading": 3.871119270807216e-43, - "angularVelocity": 3.8337935220286096e-43, - "velocityX": 1.5998936301366528e-40, - "velocityY": 1.897690279064702e-41, - "timestamp": 0.6976974870178427 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NBMid1st.traj b/src/main/deploy/choreo/3NBMid1st.traj deleted file mode 100644 index 5b62fc2..0000000 --- a/src/main/deploy/choreo/3NBMid1st.traj +++ /dev/null @@ -1,1336 +0,0 @@ -{ - "samples": [ - { - "x": 0.8085346817970276, - "y": 4.440213680267334, - "heading": -0.994421194775346, - "angularVelocity": 1.5490406402833945, - "velocityX": 1.8902870667855096, - "velocityY": -2.2010802892496195, - "timestamp": 0 - }, - { - "x": 0.9215971079167389, - "y": 4.308622426592224, - "heading": -0.8609906790037924, - "angularVelocity": 2.3420643493386937, - "velocityX": 1.9845496057144765, - "velocityY": -2.309780353730473, - "timestamp": 0.05697132779858461 - }, - { - "x": 1.0571668168480761, - "y": 4.174806360601604, - "heading": -0.7788891157181965, - "angularVelocity": 1.441103208544767, - "velocityX": 2.3796129416296528, - "velocityY": -2.3488317924362083, - "timestamp": 0.11394265559716922 - }, - { - "x": 1.2076261145428426, - "y": 4.0379914622006465, - "heading": -0.737938644372034, - "angularVelocity": 0.7187908888298683, - "velocityX": 2.640965262854634, - "velocityY": -2.4014693651629555, - "timestamp": 0.17091398339575384 - }, - { - "x": 1.364794019769659, - "y": 3.9030112436724806, - "heading": -0.7095966761729068, - "angularVelocity": 0.4974777540612509, - "velocityX": 2.7587193646331154, - "velocityY": -2.369265799901533, - "timestamp": 0.22788531119433844 - }, - { - "x": 1.5254149005845012, - "y": 3.773576465253033, - "heading": -0.6782849435340634, - "angularVelocity": 0.5496051057391952, - "velocityX": 2.8193283713291417, - "velocityY": -2.271928414184925, - "timestamp": 0.2848566389929231 - }, - { - "x": 1.6863383745410894, - "y": 3.6459599816818566, - "heading": -0.6447688867134147, - "angularVelocity": 0.5882969226053631, - "velocityX": 2.8246396946462973, - "velocityY": -2.24001245016351, - "timestamp": 0.3418279667915077 - }, - { - "x": 1.8467513472707422, - "y": 3.5185431346202543, - "heading": -0.6105743641016009, - "angularVelocity": 0.6002058216495214, - "velocityX": 2.8156790253647204, - "velocityY": -2.2365082925936, - "timestamp": 0.3987992945900923 - }, - { - "x": 2.0066203974076964, - "y": 3.391336490619763, - "heading": -0.5756204436829432, - "angularVelocity": 0.6135352951967907, - "velocityX": 2.8061317212432253, - "velocityY": -2.232818663630508, - "timestamp": 0.4557706223886769 - }, - { - "x": 2.1659061752732116, - "y": 3.264353822663242, - "heading": -0.5398102091748411, - "angularVelocity": 0.6285659101136807, - "velocityX": 2.795893724447691, - "velocityY": -2.2288872817823973, - "timestamp": 0.5127419501872615 - }, - { - "x": 2.3245615366839587, - "y": 3.137612895634049, - "heading": -0.5030261268461845, - "angularVelocity": 0.6456595580622994, - "velocityX": 2.7848282204630053, - "velocityY": -2.2246440784611945, - "timestamp": 0.5697132779858461 - }, - { - "x": 2.4825289631452856, - "y": 3.0111369184355397, - "heading": -0.46512296697159733, - "angularVelocity": 0.6653023782171502, - "velocityX": 2.772753112228705, - "velocityY": -2.219993496476861, - "timestamp": 0.6266846057844307 - }, - { - "x": 2.639736737765091, - "y": 2.884956763498202, - "heading": -0.4259172061429576, - "angularVelocity": 0.6881665276829608, - "velocityX": 2.7594191796897314, - "velocityY": -2.214801020320839, - "timestamp": 0.6836559335830154 - }, - { - "x": 2.7960930159717448, - "y": 2.759114488797632, - "heading": -0.3851704679953963, - "angularVelocity": 0.7152148233514343, - "velocityX": 2.744473127946619, - "velocityY": -2.2088703135982724, - "timestamp": 0.7406272613816 - }, - { - "x": 2.9514761274080783, - "y": 2.633669229930612, - "heading": -0.3425622280740764, - "angularVelocity": 0.7478891850994993, - "velocityX": 2.7273914342609693, - "velocityY": -2.201901618135293, - "timestamp": 0.7975985891801846 - }, - { - "x": 3.105717594116411, - "y": 2.5087077543800937, - "heading": -0.29764163685561806, - "angularVelocity": 0.7884771683270175, - "velocityX": 2.707352499377148, - "velocityY": -2.193409920026875, - "timestamp": 0.8545699169787692 - }, - { - "x": 3.258569563388298, - "y": 2.384365172612816, - "heading": -0.249734366445047, - "angularVelocity": 0.8409014193936561, - "velocityX": 2.6829630829788034, - "velocityY": -2.182546669912214, - "timestamp": 0.9115412447773538 - }, - { - "x": 3.4096337882248857, - "y": 2.2608711625821383, - "heading": -0.19773775213511016, - "angularVelocity": 0.9126804011618742, - "velocityX": 2.651583360855795, - "velocityY": -2.1676519541070953, - "timestamp": 0.9685125725759384 - }, - { - "x": 3.5581727184543612, - "y": 2.1386758008181714, - "heading": -0.139571203831234, - "angularVelocity": 1.0209793338417021, - "velocityX": 2.607257649929052, - "velocityY": -2.144857184932988, - "timestamp": 1.025483900374523 - }, - { - "x": 3.702391809739322, - "y": 2.018936204532916, - "heading": -0.07007418918166924, - "angularVelocity": 1.219859486078036, - "velocityX": 2.5314328603123006, - "velocityY": -2.1017518971750273, - "timestamp": 1.0824552281731077 - }, - { - "x": 3.855921574246425, - "y": 1.8942554845290054, - "heading": -0.02632139830741084, - "angularVelocity": 0.7679791320458813, - "velocityX": 2.694860211962926, - "velocityY": -2.1884819052261575, - "timestamp": 1.1394265559716923 - }, - { - "x": 4.03019905090332, - "y": 1.7862433195114136, - "heading": 0, - "angularVelocity": 0.4620113191054108, - "velocityX": 3.059038351239285, - "velocityY": -1.8959039431107558, - "timestamp": 1.196397883770277 - }, - { - "x": 4.329561467960323, - "y": 1.669252288909222, - "heading": 2.8762828709721077e-9, - "angularVelocity": 3.3853294762370436e-8, - "velocityX": 3.5234379231905075, - "velocityY": -1.3769618709966909, - "timestamp": 1.281361042929876 - }, - { - "x": 4.6395186577016645, - "y": 1.584215240287449, - "heading": 2.8762827416187334e-9, - "angularVelocity": -1.5224642721178536e-15, - "velocityX": 3.6481363547122974, - "velocityY": -1.0008696647218007, - "timestamp": 1.3663242020894752 - }, - { - "x": 4.949475918586316, - "y": 1.4991784509810746, - "heading": 2.876282678042524e-9, - "angularVelocity": -7.482797246900931e-16, - "velocityX": 3.648137192055341, - "velocityY": -1.0008666126295613, - "timestamp": 1.4512873612490744 - }, - { - "x": 5.259433179471324, - "y": 1.4141416616759983, - "heading": 2.876282628580193e-9, - "angularVelocity": -5.82162094124467e-16, - "velocityX": 3.648137192059533, - "velocityY": -1.000866612614283, - "timestamp": 1.5362505204086736 - }, - { - "x": 5.569390440356331, - "y": 1.3291048723709222, - "heading": 2.8762825491363832e-9, - "angularVelocity": -9.350383248396003e-16, - "velocityX": 3.648137192059533, - "velocityY": -1.0008666126142827, - "timestamp": 1.6212136795682728 - }, - { - "x": 5.879347701241338, - "y": 1.244068083065846, - "heading": 2.8762824884432015e-9, - "angularVelocity": -7.143470432395581e-16, - "velocityX": 3.648137192059533, - "velocityY": -1.0008666126142824, - "timestamp": 1.706176838727872 - }, - { - "x": 6.189304962126346, - "y": 1.1590312937607696, - "heading": 2.876282331475103e-9, - "angularVelocity": -1.8474842515275816e-15, - "velocityX": 3.648137192059533, - "velocityY": -1.0008666126142827, - "timestamp": 1.7911399978874711 - }, - { - "x": 6.499262223011353, - "y": 1.0739945044556933, - "heading": 2.8762822574949493e-9, - "angularVelocity": -8.707321426249056e-16, - "velocityX": 3.648137192059533, - "velocityY": -1.0008666126142827, - "timestamp": 1.8761031570470703 - }, - { - "x": 6.809219483896361, - "y": 0.988957715150617, - "heading": 2.876282174652877e-9, - "angularVelocity": -9.750352171187047e-16, - "velocityX": 3.6481371920595334, - "velocityY": -1.0008666126142827, - "timestamp": 1.9610663162066695 - }, - { - "x": 7.119176744781355, - "y": 0.9039209258455444, - "heading": 2.876282068308637e-9, - "angularVelocity": -1.2516511966059091e-15, - "velocityX": 3.6481371920593757, - "velocityY": -1.0008666126142396, - "timestamp": 2.046029475366269 - }, - { - "x": 7.409406401112432, - "y": 0.824296405790611, - "heading": 1.9175251160573477e-9, - "angularVelocity": -1.128438445244615e-8, - "velocityX": 3.4159470905018354, - "velocityY": -0.9371652471792223, - "timestamp": 2.1309926345258683 - }, - { - "x": 7.6415901455910715, - "y": 0.7605967844204935, - "heading": 1.1505163163502184e-9, - "angularVelocity": -9.02754567148733e-9, - "velocityX": 2.732757900897883, - "velocityY": -0.7497322604313802, - "timestamp": 2.2159557936854677 - }, - { - "x": 7.815727960351833, - "y": 0.7128220666365764, - "heading": 5.752585827191569e-10, - "angularVelocity": -6.770672598819766e-9, - "velocityX": 2.049568501021147, - "velocityY": -0.5622992159951897, - "timestamp": 2.300918952845067 - }, - { - "x": 7.931819839439569, - "y": 0.680972254072654, - "heading": 1.917529656301049e-10, - "angularVelocity": -4.513787162370632e-9, - "velocityX": 1.3663790310534918, - "velocityY": -0.3748661523295523, - "timestamp": 2.3858821120046665 - }, - { - "x": 7.989865779876709, - "y": 0.6650473475456238, - "heading": 0, - "angularVelocity": -2.2568954300522894e-9, - "velocityX": 0.6831895260403813, - "velocityY": -0.18743307904919254, - "timestamp": 2.470845271164266 - }, - { - "x": 7.989865779876709, - "y": 0.6650473475456238, - "heading": 0, - "angularVelocity": 0, - "velocityX": 5.7398628141890084e-36, - "velocityY": 6.163577013070246e-34, - "timestamp": 2.5558084303238653 - }, - { - "x": 7.942973367083922, - "y": 0.667457868334859, - "heading": 1.5015515417643764e-15, - "angularVelocity": 2.0009529191708877e-14, - "velocityX": -0.6248837129789168, - "velocityY": 0.03212236460611118, - "timestamp": 2.630850253016353 - }, - { - "x": 7.84918854285797, - "y": 0.6722789098434376, - "heading": 4.5057793482934065e-15, - "angularVelocity": 4.0034046332269777e-14, - "velocityX": -1.2497674078396386, - "velocityY": 0.06424472828085026, - "timestamp": 2.7058920757088405 - }, - { - "x": 7.70851130923829, - "y": 0.6795104719665217, - "heading": 9.022031304760564e-15, - "angularVelocity": 6.018313247766123e-14, - "velocityX": -1.874651075523027, - "velocityY": 0.09636709055852906, - "timestamp": 2.780933898401328 - }, - { - "x": 7.520941669623951, - "y": 0.6891525545293808, - "heading": 1.504590019224722e-14, - "angularVelocity": 8.027348845418817e-14, - "velocityX": -2.49953469791075, - "velocityY": 0.12848945050776842, - "timestamp": 2.8559757210938157 - }, - { - "x": 7.286479630813121, - "y": 0.7012051571825528, - "heading": 2.2571401776550036e-14, - "angularVelocity": 1.0028409911019087e-13, - "velocityX": -3.124418229706744, - "velocityY": 0.16061180580010845, - "timestamp": 2.9310175437863033 - }, - { - "x": 7.005125213200534, - "y": 0.7156682788776393, - "heading": 3.160518517475634e-14, - "angularVelocity": 1.2038331525109193e-13, - "velocityX": -3.749301489724504, - "velocityY": 0.1927341471215938, - "timestamp": 3.006059366478791 - }, - { - "x": 6.721620804307546, - "y": 0.730241921622282, - "heading": 3.159325845827104e-14, - "angularVelocity": -1.5893425902210946e-16, - "velocityX": -3.777952063541342, - "velocityY": 0.19420693983358725, - "timestamp": 3.0811011891712785 - }, - { - "x": 6.438116395414555, - "y": 0.7448155643669249, - "heading": 3.1611801523052584e-14, - "angularVelocity": 2.4710307021100496e-16, - "velocityX": -3.7779520635413752, - "velocityY": 0.19420693983358922, - "timestamp": 3.156143011863766 - }, - { - "x": 6.154611986521697, - "y": 0.7593892071141327, - "heading": 3.1598377018547585e-14, - "angularVelocity": -1.7889363588638215e-16, - "velocityX": -3.777952063539618, - "velocityY": 0.19420693986776963, - "timestamp": 3.2311848345562537 - }, - { - "x": 5.8711075940921145, - "y": 0.7739631701216403, - "heading": 3.1576411220426416e-14, - "angularVelocity": -2.9271408040259687e-16, - "velocityX": -3.777951844151616, - "velocityY": 0.19421120762524555, - "timestamp": 3.3062266572487413 - }, - { - "x": 5.590080643384297, - "y": 0.8141003552309748, - "heading": 3.15610868842989e-14, - "angularVelocity": -2.0421060653488186e-16, - "velocityX": -3.744937697734655, - "velocityY": 0.5348642086401837, - "timestamp": 3.381268479941229 - }, - { - "x": 5.31951379776001, - "y": 0.9000114798545837, - "heading": 1.0126074140253907e-33, - "angularVelocity": -4.205799613054758e-13, - "velocityX": -3.60554735901122, - "velocityY": 1.1448432559489161, - "timestamp": 3.4563103026337165 - }, - { - "x": 5.087986158761807, - "y": 1.0141443752209618, - "heading": -8.06925360653917e-8, - "angularVelocity": -0.0000011825608125095831, - "velocityX": -3.393071109702901, - "velocityY": 1.6726341252821548, - "timestamp": 3.524545724619457 - }, - { - "x": 4.876193956700591, - "y": 1.1617093808932828, - "heading": -1.490061973004125e-7, - "angularVelocity": -0.0000010011466075390655, - "velocityX": -3.1038454207182142, - "velocityY": 2.162586547836665, - "timestamp": 3.5927811466051978 - }, - { - "x": 4.680065310913933, - "y": 1.329533463078152, - "heading": -2.1452736793612451e-7, - "angularVelocity": -9.602222530316319e-7, - "velocityX": -2.874293733064984, - "velocityY": 2.4594862507033355, - "timestamp": 3.6610165685909384 - }, - { - "x": 4.483936991566996, - "y": 1.4973579267580712, - "heading": -2.800485136638582e-7, - "angularVelocity": -9.602218880015987e-7, - "velocityX": -2.874288949043537, - "velocityY": 2.4594918415685947, - "timestamp": 3.729251990576679 - }, - { - "x": 4.287808672223246, - "y": 1.6651823904417138, - "heading": -3.455696595852266e-7, - "angularVelocity": -9.602218908393429e-7, - "velocityX": -2.874288948996848, - "velocityY": 2.459491841623158, - "timestamp": 3.7974874125624196 - }, - { - "x": 4.091680352879495, - "y": 1.8330068541253561, - "heading": -4.1109080579148433e-7, - "angularVelocity": -9.602218950144384e-7, - "velocityX": -2.8742889489968495, - "velocityY": 2.459491841623156, - "timestamp": 3.8657228345481602 - }, - { - "x": 3.895552033535744, - "y": 2.0008313178089985, - "heading": -4.766119523424261e-7, - "angularVelocity": -9.602219000658333e-7, - "velocityX": -2.8742889489968504, - "velocityY": 2.4594918416231537, - "timestamp": 3.933958256533901 - }, - { - "x": 3.6994237141919926, - "y": 2.1686557814926406, - "heading": -5.421330991227701e-7, - "angularVelocity": -9.60221903427755e-7, - "velocityX": -2.874288948996852, - "velocityY": 2.459491841623151, - "timestamp": 4.0021936785196415 - }, - { - "x": 3.5032953948482413, - "y": 2.3364802451762827, - "heading": -6.076542461212268e-7, - "angularVelocity": -9.602219066242283e-7, - "velocityX": -2.874288948996854, - "velocityY": 2.4594918416231493, - "timestamp": 4.0704291005053825 - }, - { - "x": 3.3071670755041227, - "y": 2.5043047088594954, - "heading": -6.731753934621747e-7, - "angularVelocity": -9.602219116434866e-7, - "velocityX": -2.874288949002235, - "velocityY": 2.45949184161686, - "timestamp": 4.138664522491123 - }, - { - "x": 3.111038718551217, - "y": 2.6721291285911115, - "heading": -7.386965721783656e-7, - "angularVelocity": -9.602223714522231e-7, - "velocityX": -2.874289500164463, - "velocityY": 2.459491197499839, - "timestamp": 4.206899944476863 - }, - { - "x": 2.91419229045963, - "y": 2.833130049750874, - "heading": -0.009424527544100834, - "angularVelocity": -0.1381069915490224, - "velocityX": -2.8848129367870357, - "velocityY": 2.3594918368557476, - "timestamp": 4.275135366462603 - }, - { - "x": 2.744128800353239, - "y": 2.9762386994865855, - "heading": -0.09849959395445267, - "angularVelocity": -1.3054080097719059, - "velocityX": -2.4923050983978627, - "velocityY": 2.0972780056319964, - "timestamp": 4.343370788448343 - }, - { - "x": 2.60131581243687, - "y": 3.0984689436943484, - "heading": -0.2549637418085224, - "angularVelocity": -2.293004766450577, - "velocityX": -2.0929450388130264, - "velocityY": 1.7913019462722148, - "timestamp": 4.411606210434083 - }, - { - "x": 2.487964457072741, - "y": 3.1954189236366886, - "heading": -0.41008490805759246, - "angularVelocity": -2.2733231763626556, - "velocityX": -1.661180543264116, - "velocityY": 1.4208160090605124, - "timestamp": 4.479841632419824 - }, - { - "x": 2.403679735462392, - "y": 3.26747969283638, - "heading": -0.5415632745482113, - "angularVelocity": -1.9268345188528966, - "velocityX": -1.2352048123621473, - "velocityY": 1.0560610179087115, - "timestamp": 4.548077054405564 - }, - { - "x": 2.3479306427779894, - "y": 3.3151481611881186, - "heading": -0.6366834880204346, - "angularVelocity": -1.3940005162143045, - "velocityX": -0.8170110341818207, - "velocityY": 0.6985883132912986, - "timestamp": 4.616312476391304 - }, - { - "x": 2.3202381134033203, - "y": 3.3388166427612305, - "heading": -0.6871242163258895, - "angularVelocity": -0.7392161847551203, - "velocityX": -0.40583803204816526, - "velocityY": 0.3468650282262215, - "timestamp": 4.684547898377044 - }, - { - "x": 2.3202381134033203, - "y": 3.3388166427612305, - "heading": -0.6871242163258895, - "angularVelocity": -2.336499408021271e-31, - "velocityX": 1.807664942161883e-33, - "velocityY": -2.7160751304258966e-32, - "timestamp": 4.752783320362784 - }, - { - "x": 2.343794886689721, - "y": 3.323479061689414, - "heading": -0.6491973045996883, - "angularVelocity": 0.6336054834749958, - "velocityX": 0.39353852048358584, - "velocityY": -0.256229021242233, - "timestamp": 4.8126421958229395 - }, - { - "x": 2.3911982166575956, - "y": 3.2927250738018596, - "heading": -0.5766774436489436, - "angularVelocity": 1.2115139215907424, - "velocityX": 0.7919181508751876, - "velocityY": -0.5137749022369447, - "timestamp": 4.872501071283095 - }, - { - "x": 2.462785855058177, - "y": 3.246405346319809, - "heading": -0.4743619817187913, - "angularVelocity": 1.7092780501407518, - "velocityX": 1.195940248630853, - "velocityY": -0.773815530712448, - "timestamp": 4.93235994674325 - }, - { - "x": 2.5589185460183237, - "y": 3.184270435840568, - "heading": -0.3491026464013455, - "angularVelocity": 2.09257748921167, - "velocityX": 1.6059889234661082, - "velocityY": -1.0380233507828165, - "timestamp": 4.992218822203405 - }, - { - "x": 2.679950432279286, - "y": 3.106027779542805, - "heading": -0.21085817155844028, - "angularVelocity": 2.309506715256066, - "velocityX": 2.021953892894741, - "velocityY": -1.3071187137460396, - "timestamp": 5.05207769766356 - }, - { - "x": 2.8260176035796434, - "y": 3.011555903011265, - "heading": -0.07818278278115957, - "angularVelocity": 2.216469784261071, - "velocityX": 2.4401923721000403, - "velocityY": -1.5782434234740206, - "timestamp": 5.111936573123716 - }, - { - "x": 2.994403761571005, - "y": 2.9064091851549483, - "heading": -0.008153033218424529, - "angularVelocity": 1.1699142194769419, - "velocityX": 2.8130524787998565, - "velocityY": -1.756576899382393, - "timestamp": 5.171795448583871 - }, - { - "x": 3.1849716588184336, - "y": 2.790203147279415, - "heading": -5.969233857045932e-7, - "angularVelocity": 0.136194277496333, - "velocityX": 3.1836197352935405, - "velocityY": -1.9413334611153064, - "timestamp": 5.231654324044026 - }, - { - "x": 3.3752288967163246, - "y": 2.6674089766097917, - "heading": -5.278378990920332e-7, - "angularVelocity": 0.0000011541394000718638, - "velocityX": 3.178429872517975, - "velocityY": -2.0513945463502785, - "timestamp": 5.291513199504181 - }, - { - "x": 3.5654860836141538, - "y": 2.5446147269206905, - "heading": -4.587524705883237e-7, - "angularVelocity": 0.0000011541384293077112, - "velocityX": 3.178429020512966, - "velocityY": -2.051395866446551, - "timestamp": 5.3513720749643365 - }, - { - "x": 3.755743270511201, - "y": 2.4218204772303773, - "heading": -3.8966704288748865e-7, - "angularVelocity": 0.0000011541384158949214, - "velocityX": 3.1784290204999057, - "velocityY": -2.0513958664667884, - "timestamp": 5.411230950424492 - }, - { - "x": 3.9460004574097183, - "y": 2.2990262275423423, - "heading": -3.205816159441191e-7, - "angularVelocity": 0.0000011541384032407341, - "velocityX": 3.1784290205244665, - "velocityY": -2.0513958664287353, - "timestamp": 5.471089825884647 - }, - { - "x": 4.136257740222623, - "y": 2.176232126464018, - "heading": -2.5149617447282606e-7, - "angularVelocity": 0.0000011541386459436468, - "velocityX": 3.178430622866425, - "velocityY": -2.051393383760805, - "timestamp": 5.530948701344802 - }, - { - "x": 4.331712975970885, - "y": 2.0618928262664933, - "heading": -1.810305943700683e-7, - "angularVelocity": 0.00000117719518719764, - "velocityX": 3.2652674185027872, - "velocityY": -1.9101478154836689, - "timestamp": 5.590807576804957 - }, - { - "x": 4.540519439656892, - "y": 1.9742795609077881, - "heading": -1.0217911975724037e-7, - "angularVelocity": 0.0000013172896083775409, - "velocityX": 3.488312503047245, - "velocityY": -1.4636637371683408, - "timestamp": 5.650666452265113 - }, - { - "x": 4.759042739868164, - "y": 1.914918065071106, - "heading": 0, - "angularVelocity": 0.0000017070003232061267, - "velocityX": 3.6506415887603976, - "velocityY": -0.991690795731634, - "timestamp": 5.710525327725268 - }, - { - "x": 5.064685674327935, - "y": 1.8884875071659226, - "heading": 3.5654904377837356e-14, - "angularVelocity": 4.3965968281974806e-13, - "velocityX": 3.768874940643874, - "velocityY": -0.32591451044713626, - "timestamp": 5.791621925323583 - }, - { - "x": 5.370151366519879, - "y": 1.9168933201476963, - "heading": 3.5699569493639844e-14, - "angularVelocity": 5.507643615792375e-16, - "velocityX": 3.7666893709273386, - "velocityY": 0.35027132855156706, - "timestamp": 5.8727185229218986 - }, - { - "x": 5.6719192342055065, - "y": 1.972141248763098, - "heading": 3.5697718803372364e-14, - "angularVelocity": -2.2820812737714697e-17, - "velocityX": 3.721091594746464, - "velocityY": 0.681260746462555, - "timestamp": 5.953815120520214 - }, - { - "x": 5.973687062735465, - "y": 2.0273893912489522, - "heading": 3.565124865229906e-14, - "angularVelocity": -5.730222037609759e-16, - "velocityX": 3.7210911119189434, - "velocityY": 0.681263383693449, - "timestamp": 6.034911718118529 - }, - { - "x": 6.275454891265202, - "y": 2.0826375337360123, - "heading": 3.5597782853381526e-14, - "angularVelocity": -6.592853522950103e-16, - "velocityX": 3.7210911119162207, - "velocityY": 0.6812633837083223, - "timestamp": 6.116008315716845 - }, - { - "x": 6.577222719794938, - "y": 2.137885676223073, - "heading": 3.5548087973236424e-14, - "angularVelocity": -6.127862526596166e-16, - "velocityX": 3.72109111191622, - "velocityY": 0.6812633837083226, - "timestamp": 6.19710491331516 - }, - { - "x": 6.878990548324675, - "y": 2.193133818710133, - "heading": 3.55770700353708e-14, - "angularVelocity": 3.573770416112574e-16, - "velocityX": 3.72109111191622, - "velocityY": 0.6812633837083226, - "timestamp": 6.278201510913475 - }, - { - "x": 7.180758376854375, - "y": 2.2483819611971865, - "heading": 3.555072564787507e-14, - "angularVelocity": -3.2485194540738507e-16, - "velocityX": 3.7210911119157672, - "velocityY": 0.6812633837082396, - "timestamp": 6.359298108511791 - }, - { - "x": 7.45046082250988, - "y": 2.2977595214867708, - "heading": 2.3695885326073477e-14, - "angularVelocity": -1.4618172245055264e-13, - "velocityX": 3.32569372381546, - "velocityY": 0.6088733899066815, - "timestamp": 6.440394706110106 - }, - { - "x": 7.666222798434458, - "y": 2.3372615732702537, - "heading": 1.421890070899971e-14, - "angularVelocity": -1.1686044664925656e-13, - "velocityX": 2.660555218275404, - "velocityY": 0.48709875572268585, - "timestamp": 6.521491303708421 - }, - { - "x": 7.828044286775192, - "y": 2.366888113279094, - "heading": 7.108906150820137e-15, - "angularVelocity": -8.767315483931054e-14, - "velocityX": 1.9954164925914981, - "velocityY": 0.36532408123439786, - "timestamp": 6.602587901306737 - }, - { - "x": 7.935925281581105, - "y": 2.3866391404237772, - "heading": 2.368258984370487e-15, - "angularVelocity": -5.845679482055086e-14, - "velocityX": 1.3302776935262535, - "velocityY": 0.24354939331133554, - "timestamp": 6.683684498905052 - }, - { - "x": 7.989865779876709, - "y": 2.396514654159546, - "heading": 4.907869380861988e-36, - "angularVelocity": -2.920293889640648e-14, - "velocityX": 0.665138857770331, - "velocityY": 0.12177469867088454, - "timestamp": 6.764781096503367 - }, - { - "x": 7.989865779876709, - "y": 2.396514654159546, - "heading": 0, - "angularVelocity": 2.2807976020901832e-34, - "velocityX": -2.386905295344097e-35, - "velocityY": -2.096158053269738e-34, - "timestamp": 6.845877694101683 - }, - { - "x": 7.941752060251425, - "y": 2.3755004012341447, - "heading": 2.369857031913319e-14, - "angularVelocity": 2.9865236303183063e-13, - "velocityX": -0.6063351445610533, - "velocityY": -0.26482425770860446, - "timestamp": 6.925229386077437 - }, - { - "x": 7.845524622758667, - "y": 2.333471896151087, - "heading": 7.109189267667888e-14, - "angularVelocity": 5.972566076099065e-13, - "velocityX": -1.2126702669699776, - "velocityY": -0.5296485057419966, - "timestamp": 7.004581078053192 - }, - { - "x": 7.701183470328115, - "y": 2.2704291401899463, - "heading": 1.4219270868952152e-13, - "angularVelocity": 8.960214236460977e-13, - "velocityX": -1.819005352458695, - "velocityY": -0.7944727376500391, - "timestamp": 7.083932770028947 - }, - { - "x": 7.508728608819127, - "y": 2.186372135909869, - "heading": 2.3699994402916615e-13, - "angularVelocity": 1.1947727008595082e-12, - "velocityX": -2.425340364107035, - "velocityY": -1.0592969373073977, - "timestamp": 7.163284462004702 - }, - { - "x": 7.268160055809756, - "y": 2.081300890988283, - "heading": 3.5550661968129605e-13, - "angularVelocity": 1.4934360276569557e-12, - "velocityX": -3.0316751542345677, - "velocityY": -1.324121040212846, - "timestamp": 7.242636153980457 - }, - { - "x": 6.993070969404171, - "y": 1.9611523827505881, - "heading": 3.5550660039058023e-13, - "angularVelocity": -2.431040264205401e-19, - "velocityX": -3.4667072567228465, - "velocityY": -1.5141266083450944, - "timestamp": 7.321987845956212 - }, - { - "x": 6.717981882998517, - "y": 1.8410038745128643, - "heading": 3.555198084077173e-13, - "angularVelocity": 1.6644909274463737e-16, - "velocityX": -3.466707256723693, - "velocityY": -1.5141266083454636, - "timestamp": 7.401339537931967 - }, - { - "x": 6.442892796592864, - "y": 1.7208553662751405, - "heading": 3.554980670621707e-13, - "angularVelocity": -2.7398717034583525e-16, - "velocityX": -3.466707256723693, - "velocityY": -1.5141266083454636, - "timestamp": 7.4806912299077215 - }, - { - "x": 6.1678037101872105, - "y": 1.6007068580374166, - "heading": 3.554659707830622e-13, - "angularVelocity": -4.0448134512759275e-16, - "velocityX": -3.4667072567236934, - "velocityY": -1.5141266083454628, - "timestamp": 7.560042921883476 - }, - { - "x": 5.892714623774693, - "y": 1.4805583498154091, - "heading": 3.5548234631329863e-13, - "angularVelocity": 2.0636649110653867e-16, - "velocityX": -3.4667072568101984, - "velocityY": -1.5141266081474032, - "timestamp": 7.639394613859231 - }, - { - "x": 5.617624458493888, - "y": 1.3604123117741571, - "heading": 3.554803531945313e-13, - "angularVelocity": -2.5117533321458206e-17, - "velocityX": -3.466720852844014, - "velocityY": -1.5140954786189205, - "timestamp": 7.718746305834986 - }, - { - "x": 5.3267635091004895, - "y": 1.2861857871057627, - "heading": 3.5547733640886213e-13, - "angularVelocity": -3.801791233480102e-17, - "velocityX": -3.6654662572572203, - "velocityY": -0.9354119971515352, - "timestamp": 7.798097997810741 - }, - { - "x": 5.027418765032504, - "y": 1.2637717125502315, - "heading": 3.55535992528238e-13, - "angularVelocity": 7.3919179182428e-16, - "velocityX": -3.7723801045029637, - "velocityY": -0.2824649859057747, - "timestamp": 7.877449689786496 - }, - { - "x": 4.728747367858887, - "y": 1.293855905532837, - "heading": 0, - "angularVelocity": -4.480509283114818e-12, - "velocityX": -3.763894502273161, - "velocityY": 0.37912478276830097, - "timestamp": 7.956801381762251 - }, - { - "x": 4.491784068028983, - "y": 1.3525348599907823, - "heading": -1.9978113544551193e-7, - "angularVelocity": -0.000003095847804257088, - "velocityX": -3.6720299433276677, - "velocityY": 0.9093006299600177, - "timestamp": 8.021333344596727 - }, - { - "x": 4.265543006179473, - "y": 1.4442399350507595, - "heading": -3.147839483974445e-7, - "angularVelocity": -0.0000017821062292327031, - "velocityX": -3.50587603277802, - "velocityY": 1.4210798964104094, - "timestamp": 8.085865307431202 - }, - { - "x": 4.054601355970229, - "y": 1.5671157428156255, - "heading": -4.0045663910570385e-7, - "angularVelocity": -0.0000013276008809465377, - "velocityX": -3.2687933381215752, - "velocityY": 1.9041077067505576, - "timestamp": 8.150397270265678 - }, - { - "x": 3.8632265680311315, - "y": 1.7186760821797984, - "heading": -4.7495861737987186e-7, - "angularVelocity": -0.000001154497321974619, - "velocityX": -2.965581388403958, - "velocityY": 2.3486088553190996, - "timestamp": 8.214929233100154 - }, - { - "x": 3.6852305250099255, - "y": 1.8857456933767656, - "heading": -5.473016621730968e-7, - "angularVelocity": -0.000001121042063741103, - "velocityX": -2.7582617233845053, - "velocityY": 2.588943584832528, - "timestamp": 8.27946119593463 - }, - { - "x": 3.507234752110206, - "y": 2.052815592360835, - "heading": -6.196446913052133e-7, - "angularVelocity": -0.0000011210418210534835, - "velocityX": -2.7582575375287743, - "velocityY": 2.588948044438113, - "timestamp": 8.343993158769106 - }, - { - "x": 3.329238979213549, - "y": 2.219885491348167, - "heading": -6.919877206511343e-7, - "angularVelocity": -0.0000011210418243666412, - "velocityX": -2.758257537481309, - "velocityY": 2.5889480444886823, - "timestamp": 8.408525121603581 - }, - { - "x": 3.1512432063159417, - "y": 2.3869553903344864, - "heading": -7.643307500404873e-7, - "angularVelocity": -0.0000011210418250396714, - "velocityX": -2.758257537496045, - "velocityY": 2.5889480444729824, - "timestamp": 8.473057084438057 - }, - { - "x": 2.9732473495807805, - "y": 2.5540252000002215, - "heading": -8.366742189084055e-7, - "angularVelocity": -0.000001121048635286036, - "velocityX": -2.7582588366592833, - "velocityY": 2.5889466603436238, - "timestamp": 8.537589047272533 - }, - { - "x": 2.805693234212926, - "y": 2.7041662861189204, - "heading": -0.04706564750086009, - "angularVelocity": -0.7293255738611609, - "velocityX": -2.5964515568452473, - "velocityY": 2.326615827629635, - "timestamp": 8.602121010107009 - }, - { - "x": 2.6613205337549295, - "y": 2.837239575411043, - "heading": -0.1660200653385467, - "angularVelocity": -1.843341076464761, - "velocityX": -2.237227787853161, - "velocityY": 2.062129887997592, - "timestamp": 8.666652972941485 - }, - { - "x": 2.5406020300757204, - "y": 2.9504211910818823, - "heading": -0.3442125528113627, - "angularVelocity": -2.7613058652791787, - "velocityX": -1.8706776979471609, - "velocityY": 1.7538845976396134, - "timestamp": 8.73118493577596 - }, - { - "x": 2.4452505510198503, - "y": 3.0399707491346923, - "heading": -0.5155036654095948, - "angularVelocity": -2.6543608016014164, - "velocityX": -1.4775852905706008, - "velocityY": 1.3876775805271009, - "timestamp": 8.795716898610436 - }, - { - "x": 2.3746567136257015, - "y": 3.106440739683581, - "heading": -0.6585119351643622, - "angularVelocity": -2.2160843010708193, - "velocityX": -1.09393600153186, - "velocityY": 1.0300320589873262, - "timestamp": 8.860248861444912 - }, - { - "x": 2.328134128221924, - "y": 3.150385948901578, - "heading": -0.7608754833484198, - "angularVelocity": -1.5862456941937377, - "velocityX": -0.7209231419646691, - "velocityY": 0.6809836132013578, - "timestamp": 8.924780824279388 - }, - { - "x": 2.305089950561523, - "y": 3.1721901893615723, - "heading": -0.8148015740342935, - "angularVelocity": -0.8356493172878373, - "velocityX": -0.3570971135576525, - "velocityY": 0.33788280260313497, - "timestamp": 8.989312787113864 - }, - { - "x": 2.305089950561523, - "y": 3.1721901893615723, - "heading": -0.8148015740342935, - "angularVelocity": -8.408063134436125e-37, - "velocityX": 3.952960951142577e-37, - "velocityY": -2.430849972147944e-37, - "timestamp": 9.05384474994834 - }, - { - "x": 2.309173596952405, - "y": 3.203562462246217, - "heading": -0.7891930042181571, - "angularVelocity": 0.41051298264787395, - "velocityX": 0.0654620649273396, - "velocityY": 0.5029068552748039, - "timestamp": 9.116226625549343 - }, - { - "x": 2.3173582215057085, - "y": 3.2662983711409606, - "heading": -0.7378275898517348, - "angularVelocity": 0.8234028533376799, - "velocityX": 0.1312019633018532, - "velocityY": 1.005675258884561, - "timestamp": 9.178608501150347 - }, - { - "x": 2.3296630094846624, - "y": 3.3603796386527667, - "heading": -0.6603623761955548, - "angularVelocity": 1.2417903903956058, - "velocityX": 0.19724940714600217, - "velocityY": 1.508150670453601, - "timestamp": 9.24099037675135 - }, - { - "x": 2.3460984624180075, - "y": 3.48577693162035, - "heading": -0.5562016902946999, - "angularVelocity": 1.669726741899689, - "velocityX": 0.2634651936159632, - "velocityY": 2.010155862732131, - "timestamp": 9.303372252352354 - }, - { - "x": 2.366651701737987, - "y": 3.642454138676695, - "heading": -0.4245464321448433, - "angularVelocity": 2.1104729038916417, - "velocityX": 0.3294745328184519, - "velocityY": 2.5115821790684434, - "timestamp": 9.365754127953357 - }, - { - "x": 2.3865812265029587, - "y": 3.7987399457879576, - "heading": -0.28286003642691626, - "angularVelocity": 2.271275019432859, - "velocityX": 0.3194762031914812, - "velocityY": 2.505307921660973, - "timestamp": 9.42813600355436 - }, - { - "x": 2.4024793520828096, - "y": 3.923778755534052, - "heading": -0.16960155668210575, - "angularVelocity": 1.8155670802400325, - "velocityX": 0.25485167649552853, - "velocityY": 2.0044092701836567, - "timestamp": 9.490517879155364 - }, - { - "x": 2.4143813115901978, - "y": 4.017565970817142, - "heading": -0.08476980252965222, - "angularVelocity": 1.359878223204452, - "velocityX": 0.19079194706349162, - "velocityY": 1.5034369258621847, - "timestamp": 9.552899754756368 - }, - { - "x": 2.4223090833363603, - "y": 4.080093755108703, - "heading": -0.02826122995679143, - "angularVelocity": 0.9058492074571726, - "velocityX": 0.12708453649052126, - "velocityY": 1.0023389596601993, - "timestamp": 9.615281630357371 - }, - { - "x": 2.4262728691101074, - "y": 4.11135721206665, - "heading": 8.520612390652084e-40, - "angularVelocity": 0.4530359128274895, - "velocityX": 0.06354066362319509, - "velocityY": 0.5011625036398218, - "timestamp": 9.677663505958375 - }, - { - "x": 2.4262728691101074, - "y": 4.11135721206665, - "heading": 3.77515756333757e-40, - "angularVelocity": -3.697495905559829e-40, - "velocityX": 5.605053727452836e-41, - "velocityY": 1.9308049528794958e-41, - "timestamp": 9.740045381559378 - }, - { - "x": 2.414911964876249, - "y": 4.1934081363146305, - "heading": 6.694836686372959e-18, - "angularVelocity": 6.716930715190013e-17, - "velocityX": -0.11398397029768016, - "velocityY": 0.8232170538420872, - "timestamp": 9.839716451133356 - }, - { - "x": 2.3921901570614073, - "y": 4.3575099800953865, - "heading": 1.2510008928780832e-17, - "angularVelocity": 5.834363238263139e-17, - "velocityX": -0.22796793404506535, - "velocityY": 1.646434060376518, - "timestamp": 9.939387520707333 - }, - { - "x": 2.3581074476242065, - "y": 4.603662729263306, - "heading": 1.8930974562426156e-17, - "angularVelocity": 6.442155844308988e-17, - "velocityX": -0.34195187814156935, - "velocityY": 2.4696509249880063, - "timestamp": 10.03905859028131 - }, - { - "x": 2.324024738187006, - "y": 4.849815478431226, - "heading": 1.2303269381274456e-17, - "angularVelocity": -6.649577665294794e-17, - "velocityX": -0.34195187814156935, - "velocityY": 2.4696509249880063, - "timestamp": 10.138729659855288 - }, - { - "x": 2.301302930372164, - "y": 5.013917322211981, - "heading": 5.069755620661162e-18, - "angularVelocity": -7.257385509688344e-17, - "velocityX": -0.22796793404506527, - "velocityY": 1.646434060376518, - "timestamp": 10.238400729429266 - }, - { - "x": 2.2899420261383057, - "y": 5.095968246459961, - "heading": 3.4889982613181514e-43, - "angularVelocity": -5.086486622779004e-17, - "velocityX": -0.11398397029768013, - "velocityY": 0.8232170538420872, - "timestamp": 10.338071799003243 - }, - { - "x": 2.2899420261383057, - "y": 5.095968246459961, - "heading": 3.871119270807216e-43, - "angularVelocity": 3.8337935220286096e-43, - "velocityX": 1.5998936301366528e-40, - "velocityY": 1.897690279064702e-41, - "timestamp": 10.437742868577221 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NM.1.traj b/src/main/deploy/choreo/3NM-Close.1.traj similarity index 100% rename from src/main/deploy/choreo/3NM.1.traj rename to src/main/deploy/choreo/3NM-Close.1.traj diff --git a/src/main/deploy/choreo/3NM.2.traj b/src/main/deploy/choreo/3NM-Close.2.traj similarity index 100% rename from src/main/deploy/choreo/3NM.2.traj rename to src/main/deploy/choreo/3NM-Close.2.traj diff --git a/src/main/deploy/choreo/3NM.3.traj b/src/main/deploy/choreo/3NM-Close.3.traj similarity index 100% rename from src/main/deploy/choreo/3NM.3.traj rename to src/main/deploy/choreo/3NM-Close.3.traj diff --git a/src/main/deploy/choreo/3NM.traj b/src/main/deploy/choreo/3NM-Close.traj similarity index 100% rename from src/main/deploy/choreo/3NM.traj rename to src/main/deploy/choreo/3NM-Close.traj diff --git a/src/main/deploy/choreo/3NT-2ndN.1.traj b/src/main/deploy/choreo/3NT-2ndN.1.traj deleted file mode 100644 index 5e2ec11..0000000 --- a/src/main/deploy/choreo/3NT-2ndN.1.traj +++ /dev/null @@ -1,157 +0,0 @@ -{ - "samples": [ - { - "x": 0.7773118615150452, - "y": 6.72802209854126, - "heading": 1.030377205028743, - "angularVelocity": -2.5603966606354566e-33, - "velocityX": 0, - "velocityY": 3.3983269045643113e-34, - "timestamp": 0 - }, - { - "x": 0.8017165578496347, - "y": 6.73204375441724, - "heading": 1.0153126355108713, - "angularVelocity": -0.2257566165421289, - "velocityX": 0.36572712321443296, - "velocityY": 0.0602682620556043, - "timestamp": 0.06672924917379079 - }, - { - "x": 0.8505266341075832, - "y": 6.740088761712056, - "heading": 0.9851666091775935, - "angularVelocity": -0.45176630497916986, - "velocityX": 0.7314644906437763, - "velocityY": 0.12056193340141036, - "timestamp": 0.13345849834758158 - }, - { - "x": 0.9237415389002549, - "y": 6.752159262105969, - "heading": 0.9398639932852465, - "angularVelocity": -0.678901927614384, - "velocityX": 1.0971935950004397, - "velocityY": 0.18088769982225175, - "timestamp": 0.20018774752137236 - }, - { - "x": 1.0213592682884212, - "y": 6.768257272086683, - "heading": 0.8792594067892606, - "angularVelocity": -0.9082162207182363, - "velocityX": 1.4628926684598107, - "velocityY": 0.24124368519101108, - "timestamp": 0.26691699669516317 - }, - { - "x": 1.1433761786203043, - "y": 6.788383770319312, - "heading": 0.8031252901056259, - "angularVelocity": -1.1409407063063117, - "velocityX": 1.8285371384008848, - "velocityY": 0.30161433676873595, - "timestamp": 0.333646245868954 - }, - { - "x": 1.2897869899026333, - "y": 6.812537367144202, - "heading": 0.7111466792887886, - "angularVelocity": -1.3783852202096063, - "velocityX": 2.1941024827210964, - "velocityY": 0.3619641630012081, - "timestamp": 0.4003754950427448 - }, - { - "x": 1.4605852210940866, - "y": 6.8407125983828605, - "heading": 0.6029335592009877, - "angularVelocity": -1.6216744745017098, - "velocityX": 2.55957070259585, - "velocityY": 0.4222321034255566, - "timestamp": 0.4671047442165356 - }, - { - "x": 1.6557643007257534, - "y": 6.872898115584218, - "heading": 0.4780638213571818, - "angularVelocity": -1.871289417907176, - "velocityX": 2.924940442883433, - "velocityY": 0.48232997673228606, - "timestamp": 0.5338339933903263 - }, - { - "x": 1.8264308864004113, - "y": 6.900588544051283, - "heading": 0.35839609824636437, - "angularVelocity": -1.793332378117321, - "velocityX": 2.5575978718143664, - "velocityY": 0.4149668819882522, - "timestamp": 0.6005632425641171 - }, - { - "x": 1.9727210620901834, - "y": 6.9242976491672845, - "heading": 0.2558451219964055, - "angularVelocity": -1.5368219711699938, - "velocityX": 2.192294645917135, - "velocityY": 0.3553030404141562, - "timestamp": 0.6672924917379078 - }, - { - "x": 2.0946339267218823, - "y": 6.944038578297596, - "heading": 0.1704674226074578, - "angularVelocity": -1.2794644094763992, - "velocityX": 1.8269779165982687, - "velocityY": 0.2958362243654028, - "timestamp": 0.7340217409116986 - }, - { - "x": 2.192167334429377, - "y": 6.959821625235924, - "heading": 0.10224171092320174, - "angularVelocity": -1.0224258856347668, - "velocityX": 1.4616290294751688, - "velocityY": 0.23652367041058378, - "timestamp": 0.8007509900854893 - }, - { - "x": 2.265318973651712, - "y": 6.971654176410272, - "heading": 0.05111626576067614, - "angularVelocity": -0.7661624519312907, - "velocityX": 1.0962455014564527, - "velocityY": 0.17732180896462885, - "timestamp": 0.8674802392592801 - }, - { - "x": 2.314087093590593, - "y": 6.979540946456479, - "heading": 0.01704319032984002, - "angularVelocity": -0.5106167962731848, - "velocityX": 0.7308357360933072, - "velocityY": 0.11819060073141874, - "timestamp": 0.9342094884330708 - }, - { - "x": 2.338470935821533, - "y": 6.983484268188477, - "heading": 0, - "angularVelocity": -0.2554080937648863, - "velocityX": 0.3654146050322646, - "velocityY": 0.059094351889496924, - "timestamp": 1.0009387376068617 - }, - { - "x": 2.338470935821533, - "y": 6.983484268188477, - "heading": 0, - "angularVelocity": 0, - "velocityX": -3.661539873949153e-32, - "velocityY": -1.6264049380013454e-33, - "timestamp": 1.0676679867806524 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NT-2ndN.2.traj b/src/main/deploy/choreo/3NT-2ndN.2.traj deleted file mode 100644 index 6831718..0000000 --- a/src/main/deploy/choreo/3NT-2ndN.2.traj +++ /dev/null @@ -1,274 +0,0 @@ -{ - "samples": [ - { - "x": 2.338470935821533, - "y": 6.983484268188477, - "heading": 0, - "angularVelocity": 0, - "velocityX": -3.661539873949153e-32, - "velocityY": -1.6264049380013454e-33, - "timestamp": 0 - }, - { - "x": 2.3668454143263347, - "y": 6.982856207850841, - "heading": -5.424481691988399e-19, - "angularVelocity": -7.628324804228275e-18, - "velocityX": 0.39902381550093635, - "velocityY": -0.008832269190277171, - "timestamp": 0.07110973681904165 - }, - { - "x": 2.423594370858518, - "y": 6.981600087186138, - "heading": -1.6273462227067539e-18, - "angularVelocity": -1.5256673727660342e-17, - "velocityX": 0.7980476242880341, - "velocityY": -0.01766453823194559, - "timestamp": 0.1422194736380833 - }, - { - "x": 2.5087178048213095, - "y": 6.979715906207575, - "heading": -3.2546963346700688e-18, - "angularVelocity": -2.2885053225616023e-17, - "velocityX": 1.1970714246828327, - "velocityY": -0.02649680708785306, - "timestamp": 0.21332921045712494 - }, - { - "x": 2.6222157154474277, - "y": 6.977203664932138, - "heading": -5.22299810707614e-18, - "angularVelocity": -2.7679778613369908e-17, - "velocityX": 1.5960952142875302, - "velocityY": -0.03532907570492495, - "timestamp": 0.2844389472761666 - }, - { - "x": 2.764088101713831, - "y": 6.974063363382471, - "heading": -7.733758581198458e-18, - "angularVelocity": -3.5308251533986704e-17, - "velocityX": 1.9951189895054233, - "velocityY": -0.04416134400354938, - "timestamp": 0.35554868409520823 - }, - { - "x": 2.9343349621882595, - "y": 6.970295001590276, - "heading": -1.1212959884643953e-17, - "angularVelocity": -4.892721389616843e-17, - "velocityX": 2.394142744581784, - "velocityY": -0.0529936118563472, - "timestamp": 0.4266584209142499 - }, - { - "x": 3.1329562947223244, - "y": 6.965898579603107, - "heading": -1.5058318284360188e-17, - "angularVelocity": -5.4076397575508025e-17, - "velocityX": 2.7931664694458354, - "velocityY": -0.06182587904040484, - "timestamp": 0.4977681577332915 - }, - { - "x": 3.3599520957353746, - "y": 6.960874097500221, - "heading": -1.921107651348465e-17, - "angularVelocity": -5.839929122072696e-17, - "velocityX": 3.192190143956008, - "velocityY": -0.0706581451098949, - "timestamp": 0.5688778945523332 - }, - { - "x": 3.6153223580661016, - "y": 6.955221555440131, - "heading": -1.7381335638599224e-17, - "angularVelocity": 2.573122833433782e-17, - "velocityX": 3.5912137177583303, - "velocityY": -0.07949040895024774, - "timestamp": 0.6399876313713748 - }, - { - "x": 3.89906706023054, - "y": 6.948940953898378, - "heading": -1.600425363722977e-17, - "angularVelocity": 1.9365589903304247e-17, - "velocityX": 3.990236989436556, - "velocityY": -0.0883226661031768, - "timestamp": 0.7110973681904165 - }, - { - "x": 4.189757421654959, - "y": 6.942506612329325, - "heading": -2.309450754421387e-17, - "angularVelocity": -9.970862253403092e-17, - "velocityX": 4.087912210449614, - "velocityY": -0.09048467702006271, - "timestamp": 0.7822071050094581 - }, - { - "x": 4.480447783078946, - "y": 6.936072270739366, - "heading": -3.567945640397763e-17, - "angularVelocity": -1.769792636385315e-16, - "velocityX": 4.08791221044355, - "velocityY": -0.09048467731405441, - "timestamp": 0.8533168418284998 - }, - { - "x": 4.77113812835654, - "y": 6.9296371997299495, - "heading": -5.239589681395825e-17, - "angularVelocity": -2.350794864073287e-16, - "velocityX": 4.0879119833805015, - "velocityY": -0.0904949349734297, - "timestamp": 0.9244265786475414 - }, - { - "x": 5.060999121162208, - "y": 6.906770374050763, - "heading": -6.413834291103341e-17, - "angularVelocity": -1.6513133956545303e-16, - "velocityX": 4.0762489888452125, - "velocityY": -0.3215709507880496, - "timestamp": 0.995536315466583 - }, - { - "x": 5.347249984741211, - "y": 6.855753421783447, - "heading": -7.100878289298809e-17, - "angularVelocity": -9.661742947296258e-17, - "velocityX": 4.025480565445591, - "velocityY": -0.7174397564871019, - "timestamp": 1.0666460522856247 - }, - { - "x": 5.646959680408469, - "y": 6.7691984973790005, - "heading": -7.924873015338203e-17, - "angularVelocity": -1.0800318016891037e-16, - "velocityX": 3.9283746893754534, - "velocityY": -1.134498413587298, - "timestamp": 1.1429396137113823 - }, - { - "x": 5.9359740719099685, - "y": 6.651774740866165, - "heading": -9.202999259689669e-17, - "angularVelocity": -1.6752740604398516e-16, - "velocityX": 3.788188493241898, - "velocityY": -1.5391044056463643, - "timestamp": 1.21923317513714 - }, - { - "x": 6.220380450256188, - "y": 6.523592569288839, - "heading": -1.0221846207986328e-16, - "angularVelocity": -1.3354297915271803e-16, - "velocityX": 3.7277900393073033, - "velocityY": -1.68011781311405, - "timestamp": 1.2955267365628975 - }, - { - "x": 6.50478671019526, - "y": 6.395410134994128, - "heading": -1.0951189331938173e-16, - "angularVelocity": -9.559694295587159e-17, - "velocityX": 3.727788487313321, - "velocityY": -1.6801212566206856, - "timestamp": 1.3718202979886551 - }, - { - "x": 6.772850174759086, - "y": 6.274593429192974, - "heading": -1.0951189787115588e-16, - "angularVelocity": -5.966131443872117e-23, - "velocityX": 3.5135791219378683, - "velocityY": -1.5835766943285492, - "timestamp": 1.4481138594144127 - }, - { - "x": 7.011128834517843, - "y": 6.167200790731002, - "heading": -1.0951190457325102e-16, - "angularVelocity": -8.784614336894978e-23, - "velocityX": 3.1231817640421453, - "velocityY": -1.40762387356209, - "timestamp": 1.5244074208401703 - }, - { - "x": 7.21962267153951, - "y": 6.073232227690204, - "heading": -1.0951190901705372e-16, - "angularVelocity": -5.824610389344753e-23, - "velocityX": 2.732784171106696, - "velocityY": -1.231670946862785, - "timestamp": 1.600700982265928 - }, - { - "x": 7.398331679846744, - "y": 5.992687742764583, - "heading": -1.095119119553291e-16, - "angularVelocity": -3.85127569143241e-23, - "velocityX": 2.342386499824594, - "velocityY": -1.0557179848524971, - "timestamp": 1.6769945436916855 - }, - { - "x": 7.54725585645087, - "y": 5.925567337301139, - "heading": -1.0951191376445854e-16, - "angularVelocity": -2.3712740836183491e-23, - "velocityX": 1.951988789369157, - "velocityY": -0.8797650051867136, - "timestamp": 1.7532881051174432 - }, - { - "x": 7.666395199558686, - "y": 5.8718710121080715, - "heading": -1.0951191500361807e-16, - "angularVelocity": -1.6241993379413765e-23, - "velocityX": 1.5615910554097168, - "velocityY": -0.7038120149276318, - "timestamp": 1.8295816665432008 - }, - { - "x": 7.75574970797472, - "y": 5.831598767724182, - "heading": -1.0951191576780229e-16, - "angularVelocity": -1.0016365829374042e-23, - "velocityX": 1.1711933057809407, - "velocityY": -0.5278590176063511, - "timestamp": 1.9058752279689584 - }, - { - "x": 7.815319380845067, - "y": 5.804750604534326, - "heading": -1.095119162439172e-16, - "angularVelocity": -6.240564641473135e-24, - "velocityX": 0.7807955449597819, - "velocityY": -0.3519060152406423, - "timestamp": 1.982168789394716 - }, - { - "x": 7.845104217529297, - "y": 5.791326522827148, - "heading": -1.0951191646797131e-16, - "angularVelocity": -2.9367367517813784e-24, - "velocityX": 0.3903977757443359, - "velocityY": -0.17595300909161263, - "timestamp": 2.0584623508204736 - }, - { - "x": 7.845104217529297, - "y": 5.791326522827148, - "heading": -1.0951191646797131e-16, - "angularVelocity": -3.8529205658537675e-42, - "velocityX": 0, - "velocityY": -4.029280467146476e-44, - "timestamp": 2.134755912246231 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NT-2ndN.3.traj b/src/main/deploy/choreo/3NT-2ndN.3.traj deleted file mode 100644 index f60d8bd..0000000 --- a/src/main/deploy/choreo/3NT-2ndN.3.traj +++ /dev/null @@ -1,265 +0,0 @@ -{ - "samples": [ - { - "x": 7.845104217529297, - "y": 5.791326522827148, - "heading": -1.0951191646797131e-16, - "angularVelocity": -3.8529205658537675e-42, - "velocityX": 0, - "velocityY": -4.029280467146476e-44, - "timestamp": 0 - }, - { - "x": 7.819330555877964, - "y": 5.803041575931587, - "heading": -1.0951188196648234e-16, - "angularVelocity": 4.857879592349432e-22, - "velocityX": -0.3628983811361795, - "velocityY": 0.1649503226991147, - "timestamp": 0.07102170467291646 - }, - { - "x": 7.767783233011056, - "y": 5.8264716819423965, - "heading": -1.0951180912999703e-16, - "angularVelocity": 1.0255524790956886e-21, - "velocityX": -0.7257967561367829, - "velocityY": 0.32990064260939006, - "timestamp": 0.14204340934583293 - }, - { - "x": 7.690462249473273, - "y": 5.861616840611991, - "heading": -1.0951169316662303e-16, - "angularVelocity": 1.6327878151424353e-21, - "velocityX": -1.0886951234679147, - "velocityY": 0.4948509590336158, - "timestamp": 0.2130651140187494 - }, - { - "x": 7.587367605964943, - "y": 5.9084770516220475, - "heading": -1.0951152779403229e-16, - "angularVelocity": 2.3284796037986105e-21, - "velocityX": -1.4515934809382964, - "velocityY": 0.6598012709757769, - "timestamp": 0.28408681869166585 - }, - { - "x": 7.458499303419834, - "y": 5.967052314548134, - "heading": -1.0951130436615097e-16, - "angularVelocity": 3.1459098644356444e-21, - "velocityX": -1.8144918252610083, - "velocityY": 0.824751576941851, - "timestamp": 0.3551085233645823 - }, - { - "x": 7.303857343145226, - "y": 6.037342628796045, - "heading": -1.0951101137046541e-16, - "angularVelocity": 4.1254386513769835e-21, - "velocityX": -2.1773901511769775, - "velocityY": 0.9897018745414005, - "timestamp": 0.4261302280374988 - }, - { - "x": 7.123441727102036, - "y": 6.119347993474474, - "heading": -1.0951063176976066e-16, - "angularVelocity": 5.344854879364384e-21, - "velocityX": -2.5402884494828224, - "velocityY": 1.154652159591159, - "timestamp": 0.49715193271041525 - }, - { - "x": 6.917252458558463, - "y": 6.213068407097906, - "heading": -1.0951013731921648e-16, - "angularVelocity": 6.961963901804915e-21, - "velocityX": -2.9031867017717694, - "velocityY": 1.3196024237245876, - "timestamp": 0.5681736373833317 - }, - { - "x": 6.685289544050908, - "y": 6.318503866695314, - "heading": -1.0950947214506759e-16, - "angularVelocity": 9.365786866840166e-21, - "velocityX": -3.2660848620268434, - "velocityY": 1.4845526460253218, - "timestamp": 0.6391953420562482 - }, - { - "x": 6.427553003188613, - "y": 6.435654363353594, - "heading": -1.1321193217013263e-16, - "angularVelocity": -5.213138775134091e-17, - "velocityX": -3.628982746179858, - "velocityY": 1.6495027428277718, - "timestamp": 0.7102170467291646 - }, - { - "x": 6.163180157507604, - "y": 6.555821298407878, - "heading": -1.1495995520201052e-16, - "angularVelocity": -2.4612518664938278e-17, - "velocityX": -3.722423263403093, - "velocityY": 1.6919748069650107, - "timestamp": 0.7812387514020811 - }, - { - "x": 5.898806927725833, - "y": 6.675987388419107, - "heading": -1.1629491145603269e-16, - "angularVelocity": -1.879645469184645e-17, - "velocityX": -3.7224286716196997, - "velocityY": 1.691962908587475, - "timestamp": 0.8522604560749976 - }, - { - "x": 5.627414904076508, - "y": 6.779328876635568, - "heading": -1.0580061371759786e-16, - "angularVelocity": 1.4776183966247067e-16, - "velocityX": -3.821254712192482, - "velocityY": 1.4550691044715158, - "timestamp": 0.923282160747914 - }, - { - "x": 5.347249984741211, - "y": 6.855753421783447, - "heading": -9.637324583502604e-17, - "angularVelocity": 1.3273925099360297e-16, - "velocityX": -3.9447788619770323, - "velocityY": 1.076073089203438, - "timestamp": 0.9943038654208305 - }, - { - "x": 5.061742585487608, - "y": 6.904479036217225, - "heading": -8.82951413614811e-17, - "angularVelocity": 1.1404224621492368e-16, - "velocityX": -4.030636794620884, - "velocityY": 0.6878814871023652, - "timestamp": 1.0651381811385425 - }, - { - "x": 4.772852740000535, - "y": 6.925247237603686, - "heading": -7.9445886933944e-17, - "angularVelocity": 1.249289181080401e-16, - "velocityX": -4.078388314476751, - "velocityY": 0.29319407092496935, - "timestamp": 1.1359724968562546 - }, - { - "x": 4.483255537650678, - "y": 6.9299505879565855, - "heading": -7.626016633533651e-17, - "angularVelocity": 4.4974255293199715e-17, - "velocityX": -4.0883743905137075, - "velocityY": 0.0663993193869991, - "timestamp": 1.2068068125739666 - }, - { - "x": 4.201672722376627, - "y": 6.934523131728666, - "heading": -7.626013411314718e-17, - "angularVelocity": 4.548951873443926e-22, - "velocityX": -3.9752316715560787, - "velocityY": 0.0645526638571946, - "timestamp": 1.2776411282916786 - }, - { - "x": 3.948248160256927, - "y": 6.938638421582927, - "heading": -7.626011484857826e-17, - "angularVelocity": 2.719666128336545e-22, - "velocityX": -3.5777089049556827, - "velocityY": 0.05809740395688512, - "timestamp": 1.3484754440093907 - }, - { - "x": 3.7229818713544702, - "y": 6.942296457194531, - "heading": -7.626010057772757e-17, - "angularVelocity": 2.014680390074246e-22, - "velocityX": -3.1801858551183724, - "velocityY": 0.05164213947067224, - "timestamp": 1.4193097597271027 - }, - { - "x": 3.525873862356881, - "y": 6.945497238455197, - "heading": -7.626008963589061e-17, - "angularVelocity": 1.544708499714273e-22, - "velocityX": -2.782662710868862, - "velocityY": 0.04518687345582658, - "timestamp": 1.4901440754448148 - }, - { - "x": 3.3569241366079687, - "y": 6.9482407653107865, - "heading": -7.626008119077207e-17, - "angularVelocity": 1.1922354961091427e-22, - "velocityX": -2.385139519413282, - "velocityY": 0.03873160667666502, - "timestamp": 1.5609783911625268 - }, - { - "x": 3.2161326961140184, - "y": 6.950527037728815, - "heading": -7.626007474301078e-17, - "angularVelocity": 9.102595569495179e-23, - "velocityX": -1.9876162996340712, - "velocityY": 0.03227633943891411, - "timestamp": 1.6318127068802388 - }, - { - "x": 3.1034995422125533, - "y": 6.952356055687626, - "heading": -7.626006995970547e-17, - "angularVelocity": 6.752807978535753e-23, - "velocityX": -1.5900930609724457, - "velocityY": 0.02582107189543705, - "timestamp": 1.7026470225979509 - }, - { - "x": 3.019024675858946, - "y": 6.9537278191717515, - "heading": -7.626006660307227e-17, - "angularVelocity": 4.7387105624513213e-23, - "velocityX": -1.1925698088233843, - "velocityY": 0.019365804133584213, - "timestamp": 1.773481338315663 - }, - { - "x": 2.962708097769727, - "y": 6.954642328169591, - "heading": -7.626006449477479e-17, - "angularVelocity": 2.9763786792475846e-23, - "velocityX": -0.7950465465587467, - "velocityY": 0.012910536207949581, - "timestamp": 1.844315654033375 - }, - { - "x": 2.9345498085021973, - "y": 6.955099582672119, - "heading": -7.626006349610785e-17, - "angularVelocity": 1.4098631913740372e-23, - "velocityX": -0.3975232764264406, - "velocityY": 0.0064552681549291165, - "timestamp": 1.915149969751087 - }, - { - "x": 2.9345498085021973, - "y": 6.955099582672119, - "heading": -7.626006349610785e-17, - "angularVelocity": -5.7559710712214985e-49, - "velocityX": -1.1684104255391741e-43, - "velocityY": -3.507282078288517e-42, - "timestamp": 1.985984285468799 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NT-2ndN.traj b/src/main/deploy/choreo/3NT-2ndN.traj deleted file mode 100644 index 71ba484..0000000 --- a/src/main/deploy/choreo/3NT-2ndN.traj +++ /dev/null @@ -1,670 +0,0 @@ -{ - "samples": [ - { - "x": 0.7773118615150452, - "y": 6.72802209854126, - "heading": 1.030377205028743, - "angularVelocity": -2.5603966606354566e-33, - "velocityX": 0, - "velocityY": 3.3983269045643113e-34, - "timestamp": 0 - }, - { - "x": 0.8017165578496347, - "y": 6.73204375441724, - "heading": 1.0153126355108713, - "angularVelocity": -0.2257566165421289, - "velocityX": 0.36572712321443296, - "velocityY": 0.0602682620556043, - "timestamp": 0.06672924917379079 - }, - { - "x": 0.8505266341075832, - "y": 6.740088761712056, - "heading": 0.9851666091775935, - "angularVelocity": -0.45176630497916986, - "velocityX": 0.7314644906437763, - "velocityY": 0.12056193340141036, - "timestamp": 0.13345849834758158 - }, - { - "x": 0.9237415389002549, - "y": 6.752159262105969, - "heading": 0.9398639932852465, - "angularVelocity": -0.678901927614384, - "velocityX": 1.0971935950004397, - "velocityY": 0.18088769982225175, - "timestamp": 0.20018774752137236 - }, - { - "x": 1.0213592682884212, - "y": 6.768257272086683, - "heading": 0.8792594067892606, - "angularVelocity": -0.9082162207182363, - "velocityX": 1.4628926684598107, - "velocityY": 0.24124368519101108, - "timestamp": 0.26691699669516317 - }, - { - "x": 1.1433761786203043, - "y": 6.788383770319312, - "heading": 0.8031252901056259, - "angularVelocity": -1.1409407063063117, - "velocityX": 1.8285371384008848, - "velocityY": 0.30161433676873595, - "timestamp": 0.333646245868954 - }, - { - "x": 1.2897869899026333, - "y": 6.812537367144202, - "heading": 0.7111466792887886, - "angularVelocity": -1.3783852202096063, - "velocityX": 2.1941024827210964, - "velocityY": 0.3619641630012081, - "timestamp": 0.4003754950427448 - }, - { - "x": 1.4605852210940866, - "y": 6.8407125983828605, - "heading": 0.6029335592009877, - "angularVelocity": -1.6216744745017098, - "velocityX": 2.55957070259585, - "velocityY": 0.4222321034255566, - "timestamp": 0.4671047442165356 - }, - { - "x": 1.6557643007257534, - "y": 6.872898115584218, - "heading": 0.4780638213571818, - "angularVelocity": -1.871289417907176, - "velocityX": 2.924940442883433, - "velocityY": 0.48232997673228606, - "timestamp": 0.5338339933903263 - }, - { - "x": 1.8264308864004113, - "y": 6.900588544051283, - "heading": 0.35839609824636437, - "angularVelocity": -1.793332378117321, - "velocityX": 2.5575978718143664, - "velocityY": 0.4149668819882522, - "timestamp": 0.6005632425641171 - }, - { - "x": 1.9727210620901834, - "y": 6.9242976491672845, - "heading": 0.2558451219964055, - "angularVelocity": -1.5368219711699938, - "velocityX": 2.192294645917135, - "velocityY": 0.3553030404141562, - "timestamp": 0.6672924917379078 - }, - { - "x": 2.0946339267218823, - "y": 6.944038578297596, - "heading": 0.1704674226074578, - "angularVelocity": -1.2794644094763992, - "velocityX": 1.8269779165982687, - "velocityY": 0.2958362243654028, - "timestamp": 0.7340217409116986 - }, - { - "x": 2.192167334429377, - "y": 6.959821625235924, - "heading": 0.10224171092320174, - "angularVelocity": -1.0224258856347668, - "velocityX": 1.4616290294751688, - "velocityY": 0.23652367041058378, - "timestamp": 0.8007509900854893 - }, - { - "x": 2.265318973651712, - "y": 6.971654176410272, - "heading": 0.05111626576067614, - "angularVelocity": -0.7661624519312907, - "velocityX": 1.0962455014564527, - "velocityY": 0.17732180896462885, - "timestamp": 0.8674802392592801 - }, - { - "x": 2.314087093590593, - "y": 6.979540946456479, - "heading": 0.01704319032984002, - "angularVelocity": -0.5106167962731848, - "velocityX": 0.7308357360933072, - "velocityY": 0.11819060073141874, - "timestamp": 0.9342094884330708 - }, - { - "x": 2.338470935821533, - "y": 6.983484268188477, - "heading": 0, - "angularVelocity": -0.2554080937648863, - "velocityX": 0.3654146050322646, - "velocityY": 0.059094351889496924, - "timestamp": 1.0009387376068617 - }, - { - "x": 2.338470935821533, - "y": 6.983484268188477, - "heading": 0, - "angularVelocity": 0, - "velocityX": -3.661539873949153e-32, - "velocityY": -1.6264049380013454e-33, - "timestamp": 1.0676679867806524 - }, - { - "x": 2.3668454143263347, - "y": 6.982856207850841, - "heading": -5.424481691988399e-19, - "angularVelocity": -7.628324804228275e-18, - "velocityX": 0.39902381550093635, - "velocityY": -0.008832269190277171, - "timestamp": 1.138777723599694 - }, - { - "x": 2.423594370858518, - "y": 6.981600087186138, - "heading": -1.6273462227067539e-18, - "angularVelocity": -1.5256673727660342e-17, - "velocityX": 0.7980476242880341, - "velocityY": -0.01766453823194559, - "timestamp": 1.2098874604187357 - }, - { - "x": 2.5087178048213095, - "y": 6.979715906207575, - "heading": -3.2546963346700688e-18, - "angularVelocity": -2.2885053225616023e-17, - "velocityX": 1.1970714246828327, - "velocityY": -0.02649680708785306, - "timestamp": 1.2809971972377774 - }, - { - "x": 2.6222157154474277, - "y": 6.977203664932138, - "heading": -5.22299810707614e-18, - "angularVelocity": -2.7679778613369908e-17, - "velocityX": 1.5960952142875302, - "velocityY": -0.03532907570492495, - "timestamp": 1.352106934056819 - }, - { - "x": 2.764088101713831, - "y": 6.974063363382471, - "heading": -7.733758581198458e-18, - "angularVelocity": -3.5308251533986704e-17, - "velocityX": 1.9951189895054233, - "velocityY": -0.04416134400354938, - "timestamp": 1.4232166708758607 - }, - { - "x": 2.9343349621882595, - "y": 6.970295001590276, - "heading": -1.1212959884643953e-17, - "angularVelocity": -4.892721389616843e-17, - "velocityX": 2.394142744581784, - "velocityY": -0.0529936118563472, - "timestamp": 1.4943264076949023 - }, - { - "x": 3.1329562947223244, - "y": 6.965898579603107, - "heading": -1.5058318284360188e-17, - "angularVelocity": -5.4076397575508025e-17, - "velocityX": 2.7931664694458354, - "velocityY": -0.06182587904040484, - "timestamp": 1.565436144513944 - }, - { - "x": 3.3599520957353746, - "y": 6.960874097500221, - "heading": -1.921107651348465e-17, - "angularVelocity": -5.839929122072696e-17, - "velocityX": 3.192190143956008, - "velocityY": -0.0706581451098949, - "timestamp": 1.6365458813329856 - }, - { - "x": 3.6153223580661016, - "y": 6.955221555440131, - "heading": -1.7381335638599224e-17, - "angularVelocity": 2.573122833433782e-17, - "velocityX": 3.5912137177583303, - "velocityY": -0.07949040895024774, - "timestamp": 1.7076556181520273 - }, - { - "x": 3.89906706023054, - "y": 6.948940953898378, - "heading": -1.600425363722977e-17, - "angularVelocity": 1.9365589903304247e-17, - "velocityX": 3.990236989436556, - "velocityY": -0.0883226661031768, - "timestamp": 1.778765354971069 - }, - { - "x": 4.189757421654959, - "y": 6.942506612329325, - "heading": -2.309450754421387e-17, - "angularVelocity": -9.970862253403092e-17, - "velocityX": 4.087912210449614, - "velocityY": -0.09048467702006271, - "timestamp": 1.8498750917901106 - }, - { - "x": 4.480447783078946, - "y": 6.936072270739366, - "heading": -3.567945640397763e-17, - "angularVelocity": -1.769792636385315e-16, - "velocityX": 4.08791221044355, - "velocityY": -0.09048467731405441, - "timestamp": 1.9209848286091522 - }, - { - "x": 4.77113812835654, - "y": 6.9296371997299495, - "heading": -5.239589681395825e-17, - "angularVelocity": -2.350794864073287e-16, - "velocityX": 4.0879119833805015, - "velocityY": -0.0904949349734297, - "timestamp": 1.9920945654281939 - }, - { - "x": 5.060999121162208, - "y": 6.906770374050763, - "heading": -6.413834291103341e-17, - "angularVelocity": -1.6513133956545303e-16, - "velocityX": 4.0762489888452125, - "velocityY": -0.3215709507880496, - "timestamp": 2.0632043022472355 - }, - { - "x": 5.347249984741211, - "y": 6.855753421783447, - "heading": -7.100878289298809e-17, - "angularVelocity": -9.661742947296258e-17, - "velocityX": 4.025480565445591, - "velocityY": -0.7174397564871019, - "timestamp": 2.134314039066277 - }, - { - "x": 5.646959680408469, - "y": 6.7691984973790005, - "heading": -7.924873015338203e-17, - "angularVelocity": -1.0800318016891037e-16, - "velocityX": 3.9283746893754534, - "velocityY": -1.134498413587298, - "timestamp": 2.2106076004920348 - }, - { - "x": 5.9359740719099685, - "y": 6.651774740866165, - "heading": -9.202999259689669e-17, - "angularVelocity": -1.6752740604398516e-16, - "velocityX": 3.788188493241898, - "velocityY": -1.5391044056463643, - "timestamp": 2.2869011619177924 - }, - { - "x": 6.220380450256188, - "y": 6.523592569288839, - "heading": -1.0221846207986328e-16, - "angularVelocity": -1.3354297915271803e-16, - "velocityX": 3.7277900393073033, - "velocityY": -1.68011781311405, - "timestamp": 2.36319472334355 - }, - { - "x": 6.50478671019526, - "y": 6.395410134994128, - "heading": -1.0951189331938173e-16, - "angularVelocity": -9.559694295587159e-17, - "velocityX": 3.727788487313321, - "velocityY": -1.6801212566206856, - "timestamp": 2.4394882847693076 - }, - { - "x": 6.772850174759086, - "y": 6.274593429192974, - "heading": -1.0951189787115588e-16, - "angularVelocity": -5.966131443872117e-23, - "velocityX": 3.5135791219378683, - "velocityY": -1.5835766943285492, - "timestamp": 2.515781846195065 - }, - { - "x": 7.011128834517843, - "y": 6.167200790731002, - "heading": -1.0951190457325102e-16, - "angularVelocity": -8.784614336894978e-23, - "velocityX": 3.1231817640421453, - "velocityY": -1.40762387356209, - "timestamp": 2.592075407620823 - }, - { - "x": 7.21962267153951, - "y": 6.073232227690204, - "heading": -1.0951190901705372e-16, - "angularVelocity": -5.824610389344753e-23, - "velocityX": 2.732784171106696, - "velocityY": -1.231670946862785, - "timestamp": 2.6683689690465804 - }, - { - "x": 7.398331679846744, - "y": 5.992687742764583, - "heading": -1.095119119553291e-16, - "angularVelocity": -3.85127569143241e-23, - "velocityX": 2.342386499824594, - "velocityY": -1.0557179848524971, - "timestamp": 2.744662530472338 - }, - { - "x": 7.54725585645087, - "y": 5.925567337301139, - "heading": -1.0951191376445854e-16, - "angularVelocity": -2.3712740836183491e-23, - "velocityX": 1.951988789369157, - "velocityY": -0.8797650051867136, - "timestamp": 2.8209560918980956 - }, - { - "x": 7.666395199558686, - "y": 5.8718710121080715, - "heading": -1.0951191500361807e-16, - "angularVelocity": -1.6241993379413765e-23, - "velocityX": 1.5615910554097168, - "velocityY": -0.7038120149276318, - "timestamp": 2.897249653323853 - }, - { - "x": 7.75574970797472, - "y": 5.831598767724182, - "heading": -1.0951191576780229e-16, - "angularVelocity": -1.0016365829374042e-23, - "velocityX": 1.1711933057809407, - "velocityY": -0.5278590176063511, - "timestamp": 2.973543214749611 - }, - { - "x": 7.815319380845067, - "y": 5.804750604534326, - "heading": -1.095119162439172e-16, - "angularVelocity": -6.240564641473135e-24, - "velocityX": 0.7807955449597819, - "velocityY": -0.3519060152406423, - "timestamp": 3.0498367761753684 - }, - { - "x": 7.845104217529297, - "y": 5.791326522827148, - "heading": -1.0951191646797131e-16, - "angularVelocity": -2.9367367517813784e-24, - "velocityX": 0.3903977757443359, - "velocityY": -0.17595300909161263, - "timestamp": 3.126130337601126 - }, - { - "x": 7.845104217529297, - "y": 5.791326522827148, - "heading": -1.0951191646797131e-16, - "angularVelocity": -3.8529205658537675e-42, - "velocityX": 0, - "velocityY": -4.029280467146476e-44, - "timestamp": 3.2024238990268836 - }, - { - "x": 7.819330555877964, - "y": 5.803041575931587, - "heading": -1.0951188196648234e-16, - "angularVelocity": 4.857879592349432e-22, - "velocityX": -0.3628983811361795, - "velocityY": 0.1649503226991147, - "timestamp": 3.2734456036998 - }, - { - "x": 7.767783233011056, - "y": 5.8264716819423965, - "heading": -1.0951180912999703e-16, - "angularVelocity": 1.0255524790956886e-21, - "velocityX": -0.7257967561367829, - "velocityY": 0.32990064260939006, - "timestamp": 3.3444673083727166 - }, - { - "x": 7.690462249473273, - "y": 5.861616840611991, - "heading": -1.0951169316662303e-16, - "angularVelocity": 1.6327878151424353e-21, - "velocityX": -1.0886951234679147, - "velocityY": 0.4948509590336158, - "timestamp": 3.415489013045633 - }, - { - "x": 7.587367605964943, - "y": 5.9084770516220475, - "heading": -1.0951152779403229e-16, - "angularVelocity": 2.3284796037986105e-21, - "velocityX": -1.4515934809382964, - "velocityY": 0.6598012709757769, - "timestamp": 3.4865107177185495 - }, - { - "x": 7.458499303419834, - "y": 5.967052314548134, - "heading": -1.0951130436615097e-16, - "angularVelocity": 3.1459098644356444e-21, - "velocityX": -1.8144918252610083, - "velocityY": 0.824751576941851, - "timestamp": 3.557532422391466 - }, - { - "x": 7.303857343145226, - "y": 6.037342628796045, - "heading": -1.0951101137046541e-16, - "angularVelocity": 4.1254386513769835e-21, - "velocityX": -2.1773901511769775, - "velocityY": 0.9897018745414005, - "timestamp": 3.6285541270643824 - }, - { - "x": 7.123441727102036, - "y": 6.119347993474474, - "heading": -1.0951063176976066e-16, - "angularVelocity": 5.344854879364384e-21, - "velocityX": -2.5402884494828224, - "velocityY": 1.154652159591159, - "timestamp": 3.699575831737299 - }, - { - "x": 6.917252458558463, - "y": 6.213068407097906, - "heading": -1.0951013731921648e-16, - "angularVelocity": 6.961963901804915e-21, - "velocityX": -2.9031867017717694, - "velocityY": 1.3196024237245876, - "timestamp": 3.7705975364102153 - }, - { - "x": 6.685289544050908, - "y": 6.318503866695314, - "heading": -1.0950947214506759e-16, - "angularVelocity": 9.365786866840166e-21, - "velocityX": -3.2660848620268434, - "velocityY": 1.4845526460253218, - "timestamp": 3.841619241083132 - }, - { - "x": 6.427553003188613, - "y": 6.435654363353594, - "heading": -1.1321193217013263e-16, - "angularVelocity": -5.213138775134091e-17, - "velocityX": -3.628982746179858, - "velocityY": 1.6495027428277718, - "timestamp": 3.9126409457560483 - }, - { - "x": 6.163180157507604, - "y": 6.555821298407878, - "heading": -1.1495995520201052e-16, - "angularVelocity": -2.4612518664938278e-17, - "velocityX": -3.722423263403093, - "velocityY": 1.6919748069650107, - "timestamp": 3.9836626504289647 - }, - { - "x": 5.898806927725833, - "y": 6.675987388419107, - "heading": -1.1629491145603269e-16, - "angularVelocity": -1.879645469184645e-17, - "velocityX": -3.7224286716196997, - "velocityY": 1.691962908587475, - "timestamp": 4.054684355101881 - }, - { - "x": 5.627414904076508, - "y": 6.779328876635568, - "heading": -1.0580061371759786e-16, - "angularVelocity": 1.4776183966247067e-16, - "velocityX": -3.821254712192482, - "velocityY": 1.4550691044715158, - "timestamp": 4.125706059774798 - }, - { - "x": 5.347249984741211, - "y": 6.855753421783447, - "heading": -9.637324583502604e-17, - "angularVelocity": 1.3273925099360297e-16, - "velocityX": -3.9447788619770323, - "velocityY": 1.076073089203438, - "timestamp": 4.196727764447714 - }, - { - "x": 5.061742585487608, - "y": 6.904479036217225, - "heading": -8.82951413614811e-17, - "angularVelocity": 1.1404224621492368e-16, - "velocityX": -4.030636794620884, - "velocityY": 0.6878814871023652, - "timestamp": 4.267562080165426 - }, - { - "x": 4.772852740000535, - "y": 6.925247237603686, - "heading": -7.9445886933944e-17, - "angularVelocity": 1.249289181080401e-16, - "velocityX": -4.078388314476751, - "velocityY": 0.29319407092496935, - "timestamp": 4.338396395883138 - }, - { - "x": 4.483255537650678, - "y": 6.9299505879565855, - "heading": -7.626016633533651e-17, - "angularVelocity": 4.4974255293199715e-17, - "velocityX": -4.0883743905137075, - "velocityY": 0.0663993193869991, - "timestamp": 4.40923071160085 - }, - { - "x": 4.201672722376627, - "y": 6.934523131728666, - "heading": -7.626013411314718e-17, - "angularVelocity": 4.548951873443926e-22, - "velocityX": -3.9752316715560787, - "velocityY": 0.0645526638571946, - "timestamp": 4.480065027318562 - }, - { - "x": 3.948248160256927, - "y": 6.938638421582927, - "heading": -7.626011484857826e-17, - "angularVelocity": 2.719666128336545e-22, - "velocityX": -3.5777089049556827, - "velocityY": 0.05809740395688512, - "timestamp": 4.550899343036274 - }, - { - "x": 3.7229818713544702, - "y": 6.942296457194531, - "heading": -7.626010057772757e-17, - "angularVelocity": 2.014680390074246e-22, - "velocityX": -3.1801858551183724, - "velocityY": 0.05164213947067224, - "timestamp": 4.621733658753986 - }, - { - "x": 3.525873862356881, - "y": 6.945497238455197, - "heading": -7.626008963589061e-17, - "angularVelocity": 1.544708499714273e-22, - "velocityX": -2.782662710868862, - "velocityY": 0.04518687345582658, - "timestamp": 4.692567974471698 - }, - { - "x": 3.3569241366079687, - "y": 6.9482407653107865, - "heading": -7.626008119077207e-17, - "angularVelocity": 1.1922354961091427e-22, - "velocityX": -2.385139519413282, - "velocityY": 0.03873160667666502, - "timestamp": 4.76340229018941 - }, - { - "x": 3.2161326961140184, - "y": 6.950527037728815, - "heading": -7.626007474301078e-17, - "angularVelocity": 9.102595569495179e-23, - "velocityX": -1.9876162996340712, - "velocityY": 0.03227633943891411, - "timestamp": 4.8342366059071225 - }, - { - "x": 3.1034995422125533, - "y": 6.952356055687626, - "heading": -7.626006995970547e-17, - "angularVelocity": 6.752807978535753e-23, - "velocityX": -1.5900930609724457, - "velocityY": 0.02582107189543705, - "timestamp": 4.9050709216248345 - }, - { - "x": 3.019024675858946, - "y": 6.9537278191717515, - "heading": -7.626006660307227e-17, - "angularVelocity": 4.7387105624513213e-23, - "velocityX": -1.1925698088233843, - "velocityY": 0.019365804133584213, - "timestamp": 4.9759052373425465 - }, - { - "x": 2.962708097769727, - "y": 6.954642328169591, - "heading": -7.626006449477479e-17, - "angularVelocity": 2.9763786792475846e-23, - "velocityX": -0.7950465465587467, - "velocityY": 0.012910536207949581, - "timestamp": 5.046739553060259 - }, - { - "x": 2.9345498085021973, - "y": 6.955099582672119, - "heading": -7.626006349610785e-17, - "angularVelocity": 1.4098631913740372e-23, - "velocityX": -0.3975232764264406, - "velocityY": 0.0064552681549291165, - "timestamp": 5.117573868777971 - }, - { - "x": 2.9345498085021973, - "y": 6.955099582672119, - "heading": -7.626006349610785e-17, - "angularVelocity": -5.7559710712214985e-49, - "velocityX": -1.1684104255391741e-43, - "velocityY": -3.507282078288517e-42, - "timestamp": 5.188408184495683 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NT-Close.1.traj b/src/main/deploy/choreo/3NT-Close.1.traj new file mode 100644 index 0000000..547d55e --- /dev/null +++ b/src/main/deploy/choreo/3NT-Close.1.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 0.735096275806427, + "y": 6.688129901885986, + "heading": 1.030377205028743, + "angularVelocity": 1.0391167973510024e-17, + "velocityX": -2.2461159941573586e-16, + "velocityY": -4.4539976704222156e-17, + "timestamp": 0 + }, + { + "x": 0.7596859244793368, + "y": 6.693079217230425, + "heading": 1.0146123815882935, + "angularVelocity": -0.2516963968106555, + "velocityX": 0.39259088395060876, + "velocityY": 0.07901926993222823, + "timestamp": 0.06263428336762733 + }, + { + "x": 0.8088699810683173, + "y": 6.702981947714709, + "heading": 0.9831479860517711, + "angularVelocity": -0.5023510104178023, + "velocityX": 0.7852577525362323, + "velocityY": 0.1581039959563583, + "timestamp": 0.12526856673525466 + }, + { + "x": 0.882652643462703, + "y": 6.717843001519701, + "heading": 0.9360140194508846, + "angularVelocity": -0.7525266366382326, + "velocityX": 1.177991643351673, + "velocityY": 0.23726708450970968, + "timestamp": 0.187902850102882 + }, + { + "x": 0.9810383785491245, + "y": 6.737666993774571, + "heading": 0.8732159109864319, + "angularVelocity": -1.0026155818829086, + "velocityX": 1.570796851126302, + "velocityY": 0.31650385681776916, + "timestamp": 0.2505371334705093 + }, + { + "x": 1.1040342419063196, + "y": 6.76245681515317, + "heading": 0.7947797985022379, + "angularVelocity": -1.2522872182286973, + "velocityX": 1.96371470613434, + "velocityY": 0.3957867807490719, + "timestamp": 0.31317141683813665 + }, + { + "x": 1.2516562548035544, + "y": 6.792212632567885, + "heading": 0.7009027883845247, + "angularVelocity": -1.4988119137040175, + "velocityX": 2.356888351875573, + "velocityY": 0.4750723695530488, + "timestamp": 0.375805700205764 + }, + { + "x": 1.42394926193708, + "y": 6.826935222209109, + "heading": 0.592493836735336, + "angularVelocity": -1.7308244913235529, + "velocityX": 2.7507779744563265, + "velocityY": 0.5543703507777238, + "timestamp": 0.4384399835733913 + }, + { + "x": 1.6210817820852441, + "y": 6.866668873496111, + "heading": 0.474890142906328, + "angularVelocity": -1.8776249604189084, + "velocityX": 3.1473581168177396, + "velocityY": 0.6343754434578357, + "timestamp": 0.5010742669410186 + }, + { + "x": 1.7933561506575397, + "y": 6.900954234351732, + "heading": 0.3592267918392059, + "angularVelocity": -1.846646035498528, + "velocityX": 2.7504803968322524, + "velocityY": 0.5473896883977395, + "timestamp": 0.563708550308646 + }, + { + "x": 1.9409491865805881, + "y": 6.930222536079952, + "heading": 0.2578351683787445, + "angularVelocity": -1.6187879545990924, + "velocityX": 2.356425714281139, + "velocityY": 0.46728884174232477, + "timestamp": 0.6263428336762733 + }, + { + "x": 2.0639150364587264, + "y": 6.954550982387355, + "heading": 0.17246455594955176, + "angularVelocity": -1.3630013442976032, + "velocityX": 1.9632355200170402, + "velocityY": 0.38842060608578943, + "timestamp": 0.6889771170439006 + }, + { + "x": 2.1622742220184694, + "y": 6.973979114751976, + "heading": 0.1037576955884256, + "angularVelocity": -1.0969529252511232, + "velocityX": 1.5703729694236497, + "velocityY": 0.3101836776927658, + "timestamp": 0.751611400411528 + }, + { + "x": 2.2360360999783158, + "y": 6.988531733503946, + "heading": 0.05200847775326007, + "angularVelocity": -0.8262123401560716, + "velocityX": 1.1776598053642056, + "velocityY": 0.23234270386001926, + "timestamp": 0.8142456837791553 + }, + { + "x": 2.285206028756279, + "y": 6.998224972989151, + "heading": 0.017380497120516848, + "angularVelocity": -0.5528598519998558, + "velocityX": 0.7850321921840192, + "velocityY": 0.15475932610756862, + "timestamp": 0.8768799671467826 + }, + { + "x": 2.309788465499878, + "y": 7.003068923950195, + "heading": -1.979989072741127e-18, + "angularVelocity": -0.27749175349390415, + "velocityX": 0.39247574047129724, + "velocityY": 0.0773370540956569, + "timestamp": 0.93951425051441 + }, + { + "x": 2.309788465499878, + "y": 7.003068923950195, + "heading": -1.2084937260384568e-18, + "angularVelocity": -8.586750612633434e-18, + "velocityX": 2.255152192952613e-16, + "velocityY": 4.541807880719648e-17, + "timestamp": 1.0021485338820373 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NT-Close.2.traj b/src/main/deploy/choreo/3NT-Close.2.traj new file mode 100644 index 0000000..a97b5ec --- /dev/null +++ b/src/main/deploy/choreo/3NT-Close.2.traj @@ -0,0 +1,193 @@ +{ + "samples": [ + { + "x": 2.309788465499878, + "y": 7.003068923950195, + "heading": -1.2084937260384568e-18, + "angularVelocity": -8.586750612633434e-18, + "velocityX": 2.255152192952613e-16, + "velocityY": 4.541807880719648e-17, + "timestamp": 0 + }, + { + "x": 2.3737994944405516, + "y": 7.008086478053747, + "heading": -4.812715970258146e-18, + "angularVelocity": -3.623090443805201e-17, + "velocityX": 0.6434612838287803, + "velocityY": 0.05043821132986589, + "timestamp": 0.09947922361356332 + }, + { + "x": 2.5018215509422213, + "y": 7.018121586152704, + "heading": -9.261038230044222e-18, + "angularVelocity": -4.4716093454457826e-17, + "velocityX": 1.2869225537885558, + "velocityY": 0.10087642157259902, + "timestamp": 0.19895844722712663 + }, + { + "x": 2.69385463293537, + "y": 7.033174248084845, + "heading": -2.3019112293489812e-17, + "angularVelocity": -1.383009794787412e-16, + "velocityX": 1.930383802944823, + "velocityY": 0.15131463018463287, + "timestamp": 0.29843767084068995 + }, + { + "x": 2.9498987369708023, + "y": 7.0532444635798015, + "heading": -3.165953990708067e-17, + "angularVelocity": -8.685660482484638e-17, + "velocityX": 2.573845017428572, + "velocityY": 0.20175283607883435, + "timestamp": 0.39791689445425327 + }, + { + "x": 3.269953856150127, + "y": 7.078332232096839, + "heading": -4.072669650039425e-17, + "angularVelocity": -9.114623399787232e-17, + "velocityX": 3.217306162567271, + "velocityY": 0.25219103653737, + "timestamp": 0.4973961180678166 + }, + { + "x": 3.6540199697781635, + "y": 7.108437552013749, + "heading": -5.11540764570592e-17, + "angularVelocity": -1.0481967568691581e-16, + "velocityX": 3.860767099670771, + "velocityY": 0.3026292206889044, + "timestamp": 0.5968753416813799 + }, + { + "x": 4.059538019609511, + "y": 7.140224398572357, + "heading": -5.1600847084745084e-17, + "angularVelocity": -4.491094839223168e-18, + "velocityX": 4.0764094762804115, + "velocityY": 0.3195325154736649, + "timestamp": 0.6963545652949432 + }, + { + "x": 4.465056069440878, + "y": 7.17201124513096, + "heading": 2.166741952023924e-17, + "angularVelocity": 7.36518279329432e-16, + "velocityX": 4.076409476280607, + "velocityY": 0.31953251547360895, + "timestamp": 0.7958337889085065 + }, + { + "x": 4.870574119272244, + "y": 7.203798091689569, + "heading": 3.259173052122981e-17, + "angularVelocity": 1.0981500060085112e-16, + "velocityX": 4.076409476280603, + "velocityY": 0.3195325154736528, + "timestamp": 0.8953130125220699 + }, + { + "x": 5.2760921691036105, + "y": 7.235584938248175, + "heading": 1.0710070167127387e-16, + "angularVelocity": 7.489902759935735e-16, + "velocityX": 4.076409476280604, + "velocityY": 0.3195325154736477, + "timestamp": 0.9947922361356332 + }, + { + "x": 5.681610218934977, + "y": 7.267371784806784, + "heading": 9.77928108998134e-17, + "angularVelocity": -9.356617827771563e-17, + "velocityX": 4.076409476280603, + "velocityY": 0.3195325154736528, + "timestamp": 1.0942714597491965 + }, + { + "x": 6.087128268766344, + "y": 7.299158631365387, + "heading": 1.6513103694525856e-16, + "angularVelocity": 6.769074345305727e-16, + "velocityX": 4.076409476280607, + "velocityY": 0.31953251547360895, + "timestamp": 1.1937506833627598 + }, + { + "x": 6.492646318597691, + "y": 7.330945477923995, + "heading": 7.396242511112854e-17, + "angularVelocity": -9.16458819465443e-16, + "velocityX": 4.0764094762804115, + "velocityY": 0.31953251547366457, + "timestamp": 1.2932299069763231 + }, + { + "x": 6.876712432225728, + "y": 7.361050797840905, + "heading": 5.635610906288318e-17, + "angularVelocity": -1.7698485581961922e-16, + "velocityX": 3.860767099670771, + "velocityY": 0.3026292206889045, + "timestamp": 1.3927091305898864 + }, + { + "x": 7.196767551405052, + "y": 7.386138566357943, + "heading": 4.322206142605105e-17, + "angularVelocity": -1.3202804726242542e-16, + "velocityX": 3.2173061625672705, + "velocityY": 0.2521910365373701, + "timestamp": 1.4921883542034498 + }, + { + "x": 7.452811655440485, + "y": 7.406208781852899, + "heading": 2.809460919160464e-17, + "angularVelocity": -1.520664485004353e-16, + "velocityX": 2.5738450174285714, + "velocityY": 0.20175283607883435, + "timestamp": 1.591667577817013 + }, + { + "x": 7.644844737433633, + "y": 7.42126144378504, + "heading": 1.597946349618214e-17, + "angularVelocity": -1.2178568806018996e-16, + "velocityX": 1.9303838029448228, + "velocityY": 0.15131463018463284, + "timestamp": 1.6911468014305764 + }, + { + "x": 7.772866793935303, + "y": 7.431296551883997, + "heading": 8.20050547918977e-18, + "angularVelocity": -7.81968106954602e-17, + "velocityX": 1.2869225537885556, + "velocityY": 0.10087642157259895, + "timestamp": 1.7906260250441397 + }, + { + "x": 7.836877822875977, + "y": 7.436314105987549, + "heading": -5.795186749052117e-29, + "angularVelocity": -8.243435343999241e-17, + "velocityX": 0.6434612838287801, + "velocityY": 0.05043821132986585, + "timestamp": 1.890105248657703 + }, + { + "x": 7.836877822875977, + "y": 7.436314105987549, + "heading": -2.878299648600445e-30, + "angularVelocity": 6.318523951927698e-29, + "velocityX": -2.685897796857194e-25, + "velocityY": -2.998497537131158e-25, + "timestamp": 1.9895844722712663 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NT-Close.3.traj b/src/main/deploy/choreo/3NT-Close.3.traj new file mode 100644 index 0000000..56a2f1b --- /dev/null +++ b/src/main/deploy/choreo/3NT-Close.3.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 7.836877822875977, + "y": 7.436314105987549, + "heading": -2.878299648600445e-30, + "angularVelocity": 6.318523951927698e-29, + "velocityX": -2.685897796857194e-25, + "velocityY": -2.998497537131158e-25, + "timestamp": 0 + }, + { + "x": 7.772829081039793, + "y": 7.430431483450002, + "heading": 2.3106084226064385e-18, + "angularVelocity": 2.320700449568332e-17, + "velocityX": -0.6432848704351871, + "velocityY": -0.05908316023699325, + "timestamp": 0.09956513013099988 + }, + { + "x": 7.644731598748364, + "y": 7.418666238501744, + "heading": 5.038306873108203e-18, + "angularVelocity": 2.7396121984342994e-17, + "velocityX": -1.2865697270007066, + "velocityY": -0.11816631920009113, + "timestamp": 0.19913026026199976 + }, + { + "x": 7.452585378073089, + "y": 7.401018371333026, + "heading": 8.895741825302513e-18, + "angularVelocity": 3.874283041736699e-17, + "velocityX": -1.929854562761723, + "velocityY": -0.1772494762523457, + "timestamp": 0.29869539039299964 + }, + { + "x": 7.196390422466309, + "y": 7.377487882260941, + "heading": 1.1475783692536467e-17, + "angularVelocity": 2.591310696619824e-17, + "velocityX": -2.5731393638485627, + "velocityY": -0.236332630119861, + "timestamp": 0.3982605205239995 + }, + { + "x": 6.876146738832702, + "y": 7.3480747719196655, + "heading": 1.4866814385424972e-17, + "angularVelocity": 3.405841672060239e-17, + "velocityX": -3.216424095587036, + "velocityY": -0.29541577761789645, + "timestamp": 0.4978256506549994 + }, + { + "x": 6.491854347886312, + "y": 7.312779042211733, + "heading": 1.3228269289281303e-17, + "angularVelocity": -1.6457017572655323e-17, + "velocityX": -3.8597086192803536, + "velocityY": -0.35449890600748984, + "timestamp": 0.5973907807859993 + }, + { + "x": 6.086447476209309, + "y": 7.27554403131357, + "heading": 1.2208912555752519e-17, + "angularVelocity": -1.0238089703026631e-17, + "velocityX": -4.071775642171123, + "velocityY": -0.3739764197482683, + "timestamp": 0.6969559109169992 + }, + { + "x": 5.681040604532077, + "y": 7.2383090204177005, + "heading": 7.247479687391899e-17, + "angularVelocity": 6.05291071671654e-16, + "velocityX": -4.071775642173424, + "velocityY": -0.3739764197252527, + "timestamp": 0.796521041047999 + }, + { + "x": 5.275633732854971, + "y": 7.201074009520451, + "heading": 1.0354124283363177e-16, + "angularVelocity": 3.1202134641708547e-16, + "velocityX": -4.0717756421721525, + "velocityY": -0.3739764197391007, + "timestamp": 0.8960861711789989 + }, + { + "x": 4.8702268611778505, + "y": 7.163838998623373, + "heading": 8.838275427260066e-18, + "angularVelocity": -9.511660084390495e-16, + "velocityX": -4.071775642172309, + "velocityY": -0.3739764197373963, + "timestamp": 0.9956513013099988 + }, + { + "x": 4.464819989500739, + "y": 7.12660398772638, + "heading": -4.197248226534058e-17, + "angularVelocity": -5.103268345634044e-16, + "velocityX": -4.071775642172202, + "velocityY": -0.37397641973651996, + "timestamp": 1.0952164314409982 + }, + { + "x": 4.080527598554363, + "y": 7.091308258018297, + "heading": -2.975056105500731e-17, + "angularVelocity": 1.2275302803554645e-16, + "velocityX": -3.859708619280215, + "velocityY": -0.3544989060089996, + "timestamp": 1.1947815615719977 + }, + { + "x": 3.7602839149207647, + "y": 7.061895147676932, + "heading": -1.550761080609776e-17, + "angularVelocity": 1.4305159075441368e-16, + "velocityX": -3.2164240955869543, + "velocityY": -0.2954157776187877, + "timestamp": 1.294346691702997 + }, + { + "x": 3.50408895931399, + "y": 7.038364658604788, + "heading": -1.759561477000234e-18, + "angularVelocity": 1.3808096580609213e-16, + "velocityX": -2.5731393638485094, + "velocityY": -0.23633263012044387, + "timestamp": 1.3939118218339965 + }, + { + "x": 3.3119427386387184, + "y": 7.020716791436034, + "heading": -4.625194214758358e-19, + "angularVelocity": 1.3027071363418387e-17, + "velocityX": -1.9298545627616883, + "velocityY": -0.17724947625272364, + "timestamp": 1.493476951964996 + }, + { + "x": 3.1838452563472903, + "y": 7.0089515464877525, + "heading": 1.6084993167813846e-19, + "angularVelocity": 6.260920387830223e-18, + "velocityX": -1.2865697270006862, + "velocityY": -0.11816631920031569, + "timestamp": 1.5930420820959954 + }, + { + "x": 3.1197965145111084, + "y": 7.003068923950195, + "heading": 4.3763650818108325e-31, + "angularVelocity": -1.6155247471267731e-18, + "velocityX": -0.6432848704351777, + "velocityY": -0.05908316023709528, + "timestamp": 1.6926072122269948 + }, + { + "x": 3.1197965145111084, + "y": 7.003068923950195, + "heading": 1.7233142036729875e-31, + "angularVelocity": -9.337973840266154e-31, + "velocityX": 1.535724623502671e-24, + "velocityY": 1.8094635509087927e-25, + "timestamp": 1.7921723423579943 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NT-Close.traj b/src/main/deploy/choreo/3NT-Close.traj new file mode 100644 index 0000000..104d251 --- /dev/null +++ b/src/main/deploy/choreo/3NT-Close.traj @@ -0,0 +1,499 @@ +{ + "samples": [ + { + "x": 0.735096275806427, + "y": 6.688129901885986, + "heading": 1.030377205028743, + "angularVelocity": 1.0391167973510024e-17, + "velocityX": -2.2461159941573586e-16, + "velocityY": -4.4539976704222156e-17, + "timestamp": 0 + }, + { + "x": 0.7596859244793368, + "y": 6.693079217230425, + "heading": 1.0146123815882935, + "angularVelocity": -0.2516963968106555, + "velocityX": 0.39259088395060876, + "velocityY": 0.07901926993222823, + "timestamp": 0.06263428336762733 + }, + { + "x": 0.8088699810683173, + "y": 6.702981947714709, + "heading": 0.9831479860517711, + "angularVelocity": -0.5023510104178023, + "velocityX": 0.7852577525362323, + "velocityY": 0.1581039959563583, + "timestamp": 0.12526856673525466 + }, + { + "x": 0.882652643462703, + "y": 6.717843001519701, + "heading": 0.9360140194508846, + "angularVelocity": -0.7525266366382326, + "velocityX": 1.177991643351673, + "velocityY": 0.23726708450970968, + "timestamp": 0.187902850102882 + }, + { + "x": 0.9810383785491245, + "y": 6.737666993774571, + "heading": 0.8732159109864319, + "angularVelocity": -1.0026155818829086, + "velocityX": 1.570796851126302, + "velocityY": 0.31650385681776916, + "timestamp": 0.2505371334705093 + }, + { + "x": 1.1040342419063196, + "y": 6.76245681515317, + "heading": 0.7947797985022379, + "angularVelocity": -1.2522872182286973, + "velocityX": 1.96371470613434, + "velocityY": 0.3957867807490719, + "timestamp": 0.31317141683813665 + }, + { + "x": 1.2516562548035544, + "y": 6.792212632567885, + "heading": 0.7009027883845247, + "angularVelocity": -1.4988119137040175, + "velocityX": 2.356888351875573, + "velocityY": 0.4750723695530488, + "timestamp": 0.375805700205764 + }, + { + "x": 1.42394926193708, + "y": 6.826935222209109, + "heading": 0.592493836735336, + "angularVelocity": -1.7308244913235529, + "velocityX": 2.7507779744563265, + "velocityY": 0.5543703507777238, + "timestamp": 0.4384399835733913 + }, + { + "x": 1.6210817820852441, + "y": 6.866668873496111, + "heading": 0.474890142906328, + "angularVelocity": -1.8776249604189084, + "velocityX": 3.1473581168177396, + "velocityY": 0.6343754434578357, + "timestamp": 0.5010742669410186 + }, + { + "x": 1.7933561506575397, + "y": 6.900954234351732, + "heading": 0.3592267918392059, + "angularVelocity": -1.846646035498528, + "velocityX": 2.7504803968322524, + "velocityY": 0.5473896883977395, + "timestamp": 0.563708550308646 + }, + { + "x": 1.9409491865805881, + "y": 6.930222536079952, + "heading": 0.2578351683787445, + "angularVelocity": -1.6187879545990924, + "velocityX": 2.356425714281139, + "velocityY": 0.46728884174232477, + "timestamp": 0.6263428336762733 + }, + { + "x": 2.0639150364587264, + "y": 6.954550982387355, + "heading": 0.17246455594955176, + "angularVelocity": -1.3630013442976032, + "velocityX": 1.9632355200170402, + "velocityY": 0.38842060608578943, + "timestamp": 0.6889771170439006 + }, + { + "x": 2.1622742220184694, + "y": 6.973979114751976, + "heading": 0.1037576955884256, + "angularVelocity": -1.0969529252511232, + "velocityX": 1.5703729694236497, + "velocityY": 0.3101836776927658, + "timestamp": 0.751611400411528 + }, + { + "x": 2.2360360999783158, + "y": 6.988531733503946, + "heading": 0.05200847775326007, + "angularVelocity": -0.8262123401560716, + "velocityX": 1.1776598053642056, + "velocityY": 0.23234270386001926, + "timestamp": 0.8142456837791553 + }, + { + "x": 2.285206028756279, + "y": 6.998224972989151, + "heading": 0.017380497120516848, + "angularVelocity": -0.5528598519998558, + "velocityX": 0.7850321921840192, + "velocityY": 0.15475932610756862, + "timestamp": 0.8768799671467826 + }, + { + "x": 2.309788465499878, + "y": 7.003068923950195, + "heading": -1.979989072741127e-18, + "angularVelocity": -0.27749175349390415, + "velocityX": 0.39247574047129724, + "velocityY": 0.0773370540956569, + "timestamp": 0.93951425051441 + }, + { + "x": 2.309788465499878, + "y": 7.003068923950195, + "heading": -1.2084937260384568e-18, + "angularVelocity": -8.586750612633434e-18, + "velocityX": 2.255152192952613e-16, + "velocityY": 4.541807880719648e-17, + "timestamp": 1.0021485338820373 + }, + { + "x": 2.3737994944405516, + "y": 7.008086478053747, + "heading": -4.812715970258146e-18, + "angularVelocity": -3.623090443805201e-17, + "velocityX": 0.6434612838287803, + "velocityY": 0.05043821132986589, + "timestamp": 1.1016277574956006 + }, + { + "x": 2.5018215509422213, + "y": 7.018121586152704, + "heading": -9.261038230044222e-18, + "angularVelocity": -4.4716093454457826e-17, + "velocityX": 1.2869225537885558, + "velocityY": 0.10087642157259902, + "timestamp": 1.201106981109164 + }, + { + "x": 2.69385463293537, + "y": 7.033174248084845, + "heading": -2.3019112293489812e-17, + "angularVelocity": -1.383009794787412e-16, + "velocityX": 1.930383802944823, + "velocityY": 0.15131463018463287, + "timestamp": 1.3005862047227272 + }, + { + "x": 2.9498987369708023, + "y": 7.0532444635798015, + "heading": -3.165953990708067e-17, + "angularVelocity": -8.685660482484638e-17, + "velocityX": 2.573845017428572, + "velocityY": 0.20175283607883435, + "timestamp": 1.4000654283362906 + }, + { + "x": 3.269953856150127, + "y": 7.078332232096839, + "heading": -4.072669650039425e-17, + "angularVelocity": -9.114623399787232e-17, + "velocityX": 3.217306162567271, + "velocityY": 0.25219103653737, + "timestamp": 1.4995446519498539 + }, + { + "x": 3.6540199697781635, + "y": 7.108437552013749, + "heading": -5.11540764570592e-17, + "angularVelocity": -1.0481967568691581e-16, + "velocityX": 3.860767099670771, + "velocityY": 0.3026292206889044, + "timestamp": 1.5990238755634172 + }, + { + "x": 4.059538019609511, + "y": 7.140224398572357, + "heading": -5.1600847084745084e-17, + "angularVelocity": -4.491094839223168e-18, + "velocityX": 4.0764094762804115, + "velocityY": 0.3195325154736649, + "timestamp": 1.6985030991769805 + }, + { + "x": 4.465056069440878, + "y": 7.17201124513096, + "heading": 2.166741952023924e-17, + "angularVelocity": 7.36518279329432e-16, + "velocityX": 4.076409476280607, + "velocityY": 0.31953251547360895, + "timestamp": 1.7979823227905438 + }, + { + "x": 4.870574119272244, + "y": 7.203798091689569, + "heading": 3.259173052122981e-17, + "angularVelocity": 1.0981500060085112e-16, + "velocityX": 4.076409476280603, + "velocityY": 0.3195325154736528, + "timestamp": 1.8974615464041071 + }, + { + "x": 5.2760921691036105, + "y": 7.235584938248175, + "heading": 1.0710070167127387e-16, + "angularVelocity": 7.489902759935735e-16, + "velocityX": 4.076409476280604, + "velocityY": 0.3195325154736477, + "timestamp": 1.9969407700176705 + }, + { + "x": 5.681610218934977, + "y": 7.267371784806784, + "heading": 9.77928108998134e-17, + "angularVelocity": -9.356617827771563e-17, + "velocityX": 4.076409476280603, + "velocityY": 0.3195325154736528, + "timestamp": 2.0964199936312338 + }, + { + "x": 6.087128268766344, + "y": 7.299158631365387, + "heading": 1.6513103694525856e-16, + "angularVelocity": 6.769074345305727e-16, + "velocityX": 4.076409476280607, + "velocityY": 0.31953251547360895, + "timestamp": 2.195899217244797 + }, + { + "x": 6.492646318597691, + "y": 7.330945477923995, + "heading": 7.396242511112854e-17, + "angularVelocity": -9.16458819465443e-16, + "velocityX": 4.0764094762804115, + "velocityY": 0.31953251547366457, + "timestamp": 2.2953784408583604 + }, + { + "x": 6.876712432225728, + "y": 7.361050797840905, + "heading": 5.635610906288318e-17, + "angularVelocity": -1.7698485581961922e-16, + "velocityX": 3.860767099670771, + "velocityY": 0.3026292206889045, + "timestamp": 2.3948576644719237 + }, + { + "x": 7.196767551405052, + "y": 7.386138566357943, + "heading": 4.322206142605105e-17, + "angularVelocity": -1.3202804726242542e-16, + "velocityX": 3.2173061625672705, + "velocityY": 0.2521910365373701, + "timestamp": 2.494336888085487 + }, + { + "x": 7.452811655440485, + "y": 7.406208781852899, + "heading": 2.809460919160464e-17, + "angularVelocity": -1.520664485004353e-16, + "velocityX": 2.5738450174285714, + "velocityY": 0.20175283607883435, + "timestamp": 2.5938161116990504 + }, + { + "x": 7.644844737433633, + "y": 7.42126144378504, + "heading": 1.597946349618214e-17, + "angularVelocity": -1.2178568806018996e-16, + "velocityX": 1.9303838029448228, + "velocityY": 0.15131463018463284, + "timestamp": 2.6932953353126137 + }, + { + "x": 7.772866793935303, + "y": 7.431296551883997, + "heading": 8.20050547918977e-18, + "angularVelocity": -7.81968106954602e-17, + "velocityX": 1.2869225537885556, + "velocityY": 0.10087642157259895, + "timestamp": 2.792774558926177 + }, + { + "x": 7.836877822875977, + "y": 7.436314105987549, + "heading": -5.795186749052117e-29, + "angularVelocity": -8.243435343999241e-17, + "velocityX": 0.6434612838287801, + "velocityY": 0.05043821132986585, + "timestamp": 2.8922537825397403 + }, + { + "x": 7.836877822875977, + "y": 7.436314105987549, + "heading": -2.878299648600445e-30, + "angularVelocity": 6.318523951927698e-29, + "velocityX": -2.685897796857194e-25, + "velocityY": -2.998497537131158e-25, + "timestamp": 2.9917330061533036 + }, + { + "x": 7.772829081039793, + "y": 7.430431483450002, + "heading": 2.3106084226064385e-18, + "angularVelocity": 2.320700449568332e-17, + "velocityX": -0.6432848704351871, + "velocityY": -0.05908316023699325, + "timestamp": 3.0912981362843035 + }, + { + "x": 7.644731598748364, + "y": 7.418666238501744, + "heading": 5.038306873108203e-18, + "angularVelocity": 2.7396121984342994e-17, + "velocityX": -1.2865697270007066, + "velocityY": -0.11816631920009113, + "timestamp": 3.1908632664153034 + }, + { + "x": 7.452585378073089, + "y": 7.401018371333026, + "heading": 8.895741825302513e-18, + "angularVelocity": 3.874283041736699e-17, + "velocityX": -1.929854562761723, + "velocityY": -0.1772494762523457, + "timestamp": 3.2904283965463033 + }, + { + "x": 7.196390422466309, + "y": 7.377487882260941, + "heading": 1.1475783692536467e-17, + "angularVelocity": 2.591310696619824e-17, + "velocityX": -2.5731393638485627, + "velocityY": -0.236332630119861, + "timestamp": 3.389993526677303 + }, + { + "x": 6.876146738832702, + "y": 7.3480747719196655, + "heading": 1.4866814385424972e-17, + "angularVelocity": 3.405841672060239e-17, + "velocityX": -3.216424095587036, + "velocityY": -0.29541577761789645, + "timestamp": 3.489558656808303 + }, + { + "x": 6.491854347886312, + "y": 7.312779042211733, + "heading": 1.3228269289281303e-17, + "angularVelocity": -1.6457017572655323e-17, + "velocityX": -3.8597086192803536, + "velocityY": -0.35449890600748984, + "timestamp": 3.589123786939303 + }, + { + "x": 6.086447476209309, + "y": 7.27554403131357, + "heading": 1.2208912555752519e-17, + "angularVelocity": -1.0238089703026631e-17, + "velocityX": -4.071775642171123, + "velocityY": -0.3739764197482683, + "timestamp": 3.6886889170703028 + }, + { + "x": 5.681040604532077, + "y": 7.2383090204177005, + "heading": 7.247479687391899e-17, + "angularVelocity": 6.05291071671654e-16, + "velocityX": -4.071775642173424, + "velocityY": -0.3739764197252527, + "timestamp": 3.7882540472013027 + }, + { + "x": 5.275633732854971, + "y": 7.201074009520451, + "heading": 1.0354124283363177e-16, + "angularVelocity": 3.1202134641708547e-16, + "velocityX": -4.0717756421721525, + "velocityY": -0.3739764197391007, + "timestamp": 3.8878191773323025 + }, + { + "x": 4.8702268611778505, + "y": 7.163838998623373, + "heading": 8.838275427260066e-18, + "angularVelocity": -9.511660084390495e-16, + "velocityX": -4.071775642172309, + "velocityY": -0.3739764197373963, + "timestamp": 3.9873843074633024 + }, + { + "x": 4.464819989500739, + "y": 7.12660398772638, + "heading": -4.197248226534058e-17, + "angularVelocity": -5.103268345634044e-16, + "velocityX": -4.071775642172202, + "velocityY": -0.37397641973651996, + "timestamp": 4.086949437594302 + }, + { + "x": 4.080527598554363, + "y": 7.091308258018297, + "heading": -2.975056105500731e-17, + "angularVelocity": 1.2275302803554645e-16, + "velocityX": -3.859708619280215, + "velocityY": -0.3544989060089996, + "timestamp": 4.186514567725301 + }, + { + "x": 3.7602839149207647, + "y": 7.061895147676932, + "heading": -1.550761080609776e-17, + "angularVelocity": 1.4305159075441368e-16, + "velocityX": -3.2164240955869543, + "velocityY": -0.2954157776187877, + "timestamp": 4.286079697856301 + }, + { + "x": 3.50408895931399, + "y": 7.038364658604788, + "heading": -1.759561477000234e-18, + "angularVelocity": 1.3808096580609213e-16, + "velocityX": -2.5731393638485094, + "velocityY": -0.23633263012044387, + "timestamp": 4.3856448279873 + }, + { + "x": 3.3119427386387184, + "y": 7.020716791436034, + "heading": -4.625194214758358e-19, + "angularVelocity": 1.3027071363418387e-17, + "velocityX": -1.9298545627616883, + "velocityY": -0.17724947625272364, + "timestamp": 4.4852099581183 + }, + { + "x": 3.1838452563472903, + "y": 7.0089515464877525, + "heading": 1.6084993167813846e-19, + "angularVelocity": 6.260920387830223e-18, + "velocityX": -1.2865697270006862, + "velocityY": -0.11816631920031569, + "timestamp": 4.584775088249299 + }, + { + "x": 3.1197965145111084, + "y": 7.003068923950195, + "heading": 4.3763650818108325e-31, + "angularVelocity": -1.6155247471267731e-18, + "velocityX": -0.6432848704351777, + "velocityY": -0.05908316023709528, + "timestamp": 4.6843402183802985 + }, + { + "x": 3.1197965145111084, + "y": 7.003068923950195, + "heading": 1.7233142036729875e-31, + "angularVelocity": -9.337973840266154e-31, + "velocityX": 1.535724623502671e-24, + "velocityY": 1.8094635509087927e-25, + "timestamp": 4.783905348511298 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NTMid1st.1.traj b/src/main/deploy/choreo/3NTMid1st.1.traj deleted file mode 100644 index e1314cf..0000000 --- a/src/main/deploy/choreo/3NTMid1st.1.traj +++ /dev/null @@ -1,355 +0,0 @@ -{ - "samples": [ - { - "x": 0.6889447569847107, - "y": 6.680017471313477, - "heading": 1.057124176286416, - "angularVelocity": 2.8387025507521403e-19, - "velocityX": 4.1180279564081185e-19, - "velocityY": -9.327641015842875e-18, - "timestamp": 0 - }, - { - "x": 0.7066606142239513, - "y": 6.682284626893522, - "heading": 1.0367126164662654, - "angularVelocity": -0.3553197163703083, - "velocityX": 0.3083935488011813, - "velocityY": 0.03946612041250281, - "timestamp": 0.05744561553932452 - }, - { - "x": 0.7421443739769805, - "y": 6.686835073027394, - "heading": 0.996657534155969, - "angularVelocity": -0.6972696163883332, - "velocityX": 0.6176930897143716, - "velocityY": 0.07921311471990149, - "timestamp": 0.11489123107864904 - }, - { - "x": 0.7954510899599107, - "y": 6.693681954510511, - "heading": 0.9377659189851129, - "angularVelocity": -1.025171627424583, - "velocityX": 0.9279509929950843, - "velocityY": 0.11918893058827323, - "timestamp": 0.17233684661797355 - }, - { - "x": 0.866640472022295, - "y": 6.702826879784641, - "heading": 0.8609043116521772, - "angularVelocity": -1.3379890982336122, - "velocityX": 1.2392483115382664, - "velocityY": 0.15919274583921428, - "timestamp": 0.22978246215729808 - }, - { - "x": 0.9557824403052647, - "y": 6.714252443844757, - "heading": 0.7671249958790186, - "angularVelocity": -1.6324886571884958, - "velocityX": 1.5517627837401693, - "velocityY": 0.19889357878488217, - "timestamp": 0.2872280776966226 - }, - { - "x": 1.0629657750286219, - "y": 6.727919888213016, - "heading": 0.6579512096451294, - "angularVelocity": -1.9004720414067817, - "velocityX": 1.8658227214918544, - "velocityY": 0.23791971310503898, - "timestamp": 0.34467369323594715 - }, - { - "x": 1.18830476238212, - "y": 6.743775124135866, - "heading": 0.5358410167997394, - "angularVelocity": -2.1256660181802616, - "velocityX": 2.1818721268239685, - "velocityY": 0.27600428290297147, - "timestamp": 0.4021193087752717 - }, - { - "x": 1.3319325274539577, - "y": 6.761759462137426, - "heading": 0.40481020591714667, - "angularVelocity": -2.280954075475008, - "velocityX": 2.5002389429271075, - "velocityY": 0.31306719986746534, - "timestamp": 0.4595649243145962 - }, - { - "x": 1.4939586342232587, - "y": 6.781807678999511, - "heading": 0.27131178001389583, - "angularVelocity": -2.3239097475048234, - "velocityX": 2.8205130234593025, - "velocityY": 0.34899472612252225, - "timestamp": 0.5170105398539208 - }, - { - "x": 1.6743020838003386, - "y": 6.803802865918695, - "heading": 0.14646817576698382, - "angularVelocity": -2.1732486121843375, - "velocityX": 3.139377094038195, - "velocityY": 0.3828871309443344, - "timestamp": 0.5744561553932452 - }, - { - "x": 1.871509949404293, - "y": 6.8273220632242, - "heading": 0.05699752908313393, - "angularVelocity": -1.5574843413872477, - "velocityX": 3.4329489509770403, - "velocityY": 0.4094167515605277, - "timestamp": 0.6319017709325697 - }, - { - "x": 2.0850662670440805, - "y": 6.8523948454346995, - "heading": 0.008424789779770448, - "angularVelocity": -0.8455430209484467, - "velocityX": 3.7175390259957597, - "velocityY": 0.43646119856329335, - "timestamp": 0.6893473864718942 - }, - { - "x": 2.315008723320705, - "y": 6.879297231348836, - "heading": 3.6268922648533895e-7, - "angularVelocity": -0.14665047996181718, - "velocityX": 4.002785140655635, - "velocityY": 0.4683105170266588, - "timestamp": 0.7467930020112187 - }, - { - "x": 2.547959277648203, - "y": 6.909420608999938, - "heading": 3.2238006768257296e-7, - "angularVelocity": -7.016925212653802e-7, - "velocityX": 4.055149416373324, - "velocityY": 0.5243807968335108, - "timestamp": 0.8042386175505432 - }, - { - "x": 2.7809074860808103, - "y": 6.939562122456881, - "heading": 2.820828435268406e-7, - "angularVelocity": -7.014847656878403e-7, - "velocityX": 4.05510857957718, - "velocityY": 0.5246965007505043, - "timestamp": 0.8616842330898676 - }, - { - "x": 3.013855691132565, - "y": 6.969703662042265, - "heading": 2.417856204933752e-7, - "angularVelocity": -7.01484746151677e-7, - "velocityX": 4.055108520724085, - "velocityY": 0.5246969555883743, - "timestamp": 0.9191298486291921 - }, - { - "x": 3.246803890458745, - "y": 6.999845245880575, - "heading": 2.0148839914997936e-7, - "angularVelocity": -7.014847167313357e-7, - "velocityX": 4.055108421054599, - "velocityY": 0.5246977259330878, - "timestamp": 0.9765754641685166 - }, - { - "x": 3.479752088366951, - "y": 7.029986840683357, - "heading": 1.6119117842486052e-7, - "angularVelocity": -7.01484705968513e-7, - "velocityX": 4.055108396370846, - "velocityY": 0.5246979168000837, - "timestamp": 1.034021079707841 - }, - { - "x": 3.71270026445834, - "y": 7.06012860409886, - "heading": 1.208939635051467e-7, - "angularVelocity": -7.014846049093638e-7, - "velocityX": 4.0551080165887265, - "velocityY": 0.5247008519713516, - "timestamp": 1.0914666952471657 - }, - { - "x": 3.945648423411925, - "y": 7.090270499956354, - "heading": 8.059675341964947e-8, - "angularVelocity": -7.014845207564386e-7, - "velocityX": 4.055107718257797, - "velocityY": 0.5247031574909511, - "timestamp": 1.1489123107864903 - }, - { - "x": 4.178596822480232, - "y": 7.12041054039277, - "heading": 4.0299480513851105e-8, - "angularVelocity": -7.014856143177233e-7, - "velocityX": 4.05511189811939, - "velocityY": 0.5246708587495975, - "timestamp": 1.2063579263258148 - }, - { - "x": 4.411551475524902, - "y": 7.1505022048950195, - "heading": -6.644547124099774e-19, - "angularVelocity": -7.015240438601083e-7, - "velocityX": 4.055220765894668, - "velocityY": 0.5238287416669793, - "timestamp": 1.2638035418651394 - }, - { - "x": 4.733129802794184, - "y": 7.173132492258203, - "heading": 6.90202073091001e-17, - "angularVelocity": 8.838643936853847e-16, - "velocityX": 4.078826301961981, - "velocityY": 0.2870374135649262, - "timestamp": 1.3426444395799053 - }, - { - "x": 5.054711574817068, - "y": 7.1957137759487155, - "heading": -1.8089714781984044e-17, - "angularVelocity": -1.1048824198785035e-15, - "velocityX": 4.078869994432532, - "velocityY": 0.2864158621355202, - "timestamp": 1.4214853372946714 - }, - { - "x": 5.376293348799719, - "y": 7.2182950317363055, - "heading": -9.56038449707963e-17, - "angularVelocity": -9.831715772484128e-16, - "velocityX": 4.078870019289777, - "velocityY": 0.28641550822118905, - "timestamp": 1.5003262350094375 - }, - { - "x": 5.69787511780094, - "y": 7.240876358458176, - "heading": -1.30423601604514e-16, - "angularVelocity": -4.4164586162869085e-16, - "velocityX": 4.07886995610646, - "velocityY": 0.2864164079354569, - "timestamp": 1.5791671327242036 - }, - { - "x": 6.019456886422846, - "y": 7.2634576905792185, - "heading": -1.4442306040452632e-16, - "angularVelocity": -1.7756595252798448e-16, - "velocityX": 4.078869951295308, - "velocityY": 0.2864164764173412, - "timestamp": 1.6580080304389697 - }, - { - "x": 6.34103865597215, - "y": 7.286039009493073, - "heading": -1.2616330241810545e-16, - "angularVelocity": 2.316026100003538e-16, - "velocityX": 4.078869963058216, - "velocityY": 0.2864163089003463, - "timestamp": 1.7368489281537358 - }, - { - "x": 6.654261604662237, - "y": 7.308033380004462, - "heading": -7.780878230098179e-16, - "angularVelocity": -8.268862240241936e-15, - "velocityX": 3.972848582004707, - "velocityY": 0.27897158897101676, - "timestamp": 1.815689825868502 - }, - { - "x": 6.932682031084689, - "y": 7.327583933385895, - "heading": -8.681753470357217e-16, - "angularVelocity": -1.1426496494529816e-15, - "velocityX": 3.5314213116869153, - "velocityY": 0.24797476878262784, - "timestamp": 1.894530723583268 - }, - { - "x": 7.176299915122605, - "y": 7.34469066831624, - "heading": -7.853180398656596e-16, - "angularVelocity": 1.0509432267454012e-15, - "velocityX": 3.089993786210898, - "velocityY": 0.21697793183727082, - "timestamp": 1.9733716212980341 - }, - { - "x": 7.385115250070514, - "y": 7.359353584352904, - "heading": -6.358392667251897e-16, - "angularVelocity": 1.8959547377957107e-15, - "velocityX": 2.6485661756842305, - "velocityY": 0.18598108927821216, - "timestamp": 2.0522125190128 - }, - { - "x": 7.559128032575715, - "y": 7.371572681274155, - "heading": -4.621266314308384e-16, - "angularVelocity": 2.2033315294472333e-15, - "velocityX": 2.2071385226326603, - "velocityY": 0.1549842439067266, - "timestamp": 2.1310534167275663 - }, - { - "x": 7.698338260626599, - "y": 7.381347958946806, - "heading": -2.953981147731697e-16, - "angularVelocity": 2.1147465581984e-15, - "velocityX": 1.7657108440662903, - "velocityY": 0.1239873968459356, - "timestamp": 2.2098943144423324 - }, - { - "x": 7.802745932882096, - "y": 7.388679417282003, - "heading": -1.5405031499378866e-16, - "angularVelocity": 1.7928233220485274e-15, - "velocityX": 1.3242831484901139, - "velocityY": 0.09299054865815141, - "timestamp": 2.2887352121570985 - }, - { - "x": 7.872351048384304, - "y": 7.393567056216249, - "heading": -4.730665578719817e-17, - "angularVelocity": 1.3539122789856364e-15, - "velocityX": 0.8828554407641064, - "velocityY": 0.061993699664978266, - "timestamp": 2.3675761098718646 - }, - { - "x": 7.907153606414795, - "y": 7.396010875701904, - "heading": 6.668503247685733e-18, - "angularVelocity": 6.84608633328086e-16, - "velocityX": 0.4414277239257422, - "velocityY": 0.030996850067544837, - "timestamp": 2.4464170075866307 - }, - { - "x": 7.907153606414795, - "y": 7.396010875701904, - "heading": 6.8565295403527305e-18, - "angularVelocity": 2.384883844975877e-18, - "velocityX": -5.0745498315179014e-17, - "velocityY": 9.257132972080978e-18, - "timestamp": 2.525257905301397 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NTMid1st.2.traj b/src/main/deploy/choreo/3NTMid1st.2.traj deleted file mode 100644 index 65ca6b9..0000000 --- a/src/main/deploy/choreo/3NTMid1st.2.traj +++ /dev/null @@ -1,229 +0,0 @@ -{ - "samples": [ - { - "x": 7.907153606414795, - "y": 7.396010875701904, - "heading": 6.8565295403527305e-18, - "angularVelocity": 2.384883844975877e-18, - "velocityX": -5.0745498315179014e-17, - "velocityY": 9.257132972080978e-18, - "timestamp": 0 - }, - { - "x": 7.864379482827224, - "y": 7.392836087745542, - "heading": 0.011146441893639504, - "angularVelocity": 0.12739966793783036, - "velocityX": -0.48889225758201854, - "velocityY": -0.03628664064038576, - "timestamp": 0.08749192265617856 - }, - { - "x": 7.778824354644398, - "y": 7.386483417352748, - "heading": 0.03288500177532937, - "angularVelocity": 0.24846362066035474, - "velocityX": -0.9778631625120074, - "velocityY": -0.0726086500322888, - "timestamp": 0.1749838453123571 - }, - { - "x": 7.650479577823578, - "y": 7.376948909938168, - "heading": 0.06447310235121183, - "angularVelocity": 0.36104019224741246, - "velocityX": -1.4669328656221532, - "velocityY": -0.1089758588578287, - "timestamp": 0.26247576796853567 - }, - { - "x": 7.479334044381584, - "y": 7.364227520798733, - "heading": 0.10486253590537926, - "angularVelocity": 0.46163614111999585, - "velocityX": -1.956129528831516, - "velocityY": -0.14540072675540094, - "timestamp": 0.3499676906247142 - }, - { - "x": 7.265373349858742, - "y": 7.348312565522, - "heading": 0.1524685401703482, - "angularVelocity": 0.5441188491427787, - "velocityX": -2.4454908296352564, - "velocityY": -0.18190199499072268, - "timestamp": 0.4374596132808928 - }, - { - "x": 7.0085798004636075, - "y": 7.329194198798068, - "heading": 0.20463142630378972, - "angularVelocity": 0.5962023070224285, - "velocityX": -2.9350543638670454, - "velocityY": -0.21851579144124697, - "timestamp": 0.5249515359370713 - }, - { - "x": 6.7089429147208905, - "y": 7.306854123718901, - "heading": 0.2559598531940128, - "angularVelocity": 0.5866647495212898, - "velocityX": -3.424737697446822, - "velocityY": -0.2553387147172141, - "timestamp": 0.6124434585932499 - }, - { - "x": 6.366659484955159, - "y": 7.281232741940888, - "heading": 0.2892524000892399, - "angularVelocity": 0.38052137711110223, - "velocityX": -3.912171768253631, - "velocityY": -0.2928428248022215, - "timestamp": 0.6999353812494284 - }, - { - "x": 6.009887187340099, - "y": 7.254843656864203, - "heading": 0.2892524060420795, - "angularVelocity": 6.803873294167354e-8, - "velocityX": -4.0777741165557195, - "velocityY": -0.3016173867888123, - "timestamp": 0.787427303905607 - }, - { - "x": 5.653114904073964, - "y": 7.228454377814743, - "heading": 0.28925241199532503, - "angularVelocity": 6.804337339458498e-8, - "velocityX": -4.077773952552866, - "velocityY": -0.3016196038251856, - "timestamp": 0.8749192265617856 - }, - { - "x": 5.296342618629369, - "y": 7.202065128217197, - "heading": 0.2892524179485705, - "angularVelocity": 6.804337236729352e-8, - "velocityX": -4.077773977451856, - "velocityY": -0.30161926720079874, - "timestamp": 0.9624111492179641 - }, - { - "x": 4.93957033303766, - "y": 7.175675880608572, - "heading": 0.289252423901816, - "angularVelocity": 6.804337255673262e-8, - "velocityX": -4.077773979133303, - "velocityY": -0.3016192444681868, - "timestamp": 1.0499030718741427 - }, - { - "x": 4.5827980476089545, - "y": 7.1492866307962055, - "heading": 0.28925242985506144, - "angularVelocity": 6.804337216066887e-8, - "velocityX": -4.077773977270241, - "velocityY": -0.30161926965611885, - "timestamp": 1.1373949945303212 - }, - { - "x": 4.226025763989892, - "y": 7.122897356518204, - "heading": 0.289252435808307, - "angularVelocity": 6.804337356855604e-8, - "velocityX": -4.077773956586706, - "velocityY": -0.3016195492891973, - "timestamp": 1.2248869171864998 - }, - { - "x": 3.86925346440752, - "y": 7.096508298041054, - "heading": 0.28925244176109, - "angularVelocity": 6.803808613081094e-8, - "velocityX": -4.077774139041357, - "velocityY": -0.3016170827660528, - "timestamp": 1.3123788398426783 - }, - { - "x": 3.5270651589106934, - "y": 7.070826464452039, - "heading": 0.32204631423630514, - "angularVelocity": 0.374821714732306, - "velocityX": -3.911084533386486, - "velocityY": -0.29353376642480367, - "timestamp": 1.399870762498857 - }, - { - "x": 3.2274696432203713, - "y": 7.0484355191686605, - "heading": 0.3751215132796767, - "angularVelocity": 0.6066297028577579, - "velocityX": -3.424264853198595, - "velocityY": -0.2559201421526573, - "timestamp": 1.4873626851550354 - }, - { - "x": 2.970717055574845, - "y": 7.029266180646158, - "heading": 0.42936926776546197, - "angularVelocity": 0.6200315736455514, - "velocityX": -2.934586186367169, - "velocityY": -0.2190983800622664, - "timestamp": 1.5748546078112144 - }, - { - "x": 2.756796090717204, - "y": 7.013301299452356, - "heading": 0.4789124689781157, - "angularVelocity": 0.5662602867620836, - "velocityX": -2.445036734400004, - "velocityY": -0.18247262957679933, - "timestamp": 1.6623465304673934 - }, - { - "x": 2.58568613499147, - "y": 7.00053390938676, - "heading": 0.5209305288090914, - "angularVelocity": 0.48025073121431167, - "velocityX": -1.955722888822021, - "velocityY": -0.14592650016126135, - "timestamp": 1.7498384531235724 - }, - { - "x": 2.457370465153117, - "y": 6.990960629174714, - "heading": 0.5537832816752974, - "angularVelocity": 0.3754946956110353, - "velocityX": -1.466600183683258, - "velocityY": -0.10941901745223155, - "timestamp": 1.8373303757797514 - }, - { - "x": 2.3718361943351365, - "y": 6.9845795744226535, - "heading": 0.576397068559217, - "angularVelocity": 0.25846713842129254, - "velocityX": -0.9776247706214999, - "velocityY": -0.07293307265789097, - "timestamp": 1.9248222984359304 - }, - { - "x": 2.3290731906890865, - "y": 6.98138952255249, - "heading": 0.5880023600681887, - "angularVelocity": 0.13264414767266713, - "velocityX": -0.48876516080344745, - "velocityY": -0.03646110147446347, - "timestamp": 2.0123142210921094 - }, - { - "x": 2.3290731906890865, - "y": 6.98138952255249, - "heading": 0.5880023600681887, - "angularVelocity": -4.727611122290784e-19, - "velocityX": 5.894237907108255e-17, - "velocityY": -1.7416397125878645e-17, - "timestamp": 2.0998061437482884 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NTMid1st.3.traj b/src/main/deploy/choreo/3NTMid1st.3.traj deleted file mode 100644 index 442c996..0000000 --- a/src/main/deploy/choreo/3NTMid1st.3.traj +++ /dev/null @@ -1,238 +0,0 @@ -{ - "samples": [ - { - "x": 2.3290731906890865, - "y": 6.98138952255249, - "heading": 0.5880023600681887, - "angularVelocity": -4.727611122290784e-19, - "velocityX": 5.894237907108255e-17, - "velocityY": -1.7416397125878645e-17, - "timestamp": 0 - }, - { - "x": 2.369225863639807, - "y": 6.972681044615008, - "heading": 0.5777100333288744, - "angularVelocity": -0.12019172387184421, - "velocityX": 0.468894847806877, - "velocityY": -0.10169585576872482, - "timestamp": 0.08563257442158534 - }, - { - "x": 2.4495365738113324, - "y": 6.955262677241521, - "heading": 0.5575096279601734, - "angularVelocity": -0.2358962755136905, - "velocityX": 0.9378523384820929, - "velocityY": -0.20340819473362512, - "timestamp": 0.17126514884317068 - }, - { - "x": 2.5700119937448345, - "y": 6.929132707142634, - "heading": 0.52790113095987, - "angularVelocity": -0.34576207944578496, - "velocityX": 1.4068877497525563, - "velocityY": -0.30514054114786776, - "timestamp": 0.256897723264756 - }, - { - "x": 2.7306608435827027, - "y": 6.894288978936286, - "heading": 0.4895909495937422, - "angularVelocity": -0.4473786012495606, - "velocityX": 1.876024993093903, - "velocityY": -0.4068980576807838, - "timestamp": 0.34253029768634136 - }, - { - "x": 2.9314950654935967, - "y": 6.850728824040358, - "heading": 0.4436739187143449, - "angularVelocity": -0.5362098616040543, - "velocityX": 2.34530169468162, - "velocityY": -0.5086867373795613, - "timestamp": 0.4281628721079267 - }, - { - "x": 3.1725314152869473, - "y": 6.798449478849849, - "heading": 0.3920593457901536, - "angularVelocity": -0.6027446129329584, - "velocityX": 2.8147740672455193, - "velocityY": -0.6105076899022854, - "timestamp": 0.513795446529512 - }, - { - "x": 3.453789902204572, - "y": 6.737451911247368, - "heading": 0.3387609345614091, - "angularVelocity": -0.6224081383603658, - "velocityX": 3.2844801037153855, - "velocityY": -0.7123173396864007, - "timestamp": 0.5994280209510974 - }, - { - "x": 3.7752032898636747, - "y": 6.667792428626339, - "heading": 0.2965850468038582, - "angularVelocity": -0.4925215438451132, - "velocityX": 3.753400967215154, - "velocityY": -0.8134694430425643, - "timestamp": 0.6850605953726827 - }, - { - "x": 4.117354039283791, - "y": 6.5934027343019475, - "heading": 0.2965850415443264, - "angularVelocity": -6.141975552750511e-8, - "velocityX": 3.9955677116005384, - "velocityY": -0.8687079049866928, - "timestamp": 0.770693169794268 - }, - { - "x": 4.4595048423406825, - "y": 6.519013286687181, - "heading": 0.29658503628387156, - "angularVelocity": -6.143053505137436e-8, - "velocityX": 3.9955683379600107, - "velocityY": -0.868705023961243, - "timestamp": 0.8563257442158534 - }, - { - "x": 4.801655639434736, - "y": 6.444623811646621, - "heading": 0.2965850310234168, - "angularVelocity": -6.143053372565425e-8, - "velocityX": 3.9955682683271863, - "velocityY": -0.868705344234141, - "timestamp": 0.9419583186374387 - }, - { - "x": 5.143806436012128, - "y": 6.370234334229709, - "heading": 0.2965850257629621, - "angularVelocity": -6.143053357288208e-8, - "velocityX": 3.995568262293729, - "velocityY": -0.8687053719847133, - "timestamp": 1.027590893059024 - }, - { - "x": 5.485957232575462, - "y": 6.29584485674813, - "heading": 0.29658502050250735, - "angularVelocity": -6.1430533960204e-8, - "velocityX": 3.9955682621295425, - "velocityY": -0.868705372739879, - "timestamp": 1.1132234674806094 - }, - { - "x": 5.828108029592984, - "y": 6.2214553813555735, - "heading": 0.2965850152420526, - "angularVelocity": -6.143053356332505e-8, - "velocityX": 3.995568267433484, - "velocityY": -0.868705348344694, - "timestamp": 1.1988560419021947 - }, - { - "x": 6.170258832871027, - "y": 6.1470659347579835, - "heading": 0.2965850099815977, - "angularVelocity": -6.143053548401422e-8, - "velocityX": 3.995568340542574, - "velocityY": -0.8687050120828734, - "timestamp": 1.28448861632378 - }, - { - "x": 6.512409595508601, - "y": 6.072676301228697, - "heading": 0.2965850047217149, - "angularVelocity": -6.142385454674703e-8, - "velocityX": 3.9955678659513554, - "velocityY": -0.8687071950336539, - "timestamp": 1.3701211907453654 - }, - { - "x": 6.833835100702535, - "y": 6.002994889630869, - "heading": 0.2538296262850911, - "angularVelocity": -0.4992887195721912, - "velocityX": 3.753542473352417, - "velocityY": -0.813725525227982, - "timestamp": 1.4557537651669508 - }, - { - "x": 7.115102384879702, - "y": 5.9419710655129725, - "heading": 0.2000312974914686, - "angularVelocity": -0.6282460752465883, - "velocityX": 3.2845828363449043, - "velocityY": -0.7126239579983176, - "timestamp": 1.541386339588536 - }, - { - "x": 7.356146388960769, - "y": 5.8896622648493135, - "heading": 0.1477223466018653, - "angularVelocity": -0.6108534193083621, - "velocityX": 2.8148634524796803, - "velocityY": -0.610851664999963, - "timestamp": 1.6270189140101214 - }, - { - "x": 7.556986217467668, - "y": 5.846072534731234, - "heading": 0.1009603902610464, - "angularVelocity": -0.5460767313919686, - "velocityX": 2.3453671673833587, - "velocityY": -0.5090321108820021, - "timestamp": 1.7126514884317068 - }, - { - "x": 7.7176386942846635, - "y": 5.81120177694534, - "heading": 0.061773947552466436, - "angularVelocity": -0.4576114051605831, - "velocityX": 1.8760673482274681, - "velocityY": -0.4072137036803266, - "timestamp": 1.7982840628532921 - }, - { - "x": 7.838116264571975, - "y": 5.785049474927425, - "heading": 0.03138354406077533, - "angularVelocity": -0.35489302636253056, - "velocityX": 1.4069128611523227, - "velocityY": -0.30540132881164084, - "timestamp": 1.8839166372748775 - }, - { - "x": 7.91842817978787, - "y": 5.7676151344306845, - "heading": 0.01060107904356323, - "angularVelocity": -0.24269345114974641, - "velocityX": 0.9378664107481443, - "velocityY": -0.2035947256578809, - "timestamp": 1.9695492116964628 - }, - { - "x": 7.958581447601322, - "y": 5.758898258209228, - "heading": 1.8857923302222142e-17, - "angularVelocity": -0.12379727125068163, - "velocityX": 0.46890179449433855, - "velocityY": -0.10179392924172202, - "timestamp": 2.055181786118048 - }, - { - "x": 7.958581447601321, - "y": 5.758898258209228, - "heading": 9.223143488076806e-18, - "angularVelocity": -4.402147521794544e-18, - "velocityX": -8.677336320987788e-16, - "velocityY": 6.30351391233509e-17, - "timestamp": 2.1408143605396335 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NTMid1st.4.traj b/src/main/deploy/choreo/3NTMid1st.4.traj deleted file mode 100644 index f973f7c..0000000 --- a/src/main/deploy/choreo/3NTMid1st.4.traj +++ /dev/null @@ -1,274 +0,0 @@ -{ - "samples": [ - { - "x": 7.958581447601321, - "y": 5.758898258209228, - "heading": 9.223143488076806e-18, - "angularVelocity": -4.402147521794544e-18, - "velocityX": -8.677336320987788e-16, - "velocityY": 6.30351391233509e-17, - "timestamp": 0 - }, - { - "x": 7.9330263948013044, - "y": 5.765824370875185, - "heading": -2.1346547777556126e-16, - "angularVelocity": -3.2422884449291236e-15, - "velocityX": -0.37207493315591844, - "velocityY": 0.1008424019853738, - "timestamp": 0.06868254354912473 - }, - { - "x": 7.881916289817926, - "y": 5.7796765966969605, - "heading": -6.253127415258978e-16, - "angularVelocity": -5.996389119459277e-15, - "velocityX": -0.7441498573334772, - "velocityY": 0.20168481110296488, - "timestamp": 0.13736508709824946 - }, - { - "x": 7.805251133422258, - "y": 5.800454936287824, - "heading": -1.1913919936036707e-15, - "angularVelocity": -8.241966786851453e-15, - "velocityX": -1.116224770284402, - "velocityY": 0.30252722914963337, - "timestamp": 0.2060476306473742 - }, - { - "x": 7.703030926606096, - "y": 5.82815939043782, - "heading": -1.851221230426738e-15, - "angularVelocity": -9.606942197740195e-15, - "velocityX": -1.4882996687950614, - "velocityY": 0.40336965869910185, - "timestamp": 0.2747301741964989 - }, - { - "x": 7.575255670692555, - "y": 5.862789960203048, - "heading": -2.5464780342778514e-15, - "angularVelocity": -1.012275836510253e-14, - "velocityX": -1.8603745480414884, - "velocityY": 0.5042121036251386, - "timestamp": 0.34341271774562365 - }, - { - "x": 7.4219253675353904, - "y": 5.904346647067262, - "heading": -3.1806793576894922e-15, - "angularVelocity": -9.233806434397994e-15, - "velocityX": -2.2324494002977344, - "velocityY": 0.6050545701542501, - "timestamp": 0.4120952612947484 - }, - { - "x": 7.2430400199182765, - "y": 5.952829453267497, - "heading": -3.623858903243655e-15, - "angularVelocity": -6.452578990774704e-15, - "velocityX": -2.6045242120243945, - "velocityY": 0.7058970692539853, - "timestamp": 0.4807778048438731 - }, - { - "x": 7.038599632488783, - "y": 6.008238382562526, - "heading": -3.650395176878746e-15, - "angularVelocity": -3.8636122947275614e-16, - "velocityX": -2.97659895608365, - "velocityY": 0.8067396230805047, - "timestamp": 0.5494603483929978 - }, - { - "x": 6.808604214572368, - "y": 6.070573442583485, - "heading": -2.8238139058641448e-15, - "angularVelocity": 1.2034808505527204e-14, - "velocityX": -3.348673564366667, - "velocityY": 0.9075822880143853, - "timestamp": 0.6181428919421226 - }, - { - "x": 6.553053794384004, - "y": 6.139834657117734, - "heading": 1.596126099324249e-17, - "angularVelocity": 4.1346388378013e-14, - "velocityX": -3.720747761846995, - "velocityY": 1.008425299286029, - "timestamp": 0.6868254354912473 - }, - { - "x": 6.281995287149088, - "y": 6.213296859163915, - "heading": 2.79485990380299e-17, - "angularVelocity": 1.7453253668100859e-16, - "velocityX": -3.9465414824226293, - "velocityY": 1.0695905866332982, - "timestamp": 0.755507979040372 - }, - { - "x": 6.010911377533634, - "y": 6.286665266952668, - "heading": 4.354488243724968e-17, - "angularVelocity": 2.2707784403071564e-16, - "velocityX": -3.946911334487284, - "velocityY": 1.0682249665997634, - "timestamp": 0.8241905225894968 - }, - { - "x": 5.735304320584762, - "y": 6.340611159587319, - "heading": -1.2228292980098705e-17, - "angularVelocity": -8.120429446664662e-16, - "velocityX": -4.01276718518365, - "velocityY": 0.7854381891967174, - "timestamp": 0.8928730661386215 - }, - { - "x": 5.45584440231323, - "y": 6.368388175964356, - "heading": 4.246681415993057e-24, - "angularVelocity": 1.780408398032683e-16, - "velocityX": -4.068863845609499, - "velocityY": 0.4044261458833793, - "timestamp": 0.9615556096877462 - }, - { - "x": 5.136152723997814, - "y": 6.365732339700935, - "heading": 1.6384633695167614e-17, - "angularVelocity": 2.0955505420330122e-16, - "velocityX": -4.088772502150643, - "velocityY": -0.033967447441583046, - "timestamp": 1.0397433000391336 - }, - { - "x": 4.818581346862357, - "y": 6.328877980423311, - "heading": 1.0499775957498806e-16, - "angularVelocity": 1.1333385308675126e-15, - "velocityX": -4.061654407600595, - "velocityY": -0.4713575642002044, - "timestamp": 1.117930990390521 - }, - { - "x": 4.503030122212764, - "y": 6.277523836631423, - "heading": 9.793507555323466e-17, - "angularVelocity": -9.032990448793397e-17, - "velocityX": -4.035817188510941, - "velocityY": -0.6568059953935135, - "timestamp": 1.1961186807419084 - }, - { - "x": 4.187477597853484, - "y": 6.226177681156534, - "heading": 1.280731439045987e-16, - "angularVelocity": 3.8545788718687876e-16, - "velocityX": -4.035833811447281, - "velocityY": -0.6567038269737849, - "timestamp": 1.2743063710932967 - }, - { - "x": 3.8719249552864756, - "y": 6.174832252105637, - "heading": 5.2578025910772326e-17, - "angularVelocity": -9.655627841710176e-16, - "velocityX": -4.035835323292801, - "velocityY": -0.6566945362034219, - "timestamp": 1.352494061444685 - }, - { - "x": 3.5671208452798924, - "y": 6.125235876580712, - "heading": 4.8122010318066635e-17, - "angularVelocity": -5.6991340073129e-17, - "velocityX": -3.898364418191803, - "velocityY": -0.6343246015194628, - "timestamp": 1.4306817517960733 - }, - { - "x": 3.2961838319651493, - "y": 6.081150202022552, - "heading": 4.0696028624236024e-17, - "angularVelocity": -9.497633857848639e-17, - "velocityX": -3.465213156917534, - "velocityY": -0.563844185202012, - "timestamp": 1.5088694421474615 - }, - { - "x": 3.059113934767495, - "y": 6.042575233860294, - "heading": 3.553772215118994e-17, - "angularVelocity": -6.597338124206766e-17, - "velocityX": -3.0320616471994413, - "velocityY": -0.49336369944725295, - "timestamp": 1.5870571324988498 - }, - { - "x": 2.855911160163984, - "y": 6.009510973892544, - "heading": 2.649178608062371e-17, - "angularVelocity": -1.1569513470330116e-16, - "velocityX": -2.5989100546415322, - "velocityY": -0.4228831906888374, - "timestamp": 1.6652448228502381 - }, - { - "x": 2.6865755113935372, - "y": 5.981957423016371, - "heading": 1.7566522066077413e-17, - "angularVelocity": -1.1415175421837036e-16, - "velocityX": -2.165758420658685, - "velocityY": -0.3524026704571279, - "timestamp": 1.7434325132016264 - }, - { - "x": 2.5511069903996364, - "y": 5.959914581769268, - "heading": 1.0131326907565385e-17, - "angularVelocity": -9.509415935280826e-17, - "velocityX": -1.7326067618191963, - "velocityY": -0.28192214335099686, - "timestamp": 1.8216202035530147 - }, - { - "x": 2.449505598477995, - "y": 5.943382450509244, - "heading": 7.634100326841493e-18, - "angularVelocity": -3.1938869105023585e-17, - "velocityX": -1.2994550864078938, - "velocityY": -0.2114416116660293, - "timestamp": 1.899807893904403 - }, - { - "x": 2.3817713365541486, - "y": 5.932361029491859, - "heading": 6.441956734413491e-18, - "angularVelocity": -1.5247224888248296e-17, - "velocityX": -0.8663033991592198, - "velocityY": -0.14096107671253, - "timestamp": 1.9779955842557913 - }, - { - "x": 2.347904205322266, - "y": 5.926850318908691, - "heading": 9.78440936544715e-29, - "angularVelocity": -8.239093217325014e-17, - "velocityX": -0.43315170303231587, - "velocityY": -0.0704805393087875, - "timestamp": 2.0561832746071795 - }, - { - "x": 2.347904205322266, - "y": 5.926850318908691, - "heading": 1.1645894687727951e-28, - "angularVelocity": 7.55747290243908e-29, - "velocityX": 1.765775264015562e-17, - "velocityY": 1.8435301762989098e-16, - "timestamp": 2.134370964958568 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NTMid1st.5.traj b/src/main/deploy/choreo/3NTMid1st.5.traj deleted file mode 100644 index 6fe6a80..0000000 --- a/src/main/deploy/choreo/3NTMid1st.5.traj +++ /dev/null @@ -1,94 +0,0 @@ -{ - "samples": [ - { - "x": 2.347904205322266, - "y": 5.926850318908691, - "heading": 1.1645894687727951e-28, - "angularVelocity": 7.55747290243908e-29, - "velocityX": 1.765775264015562e-17, - "velocityY": 1.8435301762989098e-16, - "timestamp": 0 - }, - { - "x": 2.3488457561194225, - "y": 5.978635720040668, - "heading": 3.476477232733492e-18, - "angularVelocity": 3.618994255933981e-17, - "velocityX": 0.009801493577281458, - "velocityY": 0.5390832636166636, - "timestamp": 0.09606197154879581 - }, - { - "x": 2.350728857677348, - "y": 6.0822065203033, - "heading": 1.0600655166660911e-17, - "angularVelocity": 7.416231229628075e-17, - "velocityX": 0.019602986775770496, - "velocityY": 1.0781665063997017, - "timestamp": 0.19212394309759162 - }, - { - "x": 2.353553509923267, - "y": 6.237562715693952, - "heading": 2.1010600692164996e-17, - "angularVelocity": 1.0836697766594381e-16, - "velocityX": 0.02940447921667478, - "velocityY": 1.6172497075154921, - "timestamp": 0.28818591464638743 - }, - { - "x": 2.357319712638855, - "y": 6.444704294204712, - "heading": 3.468807748402165e-17, - "angularVelocity": 1.4238180386272635e-16, - "velocityX": 0.039205969384825255, - "velocityY": 2.156332783629564, - "timestamp": 0.38424788619518324 - }, - { - "x": 2.361085915354443, - "y": 6.651845872715471, - "heading": 1.3778053809890579e-17, - "angularVelocity": -2.1767223113507178e-16, - "velocityX": 0.039205969384825234, - "velocityY": 2.1563327836295634, - "timestamp": 0.48030985774397905 - }, - { - "x": 2.363910567600362, - "y": 6.807202068106124, - "heading": -1.1169584961898105e-17, - "angularVelocity": -2.59703588940451e-16, - "velocityX": 0.029404479216674762, - "velocityY": 1.6172497075154917, - "timestamp": 0.5763718292927749 - }, - { - "x": 2.365793669158288, - "y": 6.910772868368756, - "heading": -3.7105010844358794e-18, - "angularVelocity": 7.764866530502248e-17, - "velocityX": 0.01960298677577048, - "velocityY": 1.0781665063997015, - "timestamp": 0.6724338008415707 - }, - { - "x": 2.3667352199554443, - "y": 6.962558269500732, - "heading": -4.000836031847083e-20, - "angularVelocity": 3.820963347732001e-17, - "velocityX": 0.00980149357728144, - "velocityY": 0.5390832636166634, - "timestamp": 0.7684957723903665 - }, - { - "x": 2.3667352199554443, - "y": 6.962558269500732, - "heading": -3.5257727991835196e-20, - "angularVelocity": 4.945382777465005e-20, - "velocityX": 8.227899776891578e-20, - "velocityY": 1.1821145226159077e-19, - "timestamp": 0.8645577439391623 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NTMid1st.6.traj b/src/main/deploy/choreo/3NTMid1st.6.traj deleted file mode 100644 index 069a1c7..0000000 --- a/src/main/deploy/choreo/3NTMid1st.6.traj +++ /dev/null @@ -1,94 +0,0 @@ -{ - "samples": [ - { - "x": 2.3667352199554443, - "y": 6.962558269500732, - "heading": -3.5257727991835196e-20, - "angularVelocity": 4.945382777465005e-20, - "velocityX": 8.227899776891578e-20, - "velocityY": 1.1821145226159077e-19, - "timestamp": 0 - }, - { - "x": 2.3667489790575416, - "y": 6.937171768983989, - "heading": 0.02562390557481022, - "angularVelocity": 0.37556373996212167, - "velocityX": 0.00020166402139990016, - "velocityY": -0.3720841481710473, - "timestamp": 0.06822784749506106 - }, - { - "x": 2.366781276266347, - "y": 6.886396802207365, - "heading": 0.07683165255040712, - "angularVelocity": 0.7505402684630225, - "velocityX": 0.00047337282343297855, - "velocityY": -0.7441971077909122, - "timestamp": 0.1364556949901221 - }, - { - "x": 2.366837253346187, - "y": 6.810226488063326, - "heading": 0.15348515850807642, - "angularVelocity": 1.123492954445305, - "velocityX": 0.0008204432925090531, - "velocityY": -1.1164109222345555, - "timestamp": 0.20468354248518317 - }, - { - "x": 2.366917007756982, - "y": 6.7086457343302905, - "heading": 0.25528804756869294, - "angularVelocity": 1.4921017267617398, - "velocityX": 0.0011689422094309653, - "velocityY": -1.4888459399278209, - "timestamp": 0.2729113899802442 - }, - { - "x": 2.366858285903453, - "y": 6.606843724969224, - "heading": 0.35303920041696696, - "angularVelocity": 1.4327163531775036, - "velocityX": -0.0008606728144779493, - "velocityY": -1.4920888332060707, - "timestamp": 0.3411392374753053 - }, - { - "x": 2.366800542269629, - "y": 6.530484012907421, - "heading": 0.42622373934358737, - "angularVelocity": 1.072649095839037, - "velocityX": -0.0008463352713547739, - "velocityY": -1.1191868843191977, - "timestamp": 0.40936708497036634 - }, - { - "x": 2.3667570038160806, - "y": 6.479574895496538, - "heading": 0.47497446510426156, - "angularVelocity": 0.7145282688890857, - "velocityX": -0.000638133183841856, - "velocityY": -0.7461633230415036, - "timestamp": 0.4775949324654274 - }, - { - "x": 2.3667352199554443, - "y": 6.45412015914917, - "heading": 0.49934664103730353, - "angularVelocity": 0.35721742408488544, - "velocityX": -0.00031928107709780734, - "velocityY": -0.3730842651779495, - "timestamp": 0.5458227799604884 - }, - { - "x": 2.3667352199554443, - "y": 6.45412015914917, - "heading": 0.49934664103730353, - "angularVelocity": -2.3249470264091964e-20, - "velocityX": -2.5425798784781645e-20, - "velocityY": -4.57825100706316e-20, - "timestamp": 0.6140506274555495 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/3NTMid1st.traj b/src/main/deploy/choreo/3NTMid1st.traj deleted file mode 100644 index b61fc12..0000000 --- a/src/main/deploy/choreo/3NTMid1st.traj +++ /dev/null @@ -1,1219 +0,0 @@ -{ - "samples": [ - { - "x": 0.6889447569847107, - "y": 6.680017471313477, - "heading": 1.057124176286416, - "angularVelocity": 2.8387025507521403e-19, - "velocityX": 4.1180279564081185e-19, - "velocityY": -9.327641015842875e-18, - "timestamp": 0 - }, - { - "x": 0.7066606142239513, - "y": 6.682284626893522, - "heading": 1.0367126164662654, - "angularVelocity": -0.3553197163703083, - "velocityX": 0.3083935488011813, - "velocityY": 0.03946612041250281, - "timestamp": 0.05744561553932452 - }, - { - "x": 0.7421443739769805, - "y": 6.686835073027394, - "heading": 0.996657534155969, - "angularVelocity": -0.6972696163883332, - "velocityX": 0.6176930897143716, - "velocityY": 0.07921311471990149, - "timestamp": 0.11489123107864904 - }, - { - "x": 0.7954510899599107, - "y": 6.693681954510511, - "heading": 0.9377659189851129, - "angularVelocity": -1.025171627424583, - "velocityX": 0.9279509929950843, - "velocityY": 0.11918893058827323, - "timestamp": 0.17233684661797355 - }, - { - "x": 0.866640472022295, - "y": 6.702826879784641, - "heading": 0.8609043116521772, - "angularVelocity": -1.3379890982336122, - "velocityX": 1.2392483115382664, - "velocityY": 0.15919274583921428, - "timestamp": 0.22978246215729808 - }, - { - "x": 0.9557824403052647, - "y": 6.714252443844757, - "heading": 0.7671249958790186, - "angularVelocity": -1.6324886571884958, - "velocityX": 1.5517627837401693, - "velocityY": 0.19889357878488217, - "timestamp": 0.2872280776966226 - }, - { - "x": 1.0629657750286219, - "y": 6.727919888213016, - "heading": 0.6579512096451294, - "angularVelocity": -1.9004720414067817, - "velocityX": 1.8658227214918544, - "velocityY": 0.23791971310503898, - "timestamp": 0.34467369323594715 - }, - { - "x": 1.18830476238212, - "y": 6.743775124135866, - "heading": 0.5358410167997394, - "angularVelocity": -2.1256660181802616, - "velocityX": 2.1818721268239685, - "velocityY": 0.27600428290297147, - "timestamp": 0.4021193087752717 - }, - { - "x": 1.3319325274539577, - "y": 6.761759462137426, - "heading": 0.40481020591714667, - "angularVelocity": -2.280954075475008, - "velocityX": 2.5002389429271075, - "velocityY": 0.31306719986746534, - "timestamp": 0.4595649243145962 - }, - { - "x": 1.4939586342232587, - "y": 6.781807678999511, - "heading": 0.27131178001389583, - "angularVelocity": -2.3239097475048234, - "velocityX": 2.8205130234593025, - "velocityY": 0.34899472612252225, - "timestamp": 0.5170105398539208 - }, - { - "x": 1.6743020838003386, - "y": 6.803802865918695, - "heading": 0.14646817576698382, - "angularVelocity": -2.1732486121843375, - "velocityX": 3.139377094038195, - "velocityY": 0.3828871309443344, - "timestamp": 0.5744561553932452 - }, - { - "x": 1.871509949404293, - "y": 6.8273220632242, - "heading": 0.05699752908313393, - "angularVelocity": -1.5574843413872477, - "velocityX": 3.4329489509770403, - "velocityY": 0.4094167515605277, - "timestamp": 0.6319017709325697 - }, - { - "x": 2.0850662670440805, - "y": 6.8523948454346995, - "heading": 0.008424789779770448, - "angularVelocity": -0.8455430209484467, - "velocityX": 3.7175390259957597, - "velocityY": 0.43646119856329335, - "timestamp": 0.6893473864718942 - }, - { - "x": 2.315008723320705, - "y": 6.879297231348836, - "heading": 3.6268922648533895e-7, - "angularVelocity": -0.14665047996181718, - "velocityX": 4.002785140655635, - "velocityY": 0.4683105170266588, - "timestamp": 0.7467930020112187 - }, - { - "x": 2.547959277648203, - "y": 6.909420608999938, - "heading": 3.2238006768257296e-7, - "angularVelocity": -7.016925212653802e-7, - "velocityX": 4.055149416373324, - "velocityY": 0.5243807968335108, - "timestamp": 0.8042386175505432 - }, - { - "x": 2.7809074860808103, - "y": 6.939562122456881, - "heading": 2.820828435268406e-7, - "angularVelocity": -7.014847656878403e-7, - "velocityX": 4.05510857957718, - "velocityY": 0.5246965007505043, - "timestamp": 0.8616842330898676 - }, - { - "x": 3.013855691132565, - "y": 6.969703662042265, - "heading": 2.417856204933752e-7, - "angularVelocity": -7.01484746151677e-7, - "velocityX": 4.055108520724085, - "velocityY": 0.5246969555883743, - "timestamp": 0.9191298486291921 - }, - { - "x": 3.246803890458745, - "y": 6.999845245880575, - "heading": 2.0148839914997936e-7, - "angularVelocity": -7.014847167313357e-7, - "velocityX": 4.055108421054599, - "velocityY": 0.5246977259330878, - "timestamp": 0.9765754641685166 - }, - { - "x": 3.479752088366951, - "y": 7.029986840683357, - "heading": 1.6119117842486052e-7, - "angularVelocity": -7.01484705968513e-7, - "velocityX": 4.055108396370846, - "velocityY": 0.5246979168000837, - "timestamp": 1.034021079707841 - }, - { - "x": 3.71270026445834, - "y": 7.06012860409886, - "heading": 1.208939635051467e-7, - "angularVelocity": -7.014846049093638e-7, - "velocityX": 4.0551080165887265, - "velocityY": 0.5247008519713516, - "timestamp": 1.0914666952471657 - }, - { - "x": 3.945648423411925, - "y": 7.090270499956354, - "heading": 8.059675341964947e-8, - "angularVelocity": -7.014845207564386e-7, - "velocityX": 4.055107718257797, - "velocityY": 0.5247031574909511, - "timestamp": 1.1489123107864903 - }, - { - "x": 4.178596822480232, - "y": 7.12041054039277, - "heading": 4.0299480513851105e-8, - "angularVelocity": -7.014856143177233e-7, - "velocityX": 4.05511189811939, - "velocityY": 0.5246708587495975, - "timestamp": 1.2063579263258148 - }, - { - "x": 4.411551475524902, - "y": 7.1505022048950195, - "heading": -6.644547124099774e-19, - "angularVelocity": -7.015240438601083e-7, - "velocityX": 4.055220765894668, - "velocityY": 0.5238287416669793, - "timestamp": 1.2638035418651394 - }, - { - "x": 4.733129802794184, - "y": 7.173132492258203, - "heading": 6.90202073091001e-17, - "angularVelocity": 8.838643936853847e-16, - "velocityX": 4.078826301961981, - "velocityY": 0.2870374135649262, - "timestamp": 1.3426444395799053 - }, - { - "x": 5.054711574817068, - "y": 7.1957137759487155, - "heading": -1.8089714781984044e-17, - "angularVelocity": -1.1048824198785035e-15, - "velocityX": 4.078869994432532, - "velocityY": 0.2864158621355202, - "timestamp": 1.4214853372946714 - }, - { - "x": 5.376293348799719, - "y": 7.2182950317363055, - "heading": -9.56038449707963e-17, - "angularVelocity": -9.831715772484128e-16, - "velocityX": 4.078870019289777, - "velocityY": 0.28641550822118905, - "timestamp": 1.5003262350094375 - }, - { - "x": 5.69787511780094, - "y": 7.240876358458176, - "heading": -1.30423601604514e-16, - "angularVelocity": -4.4164586162869085e-16, - "velocityX": 4.07886995610646, - "velocityY": 0.2864164079354569, - "timestamp": 1.5791671327242036 - }, - { - "x": 6.019456886422846, - "y": 7.2634576905792185, - "heading": -1.4442306040452632e-16, - "angularVelocity": -1.7756595252798448e-16, - "velocityX": 4.078869951295308, - "velocityY": 0.2864164764173412, - "timestamp": 1.6580080304389697 - }, - { - "x": 6.34103865597215, - "y": 7.286039009493073, - "heading": -1.2616330241810545e-16, - "angularVelocity": 2.316026100003538e-16, - "velocityX": 4.078869963058216, - "velocityY": 0.2864163089003463, - "timestamp": 1.7368489281537358 - }, - { - "x": 6.654261604662237, - "y": 7.308033380004462, - "heading": -7.780878230098179e-16, - "angularVelocity": -8.268862240241936e-15, - "velocityX": 3.972848582004707, - "velocityY": 0.27897158897101676, - "timestamp": 1.815689825868502 - }, - { - "x": 6.932682031084689, - "y": 7.327583933385895, - "heading": -8.681753470357217e-16, - "angularVelocity": -1.1426496494529816e-15, - "velocityX": 3.5314213116869153, - "velocityY": 0.24797476878262784, - "timestamp": 1.894530723583268 - }, - { - "x": 7.176299915122605, - "y": 7.34469066831624, - "heading": -7.853180398656596e-16, - "angularVelocity": 1.0509432267454012e-15, - "velocityX": 3.089993786210898, - "velocityY": 0.21697793183727082, - "timestamp": 1.9733716212980341 - }, - { - "x": 7.385115250070514, - "y": 7.359353584352904, - "heading": -6.358392667251897e-16, - "angularVelocity": 1.8959547377957107e-15, - "velocityX": 2.6485661756842305, - "velocityY": 0.18598108927821216, - "timestamp": 2.0522125190128 - }, - { - "x": 7.559128032575715, - "y": 7.371572681274155, - "heading": -4.621266314308384e-16, - "angularVelocity": 2.2033315294472333e-15, - "velocityX": 2.2071385226326603, - "velocityY": 0.1549842439067266, - "timestamp": 2.1310534167275663 - }, - { - "x": 7.698338260626599, - "y": 7.381347958946806, - "heading": -2.953981147731697e-16, - "angularVelocity": 2.1147465581984e-15, - "velocityX": 1.7657108440662903, - "velocityY": 0.1239873968459356, - "timestamp": 2.2098943144423324 - }, - { - "x": 7.802745932882096, - "y": 7.388679417282003, - "heading": -1.5405031499378866e-16, - "angularVelocity": 1.7928233220485274e-15, - "velocityX": 1.3242831484901139, - "velocityY": 0.09299054865815141, - "timestamp": 2.2887352121570985 - }, - { - "x": 7.872351048384304, - "y": 7.393567056216249, - "heading": -4.730665578719817e-17, - "angularVelocity": 1.3539122789856364e-15, - "velocityX": 0.8828554407641064, - "velocityY": 0.061993699664978266, - "timestamp": 2.3675761098718646 - }, - { - "x": 7.907153606414795, - "y": 7.396010875701904, - "heading": 6.668503247685733e-18, - "angularVelocity": 6.84608633328086e-16, - "velocityX": 0.4414277239257422, - "velocityY": 0.030996850067544837, - "timestamp": 2.4464170075866307 - }, - { - "x": 7.907153606414795, - "y": 7.396010875701904, - "heading": 6.8565295403527305e-18, - "angularVelocity": 2.384883844975877e-18, - "velocityX": -5.0745498315179014e-17, - "velocityY": 9.257132972080978e-18, - "timestamp": 2.525257905301397 - }, - { - "x": 7.864379482827224, - "y": 7.392836087745542, - "heading": 0.011146441893639504, - "angularVelocity": 0.12739966793783036, - "velocityX": -0.48889225758201854, - "velocityY": -0.03628664064038576, - "timestamp": 2.6127498279575754 - }, - { - "x": 7.778824354644398, - "y": 7.386483417352748, - "heading": 0.03288500177532937, - "angularVelocity": 0.24846362066035474, - "velocityX": -0.9778631625120074, - "velocityY": -0.0726086500322888, - "timestamp": 2.700241750613754 - }, - { - "x": 7.650479577823578, - "y": 7.376948909938168, - "heading": 0.06447310235121183, - "angularVelocity": 0.36104019224741246, - "velocityX": -1.4669328656221532, - "velocityY": -0.1089758588578287, - "timestamp": 2.7877336732699325 - }, - { - "x": 7.479334044381584, - "y": 7.364227520798733, - "heading": 0.10486253590537926, - "angularVelocity": 0.46163614111999585, - "velocityX": -1.956129528831516, - "velocityY": -0.14540072675540094, - "timestamp": 2.875225595926111 - }, - { - "x": 7.265373349858742, - "y": 7.348312565522, - "heading": 0.1524685401703482, - "angularVelocity": 0.5441188491427787, - "velocityX": -2.4454908296352564, - "velocityY": -0.18190199499072268, - "timestamp": 2.9627175185822896 - }, - { - "x": 7.0085798004636075, - "y": 7.329194198798068, - "heading": 0.20463142630378972, - "angularVelocity": 0.5962023070224285, - "velocityX": -2.9350543638670454, - "velocityY": -0.21851579144124697, - "timestamp": 3.050209441238468 - }, - { - "x": 6.7089429147208905, - "y": 7.306854123718901, - "heading": 0.2559598531940128, - "angularVelocity": 0.5866647495212898, - "velocityX": -3.424737697446822, - "velocityY": -0.2553387147172141, - "timestamp": 3.1377013638946467 - }, - { - "x": 6.366659484955159, - "y": 7.281232741940888, - "heading": 0.2892524000892399, - "angularVelocity": 0.38052137711110223, - "velocityX": -3.912171768253631, - "velocityY": -0.2928428248022215, - "timestamp": 3.2251932865508253 - }, - { - "x": 6.009887187340099, - "y": 7.254843656864203, - "heading": 0.2892524060420795, - "angularVelocity": 6.803873294167354e-8, - "velocityX": -4.0777741165557195, - "velocityY": -0.3016173867888123, - "timestamp": 3.312685209207004 - }, - { - "x": 5.653114904073964, - "y": 7.228454377814743, - "heading": 0.28925241199532503, - "angularVelocity": 6.804337339458498e-8, - "velocityX": -4.077773952552866, - "velocityY": -0.3016196038251856, - "timestamp": 3.4001771318631824 - }, - { - "x": 5.296342618629369, - "y": 7.202065128217197, - "heading": 0.2892524179485705, - "angularVelocity": 6.804337236729352e-8, - "velocityX": -4.077773977451856, - "velocityY": -0.30161926720079874, - "timestamp": 3.487669054519361 - }, - { - "x": 4.93957033303766, - "y": 7.175675880608572, - "heading": 0.289252423901816, - "angularVelocity": 6.804337255673262e-8, - "velocityX": -4.077773979133303, - "velocityY": -0.3016192444681868, - "timestamp": 3.5751609771755395 - }, - { - "x": 4.5827980476089545, - "y": 7.1492866307962055, - "heading": 0.28925242985506144, - "angularVelocity": 6.804337216066887e-8, - "velocityX": -4.077773977270241, - "velocityY": -0.30161926965611885, - "timestamp": 3.662652899831718 - }, - { - "x": 4.226025763989892, - "y": 7.122897356518204, - "heading": 0.289252435808307, - "angularVelocity": 6.804337356855604e-8, - "velocityX": -4.077773956586706, - "velocityY": -0.3016195492891973, - "timestamp": 3.7501448224878966 - }, - { - "x": 3.86925346440752, - "y": 7.096508298041054, - "heading": 0.28925244176109, - "angularVelocity": 6.803808613081094e-8, - "velocityX": -4.077774139041357, - "velocityY": -0.3016170827660528, - "timestamp": 3.837636745144075 - }, - { - "x": 3.5270651589106934, - "y": 7.070826464452039, - "heading": 0.32204631423630514, - "angularVelocity": 0.374821714732306, - "velocityX": -3.911084533386486, - "velocityY": -0.29353376642480367, - "timestamp": 3.9251286678002537 - }, - { - "x": 3.2274696432203713, - "y": 7.0484355191686605, - "heading": 0.3751215132796767, - "angularVelocity": 0.6066297028577579, - "velocityX": -3.424264853198595, - "velocityY": -0.2559201421526573, - "timestamp": 4.012620590456432 - }, - { - "x": 2.970717055574845, - "y": 7.029266180646158, - "heading": 0.42936926776546197, - "angularVelocity": 0.6200315736455514, - "velocityX": -2.934586186367169, - "velocityY": -0.2190983800622664, - "timestamp": 4.100112513112611 - }, - { - "x": 2.756796090717204, - "y": 7.013301299452356, - "heading": 0.4789124689781157, - "angularVelocity": 0.5662602867620836, - "velocityX": -2.445036734400004, - "velocityY": -0.18247262957679933, - "timestamp": 4.18760443576879 - }, - { - "x": 2.58568613499147, - "y": 7.00053390938676, - "heading": 0.5209305288090914, - "angularVelocity": 0.48025073121431167, - "velocityX": -1.955722888822021, - "velocityY": -0.14592650016126135, - "timestamp": 4.275096358424969 - }, - { - "x": 2.457370465153117, - "y": 6.990960629174714, - "heading": 0.5537832816752974, - "angularVelocity": 0.3754946956110353, - "velocityX": -1.466600183683258, - "velocityY": -0.10941901745223155, - "timestamp": 4.362588281081148 - }, - { - "x": 2.3718361943351365, - "y": 6.9845795744226535, - "heading": 0.576397068559217, - "angularVelocity": 0.25846713842129254, - "velocityX": -0.9776247706214999, - "velocityY": -0.07293307265789097, - "timestamp": 4.450080203737327 - }, - { - "x": 2.3290731906890865, - "y": 6.98138952255249, - "heading": 0.5880023600681887, - "angularVelocity": 0.13264414767266713, - "velocityX": -0.48876516080344745, - "velocityY": -0.03646110147446347, - "timestamp": 4.537572126393506 - }, - { - "x": 2.3290731906890865, - "y": 6.98138952255249, - "heading": 0.5880023600681887, - "angularVelocity": -4.727611122290784e-19, - "velocityX": 5.894237907108255e-17, - "velocityY": -1.7416397125878645e-17, - "timestamp": 4.625064049049685 - }, - { - "x": 2.369225863639807, - "y": 6.972681044615008, - "heading": 0.5777100333288744, - "angularVelocity": -0.12019172387184421, - "velocityX": 0.468894847806877, - "velocityY": -0.10169585576872482, - "timestamp": 4.710696623471271 - }, - { - "x": 2.4495365738113324, - "y": 6.955262677241521, - "heading": 0.5575096279601734, - "angularVelocity": -0.2358962755136905, - "velocityX": 0.9378523384820929, - "velocityY": -0.20340819473362512, - "timestamp": 4.796329197892856 - }, - { - "x": 2.5700119937448345, - "y": 6.929132707142634, - "heading": 0.52790113095987, - "angularVelocity": -0.34576207944578496, - "velocityX": 1.4068877497525563, - "velocityY": -0.30514054114786776, - "timestamp": 4.881961772314441 - }, - { - "x": 2.7306608435827027, - "y": 6.894288978936286, - "heading": 0.4895909495937422, - "angularVelocity": -0.4473786012495606, - "velocityX": 1.876024993093903, - "velocityY": -0.4068980576807838, - "timestamp": 4.967594346736027 - }, - { - "x": 2.9314950654935967, - "y": 6.850728824040358, - "heading": 0.4436739187143449, - "angularVelocity": -0.5362098616040543, - "velocityX": 2.34530169468162, - "velocityY": -0.5086867373795613, - "timestamp": 5.053226921157612 - }, - { - "x": 3.1725314152869473, - "y": 6.798449478849849, - "heading": 0.3920593457901536, - "angularVelocity": -0.6027446129329584, - "velocityX": 2.8147740672455193, - "velocityY": -0.6105076899022854, - "timestamp": 5.138859495579197 - }, - { - "x": 3.453789902204572, - "y": 6.737451911247368, - "heading": 0.3387609345614091, - "angularVelocity": -0.6224081383603658, - "velocityX": 3.2844801037153855, - "velocityY": -0.7123173396864007, - "timestamp": 5.224492070000783 - }, - { - "x": 3.7752032898636747, - "y": 6.667792428626339, - "heading": 0.2965850468038582, - "angularVelocity": -0.4925215438451132, - "velocityX": 3.753400967215154, - "velocityY": -0.8134694430425643, - "timestamp": 5.310124644422368 - }, - { - "x": 4.117354039283791, - "y": 6.5934027343019475, - "heading": 0.2965850415443264, - "angularVelocity": -6.141975552750511e-8, - "velocityX": 3.9955677116005384, - "velocityY": -0.8687079049866928, - "timestamp": 5.395757218843953 - }, - { - "x": 4.4595048423406825, - "y": 6.519013286687181, - "heading": 0.29658503628387156, - "angularVelocity": -6.143053505137436e-8, - "velocityX": 3.9955683379600107, - "velocityY": -0.868705023961243, - "timestamp": 5.481389793265539 - }, - { - "x": 4.801655639434736, - "y": 6.444623811646621, - "heading": 0.2965850310234168, - "angularVelocity": -6.143053372565425e-8, - "velocityX": 3.9955682683271863, - "velocityY": -0.868705344234141, - "timestamp": 5.567022367687124 - }, - { - "x": 5.143806436012128, - "y": 6.370234334229709, - "heading": 0.2965850257629621, - "angularVelocity": -6.143053357288208e-8, - "velocityX": 3.995568262293729, - "velocityY": -0.8687053719847133, - "timestamp": 5.652654942108709 - }, - { - "x": 5.485957232575462, - "y": 6.29584485674813, - "heading": 0.29658502050250735, - "angularVelocity": -6.1430533960204e-8, - "velocityX": 3.9955682621295425, - "velocityY": -0.868705372739879, - "timestamp": 5.738287516530295 - }, - { - "x": 5.828108029592984, - "y": 6.2214553813555735, - "heading": 0.2965850152420526, - "angularVelocity": -6.143053356332505e-8, - "velocityX": 3.995568267433484, - "velocityY": -0.868705348344694, - "timestamp": 5.82392009095188 - }, - { - "x": 6.170258832871027, - "y": 6.1470659347579835, - "heading": 0.2965850099815977, - "angularVelocity": -6.143053548401422e-8, - "velocityX": 3.995568340542574, - "velocityY": -0.8687050120828734, - "timestamp": 5.909552665373465 - }, - { - "x": 6.512409595508601, - "y": 6.072676301228697, - "heading": 0.2965850047217149, - "angularVelocity": -6.142385454674703e-8, - "velocityX": 3.9955678659513554, - "velocityY": -0.8687071950336539, - "timestamp": 5.995185239795051 - }, - { - "x": 6.833835100702535, - "y": 6.002994889630869, - "heading": 0.2538296262850911, - "angularVelocity": -0.4992887195721912, - "velocityX": 3.753542473352417, - "velocityY": -0.813725525227982, - "timestamp": 6.080817814216636 - }, - { - "x": 7.115102384879702, - "y": 5.9419710655129725, - "heading": 0.2000312974914686, - "angularVelocity": -0.6282460752465883, - "velocityX": 3.2845828363449043, - "velocityY": -0.7126239579983176, - "timestamp": 6.166450388638221 - }, - { - "x": 7.356146388960769, - "y": 5.8896622648493135, - "heading": 0.1477223466018653, - "angularVelocity": -0.6108534193083621, - "velocityX": 2.8148634524796803, - "velocityY": -0.610851664999963, - "timestamp": 6.252082963059807 - }, - { - "x": 7.556986217467668, - "y": 5.846072534731234, - "heading": 0.1009603902610464, - "angularVelocity": -0.5460767313919686, - "velocityX": 2.3453671673833587, - "velocityY": -0.5090321108820021, - "timestamp": 6.337715537481392 - }, - { - "x": 7.7176386942846635, - "y": 5.81120177694534, - "heading": 0.061773947552466436, - "angularVelocity": -0.4576114051605831, - "velocityX": 1.8760673482274681, - "velocityY": -0.4072137036803266, - "timestamp": 6.423348111902977 - }, - { - "x": 7.838116264571975, - "y": 5.785049474927425, - "heading": 0.03138354406077533, - "angularVelocity": -0.35489302636253056, - "velocityX": 1.4069128611523227, - "velocityY": -0.30540132881164084, - "timestamp": 6.508980686324563 - }, - { - "x": 7.91842817978787, - "y": 5.7676151344306845, - "heading": 0.01060107904356323, - "angularVelocity": -0.24269345114974641, - "velocityX": 0.9378664107481443, - "velocityY": -0.2035947256578809, - "timestamp": 6.594613260746148 - }, - { - "x": 7.958581447601322, - "y": 5.758898258209228, - "heading": 1.8857923302222142e-17, - "angularVelocity": -0.12379727125068163, - "velocityX": 0.46890179449433855, - "velocityY": -0.10179392924172202, - "timestamp": 6.680245835167733 - }, - { - "x": 7.958581447601321, - "y": 5.758898258209228, - "heading": 9.223143488076806e-18, - "angularVelocity": -4.402147521794544e-18, - "velocityX": -8.677336320987788e-16, - "velocityY": 6.30351391233509e-17, - "timestamp": 6.765878409589319 - }, - { - "x": 7.9330263948013044, - "y": 5.765824370875185, - "heading": -2.1346547777556126e-16, - "angularVelocity": -3.2422884449291236e-15, - "velocityX": -0.37207493315591844, - "velocityY": 0.1008424019853738, - "timestamp": 6.8345609531384435 - }, - { - "x": 7.881916289817926, - "y": 5.7796765966969605, - "heading": -6.253127415258978e-16, - "angularVelocity": -5.996389119459277e-15, - "velocityX": -0.7441498573334772, - "velocityY": 0.20168481110296488, - "timestamp": 6.903243496687568 - }, - { - "x": 7.805251133422258, - "y": 5.800454936287824, - "heading": -1.1913919936036707e-15, - "angularVelocity": -8.241966786851453e-15, - "velocityX": -1.116224770284402, - "velocityY": 0.30252722914963337, - "timestamp": 6.971926040236693 - }, - { - "x": 7.703030926606096, - "y": 5.82815939043782, - "heading": -1.851221230426738e-15, - "angularVelocity": -9.606942197740195e-15, - "velocityX": -1.4882996687950614, - "velocityY": 0.40336965869910185, - "timestamp": 7.040608583785818 - }, - { - "x": 7.575255670692555, - "y": 5.862789960203048, - "heading": -2.5464780342778514e-15, - "angularVelocity": -1.012275836510253e-14, - "velocityX": -1.8603745480414884, - "velocityY": 0.5042121036251386, - "timestamp": 7.109291127334942 - }, - { - "x": 7.4219253675353904, - "y": 5.904346647067262, - "heading": -3.1806793576894922e-15, - "angularVelocity": -9.233806434397994e-15, - "velocityX": -2.2324494002977344, - "velocityY": 0.6050545701542501, - "timestamp": 7.177973670884067 - }, - { - "x": 7.2430400199182765, - "y": 5.952829453267497, - "heading": -3.623858903243655e-15, - "angularVelocity": -6.452578990774704e-15, - "velocityX": -2.6045242120243945, - "velocityY": 0.7058970692539853, - "timestamp": 7.246656214433192 - }, - { - "x": 7.038599632488783, - "y": 6.008238382562526, - "heading": -3.650395176878746e-15, - "angularVelocity": -3.8636122947275614e-16, - "velocityX": -2.97659895608365, - "velocityY": 0.8067396230805047, - "timestamp": 7.315338757982317 - }, - { - "x": 6.808604214572368, - "y": 6.070573442583485, - "heading": -2.8238139058641448e-15, - "angularVelocity": 1.2034808505527204e-14, - "velocityX": -3.348673564366667, - "velocityY": 0.9075822880143853, - "timestamp": 7.384021301531441 - }, - { - "x": 6.553053794384004, - "y": 6.139834657117734, - "heading": 1.596126099324249e-17, - "angularVelocity": 4.1346388378013e-14, - "velocityX": -3.720747761846995, - "velocityY": 1.008425299286029, - "timestamp": 7.452703845080566 - }, - { - "x": 6.281995287149088, - "y": 6.213296859163915, - "heading": 2.79485990380299e-17, - "angularVelocity": 1.7453253668100859e-16, - "velocityX": -3.9465414824226293, - "velocityY": 1.0695905866332982, - "timestamp": 7.521386388629691 - }, - { - "x": 6.010911377533634, - "y": 6.286665266952668, - "heading": 4.354488243724968e-17, - "angularVelocity": 2.2707784403071564e-16, - "velocityX": -3.946911334487284, - "velocityY": 1.0682249665997634, - "timestamp": 7.5900689321788155 - }, - { - "x": 5.735304320584762, - "y": 6.340611159587319, - "heading": -1.2228292980098705e-17, - "angularVelocity": -8.120429446664662e-16, - "velocityX": -4.01276718518365, - "velocityY": 0.7854381891967174, - "timestamp": 7.65875147572794 - }, - { - "x": 5.45584440231323, - "y": 6.368388175964356, - "heading": 4.246681415993057e-24, - "angularVelocity": 1.780408398032683e-16, - "velocityX": -4.068863845609499, - "velocityY": 0.4044261458833793, - "timestamp": 7.727434019277065 - }, - { - "x": 5.136152723997814, - "y": 6.365732339700935, - "heading": 1.6384633695167614e-17, - "angularVelocity": 2.0955505420330122e-16, - "velocityX": -4.088772502150643, - "velocityY": -0.033967447441583046, - "timestamp": 7.805621709628452 - }, - { - "x": 4.818581346862357, - "y": 6.328877980423311, - "heading": 1.0499775957498806e-16, - "angularVelocity": 1.1333385308675126e-15, - "velocityX": -4.061654407600595, - "velocityY": -0.4713575642002044, - "timestamp": 7.88380939997984 - }, - { - "x": 4.503030122212764, - "y": 6.277523836631423, - "heading": 9.793507555323466e-17, - "angularVelocity": -9.032990448793397e-17, - "velocityX": -4.035817188510941, - "velocityY": -0.6568059953935135, - "timestamp": 7.961997090331227 - }, - { - "x": 4.187477597853484, - "y": 6.226177681156534, - "heading": 1.280731439045987e-16, - "angularVelocity": 3.8545788718687876e-16, - "velocityX": -4.035833811447281, - "velocityY": -0.6567038269737849, - "timestamp": 8.040184780682615 - }, - { - "x": 3.8719249552864756, - "y": 6.174832252105637, - "heading": 5.2578025910772326e-17, - "angularVelocity": -9.655627841710176e-16, - "velocityX": -4.035835323292801, - "velocityY": -0.6566945362034219, - "timestamp": 8.118372471034004 - }, - { - "x": 3.5671208452798924, - "y": 6.125235876580712, - "heading": 4.8122010318066635e-17, - "angularVelocity": -5.6991340073129e-17, - "velocityX": -3.898364418191803, - "velocityY": -0.6343246015194628, - "timestamp": 8.196560161385392 - }, - { - "x": 3.2961838319651493, - "y": 6.081150202022552, - "heading": 4.0696028624236024e-17, - "angularVelocity": -9.497633857848639e-17, - "velocityX": -3.465213156917534, - "velocityY": -0.563844185202012, - "timestamp": 8.27474785173678 - }, - { - "x": 3.059113934767495, - "y": 6.042575233860294, - "heading": 3.553772215118994e-17, - "angularVelocity": -6.597338124206766e-17, - "velocityX": -3.0320616471994413, - "velocityY": -0.49336369944725295, - "timestamp": 8.352935542088169 - }, - { - "x": 2.855911160163984, - "y": 6.009510973892544, - "heading": 2.649178608062371e-17, - "angularVelocity": -1.1569513470330116e-16, - "velocityX": -2.5989100546415322, - "velocityY": -0.4228831906888374, - "timestamp": 8.431123232439557 - }, - { - "x": 2.6865755113935372, - "y": 5.981957423016371, - "heading": 1.7566522066077413e-17, - "angularVelocity": -1.1415175421837036e-16, - "velocityX": -2.165758420658685, - "velocityY": -0.3524026704571279, - "timestamp": 8.509310922790945 - }, - { - "x": 2.5511069903996364, - "y": 5.959914581769268, - "heading": 1.0131326907565385e-17, - "angularVelocity": -9.509415935280826e-17, - "velocityX": -1.7326067618191963, - "velocityY": -0.28192214335099686, - "timestamp": 8.587498613142333 - }, - { - "x": 2.449505598477995, - "y": 5.943382450509244, - "heading": 7.634100326841493e-18, - "angularVelocity": -3.1938869105023585e-17, - "velocityX": -1.2994550864078938, - "velocityY": -0.2114416116660293, - "timestamp": 8.665686303493722 - }, - { - "x": 2.3817713365541486, - "y": 5.932361029491859, - "heading": 6.441956734413491e-18, - "angularVelocity": -1.5247224888248296e-17, - "velocityX": -0.8663033991592198, - "velocityY": -0.14096107671253, - "timestamp": 8.74387399384511 - }, - { - "x": 2.347904205322266, - "y": 5.926850318908691, - "heading": 9.78440936544715e-29, - "angularVelocity": -8.239093217325014e-17, - "velocityX": -0.43315170303231587, - "velocityY": -0.0704805393087875, - "timestamp": 8.822061684196498 - }, - { - "x": 2.347904205322266, - "y": 5.926850318908691, - "heading": 1.1645894687727951e-28, - "angularVelocity": 7.55747290243908e-29, - "velocityX": 1.765775264015562e-17, - "velocityY": 1.8435301762989098e-16, - "timestamp": 8.900249374547887 - }, - { - "x": 2.3488457561194225, - "y": 5.978635720040668, - "heading": 3.476477232733492e-18, - "angularVelocity": 3.618994255933981e-17, - "velocityX": 0.009801493577281458, - "velocityY": 0.5390832636166636, - "timestamp": 8.996311346096682 - }, - { - "x": 2.350728857677348, - "y": 6.0822065203033, - "heading": 1.0600655166660911e-17, - "angularVelocity": 7.416231229628075e-17, - "velocityX": 0.019602986775770496, - "velocityY": 1.0781665063997017, - "timestamp": 9.092373317645478 - }, - { - "x": 2.353553509923267, - "y": 6.237562715693952, - "heading": 2.1010600692164996e-17, - "angularVelocity": 1.0836697766594381e-16, - "velocityX": 0.02940447921667478, - "velocityY": 1.6172497075154921, - "timestamp": 9.188435289194274 - }, - { - "x": 2.357319712638855, - "y": 6.444704294204712, - "heading": 3.468807748402165e-17, - "angularVelocity": 1.4238180386272635e-16, - "velocityX": 0.039205969384825255, - "velocityY": 2.156332783629564, - "timestamp": 9.28449726074307 - }, - { - "x": 2.361085915354443, - "y": 6.651845872715471, - "heading": 1.3778053809890579e-17, - "angularVelocity": -2.1767223113507178e-16, - "velocityX": 0.039205969384825234, - "velocityY": 2.1563327836295634, - "timestamp": 9.380559232291866 - }, - { - "x": 2.363910567600362, - "y": 6.807202068106124, - "heading": -1.1169584961898105e-17, - "angularVelocity": -2.59703588940451e-16, - "velocityX": 0.029404479216674762, - "velocityY": 1.6172497075154917, - "timestamp": 9.476621203840661 - }, - { - "x": 2.365793669158288, - "y": 6.910772868368756, - "heading": -3.7105010844358794e-18, - "angularVelocity": 7.764866530502248e-17, - "velocityX": 0.01960298677577048, - "velocityY": 1.0781665063997015, - "timestamp": 9.572683175389457 - }, - { - "x": 2.3667352199554443, - "y": 6.962558269500732, - "heading": -4.000836031847083e-20, - "angularVelocity": 3.820963347732001e-17, - "velocityX": 0.00980149357728144, - "velocityY": 0.5390832636166634, - "timestamp": 9.668745146938253 - }, - { - "x": 2.3667352199554443, - "y": 6.962558269500732, - "heading": -3.5257727991835196e-20, - "angularVelocity": 4.945382777465005e-20, - "velocityX": 8.227899776891578e-20, - "velocityY": 1.1821145226159077e-19, - "timestamp": 9.764807118487049 - }, - { - "x": 2.3667489790575416, - "y": 6.937171768983989, - "heading": 0.02562390557481022, - "angularVelocity": 0.37556373996212167, - "velocityX": 0.00020166402139990016, - "velocityY": -0.3720841481710473, - "timestamp": 9.83303496598211 - }, - { - "x": 2.366781276266347, - "y": 6.886396802207365, - "heading": 0.07683165255040712, - "angularVelocity": 0.7505402684630225, - "velocityX": 0.00047337282343297855, - "velocityY": -0.7441971077909122, - "timestamp": 9.901262813477171 - }, - { - "x": 2.366837253346187, - "y": 6.810226488063326, - "heading": 0.15348515850807642, - "angularVelocity": 1.123492954445305, - "velocityX": 0.0008204432925090531, - "velocityY": -1.1164109222345555, - "timestamp": 9.969490660972232 - }, - { - "x": 2.366917007756982, - "y": 6.7086457343302905, - "heading": 0.25528804756869294, - "angularVelocity": 1.4921017267617398, - "velocityX": 0.0011689422094309653, - "velocityY": -1.4888459399278209, - "timestamp": 10.037718508467293 - }, - { - "x": 2.366858285903453, - "y": 6.606843724969224, - "heading": 0.35303920041696696, - "angularVelocity": 1.4327163531775036, - "velocityX": -0.0008606728144779493, - "velocityY": -1.4920888332060707, - "timestamp": 10.105946355962354 - }, - { - "x": 2.366800542269629, - "y": 6.530484012907421, - "heading": 0.42622373934358737, - "angularVelocity": 1.072649095839037, - "velocityX": -0.0008463352713547739, - "velocityY": -1.1191868843191977, - "timestamp": 10.174174203457415 - }, - { - "x": 2.3667570038160806, - "y": 6.479574895496538, - "heading": 0.47497446510426156, - "angularVelocity": 0.7145282688890857, - "velocityX": -0.000638133183841856, - "velocityY": -0.7461633230415036, - "timestamp": 10.242402050952476 - }, - { - "x": 2.3667352199554443, - "y": 6.45412015914917, - "heading": 0.49934664103730353, - "angularVelocity": 0.35721742408488544, - "velocityX": -0.00031928107709780734, - "velocityY": -0.3730842651779495, - "timestamp": 10.310629898447537 - }, - { - "x": 2.3667352199554443, - "y": 6.45412015914917, - "heading": 0.49934664103730353, - "angularVelocity": -2.3249470264091964e-20, - "velocityX": -2.5425798784781645e-20, - "velocityY": -4.57825100706316e-20, - "timestamp": 10.378857745942598 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Skip.1.traj b/src/main/deploy/choreo/4NB-Skip.1.traj new file mode 100644 index 0000000..9c4a865 --- /dev/null +++ b/src/main/deploy/choreo/4NB-Skip.1.traj @@ -0,0 +1,382 @@ +{ + "samples": [ + { + "x": 0.8085346817970276, + "y": 4.440213680267334, + "heading": -0.994421194775346, + "angularVelocity": 9.676628377640253e-20, + "velocityX": 2.353549276621379e-17, + "velocityY": 2.6128743115300274e-17, + "timestamp": 0 + }, + { + "x": 0.822974919123913, + "y": 4.427726715046675, + "heading": -0.9705493797852635, + "angularVelocity": 0.40182084288808634, + "velocityX": 0.2430643977679354, + "velocityY": -0.21018606637847403, + "timestamp": 0.05940910087815508 + }, + { + "x": 0.8519383277486233, + "y": 4.402735349568012, + "heading": -0.9240064477296293, + "angularVelocity": 0.78343101254963, + "velocityX": 0.4875247764498045, + "velocityY": -0.4206656069399718, + "timestamp": 0.11881820175631017 + }, + { + "x": 0.8955174447652127, + "y": 4.365218664734886, + "heading": -0.856257826909993, + "angularVelocity": 1.1403744513586813, + "velocityX": 0.7335427800188714, + "velocityY": -0.6314972668931192, + "timestamp": 0.17822730263446523 + }, + { + "x": 0.9538148065445263, + "y": 4.315143986084932, + "heading": -0.7692489012801026, + "angularVelocity": 1.4645723356147615, + "velocityX": 0.981286720680567, + "velocityY": -0.842878917704278, + "timestamp": 0.23763640351262033 + }, + { + "x": 1.026938864152823, + "y": 4.2524594827203375, + "heading": -0.6656141360189669, + "angularVelocity": 1.7444257483998427, + "velocityX": 1.230856157178015, + "velocityY": -1.0551330088830848, + "timestamp": 0.29704550439077543 + }, + { + "x": 1.1149962326719975, + "y": 4.177093081809827, + "heading": -0.5488592608060379, + "angularVelocity": 1.9652691841337226, + "velocityX": 1.4822201854184571, + "velocityY": -1.2686002615171224, + "timestamp": 0.3564546052689305 + }, + { + "x": 1.2180827278312385, + "y": 4.088964134511281, + "heading": -0.4236140095866624, + "angularVelocity": 2.108182910834597, + "velocityX": 1.735197025968326, + "velocityY": -1.483425030776192, + "timestamp": 0.4158637061470856 + }, + { + "x": 1.3362653476838338, + "y": 3.988016447914785, + "heading": -0.29631937092086613, + "angularVelocity": 2.1426790977171852, + "velocityX": 1.9893016070882061, + "velocityY": -1.6991956637006456, + "timestamp": 0.4752728070252407 + }, + { + "x": 1.4695036425602637, + "y": 3.874326638244391, + "heading": -0.17703470987105152, + "angularVelocity": 2.007851647081105, + "velocityX": 2.2427253216586585, + "velocityY": -1.9136766587931984, + "timestamp": 0.5346819079033958 + }, + { + "x": 1.61738357366469, + "y": 3.7486761953014893, + "heading": -0.08320124029329157, + "angularVelocity": 1.5794460476721217, + "velocityX": 2.4891797539185125, + "velocityY": -2.115003275350498, + "timestamp": 0.5940910087815509 + }, + { + "x": 1.7795783215287542, + "y": 3.612066748468329, + "heading": -0.024373238316333756, + "angularVelocity": 0.9902186888437958, + "velocityX": 2.7301330177794627, + "velocityY": -2.299470027552942, + "timestamp": 0.653500109659706 + }, + { + "x": 1.9560238490010582, + "y": 3.464363802586248, + "heading": -3.251899068917287e-7, + "angularVelocity": 0.41025554613951887, + "velocityX": 2.9700083802675254, + "velocityY": -2.486200661158664, + "timestamp": 0.712909210537861 + }, + { + "x": 2.1407664058154356, + "y": 3.306630906711477, + "heading": -2.960600612657896e-7, + "angularVelocity": 4.903263169257326e-7, + "velocityX": 3.1096676112512296, + "velocityY": -2.655029171343601, + "timestamp": 0.7723183114160161 + }, + { + "x": 2.3254947652525764, + "y": 3.1488813838204237, + "heading": -2.6697171512847604e-7, + "angularVelocity": 4.896277793613143e-7, + "velocityX": 3.109428634780798, + "velocityY": -2.655309044562341, + "timestamp": 0.8317274122941712 + }, + { + "x": 2.5102231286576613, + "y": 2.9911318655758214, + "heading": -2.3788336891991952e-7, + "angularVelocity": 4.896277805605067e-7, + "velocityX": 3.109428701570963, + "velocityY": -2.655308966351258, + "timestamp": 0.8911365131723263 + }, + { + "x": 2.694951491657384, + "y": 2.8333823468564927, + "heading": -2.0879502283644788e-7, + "angularVelocity": 4.89627778455023e-7, + "velocityX": 3.1094286947477254, + "velocityY": -2.6553089743420633, + "timestamp": 0.9505456140504814 + }, + { + "x": 2.8796798410785964, + "y": 2.675632812236456, + "heading": -1.7970667738519104e-7, + "angularVelocity": 4.896277678133068e-7, + "velocityX": 3.1094284661883007, + "velocityY": -2.6553092419897357, + "timestamp": 1.0099547149286365 + }, + { + "x": 3.0644080904838464, + "y": 2.5178831604959693, + "heading": -1.5061833606690155e-7, + "angularVelocity": 4.896276982453926e-7, + "velocityX": 3.109426782675891, + "velocityY": -2.655311213412356, + "timestamp": 1.0693638158067915 + }, + { + "x": 3.2491359906412702, + "y": 2.360133099780758, + "heading": -1.215300088943934e-7, + "angularVelocity": 4.896274601374268e-7, + "velocityX": 3.109420903984017, + "velocityY": -2.6553180974534567, + "timestamp": 1.1287729166849465 + }, + { + "x": 3.4338955523253594, + "y": 2.202420122735163, + "heading": -9.244031535060432e-8, + "angularVelocity": 4.896504594972081e-7, + "velocityX": 3.1099538446664665, + "velocityY": -2.654693888886324, + "timestamp": 1.1881820175631015 + }, + { + "x": 3.6212771735981955, + "y": 2.0478316149306797, + "heading": -6.319311636602291e-8, + "angularVelocity": 4.923016600450938e-7, + "velocityX": 3.1540894998099027, + "velocityY": -2.6021014544794228, + "timestamp": 1.2475911184412565 + }, + { + "x": 3.820400967204548, + "y": 1.908694296792687, + "heading": -3.281153410671686e-8, + "angularVelocity": 5.113960960484033e-7, + "velocityX": 3.3517388861820665, + "velocityY": -2.342020264256331, + "timestamp": 1.3070002193194115 + }, + { + "x": 4.03019905090332, + "y": 1.7862433195114136, + "heading": -4.8954376995112275e-19, + "angularVelocity": 5.522981095615313e-7, + "velocityX": 3.5314132110677283, + "velocityY": -2.0611484683535886, + "timestamp": 1.3664093201975664 + }, + { + "x": 4.322951930029737, + "y": 1.6552955500710258, + "heading": 5.175704079338812e-15, + "angularVelocity": 6.599529748424669e-14, + "velocityX": 3.7325329660882653, + "velocityY": -1.669554430106589, + "timestamp": 1.4448420846999053 + }, + { + "x": 4.628082081291311, + "y": 1.556567853907556, + "heading": 5.190662725666785e-15, + "angularVelocity": 1.9071943375934453e-16, + "velocityX": 3.8903403851384977, + "velocityY": -1.2587557864383778, + "timestamp": 1.523274849202244 + }, + { + "x": 4.938074644435486, + "y": 1.4743718801236945, + "heading": 5.169631100164441e-15, + "angularVelocity": -2.681484041715186e-16, + "velocityX": 3.952335036397641, + "velocityY": -1.0479800668204275, + "timestamp": 1.601707613704583 + }, + { + "x": 5.248064134540723, + "y": 1.3921643177396372, + "heading": 5.1832756916526305e-15, + "angularVelocity": 1.7396546747950823e-16, + "velocityX": 3.952295855847418, + "velocityY": -1.0481278188474166, + "timestamp": 1.6801403782069217 + }, + { + "x": 5.558053461013338, + "y": 1.3099561383310128, + "heading": 5.1652975147552064e-15, + "angularVelocity": -2.2921768707255205e-16, + "velocityX": 3.9522937695684903, + "velocityY": -1.0481356857710855, + "timestamp": 1.7585731427092606 + }, + { + "x": 5.868042788859202, + "y": 1.227747964100929, + "heading": 5.17893915410426e-15, + "angularVelocity": 1.7392781023474698e-16, + "velocityX": 3.952293787077097, + "velocityY": -1.0481356197458775, + "timestamp": 1.8370059072115994 + }, + { + "x": 6.1780321171916235, + "y": 1.1455397917056027, + "heading": 5.203641884469278e-15, + "angularVelocity": 3.1495423062461055e-16, + "velocityX": 3.95229379328059, + "velocityY": -1.0481355963531385, + "timestamp": 1.9154386717139382 + }, + { + "x": 6.488021446386602, + "y": 1.0633316225647949, + "heading": 5.179220620544195e-15, + "angularVelocity": -3.1136561843441626e-16, + "velocityX": 3.952293804278011, + "velocityY": -1.048135554858763, + "timestamp": 1.993871436216277 + }, + { + "x": 6.788390273210204, + "y": 0.9836747790194914, + "heading": 1.4385064984300214e-14, + "angularVelocity": 1.1737244138358137e-13, + "velocityX": 3.8296345759259354, + "velocityY": -1.015606730818312, + "timestamp": 2.072304200718616 + }, + { + "x": 7.055384812513254, + "y": 0.9128686882327177, + "heading": 1.5703193582509516e-14, + "angularVelocity": 1.680584250857847e-14, + "velocityX": 3.40411996181899, + "velocityY": -0.9027616358546965, + "timestamp": 2.150736965220955 + }, + { + "x": 7.289005044919871, + "y": 0.8509133557721549, + "heading": 1.4172123019917492e-14, + "angularVelocity": -1.9520804221347698e-14, + "velocityX": 2.97860510067396, + "velocityY": -0.7899164699044072, + "timestamp": 2.229169729723294 + }, + { + "x": 7.48925096397141, + "y": 0.7978087834938102, + "heading": 1.1452140462826918e-14, + "angularVelocity": -3.4679163908121556e-14, + "velocityX": 2.5530901571826567, + "velocityY": -0.6770712802904464, + "timestamp": 2.307602494225633 + }, + { + "x": 7.656122566438546, + "y": 0.7535549723257088, + "heading": 8.361039725202098e-15, + "angularVelocity": -3.9410836943919146e-14, + "velocityX": 2.127575172518181, + "velocityY": -0.5642260788443735, + "timestamp": 2.386035258727972 + }, + { + "x": 7.789619850383681, + "y": 0.7181519228246735, + "heading": 5.39075057643342e-15, + "angularVelocity": -3.787051642035781e-14, + "velocityX": 1.7020601631497934, + "velocityY": -0.4513808702989361, + "timestamp": 2.464468023230311 + }, + { + "x": 7.889742814515085, + "y": 0.6915996353619231, + "heading": 2.8547195474114465e-15, + "angularVelocity": -3.233382334683438e-14, + "velocityX": 1.2765451373121304, + "velocityY": -0.33853565702054167, + "timestamp": 2.54290078773265 + }, + { + "x": 7.9564914579100945, + "y": 0.6738981102026166, + "heading": 1.001573018444963e-15, + "angularVelocity": -2.3627200058002107e-14, + "velocityX": 0.8510300997107006, + "velocityY": -0.22569044036143487, + "timestamp": 2.621333552234989 + }, + { + "x": 7.989865779876709, + "y": 0.6650473475456241, + "heading": -9.790956081407498e-26, + "angularVelocity": -1.2769829956117919e-14, + "velocityX": 0.4255150532864473, + "velocityY": -0.11284522116677391, + "timestamp": 2.6997663167373283 + }, + { + "x": 7.989865779876709, + "y": 0.665047347545624, + "heading": -4.586051220974463e-26, + "angularVelocity": 2.5919631848423095e-26, + "velocityX": -1.4783880366896059e-18, + "velocityY": 2.7911510207865606e-17, + "timestamp": 2.7781990812396673 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Skip.2.traj b/src/main/deploy/choreo/4NB-Skip.2.traj new file mode 100644 index 0000000..2675b80 --- /dev/null +++ b/src/main/deploy/choreo/4NB-Skip.2.traj @@ -0,0 +1,292 @@ +{ + "samples": [ + { + "x": 7.989865779876709, + "y": 0.665047347545624, + "heading": -4.586051220974463e-26, + "angularVelocity": 2.5919631848423095e-26, + "velocityX": -1.4783880366896059e-18, + "velocityY": 2.7911510207865606e-17, + "timestamp": 0 + }, + { + "x": 7.961825021408631, + "y": 0.6653260998514925, + "heading": -3.8642001004622653e-17, + "angularVelocity": -5.466910762418722e-16, + "velocityX": -0.39670913756880827, + "velocityY": 0.003943673170767399, + "timestamp": 0.07068341969616165 + }, + { + "x": 7.905743504974589, + "y": 0.6658836070474405, + "heading": -1.1420692112030604e-16, + "angularVelocity": -1.0690612857146226e-15, + "velocityX": -0.7934182680338816, + "velocityY": 0.007887382901887431, + "timestamp": 0.1413668393923233 + }, + { + "x": 7.821621231202305, + "y": 0.6667198723730994, + "heading": -2.271131022785688e-16, + "angularVelocity": -1.597350021728402e-15, + "velocityX": -1.190127389618255, + "velocityY": 0.011831138465963963, + "timestamp": 0.21205025908848496 + }, + { + "x": 7.709458200898961, + "y": 0.6678349000090077, + "heading": -3.725115104027876e-16, + "angularVelocity": -2.0570366115956903e-15, + "velocityX": -1.586836499782901, + "velocityY": 0.015774953174573097, + "timestamp": 0.2827336787846466 + }, + { + "x": 7.569254415141015, + "y": 0.6692286955560068, + "heading": -5.529032952361946e-16, + "angularVelocity": -2.5521084387850704e-15, + "velocityX": -1.983545594718296, + "velocityY": 0.019718847121565148, + "timestamp": 0.35341709848080827 + }, + { + "x": 7.4010098754359035, + "y": 0.6709012669067533, + "heading": -7.630982661410159e-16, + "angularVelocity": -2.9737515734334796e-15, + "velocityX": -2.3802546683270744, + "velocityY": 0.02366285273059252, + "timestamp": 0.4241005181769699 + }, + { + "x": 7.204724584045661, + "y": 0.6728526260129781, + "heading": -1.0096410558455573e-15, + "angularVelocity": -3.487985554177256e-15, + "velocityX": -2.7769637099335265, + "velocityY": 0.0276070274277582, + "timestamp": 0.4947839378731316 + }, + { + "x": 6.980398544742687, + "y": 0.6750827930954573, + "heading": -1.423077463628514e-15, + "angularVelocity": -5.849127749576447e-15, + "velocityX": -3.173672698169647, + "velocityY": 0.03155148820008718, + "timestamp": 0.5654673575692932 + }, + { + "x": 6.7280317650805745, + "y": 0.6775918097291393, + "heading": -1.9851623705937264e-15, + "angularVelocity": -7.952145739517459e-15, + "velocityX": -3.5703815795405807, + "velocityY": 0.03549653715757366, + "timestamp": 0.6361507772654549 + }, + { + "x": 6.447624267790984, + "y": 0.6803798097800222, + "heading": -6.968574786204946e-17, + "angularVelocity": 2.7099372702904392e-14, + "velocityX": -3.96709013931336, + "velocityY": 0.03944347999579872, + "timestamp": 0.7068341969616165 + }, + { + "x": 6.158924341772469, + "y": 0.693943817311479, + "heading": 9.565698789607162e-18, + "angularVelocity": 1.1212169263035645e-15, + "velocityX": -4.084408016187073, + "velocityY": 0.19189800931673312, + "timestamp": 0.7775176166577782 + }, + { + "x": 5.872883545252504, + "y": 0.7353236661281272, + "heading": 6.282540250170779e-18, + "angularVelocity": -4.644877904443354e-17, + "velocityX": -4.046787743851827, + "velocityY": 0.58542511093175, + "timestamp": 0.8482010363539398 + }, + { + "x": 5.592199247715388, + "y": 0.8042290602455044, + "heading": 2.5226905744349242e-17, + "angularVelocity": 2.6801710459780227e-16, + "velocityX": -3.971006195564076, + "velocityY": 0.9748452241499008, + "timestamp": 0.9188844560501015 + }, + { + "x": 5.31951379776001, + "y": 0.9000114798545834, + "heading": 1.0213561793054143e-23, + "angularVelocity": -3.568997601327789e-16, + "velocityX": -3.8578417842194788, + "velocityY": 1.3550903453853254, + "timestamp": 0.9895678757462631 + }, + { + "x": 5.028358906099587, + "y": 1.039292217399747, + "heading": 1.939930717479169e-17, + "angularVelocity": 2.45765977450937e-16, + "velocityX": -3.688587353353505, + "velocityY": 1.7645218465846235, + "timestamp": 1.0685018588050408 + }, + { + "x": 4.753982028496478, + "y": 1.2092558179345199, + "heading": 5.251717131242591e-17, + "angularVelocity": 4.1956390743611424e-16, + "velocityX": -3.476029803269934, + "velocityY": 2.1532373503589888, + "timestamp": 1.1474358418638184 + }, + { + "x": 4.499604125223824, + "y": 1.407906716987468, + "heading": 4.7661466331843347e-17, + "angularVelocity": -6.151615583577767e-17, + "velocityX": -3.222666504529982, + "velocityY": 2.5166714177468545, + "timestamp": 1.226369824922596 + }, + { + "x": 4.26534834796377, + "y": 1.629930428449429, + "heading": 6.383046336001005e-17, + "angularVelocity": 2.0484189779951308e-16, + "velocityX": -2.9677430199566506, + "velocityY": 2.8127772457221822, + "timestamp": 1.3053038079813737 + }, + { + "x": 4.031326138540414, + "y": 1.852200317188001, + "heading": 2.3677757755478083e-17, + "angularVelocity": -5.086873150258546e-16, + "velocityX": -2.964783992333329, + "velocityY": 2.815896020002203, + "timestamp": 1.3842377910401513 + }, + { + "x": 3.797312456863741, + "y": 2.0744791835519405, + "heading": 4.512669218284662e-17, + "angularVelocity": 2.717323991439214e-16, + "velocityX": -2.9646759558849944, + "velocityY": 2.816009755880509, + "timestamp": 1.463171774098929 + }, + { + "x": 3.563297360395116, + "y": 2.29675656041435, + "heading": 2.2571410949696857e-18, + "angularVelocity": -5.431063958209394e-16, + "velocityX": -2.964693879622175, + "velocityY": 2.815990885661535, + "timestamp": 1.5421057571577066 + }, + { + "x": 3.335095888371744, + "y": 2.5135119589023023, + "heading": -5.89134026984256e-14, + "angularVelocity": -7.463915767059434e-13, + "velocityX": -2.8910421491511853, + "velocityY": 2.7460339652011654, + "timestamp": 1.6210397402164842 + }, + { + "x": 3.1322501152043865, + "y": 2.706183443191518, + "heading": -7.201295573540544e-14, + "angularVelocity": -1.6595581004068512e-13, + "velocityX": -2.569815500330909, + "velocityY": 2.440919320463041, + "timestamp": 1.6999737232752619 + }, + { + "x": 2.9547600556706652, + "y": 2.8747709994570525, + "heading": -6.793308622264907e-14, + "angularVelocity": 5.168711075034906e-14, + "velocityX": -2.2485886642956747, + "velocityY": 2.1358045005792574, + "timestamp": 1.7789077063340395 + }, + { + "x": 2.802625714692311, + "y": 3.019274623086207, + "heading": -5.6227190966394764e-14, + "angularVelocity": 1.482998177559053e-13, + "velocityX": -1.927361765907952, + "velocityY": 1.830689622258048, + "timestamp": 1.8578416893928171 + }, + { + "x": 2.6758470947294035, + "y": 3.1396943117718017, + "heading": -4.1672293686354096e-14, + "angularVelocity": 1.843932964768755e-13, + "velocityX": -1.6061348363539316, + "velocityY": 1.5255747147076162, + "timestamp": 1.9367756724515948 + }, + { + "x": 2.57442419725774, + "y": 3.236030064129265, + "heading": -2.7119123795716724e-14, + "angularVelocity": 1.8437141252050512e-13, + "velocityX": -1.2849078881033131, + "velocityY": 1.2204597896162928, + "timestamp": 2.0157096555103724 + }, + { + "x": 2.498357023261081, + "y": 3.308281879235439, + "heading": -1.4470412940019133e-14, + "angularVelocity": 1.6024417392647582e-13, + "velocityX": -0.9636809273896104, + "velocityY": 0.9153448528296537, + "timestamp": 2.09464363856915 + }, + { + "x": 2.4476455734420637, + "y": 3.356449756430872, + "heading": -5.093331671182619e-15, + "angularVelocity": 1.1879650495017822e-13, + "velocityX": -0.6424539577743393, + "velocityY": 0.6102299076885483, + "timestamp": 2.1735776216279277 + }, + { + "x": 2.422289848327637, + "y": 3.3805336952209477, + "heading": 8.600144701827338e-26, + "angularVelocity": 6.45264751057182e-14, + "velocityX": -0.3212269814832313, + "velocityY": 0.30511495628123525, + "timestamp": 2.2525116046867053 + }, + { + "x": 2.422289848327637, + "y": 3.3805336952209477, + "heading": 4.234321675050891e-26, + "angularVelocity": -1.6322812413801202e-26, + "velocityX": 1.82145222049907e-18, + "velocityY": 1.2398127508278904e-17, + "timestamp": 2.331445587745483 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Skip.3.traj b/src/main/deploy/choreo/4NB-Skip.3.traj new file mode 100644 index 0000000..b859871 --- /dev/null +++ b/src/main/deploy/choreo/4NB-Skip.3.traj @@ -0,0 +1,292 @@ +{ + "samples": [ + { + "x": 2.422289848327637, + "y": 3.3805336952209477, + "heading": 4.234321675050891e-26, + "angularVelocity": -1.6322812413801202e-26, + "velocityX": 1.82145222049907e-18, + "velocityY": 1.2398127508278904e-17, + "timestamp": 0 + }, + { + "x": 2.4422092097338126, + "y": 3.359397040789022, + "heading": 1.4828318603888107e-18, + "angularVelocity": 2.061358359803718e-17, + "velocityX": 0.2769090563806016, + "velocityY": -0.29383125866924514, + "timestamp": 0.07193466933297632 + }, + { + "x": 2.4821232633957244, + "y": 3.317194979333885, + "heading": -2.399084815386753e-18, + "angularVelocity": -5.3964448898673995e-17, + "velocityX": 0.5548653247699834, + "velocityY": -0.5866720712936807, + "timestamp": 0.14386933866595264 + }, + { + "x": 2.5421257730353313, + "y": 3.2540169079394587, + "heading": -4.312212643467297e-18, + "angularVelocity": -2.6595363961942612e-17, + "velocityX": 0.8341250497936068, + "velocityY": -0.878270130114354, + "timestamp": 0.21580400799892896 + }, + { + "x": 2.6223366317399224, + "y": 3.169978308273709, + "heading": -6.87359010796808e-18, + "angularVelocity": -3.560699654803604e-17, + "velocityX": 1.1150514688942472, + "velocityY": -1.1682628202156935, + "timestamp": 0.2877386773319053 + }, + { + "x": 2.722914524907397, + "y": 3.0652340847546355, + "heading": -1.0050987029108054e-17, + "angularVelocity": -4.417054911796212e-17, + "velocityX": 1.3981838534826794, + "velocityY": -1.4561021061315331, + "timestamp": 0.3596733466648816 + }, + { + "x": 2.844079320063175, + "y": 2.94000283277744, + "heading": -1.3800277943608368e-17, + "angularVelocity": -5.212076319266894e-17, + "velocityX": 1.6843727270700168, + "velocityY": -1.7409025875701625, + "timestamp": 0.4316080159978579 + }, + { + "x": 2.9861556801651536, + "y": 2.7946160556206303, + "heading": -1.8054024423421994e-17, + "angularVelocity": -5.913346837366306e-17, + "velocityX": 1.9750749036440556, + "velocityY": -2.0210946752866197, + "timestamp": 0.5035426853308342 + }, + { + "x": 3.1496705290943545, + "y": 2.629635153477409, + "heading": -1.9755148692229212e-17, + "angularVelocity": -2.3648200854179725e-17, + "velocityX": 2.2731021139742653, + "velocityY": -2.2934824566998935, + "timestamp": 0.5754773546638106 + }, + { + "x": 3.3356217659937375, + "y": 2.446211186910791, + "heading": -2.1620491181128583e-17, + "angularVelocity": -2.5931074864358486e-17, + "velocityX": 2.5850016219372725, + "velocityY": -2.5498687665852504, + "timestamp": 0.6474120239967869 + }, + { + "x": 3.546522888895758, + "y": 2.247919800418475, + "heading": -2.298290975372596e-17, + "angularVelocity": -1.8939700380288258e-17, + "velocityX": 2.931842529590256, + "velocityY": -2.756548244821444, + "timestamp": 0.7193466933297632 + }, + { + "x": 3.779551655861922, + "y": 2.0684406150868386, + "heading": -2.356725503120944e-17, + "angularVelocity": -8.123277452624726e-18, + "velocityX": 3.2394500333071203, + "velocityY": -2.495030379582669, + "timestamp": 0.7912813626627395 + }, + { + "x": 4.029145048692618, + "y": 1.912818243294336, + "heading": -7.243904171356102e-18, + "angularVelocity": 2.2691910606172613e-16, + "velocityX": 3.4697232244967258, + "velocityY": -2.163384821739647, + "timestamp": 0.8632160319957158 + }, + { + "x": 4.292869509297096, + "y": 1.7825700534328885, + "heading": 2.7690024039408224e-18, + "angularVelocity": 1.391944477977289e-16, + "velocityX": 3.666166301315592, + "velocityY": -1.8106455630985843, + "timestamp": 0.9351507013286922 + }, + { + "x": 4.568153688292314, + "y": 1.6789660022880506, + "heading": 1.6169391405873456e-17, + "angularVelocity": 1.8628554390059454e-16, + "velocityX": 3.8268637577418185, + "velocityY": -1.4402519967735667, + "timestamp": 1.0070853706616685 + }, + { + "x": 4.85231351852417, + "y": 1.6030162572860713, + "heading": 1.104418666333468e-28, + "angularVelocity": -2.2477883829523805e-16, + "velocityX": 3.9502486473765894, + "velocityY": -1.055815585244564, + "timestamp": 1.0790200399946448 + }, + { + "x": 5.158309378699811, + "y": 1.5545385083826373, + "heading": -1.1441689912277495e-17, + "angularVelocity": -1.5100789546254797e-16, + "velocityX": 4.038545985692146, + "velocityY": -0.6398113298648395, + "timestamp": 1.1547888590843929 + }, + { + "x": 5.467685385105672, + "y": 1.5381052997754503, + "heading": 2.911311456658326e-17, + "angularVelocity": 5.352439824969174e-16, + "velocityX": 4.083157295078998, + "velocityY": -0.21688616511744238, + "timestamp": 1.230557678174141 + }, + { + "x": 5.777094938781223, + "y": 1.553894378826709, + "heading": -2.420664700580125e-18, + "angularVelocity": -4.1618411961572785e-16, + "velocityX": 4.083600053329194, + "velocityY": 0.2083849166488678, + "timestamp": 1.306326497263889 + }, + { + "x": 6.083191086572635, + "y": 1.6017349192235426, + "heading": -5.882315170795842e-18, + "angularVelocity": -4.568700570848447e-17, + "velocityX": 4.039869585784915, + "velocityY": 0.6314014256909969, + "timestamp": 1.3820953163536371 + }, + { + "x": 6.382662742491119, + "y": 1.6811093330008764, + "heading": -1.6858060845261797e-17, + "angularVelocity": -1.4485834418926946e-16, + "velocityX": 3.9524392687700143, + "velocityY": 1.0475867874182512, + "timestamp": 1.4578641354433852 + }, + { + "x": 6.672275412037942, + "y": 1.7911460204151195, + "heading": -2.888480028332242e-17, + "angularVelocity": -1.587294032373541e-16, + "velocityX": 3.8223199599175173, + "velocityY": 1.452268739806397, + "timestamp": 1.5336329545331333 + }, + { + "x": 6.935773457086466, + "y": 1.9122631966744312, + "heading": -2.1691436993771164e-17, + "angularVelocity": 9.493832977551161e-17, + "velocityX": 3.4776580684956064, + "velocityY": 1.5985094886787496, + "timestamp": 1.6094017736228814 + }, + { + "x": 7.170007295221654, + "y": 2.0198939541094463, + "heading": -1.6299807800034943e-17, + "angularVelocity": 7.115894340654352e-17, + "velocityX": 3.0914278584382107, + "velocityY": 1.4205151766725412, + "timestamp": 1.6851705927126295 + }, + { + "x": 7.374967182869676, + "y": 2.1140594081505015, + "heading": -1.0901160029750536e-17, + "angularVelocity": 7.125157266111349e-17, + "velocityX": 2.7050690522873455, + "velocityY": 1.2427995469958841, + "timestamp": 1.7609394118023776 + }, + { + "x": 7.550649876686179, + "y": 2.1947665963601173, + "heading": -8.565573749391565e-18, + "angularVelocity": 3.0825158228438804e-17, + "velocityX": 2.318667440342544, + "velocityY": 1.0651767993626458, + "timestamp": 1.8367082308921256 + }, + { + "x": 7.69705375584567, + "y": 2.26201903734599, + "heading": -6.268442595471868e-18, + "angularVelocity": 3.0317623225845786e-17, + "velocityX": 1.932244436675357, + "velocityY": 0.8876004904604238, + "timestamp": 1.9124770499818737 + }, + { + "x": 7.814177848123867, + "y": 2.315818842216871, + "heading": -5.860377826964177e-18, + "angularVelocity": 5.385632685743263e-18, + "velocityX": 1.5458086015515728, + "velocityY": 0.7100520440594869, + "timestamp": 1.9882458690716218 + }, + { + "x": 7.902021505484193, + "y": 2.3561674183552324, + "heading": -2.0084875089379725e-18, + "angularVelocity": 5.083743950397372e-17, + "velocityX": 1.1593642136140294, + "velocityY": 0.5325221723510412, + "timestamp": 2.06401468816137 + }, + { + "x": 7.960584265098705, + "y": 2.3830657710227903, + "heading": -5.354322583341586e-20, + "angularVelocity": 2.5801441111297213e-17, + "velocityX": 0.772913717254023, + "velocityY": 0.35500556812864675, + "timestamp": 2.139783507251118 + }, + { + "x": 7.989865779876708, + "y": 2.3965146541595463, + "heading": 4.464057669964841e-25, + "angularVelocity": 7.066509963979535e-19, + "velocityX": 0.3864586399765603, + "velocityY": 0.17749891443779842, + "timestamp": 2.215552326340866 + }, + { + "x": 7.989865779876708, + "y": 2.3965146541595463, + "heading": 4.577698718092902e-25, + "angularVelocity": 1.4924495313995464e-25, + "velocityX": 1.7316501925858062e-16, + "velocityY": 2.4957312477014177e-17, + "timestamp": 2.291321145430614 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Skip.4.traj b/src/main/deploy/choreo/4NB-Skip.4.traj new file mode 100644 index 0000000..8a3d7ad --- /dev/null +++ b/src/main/deploy/choreo/4NB-Skip.4.traj @@ -0,0 +1,310 @@ +{ + "samples": [ + { + "x": 7.989865779876708, + "y": 2.3965146541595463, + "heading": 4.577698718092902e-25, + "angularVelocity": 1.4924495313995464e-25, + "velocityX": 1.7316501925858062e-16, + "velocityY": 2.4957312477014177e-17, + "timestamp": 0 + }, + { + "x": 7.961948125314342, + "y": 2.3869413231673744, + "heading": 3.968152450731581e-15, + "angularVelocity": 5.4722575152696526e-14, + "velocityX": -0.38499664874452716, + "velocityY": -0.1320204152917069, + "timestamp": 0.07251401967632276 + }, + { + "x": 7.9061128172497455, + "y": 2.3677946595682435, + "heading": 1.1360635138080996e-14, + "angularVelocity": 1.0194560175538186e-13, + "velocityX": -0.7699932828692636, + "velocityY": -0.26404085285209106, + "timestamp": 0.14502803935264552 + }, + { + "x": 7.822359857005323, + "y": 2.3390746613517033, + "heading": 2.1504602727030266e-14, + "angularVelocity": 1.3988979395557398e-13, + "velocityX": -1.154989898757494, + "velocityY": -0.3960613181375172, + "timestamp": 0.21754205902896828 + }, + { + "x": 7.71068924627678, + "y": 2.300781325945998, + "heading": 3.351912896052837e-14, + "angularVelocity": 1.6568562398720052e-13, + "velocityX": -1.539986491261158, + "velocityY": -0.5280818188886689, + "timestamp": 0.29005607870529104 + }, + { + "x": 7.571100987317153, + "y": 2.2529146499450325, + "heading": 4.62437330221949e-14, + "angularVelocity": 1.754779109699489e-13, + "velocityX": -1.9249830526944358, + "velocityY": -0.6601023666114509, + "timestamp": 0.3625700983816138 + }, + { + "x": 7.4035950832655315, + "y": 2.195474628624213, + "heading": 5.80518545778972e-14, + "angularVelocity": 1.628392124731043e-13, + "velocityX": -2.30997957083824, + "velocityY": -0.7921229794885999, + "timestamp": 0.43508411805793656 + }, + { + "x": 7.20817153879745, + "y": 2.1284612549925805, + "heading": 6.651768983836094e-14, + "angularVelocity": 1.1674760820877797e-13, + "velocityX": -2.6949760245043035, + "velocityY": -0.9241436887743528, + "timestamp": 0.5075981377342593 + }, + { + "x": 6.9848303616174565, + "y": 2.0518745176536126, + "heading": 6.760398615570593e-14, + "angularVelocity": 1.498050653761286e-14, + "velocityX": -3.0799723719211687, + "velocityY": -1.0561645552234988, + "timestamp": 0.5801121574105821 + }, + { + "x": 6.733571566806331, + "y": 1.965714394765822, + "heading": 5.327081908950384e-14, + "angularVelocity": -1.9766070480015549e-13, + "velocityX": -3.464968511367217, + "velocityY": -1.188185722877021, + "timestamp": 0.6526261770869048 + }, + { + "x": 6.454395197014029, + "y": 1.8699808283188497, + "heading": -1.210358626215953e-16, + "angularVelocity": -7.362972657145883e-13, + "velocityX": -3.849964062651033, + "velocityY": -1.3202076905209181, + "timestamp": 0.7251401967632276 + }, + { + "x": 6.1738479950442935, + "y": 1.7740244527442883, + "heading": -1.1104225188815767e-16, + "angularVelocity": 1.3781627642234675e-16, + "velocityX": -3.868868437050859, + "velocityY": -1.3232803257055905, + "timestamp": 0.7976542164395504 + }, + { + "x": 5.89229592505603, + "y": 1.6810578437397081, + "heading": -1.3815766244496281e-16, + "angularVelocity": -3.7393339475245444e-16, + "velocityX": -3.882726005881283, + "velocityY": -1.2820501389883114, + "timestamp": 0.8701682361158731 + }, + { + "x": 5.603047274828762, + "y": 1.615869067560432, + "heading": -1.375103171712517e-16, + "angularVelocity": 8.927167451460251e-18, + "velocityX": -3.9888652086644605, + "velocityY": -0.8989816930592814, + "timestamp": 0.9426822557921959 + }, + { + "x": 5.3087508481141255, + "y": 1.5797585348826493, + "heading": -5.115064325941328e-17, + "angularVelocity": 1.1909376120637012e-15, + "velocityX": -4.058476250913794, + "velocityY": -0.49798001598624214, + "timestamp": 1.0151962754685186 + }, + { + "x": 5.012322426011832, + "y": 1.5730842246634098, + "heading": -1.6843428292277085e-17, + "angularVelocity": 4.731114706232749e-16, + "velocityX": -4.087877398404444, + "velocityY": -0.09204165275748097, + "timestamp": 1.0877102951448414 + }, + { + "x": 4.716698964119128, + "y": 1.5959123273167983, + "heading": 1.0780704155043204e-17, + "angularVelocity": 3.8094884400946135e-16, + "velocityX": -4.076776645568003, + "velocityY": 0.3148095051884876, + "timestamp": 1.1602243148211642 + }, + { + "x": 4.424809455871582, + "y": 1.6480166912078853, + "heading": -4.7825786100317035e-25, + "angularVelocity": -1.4867063166908422e-16, + "velocityX": -4.025283794091286, + "velocityY": 0.7185419333227538, + "timestamp": 1.232738334497487 + }, + { + "x": 4.128740207867581, + "y": 1.7331899442926113, + "heading": 2.436084377884843e-17, + "angularVelocity": 3.233261816100211e-16, + "velocityX": -3.929541129144622, + "velocityY": 1.1304510797896916, + "timestamp": 1.308082818312192 + }, + { + "x": 3.843051522185569, + "y": 1.8484872151278047, + "heading": 4.0057524127252557e-17, + "angularVelocity": 2.0833224449518998e-16, + "velocityX": -3.7917664468268093, + "velocityY": 1.5302682425770875, + "timestamp": 1.383427302126897 + }, + { + "x": 3.5707992295594733, + "y": 1.992675206864091, + "heading": 4.3264327263683456e-17, + "angularVelocity": 4.256188342181214e-17, + "velocityX": -3.6134336429424168, + "velocityY": 1.9137166310769615, + "timestamp": 1.458771785941602 + }, + { + "x": 3.314895397994771, + "y": 2.1642115529138155, + "heading": 5.0710064414841386e-17, + "angularVelocity": 9.88225915723796e-17, + "velocityX": -3.39645078987007, + "velocityY": 2.2766941568200596, + "timestamp": 1.534116269756307 + }, + { + "x": 3.078073502264168, + "y": 2.361256926788227, + "heading": 7.635756849280407e-17, + "angularVelocity": 3.4040320908383547e-16, + "velocityX": -3.143188243401714, + "velocityY": 2.615259457584993, + "timestamp": 1.6094607535710121 + }, + { + "x": 2.854646488956178, + "y": 2.573369772957938, + "heading": 6.019554858067844e-17, + "angularVelocity": -2.1450833681506433e-16, + "velocityX": -2.9654063840756333, + "velocityY": 2.815240551521766, + "timestamp": 1.6848052373857172 + }, + { + "x": 2.6466740813199228, + "y": 2.7708029552314177, + "heading": 4.0534261647582457e-17, + "angularVelocity": -2.6095184862579493e-16, + "velocityX": -2.7602871120227874, + "velocityY": 2.620406594855673, + "timestamp": 1.7601497212004222 + }, + { + "x": 2.4618096495885284, + "y": 2.946299083382676, + "heading": 1.8651869909581232e-17, + "angularVelocity": -2.9043121070808416e-16, + "velocityX": -2.4535894649644563, + "velocityY": 2.329249857008171, + "timestamp": 1.8354942050151273 + }, + { + "x": 2.3000532444302113, + "y": 3.0998581821870363, + "heading": 1.2585166550915731e-17, + "angularVelocity": -8.051961119619559e-17, + "velocityX": -2.1468911454186292, + "velocityY": 2.0380934479880564, + "timestamp": 1.9108386888298323 + }, + { + "x": 2.1614048826816776, + "y": 3.2314802598444294, + "heading": 1.044220916189497e-17, + "angularVelocity": -2.8442168781856105e-17, + "velocityX": -1.8401926024097508, + "velocityY": 1.7469371478004907, + "timestamp": 1.9861831726445374 + }, + { + "x": 2.04586457275109, + "y": 3.341165320443523, + "heading": 6.144588170756101e-18, + "angularVelocity": -5.703966005365595e-17, + "velocityX": -1.5334939478045977, + "velocityY": 1.4557809018792693, + "timestamp": 2.0615276564592424 + }, + { + "x": 1.9534323196800467, + "y": 3.4289133664338625, + "heading": 8.956175204831333e-18, + "angularVelocity": 3.7316381823207725e-17, + "velocityX": -1.2267952262854738, + "velocityY": 1.1646246884693368, + "timestamp": 2.1368721402739475 + }, + { + "x": 1.8841081268282298, + "y": 3.4947243994469472, + "heading": 8.125508496541916e-18, + "angularVelocity": -1.1024919871707434e-17, + "velocityX": -0.920096460175393, + "velocityY": 0.8734684967132663, + "timestamp": 2.2122166240886525 + }, + { + "x": 1.8378919965947327, + "y": 3.538598420647381, + "heading": 4.6134522575515235e-18, + "angularVelocity": -4.661329437022688e-17, + "velocityX": -0.6133976622236478, + "velocityY": 0.5823123204142531, + "timestamp": 2.2875611079033575 + }, + { + "x": 1.8147839307785034, + "y": 3.5605354309082036, + "heading": 2.574108918698912e-28, + "angularVelocity": -6.123143521392433e-17, + "velocityX": -0.3066988403955926, + "velocityY": 0.2911561557025646, + "timestamp": 2.3629055917180626 + }, + { + "x": 1.8147839307785032, + "y": 3.560535430908203, + "heading": 1.6039214960839928e-28, + "angularVelocity": -4.957933854028128e-29, + "velocityX": -1.2239411035355163e-16, + "velocityY": -5.776889799901916e-17, + "timestamp": 2.4382500755327676 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Skip.5.traj b/src/main/deploy/choreo/4NB-Skip.5.traj new file mode 100644 index 0000000..4958216 --- /dev/null +++ b/src/main/deploy/choreo/4NB-Skip.5.traj @@ -0,0 +1,85 @@ +{ + "samples": [ + { + "x": 1.8147839307785032, + "y": 3.560535430908203, + "heading": 1.6039214960839928e-28, + "angularVelocity": -4.957933854028128e-29, + "velocityX": -1.2239411035355163e-16, + "velocityY": -5.776889799901916e-17, + "timestamp": 0 + }, + { + "x": 1.8499405192639373, + "y": 3.595692011943056, + "heading": 7.674497632021469e-18, + "angularVelocity": 8.154121514620013e-17, + "velocityX": 0.3735372767099104, + "velocityY": 0.37353719754779685, + "timestamp": 0.0941180189433588 + }, + { + "x": 1.9202536945231747, + "y": 3.666005172301131, + "heading": 1.2913126269895033e-17, + "angularVelocity": 5.566020934800391e-17, + "velocityX": 0.7470745352338143, + "velocityY": 0.7470743769095909, + "timestamp": 0.1882360378867176 + }, + { + "x": 2.02572345256241, + "y": 3.771474907988624, + "heading": 1.5083295615484505e-17, + "angularVelocity": 2.3057958187881123e-17, + "velocityX": 1.1206117513237068, + "velocityY": 1.1206115138373827, + "timestamp": 0.2823540568300764 + }, + { + "x": 2.1663497734126267, + "y": 3.912101199036522, + "heading": 1.7930576325828894e-17, + "angularVelocity": 3.0252238011837634e-17, + "velocityX": 1.4941487552436352, + "velocityY": 1.494148438595255, + "timestamp": 0.3764720757734352 + }, + { + "x": 2.2718195314518623, + "y": 4.017570934724015, + "heading": 1.2801642358185404e-17, + "angularVelocity": -5.44947080834505e-17, + "velocityX": 1.120611751323707, + "velocityY": 1.1206115138373829, + "timestamp": 0.470590094716794 + }, + { + "x": 2.3421327067110993, + "y": 4.0878840950820905, + "heading": 5.291529814049623e-18, + "angularVelocity": -7.979463049177405e-17, + "velocityX": 0.7470745352338144, + "velocityY": 0.747074376909591, + "timestamp": 0.5647081136601528 + }, + { + "x": 2.377289295196533, + "y": 4.123040676116943, + "heading": -1.3564749018286617e-28, + "angularVelocity": -5.62222821268349e-17, + "velocityX": 0.37353727670991055, + "velocityY": 0.3735371975477969, + "timestamp": 0.6588261326035116 + }, + { + "x": 2.377289295196533, + "y": 4.123040676116943, + "heading": -6.710895051043041e-29, + "angularVelocity": 1.5189324842732185e-29, + "velocityX": -2.8617362878477276e-23, + "velocityY": -2.866266175914819e-23, + "timestamp": 0.7529441515468704 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Skip.traj b/src/main/deploy/choreo/4NB-Skip.traj new file mode 100644 index 0000000..d1d67c0 --- /dev/null +++ b/src/main/deploy/choreo/4NB-Skip.traj @@ -0,0 +1,1309 @@ +{ + "samples": [ + { + "x": 0.8085346817970276, + "y": 4.440213680267334, + "heading": -0.994421194775346, + "angularVelocity": 9.676628377640253e-20, + "velocityX": 2.353549276621379e-17, + "velocityY": 2.6128743115300274e-17, + "timestamp": 0 + }, + { + "x": 0.822974919123913, + "y": 4.427726715046675, + "heading": -0.9705493797852635, + "angularVelocity": 0.40182084288808634, + "velocityX": 0.2430643977679354, + "velocityY": -0.21018606637847403, + "timestamp": 0.05940910087815508 + }, + { + "x": 0.8519383277486233, + "y": 4.402735349568012, + "heading": -0.9240064477296293, + "angularVelocity": 0.78343101254963, + "velocityX": 0.4875247764498045, + "velocityY": -0.4206656069399718, + "timestamp": 0.11881820175631017 + }, + { + "x": 0.8955174447652127, + "y": 4.365218664734886, + "heading": -0.856257826909993, + "angularVelocity": 1.1403744513586813, + "velocityX": 0.7335427800188714, + "velocityY": -0.6314972668931192, + "timestamp": 0.17822730263446523 + }, + { + "x": 0.9538148065445263, + "y": 4.315143986084932, + "heading": -0.7692489012801026, + "angularVelocity": 1.4645723356147615, + "velocityX": 0.981286720680567, + "velocityY": -0.842878917704278, + "timestamp": 0.23763640351262033 + }, + { + "x": 1.026938864152823, + "y": 4.2524594827203375, + "heading": -0.6656141360189669, + "angularVelocity": 1.7444257483998427, + "velocityX": 1.230856157178015, + "velocityY": -1.0551330088830848, + "timestamp": 0.29704550439077543 + }, + { + "x": 1.1149962326719975, + "y": 4.177093081809827, + "heading": -0.5488592608060379, + "angularVelocity": 1.9652691841337226, + "velocityX": 1.4822201854184571, + "velocityY": -1.2686002615171224, + "timestamp": 0.3564546052689305 + }, + { + "x": 1.2180827278312385, + "y": 4.088964134511281, + "heading": -0.4236140095866624, + "angularVelocity": 2.108182910834597, + "velocityX": 1.735197025968326, + "velocityY": -1.483425030776192, + "timestamp": 0.4158637061470856 + }, + { + "x": 1.3362653476838338, + "y": 3.988016447914785, + "heading": -0.29631937092086613, + "angularVelocity": 2.1426790977171852, + "velocityX": 1.9893016070882061, + "velocityY": -1.6991956637006456, + "timestamp": 0.4752728070252407 + }, + { + "x": 1.4695036425602637, + "y": 3.874326638244391, + "heading": -0.17703470987105152, + "angularVelocity": 2.007851647081105, + "velocityX": 2.2427253216586585, + "velocityY": -1.9136766587931984, + "timestamp": 0.5346819079033958 + }, + { + "x": 1.61738357366469, + "y": 3.7486761953014893, + "heading": -0.08320124029329157, + "angularVelocity": 1.5794460476721217, + "velocityX": 2.4891797539185125, + "velocityY": -2.115003275350498, + "timestamp": 0.5940910087815509 + }, + { + "x": 1.7795783215287542, + "y": 3.612066748468329, + "heading": -0.024373238316333756, + "angularVelocity": 0.9902186888437958, + "velocityX": 2.7301330177794627, + "velocityY": -2.299470027552942, + "timestamp": 0.653500109659706 + }, + { + "x": 1.9560238490010582, + "y": 3.464363802586248, + "heading": -3.251899068917287e-7, + "angularVelocity": 0.41025554613951887, + "velocityX": 2.9700083802675254, + "velocityY": -2.486200661158664, + "timestamp": 0.712909210537861 + }, + { + "x": 2.1407664058154356, + "y": 3.306630906711477, + "heading": -2.960600612657896e-7, + "angularVelocity": 4.903263169257326e-7, + "velocityX": 3.1096676112512296, + "velocityY": -2.655029171343601, + "timestamp": 0.7723183114160161 + }, + { + "x": 2.3254947652525764, + "y": 3.1488813838204237, + "heading": -2.6697171512847604e-7, + "angularVelocity": 4.896277793613143e-7, + "velocityX": 3.109428634780798, + "velocityY": -2.655309044562341, + "timestamp": 0.8317274122941712 + }, + { + "x": 2.5102231286576613, + "y": 2.9911318655758214, + "heading": -2.3788336891991952e-7, + "angularVelocity": 4.896277805605067e-7, + "velocityX": 3.109428701570963, + "velocityY": -2.655308966351258, + "timestamp": 0.8911365131723263 + }, + { + "x": 2.694951491657384, + "y": 2.8333823468564927, + "heading": -2.0879502283644788e-7, + "angularVelocity": 4.89627778455023e-7, + "velocityX": 3.1094286947477254, + "velocityY": -2.6553089743420633, + "timestamp": 0.9505456140504814 + }, + { + "x": 2.8796798410785964, + "y": 2.675632812236456, + "heading": -1.7970667738519104e-7, + "angularVelocity": 4.896277678133068e-7, + "velocityX": 3.1094284661883007, + "velocityY": -2.6553092419897357, + "timestamp": 1.0099547149286365 + }, + { + "x": 3.0644080904838464, + "y": 2.5178831604959693, + "heading": -1.5061833606690155e-7, + "angularVelocity": 4.896276982453926e-7, + "velocityX": 3.109426782675891, + "velocityY": -2.655311213412356, + "timestamp": 1.0693638158067915 + }, + { + "x": 3.2491359906412702, + "y": 2.360133099780758, + "heading": -1.215300088943934e-7, + "angularVelocity": 4.896274601374268e-7, + "velocityX": 3.109420903984017, + "velocityY": -2.6553180974534567, + "timestamp": 1.1287729166849465 + }, + { + "x": 3.4338955523253594, + "y": 2.202420122735163, + "heading": -9.244031535060432e-8, + "angularVelocity": 4.896504594972081e-7, + "velocityX": 3.1099538446664665, + "velocityY": -2.654693888886324, + "timestamp": 1.1881820175631015 + }, + { + "x": 3.6212771735981955, + "y": 2.0478316149306797, + "heading": -6.319311636602291e-8, + "angularVelocity": 4.923016600450938e-7, + "velocityX": 3.1540894998099027, + "velocityY": -2.6021014544794228, + "timestamp": 1.2475911184412565 + }, + { + "x": 3.820400967204548, + "y": 1.908694296792687, + "heading": -3.281153410671686e-8, + "angularVelocity": 5.113960960484033e-7, + "velocityX": 3.3517388861820665, + "velocityY": -2.342020264256331, + "timestamp": 1.3070002193194115 + }, + { + "x": 4.03019905090332, + "y": 1.7862433195114136, + "heading": -4.8954376995112275e-19, + "angularVelocity": 5.522981095615313e-7, + "velocityX": 3.5314132110677283, + "velocityY": -2.0611484683535886, + "timestamp": 1.3664093201975664 + }, + { + "x": 4.322951930029737, + "y": 1.6552955500710258, + "heading": 5.175704079338812e-15, + "angularVelocity": 6.599529748424669e-14, + "velocityX": 3.7325329660882653, + "velocityY": -1.669554430106589, + "timestamp": 1.4448420846999053 + }, + { + "x": 4.628082081291311, + "y": 1.556567853907556, + "heading": 5.190662725666785e-15, + "angularVelocity": 1.9071943375934453e-16, + "velocityX": 3.8903403851384977, + "velocityY": -1.2587557864383778, + "timestamp": 1.523274849202244 + }, + { + "x": 4.938074644435486, + "y": 1.4743718801236945, + "heading": 5.169631100164441e-15, + "angularVelocity": -2.681484041715186e-16, + "velocityX": 3.952335036397641, + "velocityY": -1.0479800668204275, + "timestamp": 1.601707613704583 + }, + { + "x": 5.248064134540723, + "y": 1.3921643177396372, + "heading": 5.1832756916526305e-15, + "angularVelocity": 1.7396546747950823e-16, + "velocityX": 3.952295855847418, + "velocityY": -1.0481278188474166, + "timestamp": 1.6801403782069217 + }, + { + "x": 5.558053461013338, + "y": 1.3099561383310128, + "heading": 5.1652975147552064e-15, + "angularVelocity": -2.2921768707255205e-16, + "velocityX": 3.9522937695684903, + "velocityY": -1.0481356857710855, + "timestamp": 1.7585731427092606 + }, + { + "x": 5.868042788859202, + "y": 1.227747964100929, + "heading": 5.17893915410426e-15, + "angularVelocity": 1.7392781023474698e-16, + "velocityX": 3.952293787077097, + "velocityY": -1.0481356197458775, + "timestamp": 1.8370059072115994 + }, + { + "x": 6.1780321171916235, + "y": 1.1455397917056027, + "heading": 5.203641884469278e-15, + "angularVelocity": 3.1495423062461055e-16, + "velocityX": 3.95229379328059, + "velocityY": -1.0481355963531385, + "timestamp": 1.9154386717139382 + }, + { + "x": 6.488021446386602, + "y": 1.0633316225647949, + "heading": 5.179220620544195e-15, + "angularVelocity": -3.1136561843441626e-16, + "velocityX": 3.952293804278011, + "velocityY": -1.048135554858763, + "timestamp": 1.993871436216277 + }, + { + "x": 6.788390273210204, + "y": 0.9836747790194914, + "heading": 1.4385064984300214e-14, + "angularVelocity": 1.1737244138358137e-13, + "velocityX": 3.8296345759259354, + "velocityY": -1.015606730818312, + "timestamp": 2.072304200718616 + }, + { + "x": 7.055384812513254, + "y": 0.9128686882327177, + "heading": 1.5703193582509516e-14, + "angularVelocity": 1.680584250857847e-14, + "velocityX": 3.40411996181899, + "velocityY": -0.9027616358546965, + "timestamp": 2.150736965220955 + }, + { + "x": 7.289005044919871, + "y": 0.8509133557721549, + "heading": 1.4172123019917492e-14, + "angularVelocity": -1.9520804221347698e-14, + "velocityX": 2.97860510067396, + "velocityY": -0.7899164699044072, + "timestamp": 2.229169729723294 + }, + { + "x": 7.48925096397141, + "y": 0.7978087834938102, + "heading": 1.1452140462826918e-14, + "angularVelocity": -3.4679163908121556e-14, + "velocityX": 2.5530901571826567, + "velocityY": -0.6770712802904464, + "timestamp": 2.307602494225633 + }, + { + "x": 7.656122566438546, + "y": 0.7535549723257088, + "heading": 8.361039725202098e-15, + "angularVelocity": -3.9410836943919146e-14, + "velocityX": 2.127575172518181, + "velocityY": -0.5642260788443735, + "timestamp": 2.386035258727972 + }, + { + "x": 7.789619850383681, + "y": 0.7181519228246735, + "heading": 5.39075057643342e-15, + "angularVelocity": -3.787051642035781e-14, + "velocityX": 1.7020601631497934, + "velocityY": -0.4513808702989361, + "timestamp": 2.464468023230311 + }, + { + "x": 7.889742814515085, + "y": 0.6915996353619231, + "heading": 2.8547195474114465e-15, + "angularVelocity": -3.233382334683438e-14, + "velocityX": 1.2765451373121304, + "velocityY": -0.33853565702054167, + "timestamp": 2.54290078773265 + }, + { + "x": 7.9564914579100945, + "y": 0.6738981102026166, + "heading": 1.001573018444963e-15, + "angularVelocity": -2.3627200058002107e-14, + "velocityX": 0.8510300997107006, + "velocityY": -0.22569044036143487, + "timestamp": 2.621333552234989 + }, + { + "x": 7.989865779876709, + "y": 0.6650473475456241, + "heading": -9.790956081407498e-26, + "angularVelocity": -1.2769829956117919e-14, + "velocityX": 0.4255150532864473, + "velocityY": -0.11284522116677391, + "timestamp": 2.6997663167373283 + }, + { + "x": 7.989865779876709, + "y": 0.665047347545624, + "heading": -4.586051220974463e-26, + "angularVelocity": 2.5919631848423095e-26, + "velocityX": -1.4783880366896059e-18, + "velocityY": 2.7911510207865606e-17, + "timestamp": 2.7781990812396673 + }, + { + "x": 7.961825021408631, + "y": 0.6653260998514925, + "heading": -3.8642001004622653e-17, + "angularVelocity": -5.466910762418722e-16, + "velocityX": -0.39670913756880827, + "velocityY": 0.003943673170767399, + "timestamp": 2.848882500935829 + }, + { + "x": 7.905743504974589, + "y": 0.6658836070474405, + "heading": -1.1420692112030604e-16, + "angularVelocity": -1.0690612857146226e-15, + "velocityX": -0.7934182680338816, + "velocityY": 0.007887382901887431, + "timestamp": 2.9195659206319906 + }, + { + "x": 7.821621231202305, + "y": 0.6667198723730994, + "heading": -2.271131022785688e-16, + "angularVelocity": -1.597350021728402e-15, + "velocityX": -1.190127389618255, + "velocityY": 0.011831138465963963, + "timestamp": 2.9902493403281523 + }, + { + "x": 7.709458200898961, + "y": 0.6678349000090077, + "heading": -3.725115104027876e-16, + "angularVelocity": -2.0570366115956903e-15, + "velocityX": -1.586836499782901, + "velocityY": 0.015774953174573097, + "timestamp": 3.060932760024314 + }, + { + "x": 7.569254415141015, + "y": 0.6692286955560068, + "heading": -5.529032952361946e-16, + "angularVelocity": -2.5521084387850704e-15, + "velocityX": -1.983545594718296, + "velocityY": 0.019718847121565148, + "timestamp": 3.1316161797204756 + }, + { + "x": 7.4010098754359035, + "y": 0.6709012669067533, + "heading": -7.630982661410159e-16, + "angularVelocity": -2.9737515734334796e-15, + "velocityX": -2.3802546683270744, + "velocityY": 0.02366285273059252, + "timestamp": 3.202299599416637 + }, + { + "x": 7.204724584045661, + "y": 0.6728526260129781, + "heading": -1.0096410558455573e-15, + "angularVelocity": -3.487985554177256e-15, + "velocityX": -2.7769637099335265, + "velocityY": 0.0276070274277582, + "timestamp": 3.272983019112799 + }, + { + "x": 6.980398544742687, + "y": 0.6750827930954573, + "heading": -1.423077463628514e-15, + "angularVelocity": -5.849127749576447e-15, + "velocityX": -3.173672698169647, + "velocityY": 0.03155148820008718, + "timestamp": 3.3436664388089605 + }, + { + "x": 6.7280317650805745, + "y": 0.6775918097291393, + "heading": -1.9851623705937264e-15, + "angularVelocity": -7.952145739517459e-15, + "velocityX": -3.5703815795405807, + "velocityY": 0.03549653715757366, + "timestamp": 3.414349858505122 + }, + { + "x": 6.447624267790984, + "y": 0.6803798097800222, + "heading": -6.968574786204946e-17, + "angularVelocity": 2.7099372702904392e-14, + "velocityX": -3.96709013931336, + "velocityY": 0.03944347999579872, + "timestamp": 3.485033278201284 + }, + { + "x": 6.158924341772469, + "y": 0.693943817311479, + "heading": 9.565698789607162e-18, + "angularVelocity": 1.1212169263035645e-15, + "velocityX": -4.084408016187073, + "velocityY": 0.19189800931673312, + "timestamp": 3.5557166978974455 + }, + { + "x": 5.872883545252504, + "y": 0.7353236661281272, + "heading": 6.282540250170779e-18, + "angularVelocity": -4.644877904443354e-17, + "velocityX": -4.046787743851827, + "velocityY": 0.58542511093175, + "timestamp": 3.626400117593607 + }, + { + "x": 5.592199247715388, + "y": 0.8042290602455044, + "heading": 2.5226905744349242e-17, + "angularVelocity": 2.6801710459780227e-16, + "velocityX": -3.971006195564076, + "velocityY": 0.9748452241499008, + "timestamp": 3.697083537289769 + }, + { + "x": 5.31951379776001, + "y": 0.9000114798545834, + "heading": 1.0213561793054143e-23, + "angularVelocity": -3.568997601327789e-16, + "velocityX": -3.8578417842194788, + "velocityY": 1.3550903453853254, + "timestamp": 3.7677669569859304 + }, + { + "x": 5.028358906099587, + "y": 1.039292217399747, + "heading": 1.939930717479169e-17, + "angularVelocity": 2.45765977450937e-16, + "velocityX": -3.688587353353505, + "velocityY": 1.7645218465846235, + "timestamp": 3.846700940044708 + }, + { + "x": 4.753982028496478, + "y": 1.2092558179345199, + "heading": 5.251717131242591e-17, + "angularVelocity": 4.1956390743611424e-16, + "velocityX": -3.476029803269934, + "velocityY": 2.1532373503589888, + "timestamp": 3.9256349231034857 + }, + { + "x": 4.499604125223824, + "y": 1.407906716987468, + "heading": 4.7661466331843347e-17, + "angularVelocity": -6.151615583577767e-17, + "velocityX": -3.222666504529982, + "velocityY": 2.5166714177468545, + "timestamp": 4.004568906162263 + }, + { + "x": 4.26534834796377, + "y": 1.629930428449429, + "heading": 6.383046336001005e-17, + "angularVelocity": 2.0484189779951308e-16, + "velocityX": -2.9677430199566506, + "velocityY": 2.8127772457221822, + "timestamp": 4.083502889221041 + }, + { + "x": 4.031326138540414, + "y": 1.852200317188001, + "heading": 2.3677757755478083e-17, + "angularVelocity": -5.086873150258546e-16, + "velocityX": -2.964783992333329, + "velocityY": 2.815896020002203, + "timestamp": 4.162436872279819 + }, + { + "x": 3.797312456863741, + "y": 2.0744791835519405, + "heading": 4.512669218284662e-17, + "angularVelocity": 2.717323991439214e-16, + "velocityX": -2.9646759558849944, + "velocityY": 2.816009755880509, + "timestamp": 4.241370855338596 + }, + { + "x": 3.563297360395116, + "y": 2.29675656041435, + "heading": 2.2571410949696857e-18, + "angularVelocity": -5.431063958209394e-16, + "velocityX": -2.964693879622175, + "velocityY": 2.815990885661535, + "timestamp": 4.320304838397374 + }, + { + "x": 3.335095888371744, + "y": 2.5135119589023023, + "heading": -5.89134026984256e-14, + "angularVelocity": -7.463915767059434e-13, + "velocityX": -2.8910421491511853, + "velocityY": 2.7460339652011654, + "timestamp": 4.3992388214561515 + }, + { + "x": 3.1322501152043865, + "y": 2.706183443191518, + "heading": -7.201295573540544e-14, + "angularVelocity": -1.6595581004068512e-13, + "velocityX": -2.569815500330909, + "velocityY": 2.440919320463041, + "timestamp": 4.478172804514929 + }, + { + "x": 2.9547600556706652, + "y": 2.8747709994570525, + "heading": -6.793308622264907e-14, + "angularVelocity": 5.168711075034906e-14, + "velocityX": -2.2485886642956747, + "velocityY": 2.1358045005792574, + "timestamp": 4.557106787573707 + }, + { + "x": 2.802625714692311, + "y": 3.019274623086207, + "heading": -5.6227190966394764e-14, + "angularVelocity": 1.482998177559053e-13, + "velocityX": -1.927361765907952, + "velocityY": 1.830689622258048, + "timestamp": 4.636040770632484 + }, + { + "x": 2.6758470947294035, + "y": 3.1396943117718017, + "heading": -4.1672293686354096e-14, + "angularVelocity": 1.843932964768755e-13, + "velocityX": -1.6061348363539316, + "velocityY": 1.5255747147076162, + "timestamp": 4.714974753691262 + }, + { + "x": 2.57442419725774, + "y": 3.236030064129265, + "heading": -2.7119123795716724e-14, + "angularVelocity": 1.8437141252050512e-13, + "velocityX": -1.2849078881033131, + "velocityY": 1.2204597896162928, + "timestamp": 4.79390873675004 + }, + { + "x": 2.498357023261081, + "y": 3.308281879235439, + "heading": -1.4470412940019133e-14, + "angularVelocity": 1.6024417392647582e-13, + "velocityX": -0.9636809273896104, + "velocityY": 0.9153448528296537, + "timestamp": 4.872842719808817 + }, + { + "x": 2.4476455734420637, + "y": 3.356449756430872, + "heading": -5.093331671182619e-15, + "angularVelocity": 1.1879650495017822e-13, + "velocityX": -0.6424539577743393, + "velocityY": 0.6102299076885483, + "timestamp": 4.951776702867595 + }, + { + "x": 2.422289848327637, + "y": 3.3805336952209477, + "heading": 8.600144701827338e-26, + "angularVelocity": 6.45264751057182e-14, + "velocityX": -0.3212269814832313, + "velocityY": 0.30511495628123525, + "timestamp": 5.030710685926373 + }, + { + "x": 2.422289848327637, + "y": 3.3805336952209477, + "heading": 4.234321675050891e-26, + "angularVelocity": -1.6322812413801202e-26, + "velocityX": 1.82145222049907e-18, + "velocityY": 1.2398127508278904e-17, + "timestamp": 5.10964466898515 + }, + { + "x": 2.4422092097338126, + "y": 3.359397040789022, + "heading": 1.4828318603888107e-18, + "angularVelocity": 2.061358359803718e-17, + "velocityX": 0.2769090563806016, + "velocityY": -0.29383125866924514, + "timestamp": 5.181579338318127 + }, + { + "x": 2.4821232633957244, + "y": 3.317194979333885, + "heading": -2.399084815386753e-18, + "angularVelocity": -5.3964448898673995e-17, + "velocityX": 0.5548653247699834, + "velocityY": -0.5866720712936807, + "timestamp": 5.253514007651103 + }, + { + "x": 2.5421257730353313, + "y": 3.2540169079394587, + "heading": -4.312212643467297e-18, + "angularVelocity": -2.6595363961942612e-17, + "velocityX": 0.8341250497936068, + "velocityY": -0.878270130114354, + "timestamp": 5.325448676984079 + }, + { + "x": 2.6223366317399224, + "y": 3.169978308273709, + "heading": -6.87359010796808e-18, + "angularVelocity": -3.560699654803604e-17, + "velocityX": 1.1150514688942472, + "velocityY": -1.1682628202156935, + "timestamp": 5.3973833463170555 + }, + { + "x": 2.722914524907397, + "y": 3.0652340847546355, + "heading": -1.0050987029108054e-17, + "angularVelocity": -4.417054911796212e-17, + "velocityX": 1.3981838534826794, + "velocityY": -1.4561021061315331, + "timestamp": 5.469318015650032 + }, + { + "x": 2.844079320063175, + "y": 2.94000283277744, + "heading": -1.3800277943608368e-17, + "angularVelocity": -5.212076319266894e-17, + "velocityX": 1.6843727270700168, + "velocityY": -1.7409025875701625, + "timestamp": 5.541252684983008 + }, + { + "x": 2.9861556801651536, + "y": 2.7946160556206303, + "heading": -1.8054024423421994e-17, + "angularVelocity": -5.913346837366306e-17, + "velocityX": 1.9750749036440556, + "velocityY": -2.0210946752866197, + "timestamp": 5.6131873543159845 + }, + { + "x": 3.1496705290943545, + "y": 2.629635153477409, + "heading": -1.9755148692229212e-17, + "angularVelocity": -2.3648200854179725e-17, + "velocityX": 2.2731021139742653, + "velocityY": -2.2934824566998935, + "timestamp": 5.685122023648961 + }, + { + "x": 3.3356217659937375, + "y": 2.446211186910791, + "heading": -2.1620491181128583e-17, + "angularVelocity": -2.5931074864358486e-17, + "velocityX": 2.5850016219372725, + "velocityY": -2.5498687665852504, + "timestamp": 5.757056692981937 + }, + { + "x": 3.546522888895758, + "y": 2.247919800418475, + "heading": -2.298290975372596e-17, + "angularVelocity": -1.8939700380288258e-17, + "velocityX": 2.931842529590256, + "velocityY": -2.756548244821444, + "timestamp": 5.828991362314913 + }, + { + "x": 3.779551655861922, + "y": 2.0684406150868386, + "heading": -2.356725503120944e-17, + "angularVelocity": -8.123277452624726e-18, + "velocityX": 3.2394500333071203, + "velocityY": -2.495030379582669, + "timestamp": 5.90092603164789 + }, + { + "x": 4.029145048692618, + "y": 1.912818243294336, + "heading": -7.243904171356102e-18, + "angularVelocity": 2.2691910606172613e-16, + "velocityX": 3.4697232244967258, + "velocityY": -2.163384821739647, + "timestamp": 5.972860700980866 + }, + { + "x": 4.292869509297096, + "y": 1.7825700534328885, + "heading": 2.7690024039408224e-18, + "angularVelocity": 1.391944477977289e-16, + "velocityX": 3.666166301315592, + "velocityY": -1.8106455630985843, + "timestamp": 6.044795370313842 + }, + { + "x": 4.568153688292314, + "y": 1.6789660022880506, + "heading": 1.6169391405873456e-17, + "angularVelocity": 1.8628554390059454e-16, + "velocityX": 3.8268637577418185, + "velocityY": -1.4402519967735667, + "timestamp": 6.116730039646819 + }, + { + "x": 4.85231351852417, + "y": 1.6030162572860713, + "heading": 1.104418666333468e-28, + "angularVelocity": -2.2477883829523805e-16, + "velocityX": 3.9502486473765894, + "velocityY": -1.055815585244564, + "timestamp": 6.188664708979795 + }, + { + "x": 5.158309378699811, + "y": 1.5545385083826373, + "heading": -1.1441689912277495e-17, + "angularVelocity": -1.5100789546254797e-16, + "velocityX": 4.038545985692146, + "velocityY": -0.6398113298648395, + "timestamp": 6.264433528069543 + }, + { + "x": 5.467685385105672, + "y": 1.5381052997754503, + "heading": 2.911311456658326e-17, + "angularVelocity": 5.352439824969174e-16, + "velocityX": 4.083157295078998, + "velocityY": -0.21688616511744238, + "timestamp": 6.340202347159291 + }, + { + "x": 5.777094938781223, + "y": 1.553894378826709, + "heading": -2.420664700580125e-18, + "angularVelocity": -4.1618411961572785e-16, + "velocityX": 4.083600053329194, + "velocityY": 0.2083849166488678, + "timestamp": 6.415971166249039 + }, + { + "x": 6.083191086572635, + "y": 1.6017349192235426, + "heading": -5.882315170795842e-18, + "angularVelocity": -4.568700570848447e-17, + "velocityX": 4.039869585784915, + "velocityY": 0.6314014256909969, + "timestamp": 6.491739985338787 + }, + { + "x": 6.382662742491119, + "y": 1.6811093330008764, + "heading": -1.6858060845261797e-17, + "angularVelocity": -1.4485834418926946e-16, + "velocityX": 3.9524392687700143, + "velocityY": 1.0475867874182512, + "timestamp": 6.5675088044285355 + }, + { + "x": 6.672275412037942, + "y": 1.7911460204151195, + "heading": -2.888480028332242e-17, + "angularVelocity": -1.587294032373541e-16, + "velocityX": 3.8223199599175173, + "velocityY": 1.452268739806397, + "timestamp": 6.6432776235182835 + }, + { + "x": 6.935773457086466, + "y": 1.9122631966744312, + "heading": -2.1691436993771164e-17, + "angularVelocity": 9.493832977551161e-17, + "velocityX": 3.4776580684956064, + "velocityY": 1.5985094886787496, + "timestamp": 6.719046442608032 + }, + { + "x": 7.170007295221654, + "y": 2.0198939541094463, + "heading": -1.6299807800034943e-17, + "angularVelocity": 7.115894340654352e-17, + "velocityX": 3.0914278584382107, + "velocityY": 1.4205151766725412, + "timestamp": 6.79481526169778 + }, + { + "x": 7.374967182869676, + "y": 2.1140594081505015, + "heading": -1.0901160029750536e-17, + "angularVelocity": 7.125157266111349e-17, + "velocityX": 2.7050690522873455, + "velocityY": 1.2427995469958841, + "timestamp": 6.870584080787528 + }, + { + "x": 7.550649876686179, + "y": 2.1947665963601173, + "heading": -8.565573749391565e-18, + "angularVelocity": 3.0825158228438804e-17, + "velocityX": 2.318667440342544, + "velocityY": 1.0651767993626458, + "timestamp": 6.946352899877276 + }, + { + "x": 7.69705375584567, + "y": 2.26201903734599, + "heading": -6.268442595471868e-18, + "angularVelocity": 3.0317623225845786e-17, + "velocityX": 1.932244436675357, + "velocityY": 0.8876004904604238, + "timestamp": 7.022121718967024 + }, + { + "x": 7.814177848123867, + "y": 2.315818842216871, + "heading": -5.860377826964177e-18, + "angularVelocity": 5.385632685743263e-18, + "velocityX": 1.5458086015515728, + "velocityY": 0.7100520440594869, + "timestamp": 7.097890538056772 + }, + { + "x": 7.902021505484193, + "y": 2.3561674183552324, + "heading": -2.0084875089379725e-18, + "angularVelocity": 5.083743950397372e-17, + "velocityX": 1.1593642136140294, + "velocityY": 0.5325221723510412, + "timestamp": 7.17365935714652 + }, + { + "x": 7.960584265098705, + "y": 2.3830657710227903, + "heading": -5.354322583341586e-20, + "angularVelocity": 2.5801441111297213e-17, + "velocityX": 0.772913717254023, + "velocityY": 0.35500556812864675, + "timestamp": 7.249428176236268 + }, + { + "x": 7.989865779876708, + "y": 2.3965146541595463, + "heading": 4.464057669964841e-25, + "angularVelocity": 7.066509963979535e-19, + "velocityX": 0.3864586399765603, + "velocityY": 0.17749891443779842, + "timestamp": 7.325196995326016 + }, + { + "x": 7.989865779876708, + "y": 2.3965146541595463, + "heading": 4.577698718092902e-25, + "angularVelocity": 1.4924495313995464e-25, + "velocityX": 1.7316501925858062e-16, + "velocityY": 2.4957312477014177e-17, + "timestamp": 7.400965814415764 + }, + { + "x": 7.961948125314342, + "y": 2.3869413231673744, + "heading": 3.968152450731581e-15, + "angularVelocity": 5.4722575152696526e-14, + "velocityX": -0.38499664874452716, + "velocityY": -0.1320204152917069, + "timestamp": 7.473479834092087 + }, + { + "x": 7.9061128172497455, + "y": 2.3677946595682435, + "heading": 1.1360635138080996e-14, + "angularVelocity": 1.0194560175538186e-13, + "velocityX": -0.7699932828692636, + "velocityY": -0.26404085285209106, + "timestamp": 7.54599385376841 + }, + { + "x": 7.822359857005323, + "y": 2.3390746613517033, + "heading": 2.1504602727030266e-14, + "angularVelocity": 1.3988979395557398e-13, + "velocityX": -1.154989898757494, + "velocityY": -0.3960613181375172, + "timestamp": 7.618507873444733 + }, + { + "x": 7.71068924627678, + "y": 2.300781325945998, + "heading": 3.351912896052837e-14, + "angularVelocity": 1.6568562398720052e-13, + "velocityX": -1.539986491261158, + "velocityY": -0.5280818188886689, + "timestamp": 7.691021893121055 + }, + { + "x": 7.571100987317153, + "y": 2.2529146499450325, + "heading": 4.62437330221949e-14, + "angularVelocity": 1.754779109699489e-13, + "velocityX": -1.9249830526944358, + "velocityY": -0.6601023666114509, + "timestamp": 7.763535912797378 + }, + { + "x": 7.4035950832655315, + "y": 2.195474628624213, + "heading": 5.80518545778972e-14, + "angularVelocity": 1.628392124731043e-13, + "velocityX": -2.30997957083824, + "velocityY": -0.7921229794885999, + "timestamp": 7.836049932473701 + }, + { + "x": 7.20817153879745, + "y": 2.1284612549925805, + "heading": 6.651768983836094e-14, + "angularVelocity": 1.1674760820877797e-13, + "velocityX": -2.6949760245043035, + "velocityY": -0.9241436887743528, + "timestamp": 7.908563952150024 + }, + { + "x": 6.9848303616174565, + "y": 2.0518745176536126, + "heading": 6.760398615570593e-14, + "angularVelocity": 1.498050653761286e-14, + "velocityX": -3.0799723719211687, + "velocityY": -1.0561645552234988, + "timestamp": 7.9810779718263465 + }, + { + "x": 6.733571566806331, + "y": 1.965714394765822, + "heading": 5.327081908950384e-14, + "angularVelocity": -1.9766070480015549e-13, + "velocityX": -3.464968511367217, + "velocityY": -1.188185722877021, + "timestamp": 8.05359199150267 + }, + { + "x": 6.454395197014029, + "y": 1.8699808283188497, + "heading": -1.210358626215953e-16, + "angularVelocity": -7.362972657145883e-13, + "velocityX": -3.849964062651033, + "velocityY": -1.3202076905209181, + "timestamp": 8.126106011178992 + }, + { + "x": 6.1738479950442935, + "y": 1.7740244527442883, + "heading": -1.1104225188815767e-16, + "angularVelocity": 1.3781627642234675e-16, + "velocityX": -3.868868437050859, + "velocityY": -1.3232803257055905, + "timestamp": 8.198620030855315 + }, + { + "x": 5.89229592505603, + "y": 1.6810578437397081, + "heading": -1.3815766244496281e-16, + "angularVelocity": -3.7393339475245444e-16, + "velocityX": -3.882726005881283, + "velocityY": -1.2820501389883114, + "timestamp": 8.271134050531638 + }, + { + "x": 5.603047274828762, + "y": 1.615869067560432, + "heading": -1.375103171712517e-16, + "angularVelocity": 8.927167451460251e-18, + "velocityX": -3.9888652086644605, + "velocityY": -0.8989816930592814, + "timestamp": 8.34364807020796 + }, + { + "x": 5.3087508481141255, + "y": 1.5797585348826493, + "heading": -5.115064325941328e-17, + "angularVelocity": 1.1909376120637012e-15, + "velocityX": -4.058476250913794, + "velocityY": -0.49798001598624214, + "timestamp": 8.416162089884283 + }, + { + "x": 5.012322426011832, + "y": 1.5730842246634098, + "heading": -1.6843428292277085e-17, + "angularVelocity": 4.731114706232749e-16, + "velocityX": -4.087877398404444, + "velocityY": -0.09204165275748097, + "timestamp": 8.488676109560606 + }, + { + "x": 4.716698964119128, + "y": 1.5959123273167983, + "heading": 1.0780704155043204e-17, + "angularVelocity": 3.8094884400946135e-16, + "velocityX": -4.076776645568003, + "velocityY": 0.3148095051884876, + "timestamp": 8.561190129236929 + }, + { + "x": 4.424809455871582, + "y": 1.6480166912078853, + "heading": -4.7825786100317035e-25, + "angularVelocity": -1.4867063166908422e-16, + "velocityX": -4.025283794091286, + "velocityY": 0.7185419333227538, + "timestamp": 8.633704148913251 + }, + { + "x": 4.128740207867581, + "y": 1.7331899442926113, + "heading": 2.436084377884843e-17, + "angularVelocity": 3.233261816100211e-16, + "velocityX": -3.929541129144622, + "velocityY": 1.1304510797896916, + "timestamp": 8.709048632727956 + }, + { + "x": 3.843051522185569, + "y": 1.8484872151278047, + "heading": 4.0057524127252557e-17, + "angularVelocity": 2.0833224449518998e-16, + "velocityX": -3.7917664468268093, + "velocityY": 1.5302682425770875, + "timestamp": 8.784393116542661 + }, + { + "x": 3.5707992295594733, + "y": 1.992675206864091, + "heading": 4.3264327263683456e-17, + "angularVelocity": 4.256188342181214e-17, + "velocityX": -3.6134336429424168, + "velocityY": 1.9137166310769615, + "timestamp": 8.859737600357366 + }, + { + "x": 3.314895397994771, + "y": 2.1642115529138155, + "heading": 5.0710064414841386e-17, + "angularVelocity": 9.88225915723796e-17, + "velocityX": -3.39645078987007, + "velocityY": 2.2766941568200596, + "timestamp": 8.935082084172072 + }, + { + "x": 3.078073502264168, + "y": 2.361256926788227, + "heading": 7.635756849280407e-17, + "angularVelocity": 3.4040320908383547e-16, + "velocityX": -3.143188243401714, + "velocityY": 2.615259457584993, + "timestamp": 9.010426567986777 + }, + { + "x": 2.854646488956178, + "y": 2.573369772957938, + "heading": 6.019554858067844e-17, + "angularVelocity": -2.1450833681506433e-16, + "velocityX": -2.9654063840756333, + "velocityY": 2.815240551521766, + "timestamp": 9.085771051801482 + }, + { + "x": 2.6466740813199228, + "y": 2.7708029552314177, + "heading": 4.0534261647582457e-17, + "angularVelocity": -2.6095184862579493e-16, + "velocityX": -2.7602871120227874, + "velocityY": 2.620406594855673, + "timestamp": 9.161115535616187 + }, + { + "x": 2.4618096495885284, + "y": 2.946299083382676, + "heading": 1.8651869909581232e-17, + "angularVelocity": -2.9043121070808416e-16, + "velocityX": -2.4535894649644563, + "velocityY": 2.329249857008171, + "timestamp": 9.236460019430892 + }, + { + "x": 2.3000532444302113, + "y": 3.0998581821870363, + "heading": 1.2585166550915731e-17, + "angularVelocity": -8.051961119619559e-17, + "velocityX": -2.1468911454186292, + "velocityY": 2.0380934479880564, + "timestamp": 9.311804503245597 + }, + { + "x": 2.1614048826816776, + "y": 3.2314802598444294, + "heading": 1.044220916189497e-17, + "angularVelocity": -2.8442168781856105e-17, + "velocityX": -1.8401926024097508, + "velocityY": 1.7469371478004907, + "timestamp": 9.387148987060302 + }, + { + "x": 2.04586457275109, + "y": 3.341165320443523, + "heading": 6.144588170756101e-18, + "angularVelocity": -5.703966005365595e-17, + "velocityX": -1.5334939478045977, + "velocityY": 1.4557809018792693, + "timestamp": 9.462493470875007 + }, + { + "x": 1.9534323196800467, + "y": 3.4289133664338625, + "heading": 8.956175204831333e-18, + "angularVelocity": 3.7316381823207725e-17, + "velocityX": -1.2267952262854738, + "velocityY": 1.1646246884693368, + "timestamp": 9.537837954689712 + }, + { + "x": 1.8841081268282298, + "y": 3.4947243994469472, + "heading": 8.125508496541916e-18, + "angularVelocity": -1.1024919871707434e-17, + "velocityX": -0.920096460175393, + "velocityY": 0.8734684967132663, + "timestamp": 9.613182438504417 + }, + { + "x": 1.8378919965947327, + "y": 3.538598420647381, + "heading": 4.6134522575515235e-18, + "angularVelocity": -4.661329437022688e-17, + "velocityX": -0.6133976622236478, + "velocityY": 0.5823123204142531, + "timestamp": 9.688526922319122 + }, + { + "x": 1.8147839307785034, + "y": 3.5605354309082036, + "heading": 2.574108918698912e-28, + "angularVelocity": -6.123143521392433e-17, + "velocityX": -0.3066988403955926, + "velocityY": 0.2911561557025646, + "timestamp": 9.763871406133827 + }, + { + "x": 1.8147839307785032, + "y": 3.560535430908203, + "heading": 1.6039214960839928e-28, + "angularVelocity": -4.957933854028128e-29, + "velocityX": -1.2239411035355163e-16, + "velocityY": -5.776889799901916e-17, + "timestamp": 9.839215889948532 + }, + { + "x": 1.8499405192639373, + "y": 3.595692011943056, + "heading": 7.674497632021469e-18, + "angularVelocity": 8.154121514620013e-17, + "velocityX": 0.3735372767099104, + "velocityY": 0.37353719754779685, + "timestamp": 9.93333390889189 + }, + { + "x": 1.9202536945231747, + "y": 3.666005172301131, + "heading": 1.2913126269895033e-17, + "angularVelocity": 5.566020934800391e-17, + "velocityX": 0.7470745352338143, + "velocityY": 0.7470743769095909, + "timestamp": 10.02745192783525 + }, + { + "x": 2.02572345256241, + "y": 3.771474907988624, + "heading": 1.5083295615484505e-17, + "angularVelocity": 2.3057958187881123e-17, + "velocityX": 1.1206117513237068, + "velocityY": 1.1206115138373827, + "timestamp": 10.121569946778608 + }, + { + "x": 2.1663497734126267, + "y": 3.912101199036522, + "heading": 1.7930576325828894e-17, + "angularVelocity": 3.0252238011837634e-17, + "velocityX": 1.4941487552436352, + "velocityY": 1.494148438595255, + "timestamp": 10.215687965721967 + }, + { + "x": 2.2718195314518623, + "y": 4.017570934724015, + "heading": 1.2801642358185404e-17, + "angularVelocity": -5.44947080834505e-17, + "velocityX": 1.120611751323707, + "velocityY": 1.1206115138373829, + "timestamp": 10.309805984665326 + }, + { + "x": 2.3421327067110993, + "y": 4.0878840950820905, + "heading": 5.291529814049623e-18, + "angularVelocity": -7.979463049177405e-17, + "velocityX": 0.7470745352338144, + "velocityY": 0.747074376909591, + "timestamp": 10.403924003608685 + }, + { + "x": 2.377289295196533, + "y": 4.123040676116943, + "heading": -1.3564749018286617e-28, + "angularVelocity": -5.62222821268349e-17, + "velocityX": 0.37353727670991055, + "velocityY": 0.3735371975477969, + "timestamp": 10.498042022552044 + }, + { + "x": 2.377289295196533, + "y": 4.123040676116943, + "heading": -6.710895051043041e-29, + "angularVelocity": 1.5189324842732185e-29, + "velocityX": -2.8617362878477276e-23, + "velocityY": -2.866266175914819e-23, + "timestamp": 10.592160041495402 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Sweep.1.traj b/src/main/deploy/choreo/4NB-Sweep.1.traj new file mode 100644 index 0000000..c86664c --- /dev/null +++ b/src/main/deploy/choreo/4NB-Sweep.1.traj @@ -0,0 +1,166 @@ +{ + "samples": [ + { + "x": 0.7265444397926323, + "y": 4.389017105102539, + "heading": -1.05165023914004, + "angularVelocity": -2.4052301236587744e-16, + "velocityX": -2.752276616089782e-15, + "velocityY": 5.134530261429221e-16, + "timestamp": 0 + }, + { + "x": 0.7559653796741465, + "y": 4.38140433106999, + "heading": -1.0337573727433125, + "angularVelocity": 0.2597755888304552, + "velocityX": 0.4271446403391007, + "velocityY": -0.11052521228797353, + "timestamp": 0.06887816702594797 + }, + { + "x": 0.8148295651894585, + "y": 4.366167962208403, + "heading": -0.9984651090693958, + "angularVelocity": 0.5123868011850714, + "velocityX": 0.8546131242594956, + "velocityY": -0.22120752510512612, + "timestamp": 0.13775633405189594 + }, + { + "x": 0.9031640621745638, + "y": 4.34329522415188, + "heading": -0.9463836724391982, + "angularVelocity": 0.7561385396707215, + "velocityX": 1.2824745605066483, + "velocityY": -0.3320753011314312, + "timestamp": 0.20663450107784392 + }, + { + "x": 1.0210050595189104, + "y": 4.312770957025727, + "heading": -0.8783834326740236, + "angularVelocity": 0.987253910801006, + "velocityX": 1.7108614011164693, + "velocityY": -0.4431631741108122, + "timestamp": 0.2755126681037919 + }, + { + "x": 1.1684052822191313, + "y": 4.274574303329794, + "heading": -0.7959106115927722, + "angularVelocity": 1.1973724714564178, + "velocityX": 2.1400137237202137, + "velocityY": -0.5545538643841008, + "timestamp": 0.34439083512973984 + }, + { + "x": 1.3454446954006436, + "y": 4.228667204890838, + "heading": -0.7017429838335593, + "angularVelocity": 1.367162220268308, + "velocityX": 2.5703270110950878, + "velocityY": -0.6664970980087429, + "timestamp": 0.4132690021556878 + }, + { + "x": 1.552227621121227, + "y": 4.174960199018195, + "heading": -0.6022461552568997, + "angularVelocity": 1.4445336290550153, + "velocityX": 3.002154886652024, + "velocityY": -0.779739185748209, + "timestamp": 0.48214716918163575 + }, + { + "x": 1.7883744114232694, + "y": 4.113221891555757, + "heading": -0.5202856320014997, + "angularVelocity": 1.1899347325041563, + "velocityX": 3.428470885601271, + "velocityY": -0.8963407437828599, + "timestamp": 0.5510253362075838 + }, + { + "x": 2.02506710191313, + "y": 4.0518739432063136, + "heading": -0.4352693076854395, + "angularVelocity": 1.2343000400117028, + "velocityX": 3.436396476704921, + "velocityY": -0.8906733584581512, + "timestamp": 0.6199035032335317 + }, + { + "x": 2.232210219377279, + "y": 3.9985845821566475, + "heading": -0.3384412768078663, + "angularVelocity": 1.4057869867689714, + "velocityX": 3.0073842903820833, + "velocityY": -0.773675655880783, + "timestamp": 0.6887816702594797 + }, + { + "x": 2.409618392670172, + "y": 3.9530993682814772, + "heading": -0.2475826023369981, + "angularVelocity": 1.319121550325896, + "velocityX": 2.5756808137193876, + "velocityY": -0.6603720139363624, + "timestamp": 0.7576598372854276 + }, + { + "x": 2.5573654312197593, + "y": 3.9153053743884563, + "heading": -0.16800100720308944, + "angularVelocity": 1.1553965294101876, + "velocityX": 2.1450489310194327, + "velocityY": -0.5487078928622399, + "timestamp": 0.8265380043113756 + }, + { + "x": 2.67550632370007, + "y": 3.885136654824466, + "heading": -0.10223202695817188, + "angularVelocity": 0.95485961785463, + "velocityX": 1.7152153952616722, + "velocityY": -0.4380011964114144, + "timestamp": 0.8954161713373235 + }, + { + "x": 2.764078093861892, + "y": 3.8625489746450645, + "heading": -0.05172765313938082, + "angularVelocity": 0.7332421288123194, + "velocityX": 1.2859193847080135, + "velocityY": -0.3279367200769452, + "timestamp": 0.9642943383632715 + }, + { + "x": 2.8231068724147113, + "y": 3.847510964463322, + "heading": -0.017424843362383667, + "angularVelocity": 0.49802152493516094, + "velocityX": 0.857002749950966, + "velocityY": -0.21832767669437975, + "timestamp": 1.0331725053892196 + }, + { + "x": 2.8526127338409437, + "y": 3.84, + "heading": 1.0917587940036658e-15, + "angularVelocity": 0.2529806485097509, + "velocityX": 0.4283775643320707, + "velocityY": -0.10904710139125373, + "timestamp": 1.1020506724151675 + }, + { + "x": 2.852612733840943, + "y": 3.84, + "heading": 5.446844655867544e-16, + "angularVelocity": -1.5954070335877785e-17, + "velocityX": 2.949719479275081e-15, + "velocityY": -7.8346554708601e-16, + "timestamp": 1.1709288394411155 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Sweep.2.traj b/src/main/deploy/choreo/4NB-Sweep.2.traj new file mode 100644 index 0000000..416ca14 --- /dev/null +++ b/src/main/deploy/choreo/4NB-Sweep.2.traj @@ -0,0 +1,166 @@ +{ + "samples": [ + { + "x": 2.852612733840943, + "y": 3.84, + "heading": 5.446844655867544e-16, + "angularVelocity": -1.5954070335877785e-17, + "velocityX": 2.949719479275081e-15, + "velocityY": -7.8346554708601e-16, + "timestamp": 0 + }, + { + "x": 2.8147339330756247, + "y": 3.8590487798371766, + "heading": -0.0009023184418920604, + "angularVelocity": -0.011161938023000674, + "velocityX": -0.4685716338028561, + "velocityY": 0.23563887213740328, + "timestamp": 0.08083886866539336 + }, + { + "x": 2.739502624002538, + "y": 3.8981587006634606, + "heading": -0.002706656651554298, + "angularVelocity": -0.022320181361403398, + "velocityX": -0.9306328788999082, + "velocityY": 0.4838009421948592, + "timestamp": 0.16167773733078672 + }, + { + "x": 2.627837725833638, + "y": 3.95895392824561, + "heading": -0.00541087612487252, + "angularVelocity": -0.033451970790332144, + "velocityX": -1.3813268296851184, + "velocityY": 0.7520544087992126, + "timestamp": 0.24251660599618008 + }, + { + "x": 2.481698476909063, + "y": 4.044430690887332, + "heading": -0.009004988998051864, + "angularVelocity": -0.04446020747836087, + "velocityX": -1.8077844400503824, + "velocityY": 1.0573720792101367, + "timestamp": 0.32335547466157344 + }, + { + "x": 2.3073008390105594, + "y": 4.161516417890298, + "heading": -0.013430705174834927, + "angularVelocity": -0.054747378950859515, + "velocityX": -2.1573488196670145, + "velocityY": 1.4483840377282375, + "timestamp": 0.4041943433269668 + }, + { + "x": 2.142960643566143, + "y": 4.319791387587546, + "heading": -0.01814503786647799, + "angularVelocity": -0.05831764805067327, + "velocityX": -2.0329353707886444, + "velocityY": 1.9579067880375274, + "timestamp": 0.48503321199236016 + }, + { + "x": 2.0202818027696092, + "y": 4.4859414730946, + "heading": -0.02220768336419149, + "angularVelocity": -0.050256090477088365, + "velocityX": -1.5175724601530889, + "velocityY": 2.0553242301643135, + "timestamp": 0.5658720806577535 + }, + { + "x": 1.939891618406941, + "y": 4.649035227213643, + "heading": -0.02546081479381014, + "angularVelocity": -0.04024216918576554, + "velocityX": -0.9944496464370033, + "velocityY": 2.017516533959881, + "timestamp": 0.6467109493231469 + }, + { + "x": 1.9013190323048412, + "y": 4.805132132072189, + "heading": -0.027864639292684273, + "angularVelocity": -0.029735998765940026, + "velocityX": -0.4771539574824876, + "velocityY": 1.9309635010437805, + "timestamp": 0.7275498179885402 + }, + { + "x": 1.9041854143142702, + "y": 4.952258110046387, + "heading": -0.02940289163556845, + "angularVelocity": -0.019028622842945542, + "velocityX": 0.03545796789034449, + "velocityY": 1.8199905615104273, + "timestamp": 0.8083886866539336 + }, + { + "x": 1.9596283985749436, + "y": 5.1050826044619315, + "heading": -0.030026698581035755, + "angularVelocity": -0.006851792079561006, + "velocityX": 0.6089765482491194, + "velocityY": 1.678598912705159, + "timestamp": 0.8994315756125946 + }, + { + "x": 2.0658010117719163, + "y": 5.240057978189405, + "heading": -0.029521094444683208, + "angularVelocity": 0.005553472018908819, + "velocityX": 1.1661823829555888, + "velocityY": 1.4825471299440058, + "timestamp": 0.9904744645712558 + }, + { + "x": 2.2166986800614077, + "y": 5.345171108591057, + "heading": -0.027897424478062212, + "angularVelocity": 0.017834121755058083, + "velocityX": 1.657434973949558, + "velocityY": 1.1545451995639058, + "timestamp": 1.081517353529917 + }, + { + "x": 2.367639272503086, + "y": 5.396505549692408, + "heading": -0.025755785506199724, + "angularVelocity": 0.02352340744409937, + "velocityX": 1.6579064457215766, + "velocityY": 0.5638489912667554, + "timestamp": 1.1725602424885782 + }, + { + "x": 2.47239628749368, + "y": 5.420285820571503, + "heading": -0.024116176964614645, + "angularVelocity": 0.01800918842030109, + "velocityX": 1.1506336869226512, + "velocityY": 0.26119855324333596, + "timestamp": 1.2636031314472393 + }, + { + "x": 2.525370597839355, + "y": 5.429551124572755, + "heading": -0.023251268502449497, + "angularVelocity": 0.009500011171194953, + "velocityX": 0.581861043202709, + "velocityY": 0.10176856322583427, + "timestamp": 1.3546460204059005 + }, + { + "x": 2.525370597839355, + "y": 5.429551124572754, + "heading": -0.023251268502449497, + "angularVelocity": 6.144566740274697e-19, + "velocityX": -1.3368031084954793e-16, + "velocityY": -2.9798986610214504e-16, + "timestamp": 1.4456889093645617 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Sweep.3.traj b/src/main/deploy/choreo/4NB-Sweep.3.traj new file mode 100644 index 0000000..a669504 --- /dev/null +++ b/src/main/deploy/choreo/4NB-Sweep.3.traj @@ -0,0 +1,184 @@ +{ + "samples": [ + { + "x": 2.525370597839355, + "y": 5.429551124572754, + "heading": -0.023251268502449497, + "angularVelocity": 6.144566740274697e-19, + "velocityX": -1.3368031084954793e-16, + "velocityY": -2.9798986610214504e-16, + "timestamp": 0 + }, + { + "x": 2.486815565414766, + "y": 5.451901360603296, + "heading": -0.022679468371633073, + "angularVelocity": 0.00689933086837991, + "velocityX": -0.4652043799964615, + "velocityY": 0.2696775762204275, + "timestamp": 0.08287762128310572 + }, + { + "x": 2.4102574903491494, + "y": 5.497528005659964, + "heading": -0.021525109258417094, + "angularVelocity": 0.013928477861023088, + "velocityX": -0.9237484604450404, + "velocityY": 0.550530340401665, + "timestamp": 0.16575524256621144 + }, + { + "x": 2.296665771182315, + "y": 5.567944363601739, + "heading": -0.019771445415369154, + "angularVelocity": 0.0211596787636741, + "velocityX": -1.3705957942351075, + "velocityY": 0.8496426037764246, + "timestamp": 0.24863286384931715 + }, + { + "x": 2.148145849920551, + "y": 5.666038112101672, + "heading": -0.01738940046354127, + "angularVelocity": 0.028741714771119673, + "velocityX": -1.7920388997945242, + "velocityY": 1.1835975379270423, + "timestamp": 0.33151048513242287 + }, + { + "x": 1.9718789441987798, + "y": 5.799004690569831, + "heading": -0.014325680038043673, + "angularVelocity": 0.036966799698945195, + "velocityX": -2.126833552807363, + "velocityY": 1.6043725243265818, + "timestamp": 0.4143881064155286 + }, + { + "x": 1.8191804764247788, + "y": 5.969793912810861, + "heading": -0.010780972710273075, + "angularVelocity": 0.04277037966210476, + "velocityX": -1.8424571725145245, + "velocityY": 2.0607399150323507, + "timestamp": 0.4972657276986343 + }, + { + "x": 1.7110457921611644, + "y": 6.140250891131977, + "heading": -0.007470809475740908, + "angularVelocity": 0.039940374533974615, + "velocityX": -1.304751301853027, + "velocityY": 2.056731065420472, + "timestamp": 0.58014334898174 + }, + { + "x": 1.646568244424728, + "y": 6.301758541946275, + "heading": -0.004541955985918456, + "angularVelocity": 0.03533949749616519, + "velocityX": -0.7779850186117685, + "velocityY": 1.9487486285664002, + "timestamp": 0.6630209702648457 + }, + { + "x": 1.6249822101406382, + "y": 6.451168602079967, + "heading": -0.0020436909380878126, + "angularVelocity": 0.030144024516542172, + "velocityX": -0.2604567306577584, + "velocityY": 1.802779300618623, + "timestamp": 0.7458985915479515 + }, + { + "x": 1.6458026170730593, + "y": 6.586877346038818, + "heading": -8.583319940280204e-19, + "angularVelocity": 0.024659140868759512, + "velocityX": 0.2512186837662745, + "velocityY": 1.6374594475301991, + "timestamp": 0.8287762128310572 + }, + { + "x": 1.7175854091318359, + "y": 6.716530775561853, + "heading": 0.001662521310123723, + "angularVelocity": 0.0185413273788438, + "velocityX": 0.8005601128987638, + "velocityY": 1.4459644324183467, + "timestamp": 0.9184419242525976 + }, + { + "x": 1.8379608407999335, + "y": 6.827214719472097, + "heading": 0.002751500963366046, + "angularVelocity": 0.012144883880112818, + "velocityX": 1.342491235051747, + "velocityY": 1.2344065770012334, + "timestamp": 1.008107635674138 + }, + { + "x": 2.005463287958749, + "y": 6.915535164819774, + "heading": 0.003222193903074653, + "angularVelocity": 0.005249419563468997, + "velocityX": 1.8680769326788147, + "velocityY": 0.9849968728008089, + "timestamp": 1.0977733470956785 + }, + { + "x": 2.215062055192105, + "y": 6.97305024616571, + "heading": 0.002979401978971678, + "angularVelocity": -0.0027077454720852664, + "velocityX": 2.3375576227570494, + "velocityY": 0.6414389674057639, + "timestamp": 1.187439058517219 + }, + { + "x": 2.4217154464231823, + "y": 6.978484776468596, + "heading": 0.0019991193069041372, + "angularVelocity": -0.010932636974896442, + "velocityX": 2.304709213308401, + "velocityY": 0.06060879032495919, + "timestamp": 1.2771047699387594 + }, + { + "x": 2.5778319799768425, + "y": 6.970991314979555, + "heading": 0.0010539012563375312, + "angularVelocity": -0.01054157755045176, + "velocityX": 1.741095130776553, + "velocityY": -0.08357109278720787, + "timestamp": 1.3667704813602999 + }, + { + "x": 2.6817937055315064, + "y": 6.962507949629537, + "heading": 0.0003631357733237097, + "angularVelocity": -0.007703786342210105, + "velocityX": 1.1594368003830842, + "velocityY": -0.09461103040977602, + "timestamp": 1.4564361927818408 + }, + { + "x": 2.733689308166504, + "y": 6.957221508026124, + "heading": 2.397688145356894e-19, + "angularVelocity": -0.004049884482782016, + "velocityX": 0.5787675334557183, + "velocityY": -0.0589572258960861, + "timestamp": 1.5461019042033812 + }, + { + "x": 2.733689308166504, + "y": 6.957221508026123, + "heading": 1.2133947797260325e-19, + "angularVelocity": 3.240031726812772e-20, + "velocityX": 1.3625265205891428e-17, + "velocityY": -9.676993787863113e-17, + "timestamp": 1.6357676156249217 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-Sweep.traj b/src/main/deploy/choreo/4NB-Sweep.traj new file mode 100644 index 0000000..062534f --- /dev/null +++ b/src/main/deploy/choreo/4NB-Sweep.traj @@ -0,0 +1,490 @@ +{ + "samples": [ + { + "x": 0.7265444397926323, + "y": 4.389017105102539, + "heading": -1.05165023914004, + "angularVelocity": -2.4052301236587744e-16, + "velocityX": -2.752276616089782e-15, + "velocityY": 5.134530261429221e-16, + "timestamp": 0 + }, + { + "x": 0.7559653796741465, + "y": 4.38140433106999, + "heading": -1.0337573727433125, + "angularVelocity": 0.2597755888304552, + "velocityX": 0.4271446403391007, + "velocityY": -0.11052521228797353, + "timestamp": 0.06887816702594797 + }, + { + "x": 0.8148295651894585, + "y": 4.366167962208403, + "heading": -0.9984651090693958, + "angularVelocity": 0.5123868011850714, + "velocityX": 0.8546131242594956, + "velocityY": -0.22120752510512612, + "timestamp": 0.13775633405189594 + }, + { + "x": 0.9031640621745638, + "y": 4.34329522415188, + "heading": -0.9463836724391982, + "angularVelocity": 0.7561385396707215, + "velocityX": 1.2824745605066483, + "velocityY": -0.3320753011314312, + "timestamp": 0.20663450107784392 + }, + { + "x": 1.0210050595189104, + "y": 4.312770957025727, + "heading": -0.8783834326740236, + "angularVelocity": 0.987253910801006, + "velocityX": 1.7108614011164693, + "velocityY": -0.4431631741108122, + "timestamp": 0.2755126681037919 + }, + { + "x": 1.1684052822191313, + "y": 4.274574303329794, + "heading": -0.7959106115927722, + "angularVelocity": 1.1973724714564178, + "velocityX": 2.1400137237202137, + "velocityY": -0.5545538643841008, + "timestamp": 0.34439083512973984 + }, + { + "x": 1.3454446954006436, + "y": 4.228667204890838, + "heading": -0.7017429838335593, + "angularVelocity": 1.367162220268308, + "velocityX": 2.5703270110950878, + "velocityY": -0.6664970980087429, + "timestamp": 0.4132690021556878 + }, + { + "x": 1.552227621121227, + "y": 4.174960199018195, + "heading": -0.6022461552568997, + "angularVelocity": 1.4445336290550153, + "velocityX": 3.002154886652024, + "velocityY": -0.779739185748209, + "timestamp": 0.48214716918163575 + }, + { + "x": 1.7883744114232694, + "y": 4.113221891555757, + "heading": -0.5202856320014997, + "angularVelocity": 1.1899347325041563, + "velocityX": 3.428470885601271, + "velocityY": -0.8963407437828599, + "timestamp": 0.5510253362075838 + }, + { + "x": 2.02506710191313, + "y": 4.0518739432063136, + "heading": -0.4352693076854395, + "angularVelocity": 1.2343000400117028, + "velocityX": 3.436396476704921, + "velocityY": -0.8906733584581512, + "timestamp": 0.6199035032335317 + }, + { + "x": 2.232210219377279, + "y": 3.9985845821566475, + "heading": -0.3384412768078663, + "angularVelocity": 1.4057869867689714, + "velocityX": 3.0073842903820833, + "velocityY": -0.773675655880783, + "timestamp": 0.6887816702594797 + }, + { + "x": 2.409618392670172, + "y": 3.9530993682814772, + "heading": -0.2475826023369981, + "angularVelocity": 1.319121550325896, + "velocityX": 2.5756808137193876, + "velocityY": -0.6603720139363624, + "timestamp": 0.7576598372854276 + }, + { + "x": 2.5573654312197593, + "y": 3.9153053743884563, + "heading": -0.16800100720308944, + "angularVelocity": 1.1553965294101876, + "velocityX": 2.1450489310194327, + "velocityY": -0.5487078928622399, + "timestamp": 0.8265380043113756 + }, + { + "x": 2.67550632370007, + "y": 3.885136654824466, + "heading": -0.10223202695817188, + "angularVelocity": 0.95485961785463, + "velocityX": 1.7152153952616722, + "velocityY": -0.4380011964114144, + "timestamp": 0.8954161713373235 + }, + { + "x": 2.764078093861892, + "y": 3.8625489746450645, + "heading": -0.05172765313938082, + "angularVelocity": 0.7332421288123194, + "velocityX": 1.2859193847080135, + "velocityY": -0.3279367200769452, + "timestamp": 0.9642943383632715 + }, + { + "x": 2.8231068724147113, + "y": 3.847510964463322, + "heading": -0.017424843362383667, + "angularVelocity": 0.49802152493516094, + "velocityX": 0.857002749950966, + "velocityY": -0.21832767669437975, + "timestamp": 1.0331725053892196 + }, + { + "x": 2.8526127338409437, + "y": 3.84, + "heading": 1.0917587940036658e-15, + "angularVelocity": 0.2529806485097509, + "velocityX": 0.4283775643320707, + "velocityY": -0.10904710139125373, + "timestamp": 1.1020506724151675 + }, + { + "x": 2.852612733840943, + "y": 3.84, + "heading": 5.446844655867544e-16, + "angularVelocity": -1.5954070335877785e-17, + "velocityX": 2.949719479275081e-15, + "velocityY": -7.8346554708601e-16, + "timestamp": 1.1709288394411155 + }, + { + "x": 2.8147339330756247, + "y": 3.8590487798371766, + "heading": -0.0009023184418920604, + "angularVelocity": -0.011161938023000674, + "velocityX": -0.4685716338028561, + "velocityY": 0.23563887213740328, + "timestamp": 1.2517677081065088 + }, + { + "x": 2.739502624002538, + "y": 3.8981587006634606, + "heading": -0.002706656651554298, + "angularVelocity": -0.022320181361403398, + "velocityX": -0.9306328788999082, + "velocityY": 0.4838009421948592, + "timestamp": 1.3326065767719022 + }, + { + "x": 2.627837725833638, + "y": 3.95895392824561, + "heading": -0.00541087612487252, + "angularVelocity": -0.033451970790332144, + "velocityX": -1.3813268296851184, + "velocityY": 0.7520544087992126, + "timestamp": 1.4134454454372956 + }, + { + "x": 2.481698476909063, + "y": 4.044430690887332, + "heading": -0.009004988998051864, + "angularVelocity": -0.04446020747836087, + "velocityX": -1.8077844400503824, + "velocityY": 1.0573720792101367, + "timestamp": 1.494284314102689 + }, + { + "x": 2.3073008390105594, + "y": 4.161516417890298, + "heading": -0.013430705174834927, + "angularVelocity": -0.054747378950859515, + "velocityX": -2.1573488196670145, + "velocityY": 1.4483840377282375, + "timestamp": 1.5751231827680823 + }, + { + "x": 2.142960643566143, + "y": 4.319791387587546, + "heading": -0.01814503786647799, + "angularVelocity": -0.05831764805067327, + "velocityX": -2.0329353707886444, + "velocityY": 1.9579067880375274, + "timestamp": 1.6559620514334756 + }, + { + "x": 2.0202818027696092, + "y": 4.4859414730946, + "heading": -0.02220768336419149, + "angularVelocity": -0.050256090477088365, + "velocityX": -1.5175724601530889, + "velocityY": 2.0553242301643135, + "timestamp": 1.736800920098869 + }, + { + "x": 1.939891618406941, + "y": 4.649035227213643, + "heading": -0.02546081479381014, + "angularVelocity": -0.04024216918576554, + "velocityX": -0.9944496464370033, + "velocityY": 2.017516533959881, + "timestamp": 1.8176397887642624 + }, + { + "x": 1.9013190323048412, + "y": 4.805132132072189, + "heading": -0.027864639292684273, + "angularVelocity": -0.029735998765940026, + "velocityX": -0.4771539574824876, + "velocityY": 1.9309635010437805, + "timestamp": 1.8984786574296557 + }, + { + "x": 1.9041854143142702, + "y": 4.952258110046387, + "heading": -0.02940289163556845, + "angularVelocity": -0.019028622842945542, + "velocityX": 0.03545796789034449, + "velocityY": 1.8199905615104273, + "timestamp": 1.979317526095049 + }, + { + "x": 1.9596283985749436, + "y": 5.1050826044619315, + "heading": -0.030026698581035755, + "angularVelocity": -0.006851792079561006, + "velocityX": 0.6089765482491194, + "velocityY": 1.678598912705159, + "timestamp": 2.07036041505371 + }, + { + "x": 2.0658010117719163, + "y": 5.240057978189405, + "heading": -0.029521094444683208, + "angularVelocity": 0.005553472018908819, + "velocityX": 1.1661823829555888, + "velocityY": 1.4825471299440058, + "timestamp": 2.1614033040123712 + }, + { + "x": 2.2166986800614077, + "y": 5.345171108591057, + "heading": -0.027897424478062212, + "angularVelocity": 0.017834121755058083, + "velocityX": 1.657434973949558, + "velocityY": 1.1545451995639058, + "timestamp": 2.2524461929710324 + }, + { + "x": 2.367639272503086, + "y": 5.396505549692408, + "heading": -0.025755785506199724, + "angularVelocity": 0.02352340744409937, + "velocityX": 1.6579064457215766, + "velocityY": 0.5638489912667554, + "timestamp": 2.3434890819296936 + }, + { + "x": 2.47239628749368, + "y": 5.420285820571503, + "heading": -0.024116176964614645, + "angularVelocity": 0.01800918842030109, + "velocityX": 1.1506336869226512, + "velocityY": 0.26119855324333596, + "timestamp": 2.434531970888355 + }, + { + "x": 2.525370597839355, + "y": 5.429551124572755, + "heading": -0.023251268502449497, + "angularVelocity": 0.009500011171194953, + "velocityX": 0.581861043202709, + "velocityY": 0.10176856322583427, + "timestamp": 2.525574859847016 + }, + { + "x": 2.525370597839355, + "y": 5.429551124572754, + "heading": -0.023251268502449497, + "angularVelocity": 6.144566740274697e-19, + "velocityX": -1.3368031084954793e-16, + "velocityY": -2.9798986610214504e-16, + "timestamp": 2.616617748805677 + }, + { + "x": 2.486815565414766, + "y": 5.451901360603296, + "heading": -0.022679468371633073, + "angularVelocity": 0.00689933086837991, + "velocityX": -0.4652043799964615, + "velocityY": 0.2696775762204275, + "timestamp": 2.699495370088783 + }, + { + "x": 2.4102574903491494, + "y": 5.497528005659964, + "heading": -0.021525109258417094, + "angularVelocity": 0.013928477861023088, + "velocityX": -0.9237484604450404, + "velocityY": 0.550530340401665, + "timestamp": 2.7823729913718886 + }, + { + "x": 2.296665771182315, + "y": 5.567944363601739, + "heading": -0.019771445415369154, + "angularVelocity": 0.0211596787636741, + "velocityX": -1.3705957942351075, + "velocityY": 0.8496426037764246, + "timestamp": 2.8652506126549944 + }, + { + "x": 2.148145849920551, + "y": 5.666038112101672, + "heading": -0.01738940046354127, + "angularVelocity": 0.028741714771119673, + "velocityX": -1.7920388997945242, + "velocityY": 1.1835975379270423, + "timestamp": 2.9481282339381 + }, + { + "x": 1.9718789441987798, + "y": 5.799004690569831, + "heading": -0.014325680038043673, + "angularVelocity": 0.036966799698945195, + "velocityX": -2.126833552807363, + "velocityY": 1.6043725243265818, + "timestamp": 3.031005855221206 + }, + { + "x": 1.8191804764247788, + "y": 5.969793912810861, + "heading": -0.010780972710273075, + "angularVelocity": 0.04277037966210476, + "velocityX": -1.8424571725145245, + "velocityY": 2.0607399150323507, + "timestamp": 3.1138834765043115 + }, + { + "x": 1.7110457921611644, + "y": 6.140250891131977, + "heading": -0.007470809475740908, + "angularVelocity": 0.039940374533974615, + "velocityX": -1.304751301853027, + "velocityY": 2.056731065420472, + "timestamp": 3.1967610977874172 + }, + { + "x": 1.646568244424728, + "y": 6.301758541946275, + "heading": -0.004541955985918456, + "angularVelocity": 0.03533949749616519, + "velocityX": -0.7779850186117685, + "velocityY": 1.9487486285664002, + "timestamp": 3.279638719070523 + }, + { + "x": 1.6249822101406382, + "y": 6.451168602079967, + "heading": -0.0020436909380878126, + "angularVelocity": 0.030144024516542172, + "velocityX": -0.2604567306577584, + "velocityY": 1.802779300618623, + "timestamp": 3.3625163403536287 + }, + { + "x": 1.6458026170730593, + "y": 6.586877346038818, + "heading": -8.583319940280204e-19, + "angularVelocity": 0.024659140868759512, + "velocityX": 0.2512186837662745, + "velocityY": 1.6374594475301991, + "timestamp": 3.4453939616367344 + }, + { + "x": 1.7175854091318359, + "y": 6.716530775561853, + "heading": 0.001662521310123723, + "angularVelocity": 0.0185413273788438, + "velocityX": 0.8005601128987638, + "velocityY": 1.4459644324183467, + "timestamp": 3.535059673058275 + }, + { + "x": 1.8379608407999335, + "y": 6.827214719472097, + "heading": 0.002751500963366046, + "angularVelocity": 0.012144883880112818, + "velocityX": 1.342491235051747, + "velocityY": 1.2344065770012334, + "timestamp": 3.6247253844798153 + }, + { + "x": 2.005463287958749, + "y": 6.915535164819774, + "heading": 0.003222193903074653, + "angularVelocity": 0.005249419563468997, + "velocityX": 1.8680769326788147, + "velocityY": 0.9849968728008089, + "timestamp": 3.7143910959013557 + }, + { + "x": 2.215062055192105, + "y": 6.97305024616571, + "heading": 0.002979401978971678, + "angularVelocity": -0.0027077454720852664, + "velocityX": 2.3375576227570494, + "velocityY": 0.6414389674057639, + "timestamp": 3.804056807322896 + }, + { + "x": 2.4217154464231823, + "y": 6.978484776468596, + "heading": 0.0019991193069041372, + "angularVelocity": -0.010932636974896442, + "velocityX": 2.304709213308401, + "velocityY": 0.06060879032495919, + "timestamp": 3.8937225187444366 + }, + { + "x": 2.5778319799768425, + "y": 6.970991314979555, + "heading": 0.0010539012563375312, + "angularVelocity": -0.01054157755045176, + "velocityX": 1.741095130776553, + "velocityY": -0.08357109278720787, + "timestamp": 3.983388230165977 + }, + { + "x": 2.6817937055315064, + "y": 6.962507949629537, + "heading": 0.0003631357733237097, + "angularVelocity": -0.007703786342210105, + "velocityX": 1.1594368003830842, + "velocityY": -0.09461103040977602, + "timestamp": 4.073053941587518 + }, + { + "x": 2.733689308166504, + "y": 6.957221508026124, + "heading": 2.397688145356894e-19, + "angularVelocity": -0.004049884482782016, + "velocityX": 0.5787675334557183, + "velocityY": -0.0589572258960861, + "timestamp": 4.162719653009058 + }, + { + "x": 2.733689308166504, + "y": 6.957221508026123, + "heading": 1.2133947797260325e-19, + "angularVelocity": 3.240031726812772e-20, + "velocityX": 1.3625265205891428e-17, + "velocityY": -9.676993787863113e-17, + "timestamp": 4.252385364430599 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-SweepStraight.1.traj b/src/main/deploy/choreo/4NB-SweepStraight.1.traj new file mode 100644 index 0000000..a28e39b --- /dev/null +++ b/src/main/deploy/choreo/4NB-SweepStraight.1.traj @@ -0,0 +1,328 @@ +{ + "samples": [ + { + "x": 0.727, + "y": 4.389017105102539, + "heading": -1.052, + "angularVelocity": 7.985342564187253e-19, + "velocityX": -9.880504926804143e-18, + "velocityY": 4.541723618252651e-18, + "timestamp": 0 + }, + { + "x": 0.7320210369883334, + "y": 4.379190444759289, + "heading": -1.0132964977053183, + "angularVelocity": 0.8277410042322235, + "velocityX": 0.10738351706200194, + "velocityY": -0.2101600428523119, + "timestamp": 0.04675798600860395 + }, + { + "x": 0.7423767585467538, + "y": 4.35932167780024, + "heading": -0.9383484333304679, + "angularVelocity": 1.6028933402086725, + "velocityX": 0.22147492743864414, + "velocityY": -0.4249277750202023, + "timestamp": 0.0935159720172079 + }, + { + "x": 0.7584185968395437, + "y": 4.329046618725402, + "heading": -0.831097731487712, + "angularVelocity": 2.293740834408298, + "velocityX": 0.3430823194535392, + "velocityY": -0.6474842408585403, + "timestamp": 0.14027395802581186 + }, + { + "x": 0.7803726052330767, + "y": 4.287745603688506, + "heading": -0.6974263117481472, + "angularVelocity": 2.858793355958877, + "velocityX": 0.46952425173997964, + "velocityY": -0.8832932844737105, + "timestamp": 0.1870319440344158 + }, + { + "x": 0.8081702179949359, + "y": 4.234654918624728, + "heading": -0.5439140483187206, + "angularVelocity": 3.2831239438120146, + "velocityX": 0.5944997878390745, + "velocityY": -1.135435667695887, + "timestamp": 0.23378993004301976 + }, + { + "x": 0.8416609953282266, + "y": 4.169186742844337, + "heading": -0.3760252466197787, + "angularVelocity": 3.590590956331753, + "velocityX": 0.7162579099789393, + "velocityY": -1.400149607991124, + "timestamp": 0.2805479160516237 + }, + { + "x": 0.8808506607064412, + "y": 4.0909034261415895, + "heading": -0.20048698115793326, + "angularVelocity": 3.7541879034178507, + "velocityX": 0.838138438449452, + "velocityY": -1.6742234511200584, + "timestamp": 0.3273059020602277 + }, + { + "x": 0.925668806197045, + "y": 3.9996480546729565, + "heading": -0.02893711316846682, + "angularVelocity": 3.6688891595497974, + "velocityX": 0.9585131721127651, + "velocityY": -1.9516531668372759, + "timestamp": 0.3740638880688316 + }, + { + "x": 0.9754949593831373, + "y": 3.896282996276425, + "heading": 0.12253671854863268, + "angularVelocity": 3.2395285735590016, + "velocityX": 1.0656180353247193, + "velocityY": -2.2106396622276985, + "timestamp": 0.42082187407743554 + }, + { + "x": 1.0309514620598907, + "y": 3.781043042077509, + "heading": 0.2541852806699216, + "angularVelocity": 2.815531064508889, + "velocityX": 1.1860327488595344, + "velocityY": -2.4646047453299187, + "timestamp": 0.46757986008603947 + }, + { + "x": 1.0929356681499833, + "y": 3.6545108746667574, + "heading": 0.36498378647979907, + "angularVelocity": 2.3696167279161107, + "velocityX": 1.3256389203520678, + "velocityY": -2.7061081584525786, + "timestamp": 0.5143378460946434 + }, + { + "x": 1.1629696592047158, + "y": 3.5179953972400937, + "heading": 0.45299398894595083, + "angularVelocity": 1.882249642867833, + "velocityX": 1.4977974252790578, + "velocityY": -2.9196184241444185, + "timestamp": 0.5610958321032473 + }, + { + "x": 1.2444134330422476, + "y": 3.375806208333299, + "heading": 0.5186903236816397, + "angularVelocity": 1.4050291798979542, + "velocityX": 1.7418152660097566, + "velocityY": -3.0409605084502798, + "timestamp": 0.6078538181118512 + }, + { + "x": 1.3382986621093096, + "y": 3.240381730316091, + "heading": 0.5812504134266591, + "angularVelocity": 1.3379551833759529, + "velocityX": 2.00789719749598, + "velocityY": -2.896285524197266, + "timestamp": 0.6546118041204552 + }, + { + "x": 1.438124876158876, + "y": 3.1172857608464914, + "heading": 0.6580009966257694, + "angularVelocity": 1.6414433073585977, + "velocityX": 2.1349553856095276, + "velocityY": -2.632619151880301, + "timestamp": 0.7013697901290591 + }, + { + "x": 1.5413035249725158, + "y": 3.007118135101827, + "heading": 0.7526515078768561, + "angularVelocity": 2.024264074038882, + "velocityX": 2.206652972484545, + "velocityY": -2.3561242719990694, + "timestamp": 0.748127776137663 + }, + { + "x": 1.6457474356666648, + "y": 2.910401613533236, + "heading": 0.8633543586975786, + "angularVelocity": 2.3675709813660726, + "velocityX": 2.2337127752831343, + "velocityY": -2.068449260213815, + "timestamp": 0.7948857621462669 + }, + { + "x": 1.749997834476469, + "y": 2.8273646542502275, + "heading": 0.9880259417022461, + "angularVelocity": 2.666316358922014, + "velocityX": 2.2295741906093727, + "velocityY": -1.7758882785866068, + "timestamp": 0.8416437481548709 + }, + { + "x": 1.8521934124271529, + "y": 2.758362183574911, + "heading": 1.1133113758268665, + "angularVelocity": 2.6794446215418817, + "velocityX": 2.185628310247731, + "velocityY": -1.4757365867426573, + "timestamp": 0.8884017341634748 + }, + { + "x": 1.95145157552739, + "y": 2.7031033818343477, + "heading": 1.231381145561192, + "angularVelocity": 2.525125220594789, + "velocityX": 2.122806638461338, + "velocityY": -1.181804574097173, + "timestamp": 0.9351597201720787 + }, + { + "x": 2.0473220216152592, + "y": 2.6612321563456565, + "heading": 1.3371714258033476, + "angularVelocity": 2.2625072051406683, + "velocityX": 2.0503544799845086, + "velocityY": -0.8954882163008596, + "timestamp": 0.9819177061806826 + }, + { + "x": 2.139580608011859, + "y": 2.6324205589828935, + "heading": 1.4268444625557797, + "angularVelocity": 1.9178122157799795, + "velocityX": 1.973108644575945, + "velocityY": -0.6161855935675581, + "timestamp": 1.0286756921892866 + }, + { + "x": 2.2281178382739433, + "y": 2.6163741225527475, + "heading": 1.4972552356256885, + "angularVelocity": 1.5058555571031602, + "velocityX": 1.8935210392881243, + "velocityY": -0.3431806585331752, + "timestamp": 1.0754336781978906 + }, + { + "x": 2.312878410436143, + "y": 2.6128372160061697, + "heading": 1.5458711009980197, + "angularVelocity": 1.0397339475538696, + "velocityX": 1.8127507062977068, + "velocityY": -0.07564283341668805, + "timestamp": 1.1221916642064946 + }, + { + "x": 2.393836402463875, + "y": 2.621604064143261, + "heading": 1.5707963267948966, + "angularVelocity": 0.5330688492938996, + "velocityX": 1.7314259859865824, + "velocityY": 0.18749413491648587, + "timestamp": 1.1689496502150987 + }, + { + "x": 2.470987558364868, + "y": 2.642529249191284, + "heading": 1.5707963267948966, + "angularVelocity": -1.4908159190516835e-18, + "velocityX": 1.6500102439562863, + "velocityY": 0.44752109391968486, + "timestamp": 1.2157076362237027 + }, + { + "x": 2.602813969514969, + "y": 2.734000351322527, + "heading": 1.57075571228945, + "angularVelocity": -0.0004452490507007872, + "velocityX": 1.4451877174529841, + "velocityY": 1.0027801875851559, + "timestamp": 1.3069251365927887 + }, + { + "x": 2.713939411568514, + "y": 2.8753302073709235, + "heading": 1.570674784882514, + "angularVelocity": -0.0008871916749376329, + "velocityX": 1.2182469548490686, + "velocityY": 1.5493721651707555, + "timestamp": 1.3981426369618748 + }, + { + "x": 2.8004764091180525, + "y": 3.064720789521499, + "heading": 1.5705543763017473, + "angularVelocity": -0.001320016228019663, + "velocityX": 0.9486885433102401, + "velocityY": 2.076252707914343, + "timestamp": 1.4893601373309608 + }, + { + "x": 2.8524060828186255, + "y": 3.2955451367079904, + "heading": 1.5703982906404752, + "angularVelocity": -0.0017111372339782035, + "velocityX": 0.5692950748534504, + "velocityY": 2.5304831447084086, + "timestamp": 1.580577637700047 + }, + { + "x": 2.852926988468019, + "y": 3.509890434151461, + "heading": 1.5702419946817163, + "angularVelocity": -0.0017134426850825867, + "velocityX": 0.005710588946470057, + "velocityY": 2.3498264760185585, + "timestamp": 1.671795138069133 + }, + { + "x": 2.8426506520730745, + "y": 3.671341086515572, + "heading": 1.5701215497677061, + "angularVelocity": -0.0013204145424167311, + "velocityX": -0.11265750928735839, + "velocityY": 1.7699526046084872, + "timestamp": 1.763012638438219 + }, + { + "x": 2.832422777582534, + "y": 3.778806345465944, + "heading": 1.570040615176241, + "angularVelocity": -0.0008872704375541147, + "velocityX": -0.11212623070348748, + "velocityY": 1.1781210679481708, + "timestamp": 1.854230138807305 + }, + { + "x": 2.82627534866333, + "y": 3.8324406147003174, + "heading": 1.57, + "angularVelocity": -0.0004452564044901184, + "velocityX": -0.06739308679091216, + "velocityY": 0.5879822294775167, + "timestamp": 1.945447639176391 + }, + { + "x": 2.82627534866333, + "y": 3.8324406147003174, + "heading": 1.57, + "angularVelocity": -2.8295730504754553e-24, + "velocityX": -1.0072066101835765e-17, + "velocityY": 4.887478241972941e-18, + "timestamp": 2.036665139545477 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-SweepStraight.2.traj b/src/main/deploy/choreo/4NB-SweepStraight.2.traj new file mode 100644 index 0000000..231b9b0 --- /dev/null +++ b/src/main/deploy/choreo/4NB-SweepStraight.2.traj @@ -0,0 +1,112 @@ +{ + "samples": [ + { + "x": 2.82627534866333, + "y": 3.8324406147003174, + "heading": 1.57, + "angularVelocity": -2.8295730504754553e-24, + "velocityX": -1.0072066101835765e-17, + "velocityY": 4.887478241972941e-18, + "timestamp": 0 + }, + { + "x": 2.82627534866333, + "y": 3.887160790336596, + "heading": 1.57, + "angularVelocity": -7.223726232843347e-17, + "velocityX": -1.4022322773048193e-16, + "velocityY": 0.595845723766963, + "timestamp": 0.09183614726700551 + }, + { + "x": 2.82627534866333, + "y": 3.9966011399802817, + "heading": 1.57, + "angularVelocity": -1.4517284366324747e-16, + "velocityX": -2.7085764742439913e-16, + "velocityY": 1.1916914297972172, + "timestamp": 0.18367229453401102 + }, + { + "x": 2.82627534866333, + "y": 4.16076166091659, + "heading": 1.57, + "angularVelocity": -2.1926697371411643e-16, + "velocityX": -4.0231930155833035e-16, + "velocityY": 1.7875371062662906, + "timestamp": 0.2755084418010165 + }, + { + "x": 2.82627534866333, + "y": 4.379642347715951, + "heading": 1.57, + "angularVelocity": -2.958096299138811e-16, + "velocityX": -5.355279601742931e-16, + "velocityY": 2.3833827236130065, + "timestamp": 0.36734458906802203 + }, + { + "x": 2.82627534866333, + "y": 4.653243184089661, + "heading": 1.57, + "angularVelocity": -3.783476918941958e-16, + "velocityX": -6.748965699405944e-16, + "velocityY": 2.979228163592689, + "timestamp": 0.45918073633502754 + }, + { + "x": 2.82627534866333, + "y": 4.9268440204633706, + "heading": 1.57, + "angularVelocity": -3.641716874889364e-16, + "velocityX": -6.159722745525841e-16, + "velocityY": 2.979228163592689, + "timestamp": 0.551016883602033 + }, + { + "x": 2.82627534866333, + "y": 5.1457247072627315, + "heading": 1.57, + "angularVelocity": -2.879074384838342e-16, + "velocityX": -4.995322375549383e-16, + "velocityY": 2.3833827236130065, + "timestamp": 0.6428530308690386 + }, + { + "x": 2.82627534866333, + "y": 5.30988522819904, + "heading": 1.57, + "angularVelocity": -2.1475953045710015e-16, + "velocityX": -3.7669901554704897e-16, + "velocityY": 1.7875371062662906, + "timestamp": 0.7346891781360441 + }, + { + "x": 2.82627534866333, + "y": 5.419325577842725, + "heading": 1.57, + "angularVelocity": -1.426506525121705e-16, + "velocityX": -2.519305992761992e-16, + "velocityY": 1.1916914297972172, + "timestamp": 0.8265253254030496 + }, + { + "x": 2.82627534866333, + "y": 5.474045753479004, + "heading": 1.57, + "angularVelocity": -7.113697583370428e-17, + "velocityX": -1.262362732724396e-16, + "velocityY": 0.5958457237669631, + "timestamp": 0.9183614726700551 + }, + { + "x": 2.82627534866333, + "y": 5.474045753479004, + "heading": 1.57, + "angularVelocity": 1.033945689540126e-28, + "velocityX": -3.228114037955519e-28, + "velocityY": 3.3712113107609656e-23, + "timestamp": 1.0101976199370606 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-SweepStraight.3.traj b/src/main/deploy/choreo/4NB-SweepStraight.3.traj new file mode 100644 index 0000000..3f7dced --- /dev/null +++ b/src/main/deploy/choreo/4NB-SweepStraight.3.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 2.82627534866333, + "y": 5.474045753479004, + "heading": 1.57, + "angularVelocity": 1.033945689540126e-28, + "velocityX": -3.228114037955519e-28, + "velocityY": 3.3712113107609656e-23, + "timestamp": 0 + }, + { + "x": 2.82627534866333, + "y": 5.533372789188761, + "heading": 1.57, + "angularVelocity": -8.890513472633016e-17, + "velocityX": -2.241316397130285e-16, + "velocityY": 0.6204208876250313, + "timestamp": 0.09562385292484521 + }, + { + "x": 2.82627534866333, + "y": 5.652026858634037, + "heading": 1.57, + "angularVelocity": -1.7783037339245997e-16, + "velocityX": -4.482632719673846e-16, + "velocityY": 1.2408417546041586, + "timestamp": 0.19124770584969042 + }, + { + "x": 2.82627534866333, + "y": 5.830007958261196, + "heading": 1.57, + "angularVelocity": -2.666888193558669e-16, + "velocityX": -6.723948907964149e-16, + "velocityY": 1.8612625844206605, + "timestamp": 0.28687155877453563 + }, + { + "x": 2.82627534866333, + "y": 6.067316079778427, + "heading": 1.57, + "angularVelocity": -3.5609571634279607e-16, + "velocityX": -8.965264783008826e-16, + "velocityY": 2.4816833275243777, + "timestamp": 0.38249541169938084 + }, + { + "x": 2.82627534866333, + "y": 6.3639511817267, + "heading": 1.57, + "angularVelocity": -4.462891867937391e-16, + "velocityX": -1.1206579091688093e-15, + "velocityY": 3.102103637064379, + "timestamp": 0.47811926462422605 + }, + { + "x": 2.82627534866333, + "y": 6.601259303243931, + "heading": 1.57, + "angularVelocity": -3.515360973110671e-16, + "velocityX": -8.965264782972495e-16, + "velocityY": 2.4816833275243777, + "timestamp": 0.5737431175490713 + }, + { + "x": 2.82627534866333, + "y": 6.77924040287109, + "heading": 1.57, + "angularVelocity": -2.6388235640692793e-16, + "velocityX": -6.723948907917458e-16, + "velocityY": 1.8612625844206607, + "timestamp": 0.6693669704739165 + }, + { + "x": 2.82627534866333, + "y": 6.8978944723163655, + "heading": 1.57, + "angularVelocity": -1.7617660412854568e-16, + "velocityX": -4.482632719646236e-16, + "velocityY": 1.2408417546041588, + "timestamp": 0.7649908233987617 + }, + { + "x": 2.82627534866333, + "y": 6.957221508026123, + "heading": 1.57, + "angularVelocity": -8.819964498314495e-17, + "velocityX": -2.241316397116374e-16, + "velocityY": 0.6204208876250313, + "timestamp": 0.8606146763236069 + }, + { + "x": 2.82627534866333, + "y": 6.957221508026123, + "heading": 1.57, + "angularVelocity": 2.2261004408365928e-29, + "velocityX": 5.7622977196112705e-39, + "velocityY": -1.7124712130623078e-23, + "timestamp": 0.9562385292484525 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NB-SweepStraight.traj b/src/main/deploy/choreo/4NB-SweepStraight.traj new file mode 100644 index 0000000..ac0807c --- /dev/null +++ b/src/main/deploy/choreo/4NB-SweepStraight.traj @@ -0,0 +1,517 @@ +{ + "samples": [ + { + "x": 0.727, + "y": 4.389017105102539, + "heading": -1.052, + "angularVelocity": 7.985342564187253e-19, + "velocityX": -9.880504926804143e-18, + "velocityY": 4.541723618252651e-18, + "timestamp": 0 + }, + { + "x": 0.7320210369883334, + "y": 4.379190444759289, + "heading": -1.0132964977053183, + "angularVelocity": 0.8277410042322235, + "velocityX": 0.10738351706200194, + "velocityY": -0.2101600428523119, + "timestamp": 0.04675798600860395 + }, + { + "x": 0.7423767585467538, + "y": 4.35932167780024, + "heading": -0.9383484333304679, + "angularVelocity": 1.6028933402086725, + "velocityX": 0.22147492743864414, + "velocityY": -0.4249277750202023, + "timestamp": 0.0935159720172079 + }, + { + "x": 0.7584185968395437, + "y": 4.329046618725402, + "heading": -0.831097731487712, + "angularVelocity": 2.293740834408298, + "velocityX": 0.3430823194535392, + "velocityY": -0.6474842408585403, + "timestamp": 0.14027395802581186 + }, + { + "x": 0.7803726052330767, + "y": 4.287745603688506, + "heading": -0.6974263117481472, + "angularVelocity": 2.858793355958877, + "velocityX": 0.46952425173997964, + "velocityY": -0.8832932844737105, + "timestamp": 0.1870319440344158 + }, + { + "x": 0.8081702179949359, + "y": 4.234654918624728, + "heading": -0.5439140483187206, + "angularVelocity": 3.2831239438120146, + "velocityX": 0.5944997878390745, + "velocityY": -1.135435667695887, + "timestamp": 0.23378993004301976 + }, + { + "x": 0.8416609953282266, + "y": 4.169186742844337, + "heading": -0.3760252466197787, + "angularVelocity": 3.590590956331753, + "velocityX": 0.7162579099789393, + "velocityY": -1.400149607991124, + "timestamp": 0.2805479160516237 + }, + { + "x": 0.8808506607064412, + "y": 4.0909034261415895, + "heading": -0.20048698115793326, + "angularVelocity": 3.7541879034178507, + "velocityX": 0.838138438449452, + "velocityY": -1.6742234511200584, + "timestamp": 0.3273059020602277 + }, + { + "x": 0.925668806197045, + "y": 3.9996480546729565, + "heading": -0.02893711316846682, + "angularVelocity": 3.6688891595497974, + "velocityX": 0.9585131721127651, + "velocityY": -1.9516531668372759, + "timestamp": 0.3740638880688316 + }, + { + "x": 0.9754949593831373, + "y": 3.896282996276425, + "heading": 0.12253671854863268, + "angularVelocity": 3.2395285735590016, + "velocityX": 1.0656180353247193, + "velocityY": -2.2106396622276985, + "timestamp": 0.42082187407743554 + }, + { + "x": 1.0309514620598907, + "y": 3.781043042077509, + "heading": 0.2541852806699216, + "angularVelocity": 2.815531064508889, + "velocityX": 1.1860327488595344, + "velocityY": -2.4646047453299187, + "timestamp": 0.46757986008603947 + }, + { + "x": 1.0929356681499833, + "y": 3.6545108746667574, + "heading": 0.36498378647979907, + "angularVelocity": 2.3696167279161107, + "velocityX": 1.3256389203520678, + "velocityY": -2.7061081584525786, + "timestamp": 0.5143378460946434 + }, + { + "x": 1.1629696592047158, + "y": 3.5179953972400937, + "heading": 0.45299398894595083, + "angularVelocity": 1.882249642867833, + "velocityX": 1.4977974252790578, + "velocityY": -2.9196184241444185, + "timestamp": 0.5610958321032473 + }, + { + "x": 1.2444134330422476, + "y": 3.375806208333299, + "heading": 0.5186903236816397, + "angularVelocity": 1.4050291798979542, + "velocityX": 1.7418152660097566, + "velocityY": -3.0409605084502798, + "timestamp": 0.6078538181118512 + }, + { + "x": 1.3382986621093096, + "y": 3.240381730316091, + "heading": 0.5812504134266591, + "angularVelocity": 1.3379551833759529, + "velocityX": 2.00789719749598, + "velocityY": -2.896285524197266, + "timestamp": 0.6546118041204552 + }, + { + "x": 1.438124876158876, + "y": 3.1172857608464914, + "heading": 0.6580009966257694, + "angularVelocity": 1.6414433073585977, + "velocityX": 2.1349553856095276, + "velocityY": -2.632619151880301, + "timestamp": 0.7013697901290591 + }, + { + "x": 1.5413035249725158, + "y": 3.007118135101827, + "heading": 0.7526515078768561, + "angularVelocity": 2.024264074038882, + "velocityX": 2.206652972484545, + "velocityY": -2.3561242719990694, + "timestamp": 0.748127776137663 + }, + { + "x": 1.6457474356666648, + "y": 2.910401613533236, + "heading": 0.8633543586975786, + "angularVelocity": 2.3675709813660726, + "velocityX": 2.2337127752831343, + "velocityY": -2.068449260213815, + "timestamp": 0.7948857621462669 + }, + { + "x": 1.749997834476469, + "y": 2.8273646542502275, + "heading": 0.9880259417022461, + "angularVelocity": 2.666316358922014, + "velocityX": 2.2295741906093727, + "velocityY": -1.7758882785866068, + "timestamp": 0.8416437481548709 + }, + { + "x": 1.8521934124271529, + "y": 2.758362183574911, + "heading": 1.1133113758268665, + "angularVelocity": 2.6794446215418817, + "velocityX": 2.185628310247731, + "velocityY": -1.4757365867426573, + "timestamp": 0.8884017341634748 + }, + { + "x": 1.95145157552739, + "y": 2.7031033818343477, + "heading": 1.231381145561192, + "angularVelocity": 2.525125220594789, + "velocityX": 2.122806638461338, + "velocityY": -1.181804574097173, + "timestamp": 0.9351597201720787 + }, + { + "x": 2.0473220216152592, + "y": 2.6612321563456565, + "heading": 1.3371714258033476, + "angularVelocity": 2.2625072051406683, + "velocityX": 2.0503544799845086, + "velocityY": -0.8954882163008596, + "timestamp": 0.9819177061806826 + }, + { + "x": 2.139580608011859, + "y": 2.6324205589828935, + "heading": 1.4268444625557797, + "angularVelocity": 1.9178122157799795, + "velocityX": 1.973108644575945, + "velocityY": -0.6161855935675581, + "timestamp": 1.0286756921892866 + }, + { + "x": 2.2281178382739433, + "y": 2.6163741225527475, + "heading": 1.4972552356256885, + "angularVelocity": 1.5058555571031602, + "velocityX": 1.8935210392881243, + "velocityY": -0.3431806585331752, + "timestamp": 1.0754336781978906 + }, + { + "x": 2.312878410436143, + "y": 2.6128372160061697, + "heading": 1.5458711009980197, + "angularVelocity": 1.0397339475538696, + "velocityX": 1.8127507062977068, + "velocityY": -0.07564283341668805, + "timestamp": 1.1221916642064946 + }, + { + "x": 2.393836402463875, + "y": 2.621604064143261, + "heading": 1.5707963267948966, + "angularVelocity": 0.5330688492938996, + "velocityX": 1.7314259859865824, + "velocityY": 0.18749413491648587, + "timestamp": 1.1689496502150987 + }, + { + "x": 2.470987558364868, + "y": 2.642529249191284, + "heading": 1.5707963267948966, + "angularVelocity": -1.4908159190516835e-18, + "velocityX": 1.6500102439562863, + "velocityY": 0.44752109391968486, + "timestamp": 1.2157076362237027 + }, + { + "x": 2.602813969514969, + "y": 2.734000351322527, + "heading": 1.57075571228945, + "angularVelocity": -0.0004452490507007872, + "velocityX": 1.4451877174529841, + "velocityY": 1.0027801875851559, + "timestamp": 1.3069251365927887 + }, + { + "x": 2.713939411568514, + "y": 2.8753302073709235, + "heading": 1.570674784882514, + "angularVelocity": -0.0008871916749376329, + "velocityX": 1.2182469548490686, + "velocityY": 1.5493721651707555, + "timestamp": 1.3981426369618748 + }, + { + "x": 2.8004764091180525, + "y": 3.064720789521499, + "heading": 1.5705543763017473, + "angularVelocity": -0.001320016228019663, + "velocityX": 0.9486885433102401, + "velocityY": 2.076252707914343, + "timestamp": 1.4893601373309608 + }, + { + "x": 2.8524060828186255, + "y": 3.2955451367079904, + "heading": 1.5703982906404752, + "angularVelocity": -0.0017111372339782035, + "velocityX": 0.5692950748534504, + "velocityY": 2.5304831447084086, + "timestamp": 1.580577637700047 + }, + { + "x": 2.852926988468019, + "y": 3.509890434151461, + "heading": 1.5702419946817163, + "angularVelocity": -0.0017134426850825867, + "velocityX": 0.005710588946470057, + "velocityY": 2.3498264760185585, + "timestamp": 1.671795138069133 + }, + { + "x": 2.8426506520730745, + "y": 3.671341086515572, + "heading": 1.5701215497677061, + "angularVelocity": -0.0013204145424167311, + "velocityX": -0.11265750928735839, + "velocityY": 1.7699526046084872, + "timestamp": 1.763012638438219 + }, + { + "x": 2.832422777582534, + "y": 3.778806345465944, + "heading": 1.570040615176241, + "angularVelocity": -0.0008872704375541147, + "velocityX": -0.11212623070348748, + "velocityY": 1.1781210679481708, + "timestamp": 1.854230138807305 + }, + { + "x": 2.82627534866333, + "y": 3.8324406147003174, + "heading": 1.57, + "angularVelocity": -0.0004452564044901184, + "velocityX": -0.06739308679091216, + "velocityY": 0.5879822294775167, + "timestamp": 1.945447639176391 + }, + { + "x": 2.82627534866333, + "y": 3.8324406147003174, + "heading": 1.57, + "angularVelocity": -2.8295730504754553e-24, + "velocityX": -1.0072066101835765e-17, + "velocityY": 4.887478241972941e-18, + "timestamp": 2.036665139545477 + }, + { + "x": 2.82627534866333, + "y": 3.887160790336596, + "heading": 1.57, + "angularVelocity": -7.223726232843347e-17, + "velocityX": -1.4022322773048193e-16, + "velocityY": 0.595845723766963, + "timestamp": 2.1285012868124826 + }, + { + "x": 2.82627534866333, + "y": 3.9966011399802817, + "heading": 1.57, + "angularVelocity": -1.4517284366324747e-16, + "velocityX": -2.7085764742439913e-16, + "velocityY": 1.1916914297972172, + "timestamp": 2.220337434079488 + }, + { + "x": 2.82627534866333, + "y": 4.16076166091659, + "heading": 1.57, + "angularVelocity": -2.1926697371411643e-16, + "velocityX": -4.0231930155833035e-16, + "velocityY": 1.7875371062662906, + "timestamp": 2.3121735813464936 + }, + { + "x": 2.82627534866333, + "y": 4.379642347715951, + "heading": 1.57, + "angularVelocity": -2.958096299138811e-16, + "velocityX": -5.355279601742931e-16, + "velocityY": 2.3833827236130065, + "timestamp": 2.404009728613499 + }, + { + "x": 2.82627534866333, + "y": 4.653243184089661, + "heading": 1.57, + "angularVelocity": -3.783476918941958e-16, + "velocityX": -6.748965699405944e-16, + "velocityY": 2.979228163592689, + "timestamp": 2.4958458758805047 + }, + { + "x": 2.82627534866333, + "y": 4.9268440204633706, + "heading": 1.57, + "angularVelocity": -3.641716874889364e-16, + "velocityX": -6.159722745525841e-16, + "velocityY": 2.979228163592689, + "timestamp": 2.58768202314751 + }, + { + "x": 2.82627534866333, + "y": 5.1457247072627315, + "heading": 1.57, + "angularVelocity": -2.879074384838342e-16, + "velocityX": -4.995322375549383e-16, + "velocityY": 2.3833827236130065, + "timestamp": 2.6795181704145157 + }, + { + "x": 2.82627534866333, + "y": 5.30988522819904, + "heading": 1.57, + "angularVelocity": -2.1475953045710015e-16, + "velocityX": -3.7669901554704897e-16, + "velocityY": 1.7875371062662906, + "timestamp": 2.771354317681521 + }, + { + "x": 2.82627534866333, + "y": 5.419325577842725, + "heading": 1.57, + "angularVelocity": -1.426506525121705e-16, + "velocityX": -2.519305992761992e-16, + "velocityY": 1.1916914297972172, + "timestamp": 2.8631904649485267 + }, + { + "x": 2.82627534866333, + "y": 5.474045753479004, + "heading": 1.57, + "angularVelocity": -7.113697583370428e-17, + "velocityX": -1.262362732724396e-16, + "velocityY": 0.5958457237669631, + "timestamp": 2.955026612215532 + }, + { + "x": 2.82627534866333, + "y": 5.474045753479004, + "heading": 1.57, + "angularVelocity": 1.033945689540126e-28, + "velocityX": -3.228114037955519e-28, + "velocityY": 3.3712113107609656e-23, + "timestamp": 3.0468627594825377 + }, + { + "x": 2.82627534866333, + "y": 5.533372789188761, + "heading": 1.57, + "angularVelocity": -8.890513472633016e-17, + "velocityX": -2.241316397130285e-16, + "velocityY": 0.6204208876250313, + "timestamp": 3.142486612407383 + }, + { + "x": 2.82627534866333, + "y": 5.652026858634037, + "heading": 1.57, + "angularVelocity": -1.7783037339245997e-16, + "velocityX": -4.482632719673846e-16, + "velocityY": 1.2408417546041586, + "timestamp": 3.238110465332228 + }, + { + "x": 2.82627534866333, + "y": 5.830007958261196, + "heading": 1.57, + "angularVelocity": -2.666888193558669e-16, + "velocityX": -6.723948907964149e-16, + "velocityY": 1.8612625844206605, + "timestamp": 3.3337343182570733 + }, + { + "x": 2.82627534866333, + "y": 6.067316079778427, + "heading": 1.57, + "angularVelocity": -3.5609571634279607e-16, + "velocityX": -8.965264783008826e-16, + "velocityY": 2.4816833275243777, + "timestamp": 3.4293581711819185 + }, + { + "x": 2.82627534866333, + "y": 6.3639511817267, + "heading": 1.57, + "angularVelocity": -4.462891867937391e-16, + "velocityX": -1.1206579091688093e-15, + "velocityY": 3.102103637064379, + "timestamp": 3.5249820241067638 + }, + { + "x": 2.82627534866333, + "y": 6.601259303243931, + "heading": 1.57, + "angularVelocity": -3.515360973110671e-16, + "velocityX": -8.965264782972495e-16, + "velocityY": 2.4816833275243777, + "timestamp": 3.620605877031609 + }, + { + "x": 2.82627534866333, + "y": 6.77924040287109, + "heading": 1.57, + "angularVelocity": -2.6388235640692793e-16, + "velocityX": -6.723948907917458e-16, + "velocityY": 1.8612625844206607, + "timestamp": 3.716229729956454 + }, + { + "x": 2.82627534866333, + "y": 6.8978944723163655, + "heading": 1.57, + "angularVelocity": -1.7617660412854568e-16, + "velocityX": -4.482632719646236e-16, + "velocityY": 1.2408417546041588, + "timestamp": 3.8118535828812994 + }, + { + "x": 2.82627534866333, + "y": 6.957221508026123, + "heading": 1.57, + "angularVelocity": -8.819964498314495e-17, + "velocityX": -2.241316397116374e-16, + "velocityY": 0.6204208876250313, + "timestamp": 3.9074774358061446 + }, + { + "x": 2.82627534866333, + "y": 6.957221508026123, + "heading": 1.57, + "angularVelocity": 2.2261004408365928e-29, + "velocityX": 5.7622977196112705e-39, + "velocityY": -1.7124712130623078e-23, + "timestamp": 4.00310128873099 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-Sweep.1.traj b/src/main/deploy/choreo/4NM-Sweep.1.traj new file mode 100644 index 0000000..8b0280f --- /dev/null +++ b/src/main/deploy/choreo/4NM-Sweep.1.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.1708032823161308e-29, + "angularVelocity": -9.137601985120096e-30, + "velocityX": -1.7245265837493291e-19, + "velocityY": 3.781835438274059e-19, + "timestamp": 0 + }, + { + "x": 1.2920113917886342, + "y": 5.554330110103596, + "heading": -9.481227429886463e-19, + "angularVelocity": -1.3571377455648403e-17, + "velocityX": -0.07539684918457451, + "velocityY": -0.44695930001512374, + "timestamp": 0.06986193895423295 + }, + { + "x": 1.282761551546609, + "y": 5.491689489432592, + "heading": -3.571081438475658e-18, + "angularVelocity": -3.754488861622517e-17, + "velocityX": -0.13240171086698216, + "velocityY": -0.896634442282502, + "timestamp": 0.1397238779084659 + }, + { + "x": 1.271458199578559, + "y": 5.397448925207257, + "heading": -6.053079981598685e-18, + "angularVelocity": -3.552719270340507e-17, + "velocityX": -0.16179556618740504, + "velocityY": -1.3489543181312649, + "timestamp": 0.20958581686269886 + }, + { + "x": 1.2612678627955922, + "y": 5.271561332803293, + "heading": -9.412252485751232e-18, + "angularVelocity": -4.8083012570774046e-17, + "velocityX": -0.14586392727580277, + "velocityY": -1.8019481607350594, + "timestamp": 0.2794477558169318 + }, + { + "x": 1.2580444533875073, + "y": 5.114783048267347, + "heading": -4.514723077948285e-18, + "angularVelocity": 7.010296729390708e-17, + "velocityX": -0.04613970720446646, + "velocityY": -2.2441158502436007, + "timestamp": 0.3493096947711648 + }, + { + "x": 1.2734562507703713, + "y": 4.932401999052926, + "heading": -1.9192225006314126e-18, + "angularVelocity": 3.715185125877048e-17, + "velocityX": 0.22060363072603112, + "velocityY": -2.610592433369192, + "timestamp": 0.4191716337253978 + }, + { + "x": 1.3200722448212343, + "y": 4.744629254191582, + "heading": 4.5333995240179614e-18, + "angularVelocity": 9.236247783325738e-17, + "velocityX": 0.6672588071367644, + "velocityY": -2.6877688720371014, + "timestamp": 0.48903357267963077 + }, + { + "x": 1.3953620513844092, + "y": 4.570294708742183, + "heading": 7.671914334286196e-18, + "angularVelocity": 4.492452874736247e-17, + "velocityX": 1.077694202167754, + "velocityY": -2.49541521548101, + "timestamp": 0.5588955116338638 + }, + { + "x": 1.4945697873897588, + "y": 4.416713614347291, + "heading": 1.2373124312426076e-17, + "angularVelocity": 6.729286327198698e-17, + "velocityX": 1.4200541452240707, + "velocityY": -2.198351444203463, + "timestamp": 0.6287574505880967 + }, + { + "x": 1.6146607398986816, + "y": 4.286937236785889, + "heading": 1.5881178098148638e-17, + "angularVelocity": 5.0214090683168825e-17, + "velocityX": 1.7189753720920211, + "velocityY": -1.8576120202792974, + "timestamp": 0.6986193895423297 + }, + { + "x": 1.7770335829472077, + "y": 4.171588935482394, + "heading": 1.388382460128691e-17, + "angularVelocity": -2.4968323513493456e-17, + "velocityX": 2.0297748281421804, + "velocityY": -1.4419349569732145, + "timestamp": 0.7786148850018645 + }, + { + "x": 1.9580373267940145, + "y": 4.0933452292031935, + "heading": 1.0101249557116561e-17, + "angularVelocity": -4.728484899999743e-17, + "velocityX": 2.262674201928864, + "velocityY": -0.9781014022068167, + "timestamp": 0.8586103804613993 + }, + { + "x": 2.1432715777255, + "y": 4.056404838041411, + "heading": 5.258068055729367e-18, + "angularVelocity": -6.054317546655827e-17, + "velocityX": 2.3155585182316285, + "velocityY": -0.4617808909061482, + "timestamp": 0.938605875920934 + }, + { + "x": 2.30556761109126, + "y": 4.0540722338730015, + "heading": 1.7344191572744589e-18, + "angularVelocity": -4.404808962124233e-17, + "velocityX": 2.0288146530432702, + "velocityY": -0.029159193964731635, + "timestamp": 1.0186013713804687 + }, + { + "x": 2.4288986578849183, + "y": 4.066078072388016, + "heading": -3.0892891018139647e-18, + "angularVelocity": -6.029974658902888e-17, + "velocityX": 1.5417248944478947, + "velocityY": 0.15008143203621216, + "timestamp": 1.0985968668400035 + }, + { + "x": 2.5107392491251295, + "y": 4.079634262670543, + "heading": -1.4571920319979493e-18, + "angularVelocity": 2.040236208636831e-17, + "velocityX": 1.0230649959735494, + "velocityY": 0.16946192038255684, + "timestamp": 1.1785923622995382 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -2.1781827010886517e-29, + "angularVelocity": 1.821592588016304e-17, + "velocityX": 0.5077389399960676, + "velocityY": 0.1076332542399146, + "timestamp": 1.258587857759073 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -8.20915977294843e-30, + "angularVelocity": 2.3308941769124188e-29, + "velocityX": -1.0859852689087957e-19, + "velocityY": -3.0453945703275425e-19, + "timestamp": 1.3385833532186078 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-Sweep.2.traj b/src/main/deploy/choreo/4NM-Sweep.2.traj new file mode 100644 index 0000000..8a98c86 --- /dev/null +++ b/src/main/deploy/choreo/4NM-Sweep.2.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -8.20915977294843e-30, + "angularVelocity": 2.3308941769124188e-29, + "velocityX": -1.0859852689087957e-19, + "velocityY": -3.0453945703275425e-19, + "timestamp": 0 + }, + { + "x": 2.509912633384866, + "y": 4.106948680299614, + "heading": -1.0058018540616292e-19, + "angularVelocity": -1.2014784070586274e-18, + "velocityX": -0.4950617653324921, + "velocityY": 0.22343112145323105, + "timestamp": 0.08371368324417827 + }, + { + "x": 2.4277731042980624, + "y": 4.145931881900096, + "heading": -3.539291451520145e-19, + "angularVelocity": -3.0263744487771125e-18, + "velocityX": -0.9811959754203724, + "velocityY": 0.46567299502012466, + "timestamp": 0.16742736648835654 + }, + { + "x": 2.3064032345030743, + "y": 4.2079017677417365, + "heading": -8.502057746481102e-19, + "angularVelocity": -5.928261581023188e-18, + "velocityX": -1.4498211653281692, + "velocityY": 0.7402599364895339, + "timestamp": 0.2511410497325348 + }, + { + "x": 2.149653812858836, + "y": 4.298432011058232, + "heading": -1.775104690299589e-18, + "angularVelocity": -1.1048360705497396e-17, + "velocityX": -1.8724468398675924, + "velocityY": 1.081427071515116, + "timestamp": 0.3348547329767131 + }, + { + "x": 1.9757267559424647, + "y": 4.431061350920483, + "heading": 3.435416387971875e-18, + "angularVelocity": 6.224216507177934e-17, + "velocityX": -2.0776419120044674, + "velocityY": 1.5843209224875967, + "timestamp": 0.41856841622089136 + }, + { + "x": 1.8394339825531039, + "y": 4.5892066822176, + "heading": 1.6609097248007948e-18, + "angularVelocity": -2.1197330678659494e-17, + "velocityX": -1.6280823887753026, + "velocityY": 1.8891216485581488, + "timestamp": 0.5022820994650696 + }, + { + "x": 1.74837798651176, + "y": 4.751939027202194, + "heading": 3.07643951923686e-18, + "angularVelocity": 1.6909179328924933e-17, + "velocityX": -1.0877074393650743, + "velocityY": 1.9439157217576093, + "timestamp": 0.5859957827092479 + }, + { + "x": 1.7027420711484422, + "y": 4.9125681460047455, + "heading": 3.455775740084333e-18, + "angularVelocity": 4.531351906133862e-18, + "velocityX": -0.5451428439745697, + "velocityY": 1.918791678703507, + "timestamp": 0.6697094659534264 + }, + { + "x": 1.7022826671600342, + "y": 5.068050384521484, + "heading": 3.498529240472229e-18, + "angularVelocity": 5.107104898810256e-19, + "velocityX": -0.005487800447964233, + "velocityY": 1.8573097311131817, + "timestamp": 0.7534231491976049 + }, + { + "x": 1.748998056779968, + "y": 5.2201514780459135, + "heading": 3.2452497925690024e-18, + "angularVelocity": -2.9529105301285464e-18, + "velocityX": 0.5446409206134261, + "velocityY": 1.7733016951678244, + "timestamp": 0.8391959728174652 + }, + { + "x": 1.842485607000515, + "y": 5.362722985501461, + "heading": 2.572181733343048e-18, + "angularVelocity": -7.847101115669858e-18, + "velocityX": 1.089943717311651, + "velocityY": 1.6621990677072165, + "timestamp": 0.9249687964373254 + }, + { + "x": 1.9816206378320913, + "y": 5.49133902203876, + "heading": 1.2483858256210483e-18, + "angularVelocity": -1.5433744368348303e-17, + "velocityX": 1.6221342024160712, + "velocityY": 1.4994963568801014, + "timestamp": 1.0107416200571857 + }, + { + "x": 2.1614899424975276, + "y": 5.595072018302606, + "heading": 8.735519607318585e-19, + "angularVelocity": -4.370076935236176e-18, + "velocityX": 2.097043061828134, + "velocityY": 1.2093923446380141, + "timestamp": 1.096514443677046 + }, + { + "x": 2.3416630615403307, + "y": 5.651072922830644, + "heading": -1.580597953419591e-18, + "angularVelocity": -2.8612206866525546e-17, + "velocityX": 2.1005851438600054, + "velocityY": 0.6528979945469758, + "timestamp": 1.1822872672969063 + }, + { + "x": 2.481211015710272, + "y": 5.682013046713306, + "heading": -5.273562689120438e-18, + "angularVelocity": -4.305518156048475e-17, + "velocityX": 1.626948353576521, + "velocityY": 0.36072175984069416, + "timestamp": 1.2680600909167665 + }, + { + "x": 2.5751424394052407, + "y": 5.698896862460529, + "heading": -3.238292974805406e-18, + "angularVelocity": 2.372860764105119e-17, + "velocityX": 1.095118706960925, + "velocityY": 0.1968434177012898, + "timestamp": 1.3538329145366268 + }, + { + "x": 2.6223177909851074, + "y": 5.706172466278076, + "heading": -1.6431536638622417e-29, + "angularVelocity": 3.7754299539056475e-17, + "velocityX": 0.5500034811602417, + "velocityY": 0.08482411456794345, + "timestamp": 1.439605738156487 + }, + { + "x": 2.6223177909851074, + "y": 5.706172466278076, + "heading": -1.2685688932181852e-29, + "angularVelocity": 2.8790313162701036e-30, + "velocityX": 1.0395697262890221e-19, + "velocityY": -8.557247579898581e-19, + "timestamp": 1.5253785617763473 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-Sweep.3.traj b/src/main/deploy/choreo/4NM-Sweep.3.traj new file mode 100644 index 0000000..1611c44 --- /dev/null +++ b/src/main/deploy/choreo/4NM-Sweep.3.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 2.6223177909851074, + "y": 5.706172466278076, + "heading": -1.2685688932181852e-29, + "angularVelocity": 2.8790313162701036e-30, + "velocityX": 1.0395697262890221e-19, + "velocityY": -8.557247579898581e-19, + "timestamp": 0 + }, + { + "x": 2.580562376655811, + "y": 5.717941500249003, + "heading": -2.5900187158353315e-19, + "angularVelocity": -3.1674270516013714e-18, + "velocityX": -0.5106420139476558, + "velocityY": 0.14392775896647922, + "timestamp": 0.08177042465913686 + }, + { + "x": 2.4974776849678753, + "y": 5.742898235730052, + "heading": -7.872006902584448e-19, + "angularVelocity": -6.459533342935218e-18, + "velocityX": -1.016072645266023, + "velocityY": 0.30520491467523164, + "timestamp": 0.16354084931827373 + }, + { + "x": 2.373951012372342, + "y": 5.783554351890171, + "heading": -1.601704058013041e-18, + "angularVelocity": -9.960854638783481e-18, + "velocityX": -1.5106522084293756, + "velocityY": 0.49719830035854506, + "timestamp": 0.2453112739774106 + }, + { + "x": 2.212584981311779, + "y": 5.845428543659977, + "heading": -2.553510477545141e-18, + "angularVelocity": -1.1639982927649009e-17, + "velocityX": -1.9734033635415664, + "velocityY": 0.7566817957437639, + "timestamp": 0.32708169863654746 + }, + { + "x": 2.029785893220335, + "y": 5.945020714468991, + "heading": -3.86959228607625e-18, + "angularVelocity": -1.6094840727170316e-17, + "velocityX": -2.235515944223699, + "velocityY": 1.2179485581023628, + "timestamp": 0.4088521232956843 + }, + { + "x": 1.8832983601749962, + "y": 6.0683508997995, + "heading": -5.262396045953343e-18, + "angularVelocity": -1.7033099337403357e-17, + "velocityX": -1.7914488478686192, + "velocityY": 1.5082492948350907, + "timestamp": 0.4906225479548212 + }, + { + "x": 1.7796069658898477, + "y": 6.198788435444483, + "heading": -6.5612261048200405e-18, + "angularVelocity": -1.5883860411297538e-17, + "velocityX": -1.2680794396921602, + "velocityY": 1.5951676439093452, + "timestamp": 0.572392972613958 + }, + { + "x": 1.7192548891917947, + "y": 6.3311567689867, + "heading": -7.717140781068942e-18, + "angularVelocity": -1.4136096934392038e-17, + "velocityX": -0.7380673018347763, + "velocityY": 1.6187800674143442, + "timestamp": 0.6541633972730949 + }, + { + "x": 1.7022826671600342, + "y": 6.463063716888428, + "heading": -8.916494739837526e-18, + "angularVelocity": -1.4667331539141363e-17, + "velocityX": -0.207559421422963, + "velocityY": 1.613137616095136, + "timestamp": 0.7359338219322318 + }, + { + "x": 1.7347406171289421, + "y": 6.603778103628165, + "heading": -1.000751929260003e-17, + "angularVelocity": -1.231976965868251e-17, + "velocityX": 0.36651280374474055, + "velocityY": 1.5889365921324623, + "timestamp": 0.8244926652721589 + }, + { + "x": 1.8179248925163114, + "y": 6.740485682269317, + "heading": -1.0810197858207777e-17, + "angularVelocity": -9.063787776490836e-18, + "velocityX": 0.9393107706711105, + "velocityY": 1.5436920073177693, + "timestamp": 0.9130515086120861 + }, + { + "x": 1.9514234110036923, + "y": 6.869597791652038, + "heading": -1.3656868184138031e-17, + "angularVelocity": -3.21443923528337e-17, + "velocityX": 1.5074555341125644, + "velocityY": 1.4579245224232664, + "timestamp": 1.0016103519520132 + }, + { + "x": 2.132885886706102, + "y": 6.981719550529973, + "heading": -1.737159453771359e-17, + "angularVelocity": -4.194641742998263e-17, + "velocityX": 2.049061040757697, + "velocityY": 1.266070723705848, + "timestamp": 1.0901691952919403 + }, + { + "x": 2.3247702498642075, + "y": 7.044035745793493, + "heading": -1.618624975339561e-17, + "angularVelocity": 1.3384826497101663e-17, + "velocityX": 2.1667442337923255, + "velocityY": 0.7036699319154751, + "timestamp": 1.178728038631868 + }, + { + "x": 2.472806977776645, + "y": 7.080533384911558, + "heading": -8.026020803635598e-18, + "angularVelocity": 9.214470826616307e-17, + "velocityX": 1.671619934603348, + "velocityY": 0.4121286789843381, + "timestamp": 1.2672868819717955 + }, + { + "x": 2.572342101217046, + "y": 7.101642440594894, + "heading": -3.7650843441848865e-18, + "angularVelocity": 4.81141829629879e-17, + "velocityX": 1.1239433543451147, + "velocityY": 0.23836191719793084, + "timestamp": 1.355845725311723 + }, + { + "x": 2.6223177909851074, + "y": 7.111215591430664, + "heading": 1.8821880131630952e-29, + "angularVelocity": 4.251505701529514e-17, + "velocityX": 0.5643218439092824, + "velocityY": 0.10809932102460933, + "timestamp": 1.4444045686516507 + }, + { + "x": 2.6223177909851074, + "y": 7.111215591430664, + "heading": 9.186803464292479e-30, + "angularVelocity": -5.0618681731929696e-30, + "velocityX": -6.572861886784797e-20, + "velocityY": -2.9604907575354983e-19, + "timestamp": 1.5329634119915783 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-Sweep.traj b/src/main/deploy/choreo/4NM-Sweep.traj new file mode 100644 index 0000000..a8fa31f --- /dev/null +++ b/src/main/deploy/choreo/4NM-Sweep.traj @@ -0,0 +1,499 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.1708032823161308e-29, + "angularVelocity": -9.137601985120096e-30, + "velocityX": -1.7245265837493291e-19, + "velocityY": 3.781835438274059e-19, + "timestamp": 0 + }, + { + "x": 1.2920113917886342, + "y": 5.554330110103596, + "heading": -9.481227429886463e-19, + "angularVelocity": -1.3571377455648403e-17, + "velocityX": -0.07539684918457451, + "velocityY": -0.44695930001512374, + "timestamp": 0.06986193895423295 + }, + { + "x": 1.282761551546609, + "y": 5.491689489432592, + "heading": -3.571081438475658e-18, + "angularVelocity": -3.754488861622517e-17, + "velocityX": -0.13240171086698216, + "velocityY": -0.896634442282502, + "timestamp": 0.1397238779084659 + }, + { + "x": 1.271458199578559, + "y": 5.397448925207257, + "heading": -6.053079981598685e-18, + "angularVelocity": -3.552719270340507e-17, + "velocityX": -0.16179556618740504, + "velocityY": -1.3489543181312649, + "timestamp": 0.20958581686269886 + }, + { + "x": 1.2612678627955922, + "y": 5.271561332803293, + "heading": -9.412252485751232e-18, + "angularVelocity": -4.8083012570774046e-17, + "velocityX": -0.14586392727580277, + "velocityY": -1.8019481607350594, + "timestamp": 0.2794477558169318 + }, + { + "x": 1.2580444533875073, + "y": 5.114783048267347, + "heading": -4.514723077948285e-18, + "angularVelocity": 7.010296729390708e-17, + "velocityX": -0.04613970720446646, + "velocityY": -2.2441158502436007, + "timestamp": 0.3493096947711648 + }, + { + "x": 1.2734562507703713, + "y": 4.932401999052926, + "heading": -1.9192225006314126e-18, + "angularVelocity": 3.715185125877048e-17, + "velocityX": 0.22060363072603112, + "velocityY": -2.610592433369192, + "timestamp": 0.4191716337253978 + }, + { + "x": 1.3200722448212343, + "y": 4.744629254191582, + "heading": 4.5333995240179614e-18, + "angularVelocity": 9.236247783325738e-17, + "velocityX": 0.6672588071367644, + "velocityY": -2.6877688720371014, + "timestamp": 0.48903357267963077 + }, + { + "x": 1.3953620513844092, + "y": 4.570294708742183, + "heading": 7.671914334286196e-18, + "angularVelocity": 4.492452874736247e-17, + "velocityX": 1.077694202167754, + "velocityY": -2.49541521548101, + "timestamp": 0.5588955116338638 + }, + { + "x": 1.4945697873897588, + "y": 4.416713614347291, + "heading": 1.2373124312426076e-17, + "angularVelocity": 6.729286327198698e-17, + "velocityX": 1.4200541452240707, + "velocityY": -2.198351444203463, + "timestamp": 0.6287574505880967 + }, + { + "x": 1.6146607398986816, + "y": 4.286937236785889, + "heading": 1.5881178098148638e-17, + "angularVelocity": 5.0214090683168825e-17, + "velocityX": 1.7189753720920211, + "velocityY": -1.8576120202792974, + "timestamp": 0.6986193895423297 + }, + { + "x": 1.7770335829472077, + "y": 4.171588935482394, + "heading": 1.388382460128691e-17, + "angularVelocity": -2.4968323513493456e-17, + "velocityX": 2.0297748281421804, + "velocityY": -1.4419349569732145, + "timestamp": 0.7786148850018645 + }, + { + "x": 1.9580373267940145, + "y": 4.0933452292031935, + "heading": 1.0101249557116561e-17, + "angularVelocity": -4.728484899999743e-17, + "velocityX": 2.262674201928864, + "velocityY": -0.9781014022068167, + "timestamp": 0.8586103804613993 + }, + { + "x": 2.1432715777255, + "y": 4.056404838041411, + "heading": 5.258068055729367e-18, + "angularVelocity": -6.054317546655827e-17, + "velocityX": 2.3155585182316285, + "velocityY": -0.4617808909061482, + "timestamp": 0.938605875920934 + }, + { + "x": 2.30556761109126, + "y": 4.0540722338730015, + "heading": 1.7344191572744589e-18, + "angularVelocity": -4.404808962124233e-17, + "velocityX": 2.0288146530432702, + "velocityY": -0.029159193964731635, + "timestamp": 1.0186013713804687 + }, + { + "x": 2.4288986578849183, + "y": 4.066078072388016, + "heading": -3.0892891018139647e-18, + "angularVelocity": -6.029974658902888e-17, + "velocityX": 1.5417248944478947, + "velocityY": 0.15008143203621216, + "timestamp": 1.0985968668400035 + }, + { + "x": 2.5107392491251295, + "y": 4.079634262670543, + "heading": -1.4571920319979493e-18, + "angularVelocity": 2.040236208636831e-17, + "velocityX": 1.0230649959735494, + "velocityY": 0.16946192038255684, + "timestamp": 1.1785923622995382 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -2.1781827010886517e-29, + "angularVelocity": 1.821592588016304e-17, + "velocityX": 0.5077389399960676, + "velocityY": 0.1076332542399146, + "timestamp": 1.258587857759073 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -8.20915977294843e-30, + "angularVelocity": 2.3308941769124188e-29, + "velocityX": -1.0859852689087957e-19, + "velocityY": -3.0453945703275425e-19, + "timestamp": 1.3385833532186078 + }, + { + "x": 2.509912633384866, + "y": 4.106948680299614, + "heading": -1.0058018540616292e-19, + "angularVelocity": -1.2014784070586274e-18, + "velocityX": -0.4950617653324921, + "velocityY": 0.22343112145323105, + "timestamp": 1.422297036462786 + }, + { + "x": 2.4277731042980624, + "y": 4.145931881900096, + "heading": -3.539291451520145e-19, + "angularVelocity": -3.0263744487771125e-18, + "velocityX": -0.9811959754203724, + "velocityY": 0.46567299502012466, + "timestamp": 1.5060107197069643 + }, + { + "x": 2.3064032345030743, + "y": 4.2079017677417365, + "heading": -8.502057746481102e-19, + "angularVelocity": -5.928261581023188e-18, + "velocityX": -1.4498211653281692, + "velocityY": 0.7402599364895339, + "timestamp": 1.5897244029511426 + }, + { + "x": 2.149653812858836, + "y": 4.298432011058232, + "heading": -1.775104690299589e-18, + "angularVelocity": -1.1048360705497396e-17, + "velocityX": -1.8724468398675924, + "velocityY": 1.081427071515116, + "timestamp": 1.6734380861953209 + }, + { + "x": 1.9757267559424647, + "y": 4.431061350920483, + "heading": 3.435416387971875e-18, + "angularVelocity": 6.224216507177934e-17, + "velocityX": -2.0776419120044674, + "velocityY": 1.5843209224875967, + "timestamp": 1.7571517694394991 + }, + { + "x": 1.8394339825531039, + "y": 4.5892066822176, + "heading": 1.6609097248007948e-18, + "angularVelocity": -2.1197330678659494e-17, + "velocityX": -1.6280823887753026, + "velocityY": 1.8891216485581488, + "timestamp": 1.8408654526836774 + }, + { + "x": 1.74837798651176, + "y": 4.751939027202194, + "heading": 3.07643951923686e-18, + "angularVelocity": 1.6909179328924933e-17, + "velocityX": -1.0877074393650743, + "velocityY": 1.9439157217576093, + "timestamp": 1.9245791359278557 + }, + { + "x": 1.7027420711484422, + "y": 4.9125681460047455, + "heading": 3.455775740084333e-18, + "angularVelocity": 4.531351906133862e-18, + "velocityX": -0.5451428439745697, + "velocityY": 1.918791678703507, + "timestamp": 2.008292819172034 + }, + { + "x": 1.7022826671600342, + "y": 5.068050384521484, + "heading": 3.498529240472229e-18, + "angularVelocity": 5.107104898810256e-19, + "velocityX": -0.005487800447964233, + "velocityY": 1.8573097311131817, + "timestamp": 2.0920065024162127 + }, + { + "x": 1.748998056779968, + "y": 5.2201514780459135, + "heading": 3.2452497925690024e-18, + "angularVelocity": -2.9529105301285464e-18, + "velocityX": 0.5446409206134261, + "velocityY": 1.7733016951678244, + "timestamp": 2.177779326036073 + }, + { + "x": 1.842485607000515, + "y": 5.362722985501461, + "heading": 2.572181733343048e-18, + "angularVelocity": -7.847101115669858e-18, + "velocityX": 1.089943717311651, + "velocityY": 1.6621990677072165, + "timestamp": 2.263552149655933 + }, + { + "x": 1.9816206378320913, + "y": 5.49133902203876, + "heading": 1.2483858256210483e-18, + "angularVelocity": -1.5433744368348303e-17, + "velocityX": 1.6221342024160712, + "velocityY": 1.4994963568801014, + "timestamp": 2.3493249732757935 + }, + { + "x": 2.1614899424975276, + "y": 5.595072018302606, + "heading": 8.735519607318585e-19, + "angularVelocity": -4.370076935236176e-18, + "velocityX": 2.097043061828134, + "velocityY": 1.2093923446380141, + "timestamp": 2.4350977968956538 + }, + { + "x": 2.3416630615403307, + "y": 5.651072922830644, + "heading": -1.580597953419591e-18, + "angularVelocity": -2.8612206866525546e-17, + "velocityX": 2.1005851438600054, + "velocityY": 0.6528979945469758, + "timestamp": 2.520870620515514 + }, + { + "x": 2.481211015710272, + "y": 5.682013046713306, + "heading": -5.273562689120438e-18, + "angularVelocity": -4.305518156048475e-17, + "velocityX": 1.626948353576521, + "velocityY": 0.36072175984069416, + "timestamp": 2.6066434441353743 + }, + { + "x": 2.5751424394052407, + "y": 5.698896862460529, + "heading": -3.238292974805406e-18, + "angularVelocity": 2.372860764105119e-17, + "velocityX": 1.095118706960925, + "velocityY": 0.1968434177012898, + "timestamp": 2.6924162677552346 + }, + { + "x": 2.6223177909851074, + "y": 5.706172466278076, + "heading": -1.6431536638622417e-29, + "angularVelocity": 3.7754299539056475e-17, + "velocityX": 0.5500034811602417, + "velocityY": 0.08482411456794345, + "timestamp": 2.778189091375095 + }, + { + "x": 2.6223177909851074, + "y": 5.706172466278076, + "heading": -1.2685688932181852e-29, + "angularVelocity": 2.8790313162701036e-30, + "velocityX": 1.0395697262890221e-19, + "velocityY": -8.557247579898581e-19, + "timestamp": 2.863961914994955 + }, + { + "x": 2.580562376655811, + "y": 5.717941500249003, + "heading": -2.5900187158353315e-19, + "angularVelocity": -3.1674270516013714e-18, + "velocityX": -0.5106420139476558, + "velocityY": 0.14392775896647922, + "timestamp": 2.945732339654092 + }, + { + "x": 2.4974776849678753, + "y": 5.742898235730052, + "heading": -7.872006902584448e-19, + "angularVelocity": -6.459533342935218e-18, + "velocityX": -1.016072645266023, + "velocityY": 0.30520491467523164, + "timestamp": 3.027502764313229 + }, + { + "x": 2.373951012372342, + "y": 5.783554351890171, + "heading": -1.601704058013041e-18, + "angularVelocity": -9.960854638783481e-18, + "velocityX": -1.5106522084293756, + "velocityY": 0.49719830035854506, + "timestamp": 3.1092731889723657 + }, + { + "x": 2.212584981311779, + "y": 5.845428543659977, + "heading": -2.553510477545141e-18, + "angularVelocity": -1.1639982927649009e-17, + "velocityX": -1.9734033635415664, + "velocityY": 0.7566817957437639, + "timestamp": 3.1910436136315026 + }, + { + "x": 2.029785893220335, + "y": 5.945020714468991, + "heading": -3.86959228607625e-18, + "angularVelocity": -1.6094840727170316e-17, + "velocityX": -2.235515944223699, + "velocityY": 1.2179485581023628, + "timestamp": 3.2728140382906394 + }, + { + "x": 1.8832983601749962, + "y": 6.0683508997995, + "heading": -5.262396045953343e-18, + "angularVelocity": -1.7033099337403357e-17, + "velocityX": -1.7914488478686192, + "velocityY": 1.5082492948350907, + "timestamp": 3.3545844629497763 + }, + { + "x": 1.7796069658898477, + "y": 6.198788435444483, + "heading": -6.5612261048200405e-18, + "angularVelocity": -1.5883860411297538e-17, + "velocityX": -1.2680794396921602, + "velocityY": 1.5951676439093452, + "timestamp": 3.436354887608913 + }, + { + "x": 1.7192548891917947, + "y": 6.3311567689867, + "heading": -7.717140781068942e-18, + "angularVelocity": -1.4136096934392038e-17, + "velocityX": -0.7380673018347763, + "velocityY": 1.6187800674143442, + "timestamp": 3.51812531226805 + }, + { + "x": 1.7022826671600342, + "y": 6.463063716888428, + "heading": -8.916494739837526e-18, + "angularVelocity": -1.4667331539141363e-17, + "velocityX": -0.207559421422963, + "velocityY": 1.613137616095136, + "timestamp": 3.599895736927187 + }, + { + "x": 1.7347406171289421, + "y": 6.603778103628165, + "heading": -1.000751929260003e-17, + "angularVelocity": -1.231976965868251e-17, + "velocityX": 0.36651280374474055, + "velocityY": 1.5889365921324623, + "timestamp": 3.688454580267114 + }, + { + "x": 1.8179248925163114, + "y": 6.740485682269317, + "heading": -1.0810197858207777e-17, + "angularVelocity": -9.063787776490836e-18, + "velocityX": 0.9393107706711105, + "velocityY": 1.5436920073177693, + "timestamp": 3.777013423607041 + }, + { + "x": 1.9514234110036923, + "y": 6.869597791652038, + "heading": -1.3656868184138031e-17, + "angularVelocity": -3.21443923528337e-17, + "velocityX": 1.5074555341125644, + "velocityY": 1.4579245224232664, + "timestamp": 3.8655722669469683 + }, + { + "x": 2.132885886706102, + "y": 6.981719550529973, + "heading": -1.737159453771359e-17, + "angularVelocity": -4.194641742998263e-17, + "velocityX": 2.049061040757697, + "velocityY": 1.266070723705848, + "timestamp": 3.9541311102868955 + }, + { + "x": 2.3247702498642075, + "y": 7.044035745793493, + "heading": -1.618624975339561e-17, + "angularVelocity": 1.3384826497101663e-17, + "velocityX": 2.1667442337923255, + "velocityY": 0.7036699319154751, + "timestamp": 4.042689953626823 + }, + { + "x": 2.472806977776645, + "y": 7.080533384911558, + "heading": -8.026020803635598e-18, + "angularVelocity": 9.214470826616307e-17, + "velocityX": 1.671619934603348, + "velocityY": 0.4121286789843381, + "timestamp": 4.131248796966751 + }, + { + "x": 2.572342101217046, + "y": 7.101642440594894, + "heading": -3.7650843441848865e-18, + "angularVelocity": 4.81141829629879e-17, + "velocityX": 1.1239433543451147, + "velocityY": 0.23836191719793084, + "timestamp": 4.219807640306678 + }, + { + "x": 2.6223177909851074, + "y": 7.111215591430664, + "heading": 1.8821880131630952e-29, + "angularVelocity": 4.251505701529514e-17, + "velocityX": 0.5643218439092824, + "velocityY": 0.10809932102460933, + "timestamp": 4.308366483646606 + }, + { + "x": 2.6223177909851074, + "y": 7.111215591430664, + "heading": 9.186803464292479e-30, + "angularVelocity": -5.0618681731929696e-30, + "velocityX": -6.572861886784797e-20, + "velocityY": -2.9604907575354983e-19, + "timestamp": 4.396925326986533 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFender.1.traj b/src/main/deploy/choreo/4NM-SweepFender.1.traj new file mode 100644 index 0000000..fc7847a --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFender.1.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.9517716307925408e-29, + "angularVelocity": 3.5304770094188047e-29, + "velocityX": -1.724526619664194e-19, + "velocityY": 3.7818353955730183e-19, + "timestamp": 0 + }, + { + "x": 1.2920113917886342, + "y": 5.554330110103596, + "heading": 2.1731437642535974e-18, + "angularVelocity": 3.110625940380434e-17, + "velocityX": -0.07539684918457464, + "velocityY": -0.44695930001512396, + "timestamp": 0.069861938954233 + }, + { + "x": 1.282761551546609, + "y": 5.491689489432592, + "heading": 3.294425072358551e-18, + "angularVelocity": 1.604996065096705e-17, + "velocityX": -0.1324017108669824, + "velocityY": -0.8966344422825023, + "timestamp": 0.139723877908466 + }, + { + "x": 1.271458199578559, + "y": 5.397448925207256, + "heading": -1.1426328921377949e-17, + "angularVelocity": -2.1071206657810978e-16, + "velocityX": -0.16179556618740545, + "velocityY": -1.3489543181312655, + "timestamp": 0.209585816862699 + }, + { + "x": 1.261267862795592, + "y": 5.271561332803292, + "heading": -2.391418921336277e-17, + "angularVelocity": -1.7875054858841103e-16, + "velocityX": -0.14586392727580333, + "velocityY": -1.8019481607350603, + "timestamp": 0.279447755816932 + }, + { + "x": 1.2580444533875073, + "y": 5.114783048267347, + "heading": -3.501816243214742e-17, + "angularVelocity": -1.5894166558012616e-16, + "velocityX": -0.046139707204466994, + "velocityY": -2.244115850243602, + "timestamp": 0.34930969477116497 + }, + { + "x": 1.2734562507703713, + "y": 4.932401999052925, + "heading": -4.116208538702014e-17, + "angularVelocity": -8.794377668506781e-17, + "velocityX": 0.2206036307260309, + "velocityY": -2.6105924333691934, + "timestamp": 0.41917163372539795 + }, + { + "x": 1.3200722448212343, + "y": 4.744629254191581, + "heading": -4.742296059721744e-17, + "angularVelocity": -8.961782560603248e-17, + "velocityX": 0.6672588071367647, + "velocityY": -2.6877688720371022, + "timestamp": 0.48903357267963093 + }, + { + "x": 1.395362051384409, + "y": 4.570294708742184, + "heading": -4.971944913704747e-17, + "angularVelocity": -3.2871811710653476e-17, + "velocityX": 1.077694202167754, + "velocityY": -2.4954152154810103, + "timestamp": 0.558895511633864 + }, + { + "x": 1.4945697873897588, + "y": 4.416713614347291, + "heading": -4.954660865117441e-17, + "angularVelocity": 2.4740284798965177e-18, + "velocityX": 1.420054145224071, + "velocityY": -2.1983514442034626, + "timestamp": 0.628757450588097 + }, + { + "x": 1.6146607398986816, + "y": 4.286937236785889, + "heading": -4.40878360826832e-17, + "angularVelocity": 7.813657120875944e-17, + "velocityX": 1.7189753720920216, + "velocityY": -1.8576120202792965, + "timestamp": 0.6986193895423299 + }, + { + "x": 1.7770335829472077, + "y": 4.171588935482394, + "heading": -3.677251742788914e-17, + "angularVelocity": 9.144662966049345e-17, + "velocityX": 2.029774828142181, + "velocityY": -1.4419349569732136, + "timestamp": 0.7786148850018647 + }, + { + "x": 1.9580373267940145, + "y": 4.0933452292031935, + "heading": -2.7494710502210263e-17, + "angularVelocity": 1.159791154548558e-16, + "velocityX": 2.262674201928864, + "velocityY": -0.9781014022068157, + "timestamp": 0.8586103804613995 + }, + { + "x": 2.1432715777255, + "y": 4.056404838041411, + "heading": -1.4102008166543997e-17, + "angularVelocity": 1.6741820232350694e-16, + "velocityX": 2.315558518231629, + "velocityY": -0.4617808909061473, + "timestamp": 0.9386058759209343 + }, + { + "x": 2.30556761109126, + "y": 4.054072233873002, + "heading": -6.5008381019634475e-18, + "angularVelocity": 9.50199751021012e-17, + "velocityX": 2.0288146530432702, + "velocityY": -0.029159193964730743, + "timestamp": 1.018601371380469 + }, + { + "x": 2.4288986578849183, + "y": 4.066078072388016, + "heading": 5.687906951263531e-19, + "angularVelocity": 8.8375334664513e-17, + "velocityX": 1.5417248944478947, + "velocityY": 0.15008143203621294, + "timestamp": 1.0985968668400037 + }, + { + "x": 2.5107392491251295, + "y": 4.079634262670543, + "heading": 1.429916772585085e-18, + "angularVelocity": 1.0764681677136246e-17, + "velocityX": 1.0230649959735494, + "velocityY": 0.16946192038255736, + "timestamp": 1.1785923622995385 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -4.8145646612541634e-29, + "angularVelocity": -1.7874965990600758e-17, + "velocityX": 0.5077389399960675, + "velocityY": 0.10763325423991486, + "timestamp": 1.2585878577590732 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -2.9775150183346403e-29, + "angularVelocity": -1.4341236571899655e-29, + "velocityX": -2.760379088955979e-19, + "velocityY": 2.12311250171415e-20, + "timestamp": 1.338583353218608 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFender.2.traj b/src/main/deploy/choreo/4NM-SweepFender.2.traj new file mode 100644 index 0000000..f2786e7 --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFender.2.traj @@ -0,0 +1,112 @@ +{ + "samples": [ + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -2.9775150183346403e-29, + "angularVelocity": -1.4341236571899655e-29, + "velocityX": -2.760379088955979e-19, + "velocityY": 2.12311250171415e-20, + "timestamp": 0 + }, + { + "x": 2.5095534969743727, + "y": 4.138154812312414, + "heading": 2.190510097540435e-18, + "angularVelocity": 2.186765826125146e-17, + "velocityX": -0.41731125706956324, + "velocityY": 0.4982506071172713, + "timestamp": 0.10017122594148642 + }, + { + "x": 2.4259483376755, + "y": 4.237975559232393, + "heading": 3.933266220970577e-18, + "angularVelocity": 1.739777229170673e-17, + "velocityX": -0.8346225027505352, + "velocityY": 0.9965012006370843, + "timestamp": 0.20034245188297284 + }, + { + "x": 2.300540601198944, + "y": 4.387706676661202, + "heading": 1.3527765530751777e-17, + "angularVelocity": 9.578099042485691e-17, + "velocityX": -1.2519337294505222, + "velocityY": 1.494751771494467, + "timestamp": 0.30051367782445926 + }, + { + "x": 2.133330291347402, + "y": 4.587348160058594, + "heading": 2.2617014617503804e-17, + "angularVelocity": 9.073712421559476e-17, + "velocityX": -1.669244918188541, + "velocityY": 1.9930022970269912, + "timestamp": 0.4006849037659457 + }, + { + "x": 1.9243174195289612, + "y": 4.836899995803833, + "heading": 3.880271021847084e-17, + "angularVelocity": 1.6158028652305301e-16, + "velocityX": -2.0865559930406836, + "velocityY": 2.4912526865849736, + "timestamp": 0.5008561297074321 + }, + { + "x": 1.7153045477105202, + "y": 5.086451831549072, + "heading": 2.228690287655552e-17, + "angularVelocity": -1.648757621591215e-16, + "velocityX": -2.086555993040684, + "velocityY": 2.491252686584974, + "timestamp": 0.6010273556489185 + }, + { + "x": 1.548094237858978, + "y": 5.286093314946464, + "heading": 1.1611942212855913e-17, + "angularVelocity": -1.065671349922539e-16, + "velocityX": -1.6692449181885414, + "velocityY": 1.9930022970269914, + "timestamp": 0.7011985815904049 + }, + { + "x": 1.422686501382422, + "y": 5.435824432375273, + "heading": 1.3869899594487062e-18, + "angularVelocity": -1.0207474326639434e-16, + "velocityX": -1.2519337294505222, + "velocityY": 1.494751771494467, + "timestamp": 0.8013698075318914 + }, + { + "x": 1.3390813420835495, + "y": 5.535645179295252, + "heading": 4.452399801737667e-19, + "angularVelocity": -9.40140233404049e-18, + "velocityX": -0.8346225027505352, + "velocityY": 0.9965012006370843, + "timestamp": 0.9015410334733778 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.2561037494800293e-29, + "angularVelocity": -4.4447889631061314e-18, + "velocityX": -0.4173112570695632, + "velocityY": 0.4982506071172713, + "timestamp": 1.0017122594148642 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.2140312015726775e-29, + "angularVelocity": -1.6302535205902077e-30, + "velocityX": -1.8217183323138137e-19, + "velocityY": -1.1857377469208627e-19, + "timestamp": 1.1018834853563506 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFender.3.traj b/src/main/deploy/choreo/4NM-SweepFender.3.traj new file mode 100644 index 0000000..f7b386f --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFender.3.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.2140312015726775e-29, + "angularVelocity": -1.6302535205902077e-30, + "velocityX": -1.8217183323138137e-19, + "velocityY": -1.1857377469208627e-19, + "timestamp": 0 + }, + { + "x": 1.3545482975852345, + "y": 5.589358578088535, + "heading": 1.9334918613178174e-18, + "angularVelocity": 2.0557149745608903e-17, + "velocityX": 0.6088974989534299, + "velocityY": 0.040434275746134056, + "timestamp": 0.09405447685359292 + }, + { + "x": 1.469087367090714, + "y": 5.5969646272643825, + "heading": 2.3789667257742094e-18, + "angularVelocity": 4.736350526475188e-18, + "velocityX": 1.2177949773063277, + "velocityY": 0.08086855012427496, + "timestamp": 0.18810895370718583 + }, + { + "x": 1.6408959668925176, + "y": 5.608373700732222, + "heading": 4.870233499579861e-18, + "angularVelocity": 2.6487487710164578e-17, + "velocityX": 1.8266924185782696, + "velocityY": 0.12130282204002837, + "timestamp": 0.28216343056077875 + }, + { + "x": 1.8699740888528427, + "y": 5.623585797951656, + "heading": 2.7021941669251377e-18, + "angularVelocity": -2.3050888781904244e-17, + "velocityX": 2.435589773327992, + "velocityY": 0.16173708821021165, + "timestamp": 0.37621790741437167 + }, + { + "x": 2.156321692282717, + "y": 5.642600916220706, + "heading": -8.385603820696607e-19, + "angularVelocity": -3.764578410393351e-17, + "velocityX": 3.0444866954670173, + "velocityY": 0.20217132565257093, + "timestamp": 0.4702723842679646 + }, + { + "x": 2.385399814243042, + "y": 5.657813013440141, + "heading": -1.979930432977169e-18, + "angularVelocity": -1.2135202438796547e-17, + "velocityX": 2.4355897733279916, + "velocityY": 0.16173708821021165, + "timestamp": 0.5643268611215575 + }, + { + "x": 2.5572084140448452, + "y": 5.66922208690798, + "heading": -2.2142142923894204e-18, + "angularVelocity": -2.490938414552063e-18, + "velocityX": 1.8266924185782694, + "velocityY": 0.12130282204002837, + "timestamp": 0.6583813379751504 + }, + { + "x": 2.671747483550325, + "y": 5.676828136083827, + "heading": -1.281622902160523e-18, + "angularVelocity": 9.915437612097848e-18, + "velocityX": 1.2177949773063277, + "velocityY": 0.08086855012427498, + "timestamp": 0.7524358148287433 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.304583994113875e-29, + "angularVelocity": 1.3626388144391886e-17, + "velocityX": 0.60889749895343, + "velocityY": 0.040434275746134056, + "timestamp": 0.8464902916823362 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.1449818357083717e-29, + "angularVelocity": -3.0489465233384347e-30, + "velocityX": -1.0348040233346454e-19, + "velocityY": -6.871642741102469e-21, + "timestamp": 0.9405447685359292 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFender.4.traj b/src/main/deploy/choreo/4NM-SweepFender.4.traj new file mode 100644 index 0000000..fcfd189 --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFender.4.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.1449818357083717e-29, + "angularVelocity": -3.0489465233384347e-30, + "velocityX": -1.0348040233346454e-19, + "velocityY": -6.871642741102469e-21, + "timestamp": 0 + }, + { + "x": 2.671747483550325, + "y": 5.676828136083827, + "heading": -2.37591069216883e-18, + "angularVelocity": -2.5261004796916857e-17, + "velocityX": -0.6088974989534299, + "velocityY": -0.04043427574613417, + "timestamp": 0.09405447685359292 + }, + { + "x": 2.5572084140448452, + "y": 5.66922208690798, + "heading": -5.149849388984227e-18, + "angularVelocity": -2.949289312618861e-17, + "velocityX": -1.2177949773063277, + "velocityY": -0.0808685501242752, + "timestamp": 0.18810895370718583 + }, + { + "x": 2.385399814243042, + "y": 5.6578130134401405, + "heading": -9.068994008626725e-18, + "angularVelocity": -4.166887776483435e-17, + "velocityX": -1.8266924185782696, + "velocityY": -0.12130282204002872, + "timestamp": 0.28216343056077875 + }, + { + "x": 2.1563216922827166, + "y": 5.642600916220706, + "heading": -1.1203751573212612e-17, + "angularVelocity": -2.2697031803123958e-17, + "velocityX": -2.435589773327992, + "velocityY": -0.16173708821021213, + "timestamp": 0.37621790741437167 + }, + { + "x": 1.8699740888528424, + "y": 5.623585797951656, + "heading": -1.1207624225292261e-17, + "angularVelocity": -4.1174476338185276e-20, + "velocityX": -3.044486695467018, + "velocityY": -0.20217132565257148, + "timestamp": 0.4702723842679646 + }, + { + "x": 1.6408959668925174, + "y": 5.608373700732222, + "heading": -8.810275051865213e-18, + "angularVelocity": 2.5488942127572833e-17, + "velocityX": -2.4355897733279925, + "velocityY": -0.16173708821021213, + "timestamp": 0.5643268611215575 + }, + { + "x": 1.4690873670907139, + "y": 5.596964627264383, + "heading": -5.012140744625911e-18, + "angularVelocity": 4.0382279181189287e-17, + "velocityX": -1.8266924185782698, + "velocityY": -0.12130282204002875, + "timestamp": 0.6583813379751504 + }, + { + "x": 1.3545482975852343, + "y": 5.589358578088536, + "heading": -2.397406088315742e-18, + "angularVelocity": 2.780021402222337e-17, + "velocityX": -1.217794977306328, + "velocityY": -0.08086855012427523, + "timestamp": 0.7524358148287433 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 3.5803605220083744e-29, + "angularVelocity": 2.5489546637834322e-17, + "velocityX": -0.6088974989534299, + "velocityY": -0.040434275746134174, + "timestamp": 0.8464902916823362 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.6002222494720617e-29, + "angularVelocity": -2.492411377554893e-30, + "velocityX": 1.9318637428338628e-19, + "velocityY": -3.328737127233864e-19, + "timestamp": 0.9405447685359292 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFender.5.traj b/src/main/deploy/choreo/4NM-SweepFender.5.traj new file mode 100644 index 0000000..98e358c --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFender.5.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.6002222494720617e-29, + "angularVelocity": -2.492411377554893e-30, + "velocityX": 1.9318637428338628e-19, + "velocityY": -3.328737127233864e-19, + "timestamp": 0 + }, + { + "x": 1.2952633927726702, + "y": 5.615759088627331, + "heading": 7.301213567930375e-19, + "angularVelocity": 1.0689167577301925e-17, + "velocityX": -0.02950553143506986, + "velocityY": 0.44218766725785547, + "timestamp": 0.06830478872998302 + }, + { + "x": 1.292420372281359, + "y": 5.67622200565353, + "heading": 1.0899052605697139e-18, + "angularVelocity": 5.267330361290212e-18, + "velocityX": -0.04162256474505727, + "velocityY": 0.8851929440147399, + "timestamp": 0.13660957745996605 + }, + { + "x": 1.2904736626423332, + "y": 5.766942347295431, + "heading": 1.7572190046603685e-18, + "angularVelocity": 9.769647663647677e-18, + "velocityX": -0.028500339071703427, + "velocityY": 1.3281695665662407, + "timestamp": 0.20491436618994907 + }, + { + "x": 1.2921156551461344, + "y": 5.887719904619932, + "heading": 2.495299931317049e-18, + "angularVelocity": 1.080569753938027e-17, + "velocityX": 0.024039200388896183, + "velocityY": 1.7682150778907955, + "timestamp": 0.2732191549199321 + }, + { + "x": 1.3019663635740581, + "y": 6.037633895876457, + "heading": 3.044317794390009e-18, + "angularVelocity": 8.037763728377753e-18, + "velocityX": 0.1442169518577215, + "velocityY": 2.194780103180635, + "timestamp": 0.3415239436499151 + }, + { + "x": 1.3285668981441072, + "y": 6.21276212488805, + "heading": 3.684680070354539e-18, + "angularVelocity": 9.375068279246926e-18, + "velocityX": 0.3894387943311595, + "velocityY": 2.5639231489887897, + "timestamp": 0.40982873237989814 + }, + { + "x": 1.3835021976266313, + "y": 6.398541889884891, + "heading": 5.2477062652819604e-18, + "angularVelocity": 2.2883111141942298e-17, + "velocityX": 0.8042671751711217, + "velocityY": 2.719864426069018, + "timestamp": 0.47813352110988117 + }, + { + "x": 1.4670924013533833, + "y": 6.574564051095573, + "heading": 6.5266221738388946e-18, + "angularVelocity": 1.872366295041098e-17, + "velocityX": 1.2237824796910453, + "velocityY": 2.577010550556237, + "timestamp": 0.5464383098398642 + }, + { + "x": 1.5739126540179675, + "y": 6.731177977822192, + "heading": 7.49337156608948e-18, + "angularVelocity": 1.415346481933098e-17, + "velocityX": 1.5638764814405166, + "velocityY": 2.2928689135652283, + "timestamp": 0.6147430985698472 + }, + { + "x": 1.70013165473938, + "y": 6.864553928375244, + "heading": 8.152149061201394e-18, + "angularVelocity": 9.644675830649179e-18, + "velocityX": 1.8478792346517696, + "velocityY": 1.9526588550079922, + "timestamp": 0.6830478872998302 + }, + { + "x": 1.862901466815502, + "y": 6.9825514639396085, + "heading": 9.64595727993831e-18, + "angularVelocity": 1.9516578427319273e-17, + "velocityX": 2.126584724254197, + "velocityY": 1.541635721207726, + "timestamp": 0.759588365663892 + }, + { + "x": 2.0390721934747793, + "y": 7.064979265467772, + "heading": 1.0519046193421893e-17, + "angularVelocity": 1.1406891943933564e-17, + "velocityX": 2.301667436951849, + "velocityY": 1.0769177733133648, + "timestamp": 0.8361288440279537 + }, + { + "x": 2.212264721041194, + "y": 7.109513524937173, + "heading": 9.225749764850672e-18, + "angularVelocity": -1.689689286868671e-17, + "velocityX": 2.2627573183254994, + "velocityY": 0.581839314585602, + "timestamp": 0.9126693223920155 + }, + { + "x": 2.360897546409662, + "y": 7.125037248480103, + "heading": 6.641149339179356e-18, + "angularVelocity": -3.376775775195044e-17, + "velocityX": 1.9418852422309485, + "velocityY": 0.2028171743204049, + "timestamp": 0.9892098007560772 + }, + { + "x": 2.47440631167586, + "y": 7.126032006105099, + "heading": 2.8458717511366033e-18, + "angularVelocity": -4.958523477788854e-17, + "velocityX": 1.4829900164237055, + "velocityY": 0.012996490827559389, + "timestamp": 1.065750279120139 + }, + { + "x": 2.5502443215325226, + "y": 7.121956765796971, + "heading": -1.96953206231535e-18, + "angularVelocity": -6.291316501648883e-17, + "velocityX": 0.9908222613391942, + "velocityY": -0.053242942756960426, + "timestamp": 1.1422907574842007 + }, + { + "x": 2.58807373046875, + "y": 7.118251800537109, + "heading": -5.993434819714505e-29, + "angularVelocity": 2.573190102598612e-17, + "velocityX": 0.4942405606128222, + "velocityY": -0.048405305781324695, + "timestamp": 1.2188312358482625 + }, + { + "x": 2.58807373046875, + "y": 7.118251800537109, + "heading": -4.321420643442313e-29, + "angularVelocity": 3.722543235019264e-30, + "velocityX": -2.2890622227193903e-19, + "velocityY": 2.3506515476551758e-20, + "timestamp": 1.2953717142123242 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFender.6.traj b/src/main/deploy/choreo/4NM-SweepFender.6.traj new file mode 100644 index 0000000..92b6c7b --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFender.6.traj @@ -0,0 +1,121 @@ +{ + "samples": [ + { + "x": 2.58807373046875, + "y": 7.118251800537109, + "heading": -4.321420643442313e-29, + "angularVelocity": 3.722543235019264e-30, + "velocityX": -2.2890622227193903e-19, + "velocityY": 2.3506515476551758e-20, + "timestamp": 0 + }, + { + "x": 2.552218311146659, + "y": 7.07567690059573, + "heading": -2.216399626364933e-18, + "angularVelocity": -2.3929278644058663e-17, + "velocityX": -0.3871117607863663, + "velocityY": -0.45965839455282403, + "timestamp": 0.09262291398549927 + }, + { + "x": 2.4805074734431463, + "y": 6.990527101829926, + "heading": -5.2327325281333886e-18, + "angularVelocity": -3.256572939942303e-17, + "velocityX": -0.7742235114168355, + "velocityY": -0.9193167770464863, + "timestamp": 0.18524582797099853 + }, + { + "x": 2.372941218836405, + "y": 6.862802405994913, + "heading": -1.452637734592403e-17, + "angularVelocity": -1.0033850407775787e-16, + "velocityX": -1.1613352460880377, + "velocityY": -1.3789751405900372, + "timestamp": 0.2778687419564978 + }, + { + "x": 2.229519549987184, + "y": 6.692502816250075, + "heading": -9.320490492158675e-18, + "angularVelocity": 5.620517232807327e-17, + "velocityX": -1.5484469520325608, + "velocityY": -1.838633470023389, + "timestamp": 0.37049165594199707 + }, + { + "x": 2.050242473103896, + "y": 6.479628339967314, + "heading": -8.790348049291345e-20, + "angularVelocity": 9.967929575374868e-17, + "velocityX": -1.9355585909481747, + "velocityY": -2.298291719866287, + "timestamp": 0.46311456992749633 + }, + { + "x": 1.8351100192285623, + "y": 6.224179014006075, + "heading": 1.878777588983223e-17, + "angularVelocity": 2.0379059864583081e-16, + "velocityX": -2.322669894719714, + "velocityY": -2.757949571757475, + "timestamp": 0.5557374839129956 + }, + { + "x": 1.6558329423452747, + "y": 6.011304537723314, + "heading": 2.071185162570867e-17, + "angularVelocity": 2.0773214298256213e-17, + "velocityX": -1.9355585909481743, + "velocityY": -2.298291719866287, + "timestamp": 0.6483603978984949 + }, + { + "x": 1.5124112734960538, + "y": 5.841004947978476, + "heading": 1.8006256380694997e-17, + "angularVelocity": -2.921086300145266e-17, + "velocityX": -1.5484469520325606, + "velocityY": -1.838633470023389, + "timestamp": 0.7409833118839941 + }, + { + "x": 1.4048450188893125, + "y": 5.713280252143463, + "heading": 1.1160795488320093e-17, + "angularVelocity": -7.39067747686445e-17, + "velocityX": -1.1613352460880377, + "velocityY": -1.3789751405900372, + "timestamp": 0.8336062258694934 + }, + { + "x": 1.3331341811857995, + "y": 5.628130453377659, + "heading": 2.2020929810261438e-18, + "angularVelocity": -9.672231256696494e-17, + "velocityX": -0.7742235114168354, + "velocityY": -0.9193167770464864, + "timestamp": 0.9262291398549927 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 5.441647463444351e-29, + "angularVelocity": -2.377481856337291e-17, + "velocityX": -0.3871117607863663, + "velocityY": -0.4596583945528241, + "timestamp": 1.018852053840492 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.677896302038151e-29, + "angularVelocity": -9.269005124761017e-30, + "velocityX": 9.63963472983009e-20, + "velocityY": 1.144615001673186e-19, + "timestamp": 1.1114749678259912 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFender.traj b/src/main/deploy/choreo/4NM-SweepFender.traj new file mode 100644 index 0000000..38d9a10 --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFender.traj @@ -0,0 +1,724 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.9517716307925408e-29, + "angularVelocity": 3.5304770094188047e-29, + "velocityX": -1.724526619664194e-19, + "velocityY": 3.7818353955730183e-19, + "timestamp": 0 + }, + { + "x": 1.2920113917886342, + "y": 5.554330110103596, + "heading": 2.1731437642535974e-18, + "angularVelocity": 3.110625940380434e-17, + "velocityX": -0.07539684918457464, + "velocityY": -0.44695930001512396, + "timestamp": 0.069861938954233 + }, + { + "x": 1.282761551546609, + "y": 5.491689489432592, + "heading": 3.294425072358551e-18, + "angularVelocity": 1.604996065096705e-17, + "velocityX": -0.1324017108669824, + "velocityY": -0.8966344422825023, + "timestamp": 0.139723877908466 + }, + { + "x": 1.271458199578559, + "y": 5.397448925207256, + "heading": -1.1426328921377949e-17, + "angularVelocity": -2.1071206657810978e-16, + "velocityX": -0.16179556618740545, + "velocityY": -1.3489543181312655, + "timestamp": 0.209585816862699 + }, + { + "x": 1.261267862795592, + "y": 5.271561332803292, + "heading": -2.391418921336277e-17, + "angularVelocity": -1.7875054858841103e-16, + "velocityX": -0.14586392727580333, + "velocityY": -1.8019481607350603, + "timestamp": 0.279447755816932 + }, + { + "x": 1.2580444533875073, + "y": 5.114783048267347, + "heading": -3.501816243214742e-17, + "angularVelocity": -1.5894166558012616e-16, + "velocityX": -0.046139707204466994, + "velocityY": -2.244115850243602, + "timestamp": 0.34930969477116497 + }, + { + "x": 1.2734562507703713, + "y": 4.932401999052925, + "heading": -4.116208538702014e-17, + "angularVelocity": -8.794377668506781e-17, + "velocityX": 0.2206036307260309, + "velocityY": -2.6105924333691934, + "timestamp": 0.41917163372539795 + }, + { + "x": 1.3200722448212343, + "y": 4.744629254191581, + "heading": -4.742296059721744e-17, + "angularVelocity": -8.961782560603248e-17, + "velocityX": 0.6672588071367647, + "velocityY": -2.6877688720371022, + "timestamp": 0.48903357267963093 + }, + { + "x": 1.395362051384409, + "y": 4.570294708742184, + "heading": -4.971944913704747e-17, + "angularVelocity": -3.2871811710653476e-17, + "velocityX": 1.077694202167754, + "velocityY": -2.4954152154810103, + "timestamp": 0.558895511633864 + }, + { + "x": 1.4945697873897588, + "y": 4.416713614347291, + "heading": -4.954660865117441e-17, + "angularVelocity": 2.4740284798965177e-18, + "velocityX": 1.420054145224071, + "velocityY": -2.1983514442034626, + "timestamp": 0.628757450588097 + }, + { + "x": 1.6146607398986816, + "y": 4.286937236785889, + "heading": -4.40878360826832e-17, + "angularVelocity": 7.813657120875944e-17, + "velocityX": 1.7189753720920216, + "velocityY": -1.8576120202792965, + "timestamp": 0.6986193895423299 + }, + { + "x": 1.7770335829472077, + "y": 4.171588935482394, + "heading": -3.677251742788914e-17, + "angularVelocity": 9.144662966049345e-17, + "velocityX": 2.029774828142181, + "velocityY": -1.4419349569732136, + "timestamp": 0.7786148850018647 + }, + { + "x": 1.9580373267940145, + "y": 4.0933452292031935, + "heading": -2.7494710502210263e-17, + "angularVelocity": 1.159791154548558e-16, + "velocityX": 2.262674201928864, + "velocityY": -0.9781014022068157, + "timestamp": 0.8586103804613995 + }, + { + "x": 2.1432715777255, + "y": 4.056404838041411, + "heading": -1.4102008166543997e-17, + "angularVelocity": 1.6741820232350694e-16, + "velocityX": 2.315558518231629, + "velocityY": -0.4617808909061473, + "timestamp": 0.9386058759209343 + }, + { + "x": 2.30556761109126, + "y": 4.054072233873002, + "heading": -6.5008381019634475e-18, + "angularVelocity": 9.50199751021012e-17, + "velocityX": 2.0288146530432702, + "velocityY": -0.029159193964730743, + "timestamp": 1.018601371380469 + }, + { + "x": 2.4288986578849183, + "y": 4.066078072388016, + "heading": 5.687906951263531e-19, + "angularVelocity": 8.8375334664513e-17, + "velocityX": 1.5417248944478947, + "velocityY": 0.15008143203621294, + "timestamp": 1.0985968668400037 + }, + { + "x": 2.5107392491251295, + "y": 4.079634262670543, + "heading": 1.429916772585085e-18, + "angularVelocity": 1.0764681677136246e-17, + "velocityX": 1.0230649959735494, + "velocityY": 0.16946192038255736, + "timestamp": 1.1785923622995385 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -4.8145646612541634e-29, + "angularVelocity": -1.7874965990600758e-17, + "velocityX": 0.5077389399960675, + "velocityY": 0.10763325423991486, + "timestamp": 1.2585878577590732 + }, + { + "x": 2.551356077194214, + "y": 4.088244438171387, + "heading": -2.9775150183346403e-29, + "angularVelocity": -1.4341236571899655e-29, + "velocityX": -2.760379088955979e-19, + "velocityY": 2.12311250171415e-20, + "timestamp": 1.338583353218608 + }, + { + "x": 2.5095534969743727, + "y": 4.138154812312414, + "heading": 2.190510097540435e-18, + "angularVelocity": 2.186765826125146e-17, + "velocityX": -0.41731125706956324, + "velocityY": 0.4982506071172713, + "timestamp": 1.4387545791600944 + }, + { + "x": 2.4259483376755, + "y": 4.237975559232393, + "heading": 3.933266220970577e-18, + "angularVelocity": 1.739777229170673e-17, + "velocityX": -0.8346225027505352, + "velocityY": 0.9965012006370843, + "timestamp": 1.5389258051015808 + }, + { + "x": 2.300540601198944, + "y": 4.387706676661202, + "heading": 1.3527765530751777e-17, + "angularVelocity": 9.578099042485691e-17, + "velocityX": -1.2519337294505222, + "velocityY": 1.494751771494467, + "timestamp": 1.6390970310430673 + }, + { + "x": 2.133330291347402, + "y": 4.587348160058594, + "heading": 2.2617014617503804e-17, + "angularVelocity": 9.073712421559476e-17, + "velocityX": -1.669244918188541, + "velocityY": 1.9930022970269912, + "timestamp": 1.7392682569845537 + }, + { + "x": 1.9243174195289612, + "y": 4.836899995803833, + "heading": 3.880271021847084e-17, + "angularVelocity": 1.6158028652305301e-16, + "velocityX": -2.0865559930406836, + "velocityY": 2.4912526865849736, + "timestamp": 1.83943948292604 + }, + { + "x": 1.7153045477105202, + "y": 5.086451831549072, + "heading": 2.228690287655552e-17, + "angularVelocity": -1.648757621591215e-16, + "velocityX": -2.086555993040684, + "velocityY": 2.491252686584974, + "timestamp": 1.9396107088675265 + }, + { + "x": 1.548094237858978, + "y": 5.286093314946464, + "heading": 1.1611942212855913e-17, + "angularVelocity": -1.065671349922539e-16, + "velocityX": -1.6692449181885414, + "velocityY": 1.9930022970269914, + "timestamp": 2.039781934809013 + }, + { + "x": 1.422686501382422, + "y": 5.435824432375273, + "heading": 1.3869899594487062e-18, + "angularVelocity": -1.0207474326639434e-16, + "velocityX": -1.2519337294505222, + "velocityY": 1.494751771494467, + "timestamp": 2.1399531607504994 + }, + { + "x": 1.3390813420835495, + "y": 5.535645179295252, + "heading": 4.452399801737667e-19, + "angularVelocity": -9.40140233404049e-18, + "velocityX": -0.8346225027505352, + "velocityY": 0.9965012006370843, + "timestamp": 2.2401243866919858 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.2561037494800293e-29, + "angularVelocity": -4.4447889631061314e-18, + "velocityX": -0.4173112570695632, + "velocityY": 0.4982506071172713, + "timestamp": 2.340295612633472 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 1.2140312015726775e-29, + "angularVelocity": -1.6302535205902077e-30, + "velocityX": -1.8217183323138137e-19, + "velocityY": -1.1857377469208627e-19, + "timestamp": 2.4404668385749586 + }, + { + "x": 1.3545482975852345, + "y": 5.589358578088535, + "heading": 1.9334918613178174e-18, + "angularVelocity": 2.0557149745608903e-17, + "velocityX": 0.6088974989534299, + "velocityY": 0.040434275746134056, + "timestamp": 2.5345213154285515 + }, + { + "x": 1.469087367090714, + "y": 5.5969646272643825, + "heading": 2.3789667257742094e-18, + "angularVelocity": 4.736350526475188e-18, + "velocityX": 1.2177949773063277, + "velocityY": 0.08086855012427496, + "timestamp": 2.6285757922821444 + }, + { + "x": 1.6408959668925176, + "y": 5.608373700732222, + "heading": 4.870233499579861e-18, + "angularVelocity": 2.6487487710164578e-17, + "velocityX": 1.8266924185782696, + "velocityY": 0.12130282204002837, + "timestamp": 2.7226302691357374 + }, + { + "x": 1.8699740888528427, + "y": 5.623585797951656, + "heading": 2.7021941669251377e-18, + "angularVelocity": -2.3050888781904244e-17, + "velocityX": 2.435589773327992, + "velocityY": 0.16173708821021165, + "timestamp": 2.8166847459893303 + }, + { + "x": 2.156321692282717, + "y": 5.642600916220706, + "heading": -8.385603820696607e-19, + "angularVelocity": -3.764578410393351e-17, + "velocityX": 3.0444866954670173, + "velocityY": 0.20217132565257093, + "timestamp": 2.910739222842923 + }, + { + "x": 2.385399814243042, + "y": 5.657813013440141, + "heading": -1.979930432977169e-18, + "angularVelocity": -1.2135202438796547e-17, + "velocityX": 2.4355897733279916, + "velocityY": 0.16173708821021165, + "timestamp": 3.004793699696516 + }, + { + "x": 2.5572084140448452, + "y": 5.66922208690798, + "heading": -2.2142142923894204e-18, + "angularVelocity": -2.490938414552063e-18, + "velocityX": 1.8266924185782694, + "velocityY": 0.12130282204002837, + "timestamp": 3.098848176550109 + }, + { + "x": 2.671747483550325, + "y": 5.676828136083827, + "heading": -1.281622902160523e-18, + "angularVelocity": 9.915437612097848e-18, + "velocityX": 1.2177949773063277, + "velocityY": 0.08086855012427498, + "timestamp": 3.192902653403702 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.304583994113875e-29, + "angularVelocity": 1.3626388144391886e-17, + "velocityX": 0.60889749895343, + "velocityY": 0.040434275746134056, + "timestamp": 3.286957130257295 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.1449818357083717e-29, + "angularVelocity": -3.0489465233384347e-30, + "velocityX": -1.0348040233346454e-19, + "velocityY": -6.871642741102469e-21, + "timestamp": 3.3810116071108878 + }, + { + "x": 2.671747483550325, + "y": 5.676828136083827, + "heading": -2.37591069216883e-18, + "angularVelocity": -2.5261004796916857e-17, + "velocityX": -0.6088974989534299, + "velocityY": -0.04043427574613417, + "timestamp": 3.4750660839644807 + }, + { + "x": 2.5572084140448452, + "y": 5.66922208690798, + "heading": -5.149849388984227e-18, + "angularVelocity": -2.949289312618861e-17, + "velocityX": -1.2177949773063277, + "velocityY": -0.0808685501242752, + "timestamp": 3.5691205608180736 + }, + { + "x": 2.385399814243042, + "y": 5.6578130134401405, + "heading": -9.068994008626725e-18, + "angularVelocity": -4.166887776483435e-17, + "velocityX": -1.8266924185782696, + "velocityY": -0.12130282204002872, + "timestamp": 3.6631750376716665 + }, + { + "x": 2.1563216922827166, + "y": 5.642600916220706, + "heading": -1.1203751573212612e-17, + "angularVelocity": -2.2697031803123958e-17, + "velocityX": -2.435589773327992, + "velocityY": -0.16173708821021213, + "timestamp": 3.7572295145252594 + }, + { + "x": 1.8699740888528424, + "y": 5.623585797951656, + "heading": -1.1207624225292261e-17, + "angularVelocity": -4.1174476338185276e-20, + "velocityX": -3.044486695467018, + "velocityY": -0.20217132565257148, + "timestamp": 3.8512839913788524 + }, + { + "x": 1.6408959668925174, + "y": 5.608373700732222, + "heading": -8.810275051865213e-18, + "angularVelocity": 2.5488942127572833e-17, + "velocityX": -2.4355897733279925, + "velocityY": -0.16173708821021213, + "timestamp": 3.9453384682324453 + }, + { + "x": 1.4690873670907139, + "y": 5.596964627264383, + "heading": -5.012140744625911e-18, + "angularVelocity": 4.0382279181189287e-17, + "velocityX": -1.8266924185782698, + "velocityY": -0.12130282204002875, + "timestamp": 4.039392945086038 + }, + { + "x": 1.3545482975852343, + "y": 5.589358578088536, + "heading": -2.397406088315742e-18, + "angularVelocity": 2.780021402222337e-17, + "velocityX": -1.217794977306328, + "velocityY": -0.08086855012427523, + "timestamp": 4.133447421939631 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 3.5803605220083744e-29, + "angularVelocity": 2.5489546637834322e-17, + "velocityX": -0.6088974989534299, + "velocityY": -0.040434275746134174, + "timestamp": 4.227501898793224 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.6002222494720617e-29, + "angularVelocity": -2.492411377554893e-30, + "velocityX": 1.9318637428338628e-19, + "velocityY": -3.328737127233864e-19, + "timestamp": 4.321556375646817 + }, + { + "x": 1.2952633927726702, + "y": 5.615759088627331, + "heading": 7.301213567930375e-19, + "angularVelocity": 1.0689167577301925e-17, + "velocityX": -0.02950553143506986, + "velocityY": 0.44218766725785547, + "timestamp": 4.3898611643768 + }, + { + "x": 1.292420372281359, + "y": 5.67622200565353, + "heading": 1.0899052605697139e-18, + "angularVelocity": 5.267330361290212e-18, + "velocityX": -0.04162256474505727, + "velocityY": 0.8851929440147399, + "timestamp": 4.458165953106783 + }, + { + "x": 1.2904736626423332, + "y": 5.766942347295431, + "heading": 1.7572190046603685e-18, + "angularVelocity": 9.769647663647677e-18, + "velocityX": -0.028500339071703427, + "velocityY": 1.3281695665662407, + "timestamp": 4.526470741836766 + }, + { + "x": 1.2921156551461344, + "y": 5.887719904619932, + "heading": 2.495299931317049e-18, + "angularVelocity": 1.080569753938027e-17, + "velocityX": 0.024039200388896183, + "velocityY": 1.7682150778907955, + "timestamp": 4.594775530566749 + }, + { + "x": 1.3019663635740581, + "y": 6.037633895876457, + "heading": 3.044317794390009e-18, + "angularVelocity": 8.037763728377753e-18, + "velocityX": 0.1442169518577215, + "velocityY": 2.194780103180635, + "timestamp": 4.663080319296732 + }, + { + "x": 1.3285668981441072, + "y": 6.21276212488805, + "heading": 3.684680070354539e-18, + "angularVelocity": 9.375068279246926e-18, + "velocityX": 0.3894387943311595, + "velocityY": 2.5639231489887897, + "timestamp": 4.731385108026715 + }, + { + "x": 1.3835021976266313, + "y": 6.398541889884891, + "heading": 5.2477062652819604e-18, + "angularVelocity": 2.2883111141942298e-17, + "velocityX": 0.8042671751711217, + "velocityY": 2.719864426069018, + "timestamp": 4.799689896756698 + }, + { + "x": 1.4670924013533833, + "y": 6.574564051095573, + "heading": 6.5266221738388946e-18, + "angularVelocity": 1.872366295041098e-17, + "velocityX": 1.2237824796910453, + "velocityY": 2.577010550556237, + "timestamp": 4.867994685486681 + }, + { + "x": 1.5739126540179675, + "y": 6.731177977822192, + "heading": 7.49337156608948e-18, + "angularVelocity": 1.415346481933098e-17, + "velocityX": 1.5638764814405166, + "velocityY": 2.2928689135652283, + "timestamp": 4.936299474216664 + }, + { + "x": 1.70013165473938, + "y": 6.864553928375244, + "heading": 8.152149061201394e-18, + "angularVelocity": 9.644675830649179e-18, + "velocityX": 1.8478792346517696, + "velocityY": 1.9526588550079922, + "timestamp": 5.004604262946647 + }, + { + "x": 1.862901466815502, + "y": 6.9825514639396085, + "heading": 9.64595727993831e-18, + "angularVelocity": 1.9516578427319273e-17, + "velocityX": 2.126584724254197, + "velocityY": 1.541635721207726, + "timestamp": 5.081144741310709 + }, + { + "x": 2.0390721934747793, + "y": 7.064979265467772, + "heading": 1.0519046193421893e-17, + "angularVelocity": 1.1406891943933564e-17, + "velocityX": 2.301667436951849, + "velocityY": 1.0769177733133648, + "timestamp": 5.157685219674771 + }, + { + "x": 2.212264721041194, + "y": 7.109513524937173, + "heading": 9.225749764850672e-18, + "angularVelocity": -1.689689286868671e-17, + "velocityX": 2.2627573183254994, + "velocityY": 0.581839314585602, + "timestamp": 5.234225698038832 + }, + { + "x": 2.360897546409662, + "y": 7.125037248480103, + "heading": 6.641149339179356e-18, + "angularVelocity": -3.376775775195044e-17, + "velocityX": 1.9418852422309485, + "velocityY": 0.2028171743204049, + "timestamp": 5.310766176402894 + }, + { + "x": 2.47440631167586, + "y": 7.126032006105099, + "heading": 2.8458717511366033e-18, + "angularVelocity": -4.958523477788854e-17, + "velocityX": 1.4829900164237055, + "velocityY": 0.012996490827559389, + "timestamp": 5.387306654766956 + }, + { + "x": 2.5502443215325226, + "y": 7.121956765796971, + "heading": -1.96953206231535e-18, + "angularVelocity": -6.291316501648883e-17, + "velocityX": 0.9908222613391942, + "velocityY": -0.053242942756960426, + "timestamp": 5.463847133131018 + }, + { + "x": 2.58807373046875, + "y": 7.118251800537109, + "heading": -5.993434819714505e-29, + "angularVelocity": 2.573190102598612e-17, + "velocityX": 0.4942405606128222, + "velocityY": -0.048405305781324695, + "timestamp": 5.540387611495079 + }, + { + "x": 2.58807373046875, + "y": 7.118251800537109, + "heading": -4.321420643442313e-29, + "angularVelocity": 3.722543235019264e-30, + "velocityX": -2.2890622227193903e-19, + "velocityY": 2.3506515476551758e-20, + "timestamp": 5.616928089859141 + }, + { + "x": 2.552218311146659, + "y": 7.07567690059573, + "heading": -2.216399626364933e-18, + "angularVelocity": -2.3929278644058663e-17, + "velocityX": -0.3871117607863663, + "velocityY": -0.45965839455282403, + "timestamp": 5.70955100384464 + }, + { + "x": 2.4805074734431463, + "y": 6.990527101829926, + "heading": -5.2327325281333886e-18, + "angularVelocity": -3.256572939942303e-17, + "velocityX": -0.7742235114168355, + "velocityY": -0.9193167770464863, + "timestamp": 5.80217391783014 + }, + { + "x": 2.372941218836405, + "y": 6.862802405994913, + "heading": -1.452637734592403e-17, + "angularVelocity": -1.0033850407775787e-16, + "velocityX": -1.1613352460880377, + "velocityY": -1.3789751405900372, + "timestamp": 5.894796831815639 + }, + { + "x": 2.229519549987184, + "y": 6.692502816250075, + "heading": -9.320490492158675e-18, + "angularVelocity": 5.620517232807327e-17, + "velocityX": -1.5484469520325608, + "velocityY": -1.838633470023389, + "timestamp": 5.987419745801138 + }, + { + "x": 2.050242473103896, + "y": 6.479628339967314, + "heading": -8.790348049291345e-20, + "angularVelocity": 9.967929575374868e-17, + "velocityX": -1.9355585909481747, + "velocityY": -2.298291719866287, + "timestamp": 6.0800426597866375 + }, + { + "x": 1.8351100192285623, + "y": 6.224179014006075, + "heading": 1.878777588983223e-17, + "angularVelocity": 2.0379059864583081e-16, + "velocityX": -2.322669894719714, + "velocityY": -2.757949571757475, + "timestamp": 6.172665573772137 + }, + { + "x": 1.6558329423452747, + "y": 6.011304537723314, + "heading": 2.071185162570867e-17, + "angularVelocity": 2.0773214298256213e-17, + "velocityX": -1.9355585909481743, + "velocityY": -2.298291719866287, + "timestamp": 6.265288487757636 + }, + { + "x": 1.5124112734960538, + "y": 5.841004947978476, + "heading": 1.8006256380694997e-17, + "angularVelocity": -2.921086300145266e-17, + "velocityX": -1.5484469520325606, + "velocityY": -1.838633470023389, + "timestamp": 6.357911401743135 + }, + { + "x": 1.4048450188893125, + "y": 5.713280252143463, + "heading": 1.1160795488320093e-17, + "angularVelocity": -7.39067747686445e-17, + "velocityX": -1.1613352460880377, + "velocityY": -1.3789751405900372, + "timestamp": 6.450534315728635 + }, + { + "x": 1.3331341811857995, + "y": 5.628130453377659, + "heading": 2.2020929810261438e-18, + "angularVelocity": -9.672231256696494e-17, + "velocityX": -0.7742235114168354, + "velocityY": -0.9193167770464864, + "timestamp": 6.543157229714134 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 5.441647463444351e-29, + "angularVelocity": -2.377481856337291e-17, + "velocityX": -0.3871117607863663, + "velocityY": -0.4596583945528241, + "timestamp": 6.635780143699633 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": 2.677896302038151e-29, + "angularVelocity": -9.269005124761017e-30, + "velocityX": 9.63963472983009e-20, + "velocityY": 1.144615001673186e-19, + "timestamp": 6.728403057685132 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFenderStraight.1.traj b/src/main/deploy/choreo/4NM-SweepFenderStraight.1.traj new file mode 100644 index 0000000..c6d4003 --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFenderStraight.1.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -3.4870573885213374e-23, + "angularVelocity": -5.958014181544604e-22, + "velocityX": 4.75095708718707e-20, + "velocityY": -5.100796288410356e-20, + "timestamp": 0 + }, + { + "x": 1.3221899506052506, + "y": 5.558731738657612, + "heading": -0.011172212869994504, + "angularVelocity": -0.14847792516259956, + "velocityX": 0.33106795052320837, + "velocityY": -0.35648661636038936, + "timestamp": 0.07524494201922415 + }, + { + "x": 1.3720125126335756, + "y": 5.5050840215864865, + "heading": -0.03353368538631347, + "angularVelocity": -0.29718240078656566, + "velocityX": 0.662138353639718, + "velocityY": -0.712974395772926, + "timestamp": 0.1504898840384483 + }, + { + "x": 1.4467463839809327, + "y": 5.424612630178231, + "heading": -0.06713200496181293, + "angularVelocity": -0.4465193097884971, + "velocityX": 0.9932079066293052, + "velocityY": -1.0694591456751465, + "timestamp": 0.22573482605767242 + }, + { + "x": 1.5463911448663232, + "y": 5.31731803920096, + "heading": -0.11204656505352982, + "angularVelocity": -0.5969113522639407, + "velocityX": 1.3242718807588763, + "velocityY": -1.4259375859424448, + "timestamp": 0.3009797680768966 + }, + { + "x": 1.6709458752175186, + "y": 5.183200865786002, + "heading": -0.1683893711840811, + "angularVelocity": -0.7487919402762832, + "velocityX": 1.6553236271930836, + "velocityY": -1.7824078245776684, + "timestamp": 0.37622471009612074 + }, + { + "x": 1.8204089760394484, + "y": 5.022261716216735, + "heading": -0.2363043631413436, + "angularVelocity": -0.9025854779702146, + "velocityX": 1.9863541230949968, + "velocityY": -2.1388700057493355, + "timestamp": 0.4514696521153449 + }, + { + "x": 1.99477795080174, + "y": 4.83450098026474, + "heading": -0.3159644115550863, + "angularVelocity": -1.0586764542045974, + "velocityX": 2.317351440283424, + "velocityY": -2.495327006884067, + "timestamp": 0.526714594134569 + }, + { + "x": 2.1441616151551486, + "y": 4.673559909893242, + "heading": -0.3887569416939099, + "angularVelocity": -0.9674076181788537, + "velocityX": 1.9852984180018707, + "velocityY": -2.138895533076221, + "timestamp": 0.6019595361537932 + }, + { + "x": 2.268644362208996, + "y": 4.53944104449319, + "heading": -0.449547316997939, + "angularVelocity": -0.8078998225355519, + "velocityX": 1.6543669742219151, + "velocityY": -1.782430310940861, + "timestamp": 0.6772044781730173 + }, + { + "x": 2.3682284247776337, + "y": 4.432144914414375, + "heading": -0.49824500481886397, + "angularVelocity": -0.6471888543482873, + "velocityX": 1.323465204388028, + "velocityY": -1.425958040493952, + "timestamp": 0.7524494201922415 + }, + { + "x": 2.4429154717792705, + "y": 4.351672139574372, + "heading": -0.5347927169900155, + "angularVelocity": -0.48571653044551594, + "velocityX": 0.9925856143600397, + "velocityY": -1.0694775313860099, + "timestamp": 0.8276943622114656 + }, + { + "x": 2.492706577536202, + "y": 4.298023281924518, + "heading": -0.5591597915473412, + "angularVelocity": -0.3238367111918331, + "velocityX": 0.6617203019999724, + "velocityY": -0.7129895539842044, + "timestamp": 0.9029393042306898 + }, + { + "x": 2.5176022052764893, + "y": 4.271198749542236, + "heading": -0.5713377241070188, + "angularVelocity": -0.16184386927384822, + "velocityX": 0.3308611459083442, + "velocityY": -0.3564961532620849, + "timestamp": 0.978184246249914 + }, + { + "x": 2.5176022052764893, + "y": 4.271198749542236, + "heading": -0.5713377241070188, + "angularVelocity": -1.965008466563789e-21, + "velocityX": 1.6720342558000003e-19, + "velocityY": -1.796953683889986e-19, + "timestamp": 1.053429188269138 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFenderStraight.2.traj b/src/main/deploy/choreo/4NM-SweepFenderStraight.2.traj new file mode 100644 index 0000000..5013ece --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFenderStraight.2.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 2.5176022052764893, + "y": 4.271198749542236, + "heading": -0.5713377241070188, + "angularVelocity": -1.965008466563789e-21, + "velocityX": 1.6720342558000003e-19, + "velocityY": -1.796953683889986e-19, + "timestamp": 0 + }, + { + "x": 2.4927076698068604, + "y": 4.298022743670389, + "heading": -0.5590626618027412, + "angularVelocity": 0.163131980525576, + "velocityX": -0.3308410804570447, + "velocityY": 0.3564830205551893, + "timestamp": 0.07524620411479122 + }, + { + "x": 2.4429176923283147, + "y": 4.351669893774747, + "heading": -0.5345293787694578, + "angularVelocity": 0.32604014145161125, + "velocityX": -0.6616942085555453, + "velocityY": 0.712954902316633, + "timestamp": 0.15049240822958243 + }, + { + "x": 2.368230774545632, + "y": 4.432139277372316, + "heading": -0.49778036282477456, + "angularVelocity": 0.48838365173373005, + "velocityX": -0.9925672485584068, + "velocityY": 1.0694145245494298, + "timestamp": 0.22573861234437365 + }, + { + "x": 2.2686448820648275, + "y": 4.539430029093104, + "heading": -0.4488890301403053, + "angularVelocity": 0.6497514826114488, + "velocityX": -1.3234673250611078, + "velocityY": 1.4258626462686734, + "timestamp": 0.30098481645916486 + }, + { + "x": 2.14415754877083, + "y": 4.673541531401143, + "heading": -0.38796534930924853, + "angularVelocity": 0.8096578631144655, + "velocityX": -1.6544001755103406, + "velocityY": 1.7823025611158592, + "timestamp": 0.3762310205739561 + }, + { + "x": 1.9947660347325065, + "y": 4.83447364690962, + "heading": -0.31516105455016663, + "angularVelocity": 0.9675477403221032, + "velocityX": -1.9853694388413312, + "velocityY": 2.138740650132567, + "timestamp": 0.4514772246887473 + }, + { + "x": 1.8203962771969746, + "y": 5.02224100400193, + "heading": -0.2359849474173268, + "angularVelocity": 1.0522272593585371, + "velocityX": -2.3173229744522876, + "velocityY": 2.4953731460774122, + "timestamp": 0.5267234288035385 + }, + { + "x": 1.6709347760864326, + "y": 5.183186512420658, + "heading": -0.1683650413377472, + "angularVelocity": 0.8986487341796361, + "velocityX": -1.9862995465197482, + "velocityY": 2.1389186379847103, + "timestamp": 0.6019696329183297 + }, + { + "x": 1.546382775447489, + "y": 5.317309066145499, + "heading": -0.11216464144227759, + "angularVelocity": 0.7468868437500663, + "velocityX": -1.6552595855723733, + "velocityY": 1.7824494312062664, + "timestamp": 0.6772158370331209 + }, + { + "x": 1.4467409698876759, + "y": 5.424607739615935, + "heading": -0.06728177554770955, + "angularVelocity": 0.5964801337499681, + "velocityX": -1.3242103934944731, + "velocityY": 1.4259679239998275, + "timestamp": 0.7524620411479122 + }, + { + "x": 1.3720096830963446, + "y": 5.505081872672054, + "heading": -0.0336471785574408, + "angularVelocity": 0.4469939365839344, + "velocityX": -0.9931568996799567, + "velocityY": 1.0694776434616193, + "timestamp": 0.8277082452627034 + }, + { + "x": 1.3221889841070316, + "y": 5.558731132858149, + "heading": -0.011222693307519035, + "angularVelocity": 0.2980148369439646, + "velocityX": -0.6621024884299752, + "velocityY": 0.7129829446845056, + "timestamp": 0.9029544493774946 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -5.804527907102298e-22, + "angularVelocity": 0.14914630498035963, + "velocityX": -0.33104955308205936, + "velocityY": 0.3564886879503997, + "timestamp": 0.9782006534922858 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -1.7372853944454406e-22, + "angularVelocity": 2.7872047475617054e-21, + "velocityX": -2.1429242136387852e-19, + "velocityY": 2.3070553047501513e-19, + "timestamp": 1.053446857607077 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFenderStraight.3.traj b/src/main/deploy/choreo/4NM-SweepFenderStraight.3.traj new file mode 100644 index 0000000..f33c42f --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFenderStraight.3.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -1.7372853944454406e-22, + "angularVelocity": 2.7872047475617054e-21, + "velocityX": -2.1429242136387852e-19, + "velocityY": 2.3070553047501513e-19, + "timestamp": 0 + }, + { + "x": 1.3545482975852352, + "y": 5.589358578088536, + "heading": 9.197337540759096e-19, + "angularVelocity": 9.780581566888343e-18, + "velocityX": 0.6088974989534347, + "velocityY": 0.04043427574613332, + "timestamp": 0.09405447685359336 + }, + { + "x": 1.4690873670907165, + "y": 5.596964627264383, + "heading": 3.887468901363766e-18, + "angularVelocity": 3.155336403486356e-17, + "velocityX": 1.2177949773063368, + "velocityY": 0.08086855012427349, + "timestamp": 0.18810895370718672 + }, + { + "x": 1.6408959668925225, + "y": 5.608373700732223, + "heading": 3.6659024972997286e-18, + "angularVelocity": -2.3557241663966836e-18, + "velocityX": 1.8266924185782827, + "velocityY": 0.1213028220400261, + "timestamp": 0.2821634305607801 + }, + { + "x": 1.8699740888528504, + "y": 5.623585797951658, + "heading": -8.067542241443972e-19, + "angularVelocity": -4.7553895051950526e-17, + "velocityX": 2.435589773328005, + "velocityY": 0.16173708821020832, + "timestamp": 0.37621790741437344 + }, + { + "x": 2.156321692282708, + "y": 5.642600916220705, + "heading": -2.6708058042599195e-18, + "angularVelocity": -1.981885012273154e-17, + "velocityX": 3.044486695466825, + "velocityY": 0.20217132565255283, + "timestamp": 0.4702723842679668 + }, + { + "x": 2.385399814243036, + "y": 5.6578130134401405, + "heading": -2.770306305819978e-18, + "angularVelocity": -1.0579028761460627e-18, + "velocityX": 2.435589773328005, + "velocityY": 0.1617370882102083, + "timestamp": 0.5643268611215602 + }, + { + "x": 2.557208414044842, + "y": 5.66922208690798, + "heading": -1.5286567962624567e-18, + "angularVelocity": 1.3201386590800127e-17, + "velocityX": 1.8266924185782827, + "velocityY": 0.12130282204002607, + "timestamp": 0.6583813379751535 + }, + { + "x": 2.6717474835503237, + "y": 5.676828136083827, + "heading": -1.1055203544056362e-18, + "angularVelocity": 4.4988442442533515e-18, + "velocityX": 1.217794977306337, + "velocityY": 0.08086855012427348, + "timestamp": 0.7524358148287469 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.3994282719055762e-28, + "angularVelocity": 1.1754042882709242e-17, + "velocityX": 0.6088974989534347, + "velocityY": 0.04043427574613331, + "timestamp": 0.8464902916823402 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.2578191824600292e-28, + "angularVelocity": -3.8382941954344137e-29, + "velocityX": -7.718549111027777e-22, + "velocityY": -5.126840450701387e-23, + "timestamp": 0.9405447685359336 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFenderStraight.4.traj b/src/main/deploy/choreo/4NM-SweepFenderStraight.4.traj new file mode 100644 index 0000000..1bb3a75 --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFenderStraight.4.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.2578191824600292e-28, + "angularVelocity": -3.8382941954344137e-29, + "velocityX": -7.718549111027777e-22, + "velocityY": -5.126840450701387e-23, + "timestamp": 0 + }, + { + "x": 2.6717474835503237, + "y": 5.676828136083827, + "heading": 2.957978952433013e-18, + "angularVelocity": 3.144963484589081e-17, + "velocityX": -0.6088974989534347, + "velocityY": -0.04043427574613368, + "timestamp": 0.09405447685359336 + }, + { + "x": 2.557208414044842, + "y": 5.66922208690798, + "heading": 7.803415503811453e-18, + "angularVelocity": 5.15173409451729e-17, + "velocityX": -1.217794977306337, + "velocityY": -0.08086855012427421, + "timestamp": 0.18810895370718672 + }, + { + "x": 2.385399814243036, + "y": 5.6578130134401405, + "heading": 1.0918258808355902e-17, + "angularVelocity": 3.3117437987572525e-17, + "velocityX": -1.8266924185782827, + "velocityY": -0.12130282204002718, + "timestamp": 0.2821634305607801 + }, + { + "x": 2.156321692282708, + "y": 5.642600916220706, + "heading": 1.4843907441415233e-17, + "angularVelocity": 4.173803060196763e-17, + "velocityX": -2.435589773328005, + "velocityY": -0.16173708821020977, + "timestamp": 0.37621790741437344 + }, + { + "x": 1.8699740888528504, + "y": 5.623585797951657, + "heading": 1.4047394913563068e-17, + "angularVelocity": -8.468629611156265e-18, + "velocityX": -3.044486695466825, + "velocityY": -0.2021713256525547, + "timestamp": 0.4702723842679668 + }, + { + "x": 1.6408959668925225, + "y": 5.608373700732223, + "heading": 5.894670829565627e-18, + "angularVelocity": -8.668087215646387e-17, + "velocityX": -2.435589773328005, + "velocityY": -0.1617370882102098, + "timestamp": 0.5643268611215602 + }, + { + "x": 1.4690873670907165, + "y": 5.596964627264383, + "heading": -3.5801552348274217e-19, + "angularVelocity": -6.647941238005475e-17, + "velocityX": -1.8266924185782827, + "velocityY": -0.12130282204002721, + "timestamp": 0.6583813379751535 + }, + { + "x": 1.3545482975852352, + "y": 5.589358578088536, + "heading": -2.459449657169509e-18, + "angularVelocity": -2.234273374195548e-17, + "velocityX": -1.2177949773063372, + "velocityY": -0.08086855012427423, + "timestamp": 0.7524358148287469 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -2.170460361005601e-22, + "angularVelocity": 2.6146895857700576e-17, + "velocityX": -0.6088974989534347, + "velocityY": -0.04043427574613369, + "timestamp": 0.8464902916823402 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -3.6388277145419484e-23, + "angularVelocity": 1.9207790315741447e-21, + "velocityX": 1.47076337525445e-19, + "velocityY": 1.4190124023616126e-19, + "timestamp": 0.9405447685359336 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFenderStraight.5.traj b/src/main/deploy/choreo/4NM-SweepFenderStraight.5.traj new file mode 100644 index 0000000..23e6287 --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFenderStraight.5.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -3.6388277145419484e-23, + "angularVelocity": 1.9207790315741447e-21, + "velocityX": 1.47076337525445e-19, + "velocityY": 1.4190124023616126e-19, + "timestamp": 0 + }, + { + "x": 1.3227737240195483, + "y": 5.61050348195736, + "heading": 0.013550786900983634, + "angularVelocity": 0.18226929801485287, + "velocityX": 0.34292833980902226, + "velocityY": 0.33557028471401684, + "timestamp": 0.07434486799789708 + }, + { + "x": 1.3737639106778228, + "y": 5.660399482511426, + "heading": 0.040679974016250266, + "angularVelocity": 0.36491001794547456, + "velocityX": 0.6858602083968518, + "velocityY": 0.6711424997819331, + "timestamp": 0.1486897359957946 + }, + { + "x": 1.4502490112097923, + "y": 5.735243170344193, + "heading": 0.08146222479513072, + "angularVelocity": 0.548555023058672, + "velocityX": 1.0287878987710746, + "velocityY": 1.0067095395862875, + "timestamp": 0.22303460399369213 + }, + { + "x": 1.5522278856647675, + "y": 5.835033815033583, + "heading": 0.1360214110793065, + "angularVelocity": 0.7338662069548484, + "velocityX": 1.3717002558651332, + "velocityY": 1.3422667546092502, + "timestamp": 0.29737947199158965 + }, + { + "x": 1.6796982296330216, + "y": 5.959770632655783, + "heading": 0.20453087198468312, + "angularVelocity": 0.9215089454098471, + "velocityX": 1.7145816167413093, + "velocityY": 1.6778134252081405, + "timestamp": 0.3717243399894872 + }, + { + "x": 1.8326561921573887, + "y": 6.109453229425897, + "heading": 0.28720867051263665, + "angularVelocity": 1.1120848116953013, + "velocityX": 2.0574111790566834, + "velocityY": 2.01335479907434, + "timestamp": 0.4460692079873847 + }, + { + "x": 2.011096000176797, + "y": 6.28408222003575, + "heading": 0.38430411909211587, + "angularVelocity": 1.3060141364730837, + "velocityX": 2.400163088923032, + "velocityY": 2.3489044410542577, + "timestamp": 0.5204140759852822 + }, + { + "x": 2.1638766096575854, + "y": 6.433815495784424, + "heading": 0.4735075918913831, + "angularVelocity": 1.1998605310831978, + "velocityX": 2.05502563384886, + "velocityY": 2.0140364732762612, + "timestamp": 0.5947589439831797 + }, + { + "x": 2.291186482527782, + "y": 6.5585980911554875, + "heading": 0.5479625204474381, + "angularVelocity": 1.0014804055898086, + "velocityX": 1.712423147671697, + "velocityY": 1.678429173814564, + "timestamp": 0.6691038119810773 + }, + { + "x": 2.393030162177412, + "y": 6.658427500466555, + "heading": 0.6075765143590145, + "angularVelocity": 0.8018575527400554, + "velocityX": 1.3698817738503593, + "velocityY": 1.3427881708511593, + "timestamp": 0.7434486799789748 + }, + { + "x": 2.469410931527034, + "y": 6.733301422056823, + "heading": 0.6523015105520286, + "angularVelocity": 0.6015882117684191, + "velocityX": 1.027384558027357, + "velocityY": 1.007116208645172, + "timestamp": 0.8177935479768723 + }, + { + "x": 2.520330867960948, + "y": 6.783218097420482, + "heading": 0.6821175635862542, + "angularVelocity": 0.4010505881195312, + "velocityX": 0.6849152847423776, + "velocityY": 0.6714205930807574, + "timestamp": 0.8921384159747698 + }, + { + "x": 2.545790910720825, + "y": 6.808176517486572, + "heading": 0.6970216139142698, + "angularVelocity": 0.20047181102584016, + "velocityX": 0.34245864503515044, + "velocityY": 0.3357114046768724, + "timestamp": 0.9664832839726674 + }, + { + "x": 2.545790910720825, + "y": 6.808176517486572, + "heading": 0.6970216139142698, + "angularVelocity": 1.060990978792425e-20, + "velocityX": 9.444494751401817e-19, + "velocityY": 9.172807861306771e-19, + "timestamp": 1.0408281519705649 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFenderStraight.6.traj b/src/main/deploy/choreo/4NM-SweepFenderStraight.6.traj new file mode 100644 index 0000000..eb28497 --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFenderStraight.6.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 2.545790910720825, + "y": 6.808176517486572, + "heading": 0.6970216139142698, + "angularVelocity": 1.060990978792425e-20, + "velocityX": 9.444494751401817e-19, + "velocityY": 9.172807861306771e-19, + "timestamp": 0 + }, + { + "x": 2.52033376431852, + "y": 6.783218896026595, + "heading": 0.6819576043595674, + "angularVelocity": -0.20261789732027236, + "velocityX": -0.34241039592273365, + "velocityY": -0.3356915543615947, + "timestamp": 0.07434688521562816 + }, + { + "x": 2.469417510751594, + "y": 6.733305671093222, + "heading": 0.6518402320176618, + "angularVelocity": -0.405092590692349, + "velocityX": -0.6848471650056825, + "velocityY": -0.6713559658701147, + "timestamp": 0.1486937704312563 + }, + { + "x": 2.393038999153381, + "y": 6.65843948497798, + "heading": 0.6067059073438061, + "angularVelocity": -0.6070775465973136, + "velocityX": -1.0273263147029235, + "velocityY": -1.0069848373352566, + "timestamp": 0.22304065564688447 + }, + { + "x": 2.2911938790627677, + "y": 6.558623270950903, + "heading": 0.5466314564942851, + "angularVelocity": -0.8080291551594541, + "velocityX": -1.3698639801147294, + "velocityY": -1.342574254961449, + "timestamp": 0.2973875408625126 + }, + { + "x": 2.163876704340966, + "y": 6.433859756287583, + "heading": 0.471750868383845, + "angularVelocity": -1.0071785508332185, + "velocityX": -1.7124748986126783, + "velocityY": -1.6781269894692763, + "timestamp": 0.3717344260781408 + }, + { + "x": 2.0110812200123074, + "y": 6.284150815002161, + "heading": 0.3822707555656717, + "angularVelocity": -1.2035489120849394, + "velocityX": -2.0551699494270133, + "velocityY": -2.0136545176199485, + "timestamp": 0.44608131129376893 + }, + { + "x": 1.832635836912685, + "y": 6.109507130533716, + "heading": 0.28610948605635284, + "angularVelocity": -1.2934135603720627, + "velocityX": -2.4001729538779912, + "velocityY": -2.349038348572726, + "timestamp": 0.5204281965093971 + }, + { + "x": 1.6796785281931887, + "y": 5.959809271214951, + "heading": 0.2040617419906055, + "angularVelocity": -1.1035801140529855, + "velocityX": -2.0573465623458747, + "velocityY": -2.0135054600417615, + "timestamp": 0.5947750817250252 + }, + { + "x": 1.5522122020719726, + "y": 5.835058855915294, + "heading": 0.1359188756997469, + "angularVelocity": -0.9165530754008571, + "velocityX": -1.7144810539342084, + "velocityY": -1.6779507969680607, + "timestamp": 0.6691219669406534 + }, + { + "x": 1.4502385246804212, + "y": 5.735257410270185, + "heading": 0.08152405680169575, + "angularVelocity": -0.7316354779395241, + "velocityX": -1.3715931352846462, + "velocityY": -1.342375613391939, + "timestamp": 0.7434688521562816 + }, + { + "x": 1.373758307379264, + "y": 5.6604060852535225, + "heading": 0.04077083075075002, + "angularVelocity": -0.5481497433651609, + "velocityX": -1.0286943034579255, + "velocityY": -1.0067849486844118, + "timestamp": 0.8178157373719097 + }, + { + "x": 1.322771778723125, + "y": 5.6105054821115194, + "heading": 0.013600563583197408, + "angularVelocity": -0.3654526627275732, + "velocityX": -0.6857923974657818, + "velocityY": -0.6711861969371897, + "timestamp": 0.8921626225875379 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -1.098795178581163e-21, + "angularVelocity": -0.18293387199412173, + "velocityX": -0.34289287016502545, + "velocityY": -0.3355880828480881, + "timestamp": 0.966509507803166 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -9.496010380322716e-22, + "angularVelocity": -6.404372441775509e-21, + "velocityX": -5.456635982947961e-19, + "velocityY": -5.295509774626706e-19, + "timestamp": 1.0408563930187942 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NM-SweepFenderStraight.traj b/src/main/deploy/choreo/4NM-SweepFenderStraight.traj new file mode 100644 index 0000000..9beddd4 --- /dev/null +++ b/src/main/deploy/choreo/4NM-SweepFenderStraight.traj @@ -0,0 +1,697 @@ +{ + "samples": [ + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -3.4870573885213374e-23, + "angularVelocity": -5.958014181544604e-22, + "velocityX": 4.75095708718707e-20, + "velocityY": -5.100796288410356e-20, + "timestamp": 0 + }, + { + "x": 1.3221899506052506, + "y": 5.558731738657612, + "heading": -0.011172212869994504, + "angularVelocity": -0.14847792516259956, + "velocityX": 0.33106795052320837, + "velocityY": -0.35648661636038936, + "timestamp": 0.07524494201922415 + }, + { + "x": 1.3720125126335756, + "y": 5.5050840215864865, + "heading": -0.03353368538631347, + "angularVelocity": -0.29718240078656566, + "velocityX": 0.662138353639718, + "velocityY": -0.712974395772926, + "timestamp": 0.1504898840384483 + }, + { + "x": 1.4467463839809327, + "y": 5.424612630178231, + "heading": -0.06713200496181293, + "angularVelocity": -0.4465193097884971, + "velocityX": 0.9932079066293052, + "velocityY": -1.0694591456751465, + "timestamp": 0.22573482605767242 + }, + { + "x": 1.5463911448663232, + "y": 5.31731803920096, + "heading": -0.11204656505352982, + "angularVelocity": -0.5969113522639407, + "velocityX": 1.3242718807588763, + "velocityY": -1.4259375859424448, + "timestamp": 0.3009797680768966 + }, + { + "x": 1.6709458752175186, + "y": 5.183200865786002, + "heading": -0.1683893711840811, + "angularVelocity": -0.7487919402762832, + "velocityX": 1.6553236271930836, + "velocityY": -1.7824078245776684, + "timestamp": 0.37622471009612074 + }, + { + "x": 1.8204089760394484, + "y": 5.022261716216735, + "heading": -0.2363043631413436, + "angularVelocity": -0.9025854779702146, + "velocityX": 1.9863541230949968, + "velocityY": -2.1388700057493355, + "timestamp": 0.4514696521153449 + }, + { + "x": 1.99477795080174, + "y": 4.83450098026474, + "heading": -0.3159644115550863, + "angularVelocity": -1.0586764542045974, + "velocityX": 2.317351440283424, + "velocityY": -2.495327006884067, + "timestamp": 0.526714594134569 + }, + { + "x": 2.1441616151551486, + "y": 4.673559909893242, + "heading": -0.3887569416939099, + "angularVelocity": -0.9674076181788537, + "velocityX": 1.9852984180018707, + "velocityY": -2.138895533076221, + "timestamp": 0.6019595361537932 + }, + { + "x": 2.268644362208996, + "y": 4.53944104449319, + "heading": -0.449547316997939, + "angularVelocity": -0.8078998225355519, + "velocityX": 1.6543669742219151, + "velocityY": -1.782430310940861, + "timestamp": 0.6772044781730173 + }, + { + "x": 2.3682284247776337, + "y": 4.432144914414375, + "heading": -0.49824500481886397, + "angularVelocity": -0.6471888543482873, + "velocityX": 1.323465204388028, + "velocityY": -1.425958040493952, + "timestamp": 0.7524494201922415 + }, + { + "x": 2.4429154717792705, + "y": 4.351672139574372, + "heading": -0.5347927169900155, + "angularVelocity": -0.48571653044551594, + "velocityX": 0.9925856143600397, + "velocityY": -1.0694775313860099, + "timestamp": 0.8276943622114656 + }, + { + "x": 2.492706577536202, + "y": 4.298023281924518, + "heading": -0.5591597915473412, + "angularVelocity": -0.3238367111918331, + "velocityX": 0.6617203019999724, + "velocityY": -0.7129895539842044, + "timestamp": 0.9029393042306898 + }, + { + "x": 2.5176022052764893, + "y": 4.271198749542236, + "heading": -0.5713377241070188, + "angularVelocity": -0.16184386927384822, + "velocityX": 0.3308611459083442, + "velocityY": -0.3564961532620849, + "timestamp": 0.978184246249914 + }, + { + "x": 2.5176022052764893, + "y": 4.271198749542236, + "heading": -0.5713377241070188, + "angularVelocity": -1.965008466563789e-21, + "velocityX": 1.6720342558000003e-19, + "velocityY": -1.796953683889986e-19, + "timestamp": 1.053429188269138 + }, + { + "x": 2.4927076698068604, + "y": 4.298022743670389, + "heading": -0.5590626618027412, + "angularVelocity": 0.163131980525576, + "velocityX": -0.3308410804570447, + "velocityY": 0.3564830205551893, + "timestamp": 1.1286753923839292 + }, + { + "x": 2.4429176923283147, + "y": 4.351669893774747, + "heading": -0.5345293787694578, + "angularVelocity": 0.32604014145161125, + "velocityX": -0.6616942085555453, + "velocityY": 0.712954902316633, + "timestamp": 1.2039215964987204 + }, + { + "x": 2.368230774545632, + "y": 4.432139277372316, + "heading": -0.49778036282477456, + "angularVelocity": 0.48838365173373005, + "velocityX": -0.9925672485584068, + "velocityY": 1.0694145245494298, + "timestamp": 1.2791678006135117 + }, + { + "x": 2.2686448820648275, + "y": 4.539430029093104, + "heading": -0.4488890301403053, + "angularVelocity": 0.6497514826114488, + "velocityX": -1.3234673250611078, + "velocityY": 1.4258626462686734, + "timestamp": 1.3544140047283029 + }, + { + "x": 2.14415754877083, + "y": 4.673541531401143, + "heading": -0.38796534930924853, + "angularVelocity": 0.8096578631144655, + "velocityX": -1.6544001755103406, + "velocityY": 1.7823025611158592, + "timestamp": 1.429660208843094 + }, + { + "x": 1.9947660347325065, + "y": 4.83447364690962, + "heading": -0.31516105455016663, + "angularVelocity": 0.9675477403221032, + "velocityX": -1.9853694388413312, + "velocityY": 2.138740650132567, + "timestamp": 1.5049064129578853 + }, + { + "x": 1.8203962771969746, + "y": 5.02224100400193, + "heading": -0.2359849474173268, + "angularVelocity": 1.0522272593585371, + "velocityX": -2.3173229744522876, + "velocityY": 2.4953731460774122, + "timestamp": 1.5801526170726765 + }, + { + "x": 1.6709347760864326, + "y": 5.183186512420658, + "heading": -0.1683650413377472, + "angularVelocity": 0.8986487341796361, + "velocityX": -1.9862995465197482, + "velocityY": 2.1389186379847103, + "timestamp": 1.6553988211874677 + }, + { + "x": 1.546382775447489, + "y": 5.317309066145499, + "heading": -0.11216464144227759, + "angularVelocity": 0.7468868437500663, + "velocityX": -1.6552595855723733, + "velocityY": 1.7824494312062664, + "timestamp": 1.730645025302259 + }, + { + "x": 1.4467409698876759, + "y": 5.424607739615935, + "heading": -0.06728177554770955, + "angularVelocity": 0.5964801337499681, + "velocityX": -1.3242103934944731, + "velocityY": 1.4259679239998275, + "timestamp": 1.8058912294170502 + }, + { + "x": 1.3720096830963446, + "y": 5.505081872672054, + "heading": -0.0336471785574408, + "angularVelocity": 0.4469939365839344, + "velocityX": -0.9931568996799567, + "velocityY": 1.0694776434616193, + "timestamp": 1.8811374335318414 + }, + { + "x": 1.3221889841070316, + "y": 5.558731132858149, + "heading": -0.011222693307519035, + "angularVelocity": 0.2980148369439646, + "velocityX": -0.6621024884299752, + "velocityY": 0.7129829446845056, + "timestamp": 1.9563836376466326 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -5.804527907102298e-22, + "angularVelocity": 0.14914630498035963, + "velocityX": -0.33104955308205936, + "velocityY": 0.3564886879503997, + "timestamp": 2.031629841761424 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -1.7372853944454406e-22, + "angularVelocity": 2.7872047475617054e-21, + "velocityX": -2.1429242136387852e-19, + "velocityY": 2.3070553047501513e-19, + "timestamp": 2.106876045876215 + }, + { + "x": 1.3545482975852352, + "y": 5.589358578088536, + "heading": 9.197337540759096e-19, + "angularVelocity": 9.780581566888343e-18, + "velocityX": 0.6088974989534347, + "velocityY": 0.04043427574613332, + "timestamp": 2.2009305227298084 + }, + { + "x": 1.4690873670907165, + "y": 5.596964627264383, + "heading": 3.887468901363766e-18, + "angularVelocity": 3.155336403486356e-17, + "velocityX": 1.2177949773063368, + "velocityY": 0.08086855012427349, + "timestamp": 2.2949849995834017 + }, + { + "x": 1.6408959668925225, + "y": 5.608373700732223, + "heading": 3.6659024972997286e-18, + "angularVelocity": -2.3557241663966836e-18, + "velocityX": 1.8266924185782827, + "velocityY": 0.1213028220400261, + "timestamp": 2.389039476436995 + }, + { + "x": 1.8699740888528504, + "y": 5.623585797951658, + "heading": -8.067542241443972e-19, + "angularVelocity": -4.7553895051950526e-17, + "velocityX": 2.435589773328005, + "velocityY": 0.16173708821020832, + "timestamp": 2.4830939532905885 + }, + { + "x": 2.156321692282708, + "y": 5.642600916220705, + "heading": -2.6708058042599195e-18, + "angularVelocity": -1.981885012273154e-17, + "velocityX": 3.044486695466825, + "velocityY": 0.20217132565255283, + "timestamp": 2.577148430144182 + }, + { + "x": 2.385399814243036, + "y": 5.6578130134401405, + "heading": -2.770306305819978e-18, + "angularVelocity": -1.0579028761460627e-18, + "velocityX": 2.435589773328005, + "velocityY": 0.1617370882102083, + "timestamp": 2.671202906997775 + }, + { + "x": 2.557208414044842, + "y": 5.66922208690798, + "heading": -1.5286567962624567e-18, + "angularVelocity": 1.3201386590800127e-17, + "velocityX": 1.8266924185782827, + "velocityY": 0.12130282204002607, + "timestamp": 2.7652573838513685 + }, + { + "x": 2.6717474835503237, + "y": 5.676828136083827, + "heading": -1.1055203544056362e-18, + "angularVelocity": 4.4988442442533515e-18, + "velocityX": 1.217794977306337, + "velocityY": 0.08086855012427348, + "timestamp": 2.859311860704962 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.3994282719055762e-28, + "angularVelocity": 1.1754042882709242e-17, + "velocityX": 0.6088974989534347, + "velocityY": 0.04043427574613331, + "timestamp": 2.9533663375585553 + }, + { + "x": 2.7290170192718506, + "y": 5.680631160736084, + "heading": -1.2578191824600292e-28, + "angularVelocity": -3.8382941954344137e-29, + "velocityX": -7.718549111027777e-22, + "velocityY": -5.126840450701387e-23, + "timestamp": 3.0474208144121486 + }, + { + "x": 2.6717474835503237, + "y": 5.676828136083827, + "heading": 2.957978952433013e-18, + "angularVelocity": 3.144963484589081e-17, + "velocityX": -0.6088974989534347, + "velocityY": -0.04043427574613368, + "timestamp": 3.141475291265742 + }, + { + "x": 2.557208414044842, + "y": 5.66922208690798, + "heading": 7.803415503811453e-18, + "angularVelocity": 5.15173409451729e-17, + "velocityX": -1.217794977306337, + "velocityY": -0.08086855012427421, + "timestamp": 3.2355297681193353 + }, + { + "x": 2.385399814243036, + "y": 5.6578130134401405, + "heading": 1.0918258808355902e-17, + "angularVelocity": 3.3117437987572525e-17, + "velocityX": -1.8266924185782827, + "velocityY": -0.12130282204002718, + "timestamp": 3.3295842449729287 + }, + { + "x": 2.156321692282708, + "y": 5.642600916220706, + "heading": 1.4843907441415233e-17, + "angularVelocity": 4.173803060196763e-17, + "velocityX": -2.435589773328005, + "velocityY": -0.16173708821020977, + "timestamp": 3.423638721826522 + }, + { + "x": 1.8699740888528504, + "y": 5.623585797951657, + "heading": 1.4047394913563068e-17, + "angularVelocity": -8.468629611156265e-18, + "velocityX": -3.044486695466825, + "velocityY": -0.2021713256525547, + "timestamp": 3.5176931986801154 + }, + { + "x": 1.6408959668925225, + "y": 5.608373700732223, + "heading": 5.894670829565627e-18, + "angularVelocity": -8.668087215646387e-17, + "velocityX": -2.435589773328005, + "velocityY": -0.1617370882102098, + "timestamp": 3.611747675533709 + }, + { + "x": 1.4690873670907165, + "y": 5.596964627264383, + "heading": -3.5801552348274217e-19, + "angularVelocity": -6.647941238005475e-17, + "velocityX": -1.8266924185782827, + "velocityY": -0.12130282204002721, + "timestamp": 3.705802152387302 + }, + { + "x": 1.3545482975852352, + "y": 5.589358578088536, + "heading": -2.459449657169509e-18, + "angularVelocity": -2.234273374195548e-17, + "velocityX": -1.2177949773063372, + "velocityY": -0.08086855012427423, + "timestamp": 3.7998566292408955 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -2.170460361005601e-22, + "angularVelocity": 2.6146895857700576e-17, + "velocityX": -0.6088974989534347, + "velocityY": -0.04043427574613369, + "timestamp": 3.893911106094489 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -3.6388277145419484e-23, + "angularVelocity": 1.9207790315741447e-21, + "velocityX": 1.47076337525445e-19, + "velocityY": 1.4190124023616126e-19, + "timestamp": 3.9879655829480822 + }, + { + "x": 1.3227737240195483, + "y": 5.61050348195736, + "heading": 0.013550786900983634, + "angularVelocity": 0.18226929801485287, + "velocityX": 0.34292833980902226, + "velocityY": 0.33557028471401684, + "timestamp": 4.062310450945979 + }, + { + "x": 1.3737639106778228, + "y": 5.660399482511426, + "heading": 0.040679974016250266, + "angularVelocity": 0.36491001794547456, + "velocityX": 0.6858602083968518, + "velocityY": 0.6711424997819331, + "timestamp": 4.136655318943877 + }, + { + "x": 1.4502490112097923, + "y": 5.735243170344193, + "heading": 0.08146222479513072, + "angularVelocity": 0.548555023058672, + "velocityX": 1.0287878987710746, + "velocityY": 1.0067095395862875, + "timestamp": 4.211000186941774 + }, + { + "x": 1.5522278856647675, + "y": 5.835033815033583, + "heading": 0.1360214110793065, + "angularVelocity": 0.7338662069548484, + "velocityX": 1.3717002558651332, + "velocityY": 1.3422667546092502, + "timestamp": 4.285345054939672 + }, + { + "x": 1.6796982296330216, + "y": 5.959770632655783, + "heading": 0.20453087198468312, + "angularVelocity": 0.9215089454098471, + "velocityX": 1.7145816167413093, + "velocityY": 1.6778134252081405, + "timestamp": 4.359689922937569 + }, + { + "x": 1.8326561921573887, + "y": 6.109453229425897, + "heading": 0.28720867051263665, + "angularVelocity": 1.1120848116953013, + "velocityX": 2.0574111790566834, + "velocityY": 2.01335479907434, + "timestamp": 4.434034790935467 + }, + { + "x": 2.011096000176797, + "y": 6.28408222003575, + "heading": 0.38430411909211587, + "angularVelocity": 1.3060141364730837, + "velocityX": 2.400163088923032, + "velocityY": 2.3489044410542577, + "timestamp": 4.5083796589333645 + }, + { + "x": 2.1638766096575854, + "y": 6.433815495784424, + "heading": 0.4735075918913831, + "angularVelocity": 1.1998605310831978, + "velocityX": 2.05502563384886, + "velocityY": 2.0140364732762612, + "timestamp": 4.582724526931262 + }, + { + "x": 2.291186482527782, + "y": 6.5585980911554875, + "heading": 0.5479625204474381, + "angularVelocity": 1.0014804055898086, + "velocityX": 1.712423147671697, + "velocityY": 1.678429173814564, + "timestamp": 4.6570693949291595 + }, + { + "x": 2.393030162177412, + "y": 6.658427500466555, + "heading": 0.6075765143590145, + "angularVelocity": 0.8018575527400554, + "velocityX": 1.3698817738503593, + "velocityY": 1.3427881708511593, + "timestamp": 4.731414262927057 + }, + { + "x": 2.469410931527034, + "y": 6.733301422056823, + "heading": 0.6523015105520286, + "angularVelocity": 0.6015882117684191, + "velocityX": 1.027384558027357, + "velocityY": 1.007116208645172, + "timestamp": 4.8057591309249545 + }, + { + "x": 2.520330867960948, + "y": 6.783218097420482, + "heading": 0.6821175635862542, + "angularVelocity": 0.4010505881195312, + "velocityX": 0.6849152847423776, + "velocityY": 0.6714205930807574, + "timestamp": 4.880103998922852 + }, + { + "x": 2.545790910720825, + "y": 6.808176517486572, + "heading": 0.6970216139142698, + "angularVelocity": 0.20047181102584016, + "velocityX": 0.34245864503515044, + "velocityY": 0.3357114046768724, + "timestamp": 4.95444886692075 + }, + { + "x": 2.545790910720825, + "y": 6.808176517486572, + "heading": 0.6970216139142698, + "angularVelocity": 1.060990978792425e-20, + "velocityX": 9.444494751401817e-19, + "velocityY": 9.172807861306771e-19, + "timestamp": 5.028793734918647 + }, + { + "x": 2.52033376431852, + "y": 6.783218896026595, + "heading": 0.6819576043595674, + "angularVelocity": -0.20261789732027236, + "velocityX": -0.34241039592273365, + "velocityY": -0.3356915543615947, + "timestamp": 5.103140620134275 + }, + { + "x": 2.469417510751594, + "y": 6.733305671093222, + "heading": 0.6518402320176618, + "angularVelocity": -0.405092590692349, + "velocityX": -0.6848471650056825, + "velocityY": -0.6713559658701147, + "timestamp": 5.177487505349903 + }, + { + "x": 2.393038999153381, + "y": 6.65843948497798, + "heading": 0.6067059073438061, + "angularVelocity": -0.6070775465973136, + "velocityX": -1.0273263147029235, + "velocityY": -1.0069848373352566, + "timestamp": 5.251834390565532 + }, + { + "x": 2.2911938790627677, + "y": 6.558623270950903, + "heading": 0.5466314564942851, + "angularVelocity": -0.8080291551594541, + "velocityX": -1.3698639801147294, + "velocityY": -1.342574254961449, + "timestamp": 5.32618127578116 + }, + { + "x": 2.163876704340966, + "y": 6.433859756287583, + "heading": 0.471750868383845, + "angularVelocity": -1.0071785508332185, + "velocityX": -1.7124748986126783, + "velocityY": -1.6781269894692763, + "timestamp": 5.400528160996788 + }, + { + "x": 2.0110812200123074, + "y": 6.284150815002161, + "heading": 0.3822707555656717, + "angularVelocity": -1.2035489120849394, + "velocityX": -2.0551699494270133, + "velocityY": -2.0136545176199485, + "timestamp": 5.474875046212416 + }, + { + "x": 1.832635836912685, + "y": 6.109507130533716, + "heading": 0.28610948605635284, + "angularVelocity": -1.2934135603720627, + "velocityX": -2.4001729538779912, + "velocityY": -2.349038348572726, + "timestamp": 5.549221931428044 + }, + { + "x": 1.6796785281931887, + "y": 5.959809271214951, + "heading": 0.2040617419906055, + "angularVelocity": -1.1035801140529855, + "velocityX": -2.0573465623458747, + "velocityY": -2.0135054600417615, + "timestamp": 5.623568816643672 + }, + { + "x": 1.5522122020719726, + "y": 5.835058855915294, + "heading": 0.1359188756997469, + "angularVelocity": -0.9165530754008571, + "velocityX": -1.7144810539342084, + "velocityY": -1.6779507969680607, + "timestamp": 5.6979157018593005 + }, + { + "x": 1.4502385246804212, + "y": 5.735257410270185, + "heading": 0.08152405680169575, + "angularVelocity": -0.7316354779395241, + "velocityX": -1.3715931352846462, + "velocityY": -1.342375613391939, + "timestamp": 5.772262587074929 + }, + { + "x": 1.373758307379264, + "y": 5.6604060852535225, + "heading": 0.04077083075075002, + "angularVelocity": -0.5481497433651609, + "velocityX": -1.0286943034579255, + "velocityY": -1.0067849486844118, + "timestamp": 5.846609472290557 + }, + { + "x": 1.322771778723125, + "y": 5.6105054821115194, + "heading": 0.013600563583197408, + "angularVelocity": -0.3654526627275732, + "velocityX": -0.6857923974657818, + "velocityY": -0.6711861969371897, + "timestamp": 5.920956357506185 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -1.098795178581163e-21, + "angularVelocity": -0.18293387199412173, + "velocityX": -0.34289287016502545, + "velocityY": -0.3355880828480881, + "timestamp": 5.995303242721813 + }, + { + "x": 1.2972787618637085, + "y": 5.585555553436279, + "heading": -9.496010380322716e-22, + "angularVelocity": -6.404372441775509e-21, + "velocityX": -5.456635982947961e-19, + "velocityY": -5.295509774626706e-19, + "timestamp": 6.069650127937441 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Close.1.traj b/src/main/deploy/choreo/4NT-Close.1.traj new file mode 100644 index 0000000..d519efb --- /dev/null +++ b/src/main/deploy/choreo/4NT-Close.1.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 0.7644821405410767, + "y": 6.707253456115723, + "heading": 1.0390723224103011, + "angularVelocity": -4.4871567857707046e-18, + "velocityX": 3.460724762099423e-16, + "velocityY": 6.79633296625553e-17, + "timestamp": 0 + }, + { + "x": 0.7893073618901386, + "y": 6.711898127758422, + "heading": 1.0229599251421253, + "angularVelocity": -0.2562974082798586, + "velocityX": 0.3948909517207538, + "velocityY": 0.0738820725755683, + "timestamp": 0.06286601716469933 + }, + { + "x": 0.8389638482798748, + "y": 6.721192561704798, + "heading": 0.9908267229593493, + "angularVelocity": -0.5111378711748218, + "velocityX": 0.7898780395080967, + "velocityY": 0.14784512150697973, + "timestamp": 0.12573203432939867 + }, + { + "x": 0.9134574420677571, + "y": 6.735142975200245, + "heading": 0.9427373156582869, + "angularVelocity": -0.7649507551133621, + "velocityX": 1.1849580607710317, + "velocityY": 0.22190706719814038, + "timestamp": 0.188598051494098 + }, + { + "x": 1.0127948463812606, + "y": 6.753755537156801, + "heading": 0.8787447001001124, + "angularVelocity": -1.0179206261879956, + "velocityX": 1.5801447076447082, + "velocityY": 0.296067140817775, + "timestamp": 0.25146406865879734 + }, + { + "x": 1.136986588346673, + "y": 6.777034858063683, + "heading": 0.798950116173758, + "angularVelocity": -1.2692800900268044, + "velocityX": 1.9754988078860747, + "velocityY": 0.3703005527754691, + "timestamp": 0.31433008582349664 + }, + { + "x": 1.2860552286500786, + "y": 6.804983168108553, + "heading": 0.70370039577791, + "angularVelocity": -1.5151225525614787, + "velocityX": 2.3712117774673787, + "velocityY": 0.44456943998292453, + "timestamp": 0.37719610298819595 + }, + { + "x": 1.4600612279623586, + "y": 6.8376056601235335, + "heading": 0.5943219599600833, + "angularVelocity": -1.739865840894617, + "velocityX": 2.7678864855778302, + "velocityY": 0.5189209287668323, + "timestamp": 0.44006212015289525 + }, + { + "x": 1.6592143751431372, + "y": 6.8749781984647464, + "heading": 0.47881641796683205, + "angularVelocity": -1.837328770020157, + "velocityX": 3.167898272591771, + "velocityY": 0.5944791801156865, + "timestamp": 0.5029281373175946 + }, + { + "x": 1.833246757687688, + "y": 6.907207190100634, + "heading": 0.3632682477784989, + "angularVelocity": -1.8380068501150066, + "velocityX": 2.7683061595681933, + "velocityY": 0.5126615791717365, + "timestamp": 0.5657941544822939 + }, + { + "x": 1.98232093363545, + "y": 6.934701065225548, + "heading": 0.2612435638075881, + "angularVelocity": -1.6228908490179, + "velocityX": 2.371299832105078, + "velocityY": 0.43734081408223097, + "timestamp": 0.6286601716469932 + }, + { + "x": 2.1065071082491382, + "y": 6.957543379697764, + "heading": 0.1749925302220398, + "angularVelocity": -1.371981834947508, + "velocityX": 1.975410248884438, + "velocityY": 0.3633491590913955, + "timestamp": 0.6915261888116925 + }, + { + "x": 2.2058348256223446, + "y": 6.9757778777630985, + "heading": 0.10539352393852991, + "angularVelocity": -1.1071006152833613, + "velocityX": 1.5799906189855346, + "velocityY": 0.29005333704479014, + "timestamp": 0.7543922059763918 + }, + { + "x": 2.28031886152934, + "y": 6.9894324604556495, + "heading": 0.052874348065941004, + "angularVelocity": -0.8354143978130386, + "velocityX": 1.1848060250398966, + "velocityY": 0.2172013324269997, + "timestamp": 0.8172582231410911 + }, + { + "x": 2.329968057638108, + "y": 6.998525566093112, + "heading": 0.017682384897765975, + "angularVelocity": -0.5597931085081341, + "velocityX": 0.7897620741376922, + "velocityY": 0.14464262327357277, + "timestamp": 0.8801242403057904 + }, + { + "x": 2.3547892570495605, + "y": 7.003068923950195, + "heading": -1.6526519468843367e-17, + "angularVelocity": -0.28127095838489014, + "velocityX": 0.39482697538202266, + "velocityY": 0.07227048987661186, + "timestamp": 0.9429902574704897 + }, + { + "x": 2.35478925704956, + "y": 7.003068923950195, + "heading": -7.945307467511743e-18, + "angularVelocity": 9.541751832566194e-18, + "velocityX": -2.469200856331852e-16, + "velocityY": -5.7488464838897e-17, + "timestamp": 1.0058562746351891 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Close.2.traj b/src/main/deploy/choreo/4NT-Close.2.traj new file mode 100644 index 0000000..853e8fb --- /dev/null +++ b/src/main/deploy/choreo/4NT-Close.2.traj @@ -0,0 +1,193 @@ +{ + "samples": [ + { + "x": 2.35478925704956, + "y": 7.003068923950195, + "heading": -7.945307467511743e-18, + "angularVelocity": 9.541751832566194e-18, + "velocityX": -2.469200856331852e-16, + "velocityY": -5.7488464838897e-17, + "timestamp": 0 + }, + { + "x": 2.415475621976421, + "y": 7.007915755074314, + "heading": -3.517124144434822e-19, + "angularVelocity": 7.839197585347938e-17, + "velocityX": 0.6264916515173242, + "velocityY": 0.05003593870277661, + "timestamp": 0.0968669969980962 + }, + { + "x": 2.536848350474334, + "y": 7.017609417214267, + "heading": 2.0883695559124673e-17, + "angularVelocity": 2.1922232683494886e-16, + "velocityX": 1.2529832890380508, + "velocityY": 0.1000718762876886, + "timestamp": 0.19373399399619218 + }, + { + "x": 2.7189074405095877, + "y": 7.03214991020763, + "heading": 4.970708271093658e-17, + "angularVelocity": 2.975563285402562e-16, + "velocityX": 1.8794749055638844, + "velocityY": 0.1501078121958038, + "timestamp": 0.29060099099428816 + }, + { + "x": 2.9616528886926634, + "y": 7.051537233783689, + "heading": 1.0287632521899536e-16, + "angularVelocity": 5.488891512697579e-16, + "velocityX": 2.505966487098253, + "velocityY": 0.20014374530925957, + "timestamp": 0.38746798799238413 + }, + { + "x": 3.2650846882445492, + "y": 7.075771387401028, + "heading": 1.9068454520432853e-16, + "angularVelocity": 9.064823379151022e-16, + "velocityX": 3.1324579986499463, + "velocityY": 0.25017967283341686, + "timestamp": 0.4843349849904801 + }, + { + "x": 3.629202818829201, + "y": 7.104852369435471, + "heading": 3.733614670935224e-16, + "angularVelocity": 1.8858530856090668e-15, + "velocityX": 3.7589493002638363, + "velocityY": 0.300215583590495, + "timestamp": 0.5812019819885761 + }, + { + "x": 4.024026376830976, + "y": 7.136385698409197, + "heading": 4.457439879437158e-16, + "angularVelocity": 7.472361390272464e-16, + "velocityX": 4.07593473770572, + "velocityY": 0.3255322240901749, + "timestamp": 0.6780689789866721 + }, + { + "x": 4.418849934832716, + "y": 7.167919027382921, + "heading": 4.558091019327557e-16, + "angularVelocity": 1.0390653460947907e-16, + "velocityX": 4.075934737705357, + "velocityY": 0.32553222409014865, + "timestamp": 0.774935975984768 + }, + { + "x": 4.8136734928344564, + "y": 7.199452356356644, + "heading": 4.22612711995065e-16, + "angularVelocity": -3.427007234567253e-16, + "velocityX": 4.075934737705359, + "velocityY": 0.32553222409012916, + "timestamp": 0.871802972982864 + }, + { + "x": 5.208497050836196, + "y": 7.230985685330376, + "heading": 4.132801040379339e-16, + "angularVelocity": -9.634455769312537e-17, + "velocityX": 4.07593473770535, + "velocityY": 0.3255322240902326, + "timestamp": 0.96866996998096 + }, + { + "x": 5.6033206088379375, + "y": 7.262519014304082, + "heading": 4.974719423376743e-16, + "angularVelocity": 8.691488422559714e-16, + "velocityX": 4.0759347377053725, + "velocityY": 0.32553222408995947, + "timestamp": 1.065536966979056 + }, + { + "x": 5.998144166839676, + "y": 7.294052343277824, + "heading": 4.787346935143853e-16, + "angularVelocity": -1.9343274205936813e-16, + "velocityX": 4.075934737705342, + "velocityY": 0.3255322240903373, + "timestamp": 1.162403963977152 + }, + { + "x": 6.392967724841452, + "y": 7.325585672251541, + "heading": 4.705780860197851e-16, + "angularVelocity": -8.420419481072571e-17, + "velocityX": 4.075934737705728, + "velocityY": 0.32553222409008153, + "timestamp": 1.259270960975248 + }, + { + "x": 6.757085855426104, + "y": 7.3546666542859835, + "heading": 2.57250007761421e-16, + "angularVelocity": -2.20227825947358e-15, + "velocityX": 3.7589493002638363, + "velocityY": 0.3002155835905016, + "timestamp": 1.356137957973344 + }, + { + "x": 7.06051765497799, + "y": 7.378900807903323, + "heading": 1.3962913393362377e-16, + "angularVelocity": -1.2142512866829015e-15, + "velocityX": 3.132457998649947, + "velocityY": 0.25017967283342074, + "timestamp": 1.4530049549714399 + }, + { + "x": 7.303263103161066, + "y": 7.398288131479383, + "heading": 7.249700096797418e-17, + "angularVelocity": -6.930341220083241e-16, + "velocityX": 2.5059664870982536, + "velocityY": 0.20014374530926216, + "timestamp": 1.5498719519695359 + }, + { + "x": 7.485322193196319, + "y": 7.412828624472744, + "heading": 2.3826173239994654e-17, + "angularVelocity": -5.024500609510776e-16, + "velocityX": 1.8794749055638846, + "velocityY": 0.1501078121958055, + "timestamp": 1.6467389489676318 + }, + { + "x": 7.606694921694233, + "y": 7.422522286612698, + "heading": 6.1848492309357054e-18, + "angularVelocity": -1.8211904002605062e-16, + "velocityX": 1.2529832890380512, + "velocityY": 0.10007187628768963, + "timestamp": 1.7436059459657278 + }, + { + "x": 7.667381286621095, + "y": 7.427369117736816, + "heading": -2.892642650950664e-26, + "angularVelocity": -6.384888230976486e-17, + "velocityX": 0.6264916515173246, + "velocityY": 0.05003593870277713, + "timestamp": 1.8404729429638238 + }, + { + "x": 7.667381286621094, + "y": 7.427369117736816, + "heading": -2.807242222383161e-26, + "angularVelocity": -9.314848061399685e-27, + "velocityX": 2.930418041341978e-17, + "velocityY": 1.8573166632838487e-18, + "timestamp": 1.9373399399619198 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Close.3.traj b/src/main/deploy/choreo/4NT-Close.3.traj new file mode 100644 index 0000000..27f9fad --- /dev/null +++ b/src/main/deploy/choreo/4NT-Close.3.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 7.667381286621094, + "y": 7.427369117736816, + "heading": -2.807242222383161e-26, + "angularVelocity": -9.314848061399685e-27, + "velocityX": 2.930418041341978e-17, + "velocityY": 1.8573166632838487e-18, + "timestamp": 0 + }, + { + "x": 7.6043184218722315, + "y": 7.4216272524122235, + "heading": -5.4231338022379995e-19, + "angularVelocity": -5.489423433560598e-18, + "velocityX": -0.6383377994270321, + "velocityY": -0.05812057048950791, + "timestamp": 0.09879230840703412 + }, + { + "x": 7.478192693747755, + "y": 7.410143521888074, + "heading": 9.1638326322218e-18, + "angularVelocity": 9.824800347715444e-17, + "velocityX": -1.276675584953701, + "velocityY": -0.11624113971338963, + "timestamp": 0.19758461681406825 + }, + { + "x": 7.289004104307538, + "y": 7.392917926351917, + "heading": 3.509462172627408e-17, + "angularVelocity": 2.624778291044605e-16, + "velocityX": -1.915013349629827, + "velocityY": -0.17436170703883222, + "timestamp": 0.29637692522110237 + }, + { + "x": 7.0367526569847, + "y": 7.369950466116338, + "heading": 8.808387501704071e-17, + "angularVelocity": 5.363702518284207e-16, + "velocityX": -2.5533510795550636, + "velocityY": -0.23248227120021098, + "timestamp": 0.3951692336281365 + }, + { + "x": 6.721438358645465, + "y": 7.341241141806506, + "heading": 1.8168852174846708e-16, + "angularVelocity": 9.474892406751412e-16, + "velocityX": -3.191688739978706, + "velocityY": -0.2906028290334789, + "timestamp": 0.4939615420351706 + }, + { + "x": 6.343061229887728, + "y": 7.306789955297856, + "heading": 3.542313739589584e-16, + "angularVelocity": 1.7465211410774172e-15, + "velocityX": -3.830026191905404, + "velocityY": -0.3487233678831281, + "timestamp": 0.5927538504422047 + }, + { + "x": 5.940772069277239, + "y": 7.270161581516117, + "heading": 3.929421150957015e-16, + "angularVelocity": 3.9183962579211066e-16, + "velocityX": -4.072069648914557, + "velocityY": -0.37076139197828895, + "timestamp": 0.6915461588492389 + }, + { + "x": 5.5384829086667535, + "y": 7.233533207734375, + "heading": 3.5944692196834336e-16, + "angularVelocity": -3.3904656795171214e-16, + "velocityX": -4.072069648914502, + "velocityY": -0.37076139197830654, + "timestamp": 0.790338467256273 + }, + { + "x": 5.136193748056268, + "y": 7.1969048339526385, + "heading": 2.816331867279604e-16, + "angularVelocity": -7.876497311722583e-16, + "velocityX": -4.072069648914507, + "velocityY": -0.3707613919782439, + "timestamp": 0.8891307756633071 + }, + { + "x": 4.7339045874457835, + "y": 7.1602764601708975, + "heading": 3.7044270006052305e-16, + "angularVelocity": 8.989516982312421e-16, + "velocityX": -4.072069648914502, + "velocityY": -0.3707613919782984, + "timestamp": 0.9879230840703412 + }, + { + "x": 4.3316154268352935, + "y": 7.123648086389155, + "heading": 3.627714094587869e-16, + "angularVelocity": -7.765068710911665e-17, + "velocityX": -4.072069648914555, + "velocityY": -0.37076139197830443, + "timestamp": 1.0867153924773754 + }, + { + "x": 3.9532382980775562, + "y": 7.0891968998805055, + "heading": 2.0138393758473373e-16, + "angularVelocity": -1.6336036426615943e-15, + "velocityX": -3.8300261919054046, + "velocityY": -0.34872336788312547, + "timestamp": 1.18550770088441 + }, + { + "x": 3.6379239997383217, + "y": 7.060487575570674, + "heading": 1.144803164813082e-16, + "angularVelocity": -8.79659814093205e-16, + "velocityX": -3.191688739978706, + "velocityY": -0.29060282903347734, + "timestamp": 1.2843000092914445 + }, + { + "x": 3.385672552415484, + "y": 7.037520115335095, + "heading": 6.046472103882109e-17, + "angularVelocity": -5.467591390886901e-16, + "velocityX": -2.553351079555064, + "velocityY": -0.23248227120021, + "timestamp": 1.383092317698479 + }, + { + "x": 3.196483962975267, + "y": 7.020294519798938, + "heading": 3.2682810287652467e-17, + "angularVelocity": -2.8121533703852023e-16, + "velocityX": -1.9150133496298272, + "velocityY": -0.17436170703883164, + "timestamp": 1.4818846261055136 + }, + { + "x": 3.07035823485079, + "y": 7.008810789274788, + "heading": 1.6547008414870972e-17, + "angularVelocity": -1.6333055858508895e-16, + "velocityX": -1.276675584953701, + "velocityY": -0.11624113971338933, + "timestamp": 1.5806769345125482 + }, + { + "x": 3.0072953701019274, + "y": 7.003068923950195, + "heading": -5.572543390873087e-26, + "angularVelocity": -1.6749288756381388e-16, + "velocityX": -0.638337799427032, + "velocityY": -0.058120570489507827, + "timestamp": 1.6794692429195828 + }, + { + "x": 3.007295370101928, + "y": 7.003068923950195, + "heading": -2.701724786958032e-26, + "angularVelocity": 1.8342141297599884e-26, + "velocityX": 1.650330335862223e-16, + "velocityY": -1.072378048093932e-16, + "timestamp": 1.7782615513266173 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Close.4.traj b/src/main/deploy/choreo/4NT-Close.4.traj new file mode 100644 index 0000000..0156c5a --- /dev/null +++ b/src/main/deploy/choreo/4NT-Close.4.traj @@ -0,0 +1,238 @@ +{ + "samples": [ + { + "x": 3.007295370101928, + "y": 7.003068923950195, + "heading": -2.701724786958032e-26, + "angularVelocity": 1.8342141297599884e-26, + "velocityX": 1.650330335862223e-16, + "velocityY": -1.072378048093932e-16, + "timestamp": 0 + }, + { + "x": 3.036322603690972, + "y": 6.998184073615173, + "heading": -6.659182533814151e-19, + "angularVelocity": -9.886580304682387e-18, + "velocityX": 0.430953927728845, + "velocityY": -0.07252311632756721, + "timestamp": 0.06735576988941894 + }, + { + "x": 3.094377070111199, + "y": 6.988414372048806, + "heading": -6.272481275693907e-18, + "angularVelocity": -8.323805132134451e-17, + "velocityX": 0.8619078442060418, + "velocityY": -0.145046245962409, + "timestamp": 0.13471153977883787 + }, + { + "x": 3.1814587683881745, + "y": 6.973759818098456, + "heading": -1.1245385282688892e-17, + "angularVelocity": -7.383040860905551e-17, + "velocityX": 1.292861746216284, + "velocityY": -0.2175693927099312, + "timestamp": 0.2020673096682568 + }, + { + "x": 3.2975676972225916, + "y": 6.954220410226885, + "heading": -1.425981430781261e-17, + "angularVelocity": -4.475383529525523e-17, + "velocityX": 1.7238156289363036, + "velocityY": -0.2900925622801421, + "timestamp": 0.26942307955767575 + }, + { + "x": 3.442703854795298, + "y": 6.9297961462812125, + "heading": -2.2571965647820338e-17, + "angularVelocity": -1.2340667118105605e-16, + "velocityX": 2.154769484648219, + "velocityY": -0.3626157638131432, + "timestamp": 0.3367788494470947 + }, + { + "x": 3.61686723837731, + "y": 6.900487023030509, + "heading": -2.846165657687679e-17, + "angularVelocity": -8.74415093652977e-17, + "velocityX": 2.5857232998441617, + "velocityY": -0.43513901331428134, + "timestamp": 0.4041346193365136 + }, + { + "x": 3.820057843419671, + "y": 6.866293035085623, + "heading": -3.614720128617906e-17, + "angularVelocity": -1.141037318539728e-16, + "velocityX": 3.0166770475038507, + "velocityY": -0.5076623428256568, + "timestamp": 0.47149038922593256 + }, + { + "x": 4.052275660822413, + "y": 6.827214171652438, + "heading": -4.8269174033446284e-17, + "angularVelocity": -1.7996931628267424e-16, + "velocityX": 3.4476306600605273, + "velocityY": -0.5801858325923079, + "timestamp": 0.5388461591153515 + }, + { + "x": 4.313520663286318, + "y": 6.783250400223824, + "heading": -6.49281895371743e-17, + "angularVelocity": -2.4732870697850985e-16, + "velocityX": 3.878583867318316, + "velocityY": -0.6527098049772873, + "timestamp": 0.6062019290047704 + }, + { + "x": 4.585110952119839, + "y": 6.737529090718732, + "heading": -9.015940488845162e-17, + "angularVelocity": -3.745962038515257e-16, + "velocityX": 4.032175554958435, + "velocityY": -0.6788031608898043, + "timestamp": 0.6735576988941894 + }, + { + "x": 4.85667163042734, + "y": 6.691632237173498, + "heading": -4.751315953731664e-17, + "angularVelocity": 6.331491122848668e-16, + "velocityX": 4.031735941157495, + "velocityY": -0.6814093821593625, + "timestamp": 0.7409134687836083 + }, + { + "x": 5.127899646759034, + "y": 6.643808746337891, + "heading": 8.852905771561079e-23, + "angularVelocity": 7.054072252152007e-16, + "velocityX": 4.026797062478613, + "velocityY": -0.7100132759837658, + "timestamp": 0.8082692386730272 + }, + { + "x": 5.435225736063499, + "y": 6.550301165877784, + "heading": -3.054048980043161e-19, + "angularVelocity": -3.89086507141557e-18, + "velocityX": 3.9118505934900796, + "velocityY": -1.190226592691475, + "timestamp": 0.8868320779123247 + }, + { + "x": 5.741994583635364, + "y": 6.454981350959253, + "heading": -3.5555327560181985e-17, + "angularVelocity": -4.486866408095523e-16, + "velocityX": 3.9047576505906223, + "velocityY": -1.213293916587032, + "timestamp": 0.9653949171516221 + }, + { + "x": 6.048713801922417, + "y": 6.3595019612227075, + "heading": 7.714193179741933e-18, + "angularVelocity": 5.507616890435799e-16, + "velocityX": 3.9041259360904785, + "velocityY": -1.2153250908580402, + "timestamp": 1.0439577563909195 + }, + { + "x": 6.35543181351245, + "y": 6.264018695159109, + "heading": 2.1299456161841412e-17, + "angularVelocity": 1.7292071017284973e-16, + "velocityX": 3.904110576449497, + "velocityY": -1.2153744313228805, + "timestamp": 1.1225205956302169 + }, + { + "x": 6.661317138890106, + "y": 6.168794814888757, + "heading": -2.6772613029830932e-14, + "angularVelocity": -3.41050623150326e-13, + "velocityX": 3.8935115932605218, + "velocityY": -1.212072796659924, + "timestamp": 1.2010834348695143 + }, + { + "x": 6.928966825594151, + "y": 6.085473915495383, + "heading": -3.073303152925181e-14, + "angularVelocity": -5.041082116318115e-14, + "velocityX": 3.406822987759037, + "velocityY": -1.0605637499881233, + "timestamp": 1.2796462741088117 + }, + { + "x": 7.158380853190387, + "y": 6.014056000162917, + "heading": -2.7188130909040473e-14, + "angularVelocity": 4.512183847842235e-14, + "velocityX": 2.920134122157474, + "velocityY": -0.9090546627891893, + "timestamp": 1.3582091133481091 + }, + { + "x": 7.34955921485854, + "y": 5.954541069923522, + "heading": -2.0785853473910866e-14, + "angularVelocity": 8.149241754648641e-14, + "velocityX": 2.4334451697429427, + "velocityY": -0.7575455624521772, + "timestamp": 1.4367719525874065 + }, + { + "x": 7.502501907186795, + "y": 5.906929125287755, + "heading": -1.3764492993566052e-14, + "angularVelocity": 8.937251498866303e-14, + "velocityX": 1.9467561739005792, + "velocityY": -0.6060364556164637, + "timestamp": 1.515334791826704 + }, + { + "x": 7.617208928127525, + "y": 5.871220166560173, + "heading": -7.419672395660612e-15, + "angularVelocity": 8.076106815549843e-14, + "velocityX": 1.4600671519946562, + "velocityY": -0.4545273449041617, + "timestamp": 1.5938976310660014 + }, + { + "x": 7.693680276315422, + "y": 5.847414193943067, + "heading": -2.6268163161796944e-15, + "angularVelocity": 6.100663836778702e-14, + "velocityX": 0.973378112710156, + "velocityY": -0.3030182316169395, + "timestamp": 1.6724604703052988 + }, + { + "x": 7.731915950775149, + "y": 5.8355112075805655, + "heading": 4.745265421419366e-24, + "angularVelocity": 3.343585339172883e-14, + "velocityX": 0.4866890610109727, + "velocityY": -0.15150911649515025, + "timestamp": 1.7510233095445962 + }, + { + "x": 7.731915950775149, + "y": 5.8355112075805655, + "heading": 2.3401335118799086e-24, + "angularVelocity": -8.262952430355052e-25, + "velocityX": -7.030605668713669e-16, + "velocityY": 2.492642804337347e-16, + "timestamp": 1.8295861487838936 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Close.5.traj b/src/main/deploy/choreo/4NT-Close.5.traj new file mode 100644 index 0000000..2adec26 --- /dev/null +++ b/src/main/deploy/choreo/4NT-Close.5.traj @@ -0,0 +1,238 @@ +{ + "samples": [ + { + "x": 7.731915950775149, + "y": 5.8355112075805655, + "heading": 2.3401335118799086e-24, + "angularVelocity": -8.262952430355052e-25, + "velocityX": -7.030605668713669e-16, + "velocityY": 2.492642804337347e-16, + "timestamp": 0 + }, + { + "x": 7.699082568912615, + "y": 5.845746923563925, + "heading": -2.39940234007706e-18, + "angularVelocity": -3.295608955934343e-17, + "velocityX": -0.4509701424121041, + "velocityY": 0.14058869458014864, + "timestamp": 0.07280611014937577 + }, + { + "x": 7.633415805912784, + "y": 5.8662183553377405, + "heading": -7.195977584044839e-18, + "angularVelocity": -6.588149336242636e-17, + "velocityX": -0.901940274862986, + "velocityY": 0.28117738651074886, + "timestamp": 0.14561222029875154 + }, + { + "x": 7.534915662742616, + "y": 5.8969255026447325, + "heading": -1.2575357650155535e-17, + "angularVelocity": -7.388638205297987e-17, + "velocityX": -1.3529103940325595, + "velocityY": 0.42176607490759116, + "timestamp": 0.2184183304481273 + }, + { + "x": 7.40358214075581, + "y": 5.93786836512457, + "heading": -1.2739401484069817e-17, + "angularVelocity": -2.253160375109903e-18, + "velocityX": -1.8038804946089275, + "velocityY": 0.5623547583552307, + "timestamp": 0.2912244405975031 + }, + { + "x": 7.239415241982809, + "y": 5.98904694223645, + "heading": -1.5244617828039866e-17, + "angularVelocity": -3.4409424685999395e-17, + "velocityX": -2.254850567296915, + "velocityY": 0.7029434343748852, + "timestamp": 0.36403055074687884 + }, + { + "x": 7.042414969807385, + "y": 6.050461233078238, + "heading": -2.121155671219581e-17, + "angularVelocity": -8.195656807520109e-17, + "velocityX": -2.705820593508446, + "velocityY": 0.8435320980036471, + "timestamp": 0.4368366608962546 + }, + { + "x": 6.812581330995641, + "y": 6.122111235842821, + "heading": -3.0333248024884946e-17, + "angularVelocity": -1.2528744281701554e-16, + "velocityX": -3.156790526786791, + "velocityY": 0.9841207368114688, + "timestamp": 0.5096427710456304 + }, + { + "x": 6.549914345820485, + "y": 6.2039969450909735, + "heading": -3.5945148234933864e-17, + "angularVelocity": -7.708007265759047e-17, + "velocityX": -3.6077601816144935, + "velocityY": 1.124709300910954, + "timestamp": 0.5824488811950062 + }, + { + "x": 6.265706841223026, + "y": 6.29259789219859, + "heading": -5.062522731893286e-17, + "angularVelocity": -2.0163251482297616e-16, + "velocityX": -3.9036216055816446, + "velocityY": 1.21694383789813, + "timestamp": 0.6552549913443819 + }, + { + "x": 5.981499386126825, + "y": 6.381198998076362, + "heading": -3.5793594630502314e-17, + "angularVelocity": 2.0371412039541545e-16, + "velocityX": -3.90362092567637, + "velocityY": 1.2169460186238399, + "timestamp": 0.7280611014937577 + }, + { + "x": 5.697290967881506, + "y": 6.46979701439404, + "heading": -7.376404563418145e-17, + "angularVelocity": -5.215283580351126e-16, + "velocityX": -3.90363415463632, + "velocityY": 1.216903583173247, + "timestamp": 0.8008672116431335 + }, + { + "x": 5.41304748356572, + "y": 6.558282465917216, + "heading": -8.687949694144753e-18, + "angularVelocity": 8.938274014785796e-16, + "velocityX": -3.9041157910044126, + "velocityY": 1.2153574932329017, + "timestamp": 0.8736733217925092 + }, + { + "x": 5.127899646759033, + "y": 6.643808746337891, + "heading": -1.0157183102941835e-28, + "angularVelocity": 1.1932995438677598e-16, + "velocityX": -3.9165371727956653, + "velocityY": 1.1747129498500006, + "timestamp": 0.946479431941885 + }, + { + "x": 4.835131855746803, + "y": 6.69650604863309, + "heading": -2.645085913591072e-17, + "angularVelocity": -3.635805599807214e-16, + "velocityX": -4.024242721584003, + "velocityY": 0.7243513177298383, + "timestamp": 1.0192304589716352 + }, + { + "x": 4.54187305912769, + "y": 6.7463985666619255, + "heading": 4.8500280831595865e-17, + "angularVelocity": 1.0302416754610894e-15, + "velocityX": -4.030991844269019, + "velocityY": 0.6857981263744645, + "timestamp": 1.0919814860013854 + }, + { + "x": 4.248595942277107, + "y": 6.796183281920838, + "heading": 1.565041210121607e-17, + "angularVelocity": -4.515382128821962e-16, + "velocityX": -4.031243665201547, + "velocityY": 0.6843163222774555, + "timestamp": 1.1647325130311357 + }, + { + "x": 3.9777514636703137, + "y": 6.84215783779885, + "heading": 1.1250247092738965e-17, + "angularVelocity": -6.048251553582641e-17, + "velocityX": -3.7228956025052042, + "velocityY": 0.6319437368109796, + "timestamp": 1.237483540060886 + }, + { + "x": 3.74076252273117, + "y": 6.882385596024865, + "heading": 7.740492042931928e-18, + "angularVelocity": -4.824337500544115e-17, + "velocityX": -3.2575339567678285, + "velocityY": 0.5529510698119757, + "timestamp": 1.3102345670906361 + }, + { + "x": 3.5376291362506724, + "y": 6.916866540158376, + "heading": 4.67781503303015e-18, + "angularVelocity": -4.20980577468297e-17, + "velocityX": -2.792172080229617, + "velocityY": 0.4739581768297859, + "timestamp": 1.3829855941203864 + }, + { + "x": 3.368351309817325, + "y": 6.9456006646442425, + "heading": 1.1688792842416151e-18, + "angularVelocity": -4.823211315826823e-17, + "velocityX": -2.326810126874584, + "velocityY": 0.3949652074893709, + "timestamp": 1.4557366211501357 + }, + { + "x": 3.2329290462231546, + "y": 6.968587966690773, + "heading": -1.3220258587467443e-18, + "angularVelocity": -3.4238762839143694e-17, + "velocityX": -1.861448135141702, + "velocityY": 0.3159721997757365, + "timestamp": 1.528487648179885 + }, + { + "x": 3.131362347142629, + "y": 6.985828444618419, + "heading": -1.9579482778740535e-18, + "angularVelocity": -8.74107836798576e-18, + "velocityX": -1.396086120392406, + "velocityY": 0.2369791689758226, + "timestamp": 1.6012386752096344 + }, + { + "x": 3.063651213691743, + "y": 6.997322097305583, + "heading": -1.6053614795746217e-18, + "angularVelocity": 4.846485264019486e-18, + "velocityX": -0.9307240903032074, + "velocityY": 0.1579861227590071, + "timestamp": 1.6739897022393837 + }, + { + "x": 3.029795646667476, + "y": 7.003068923950196, + "heading": 1.137912307807085e-28, + "angularVelocity": 2.2066513092231192e-17, + "velocityX": -0.4653620492590992, + "velocityY": 0.07899306551733094, + "timestamp": 1.746740729269133 + }, + { + "x": 3.0297956466674782, + "y": 7.003068923950195, + "heading": 5.621189806598705e-29, + "angularVelocity": -1.879533730837788e-29, + "velocityX": -1.7673666788727504e-16, + "velocityY": 5.511519448895902e-17, + "timestamp": 1.8194917562988824 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Close.traj b/src/main/deploy/choreo/4NT-Close.traj new file mode 100644 index 0000000..cec1466 --- /dev/null +++ b/src/main/deploy/choreo/4NT-Close.traj @@ -0,0 +1,949 @@ +{ + "samples": [ + { + "x": 0.7644821405410767, + "y": 6.707253456115723, + "heading": 1.0390723224103011, + "angularVelocity": -4.4871567857707046e-18, + "velocityX": 3.460724762099423e-16, + "velocityY": 6.79633296625553e-17, + "timestamp": 0 + }, + { + "x": 0.7893073618901386, + "y": 6.711898127758422, + "heading": 1.0229599251421253, + "angularVelocity": -0.2562974082798586, + "velocityX": 0.3948909517207538, + "velocityY": 0.0738820725755683, + "timestamp": 0.06286601716469933 + }, + { + "x": 0.8389638482798748, + "y": 6.721192561704798, + "heading": 0.9908267229593493, + "angularVelocity": -0.5111378711748218, + "velocityX": 0.7898780395080967, + "velocityY": 0.14784512150697973, + "timestamp": 0.12573203432939867 + }, + { + "x": 0.9134574420677571, + "y": 6.735142975200245, + "heading": 0.9427373156582869, + "angularVelocity": -0.7649507551133621, + "velocityX": 1.1849580607710317, + "velocityY": 0.22190706719814038, + "timestamp": 0.188598051494098 + }, + { + "x": 1.0127948463812606, + "y": 6.753755537156801, + "heading": 0.8787447001001124, + "angularVelocity": -1.0179206261879956, + "velocityX": 1.5801447076447082, + "velocityY": 0.296067140817775, + "timestamp": 0.25146406865879734 + }, + { + "x": 1.136986588346673, + "y": 6.777034858063683, + "heading": 0.798950116173758, + "angularVelocity": -1.2692800900268044, + "velocityX": 1.9754988078860747, + "velocityY": 0.3703005527754691, + "timestamp": 0.31433008582349664 + }, + { + "x": 1.2860552286500786, + "y": 6.804983168108553, + "heading": 0.70370039577791, + "angularVelocity": -1.5151225525614787, + "velocityX": 2.3712117774673787, + "velocityY": 0.44456943998292453, + "timestamp": 0.37719610298819595 + }, + { + "x": 1.4600612279623586, + "y": 6.8376056601235335, + "heading": 0.5943219599600833, + "angularVelocity": -1.739865840894617, + "velocityX": 2.7678864855778302, + "velocityY": 0.5189209287668323, + "timestamp": 0.44006212015289525 + }, + { + "x": 1.6592143751431372, + "y": 6.8749781984647464, + "heading": 0.47881641796683205, + "angularVelocity": -1.837328770020157, + "velocityX": 3.167898272591771, + "velocityY": 0.5944791801156865, + "timestamp": 0.5029281373175946 + }, + { + "x": 1.833246757687688, + "y": 6.907207190100634, + "heading": 0.3632682477784989, + "angularVelocity": -1.8380068501150066, + "velocityX": 2.7683061595681933, + "velocityY": 0.5126615791717365, + "timestamp": 0.5657941544822939 + }, + { + "x": 1.98232093363545, + "y": 6.934701065225548, + "heading": 0.2612435638075881, + "angularVelocity": -1.6228908490179, + "velocityX": 2.371299832105078, + "velocityY": 0.43734081408223097, + "timestamp": 0.6286601716469932 + }, + { + "x": 2.1065071082491382, + "y": 6.957543379697764, + "heading": 0.1749925302220398, + "angularVelocity": -1.371981834947508, + "velocityX": 1.975410248884438, + "velocityY": 0.3633491590913955, + "timestamp": 0.6915261888116925 + }, + { + "x": 2.2058348256223446, + "y": 6.9757778777630985, + "heading": 0.10539352393852991, + "angularVelocity": -1.1071006152833613, + "velocityX": 1.5799906189855346, + "velocityY": 0.29005333704479014, + "timestamp": 0.7543922059763918 + }, + { + "x": 2.28031886152934, + "y": 6.9894324604556495, + "heading": 0.052874348065941004, + "angularVelocity": -0.8354143978130386, + "velocityX": 1.1848060250398966, + "velocityY": 0.2172013324269997, + "timestamp": 0.8172582231410911 + }, + { + "x": 2.329968057638108, + "y": 6.998525566093112, + "heading": 0.017682384897765975, + "angularVelocity": -0.5597931085081341, + "velocityX": 0.7897620741376922, + "velocityY": 0.14464262327357277, + "timestamp": 0.8801242403057904 + }, + { + "x": 2.3547892570495605, + "y": 7.003068923950195, + "heading": -1.6526519468843367e-17, + "angularVelocity": -0.28127095838489014, + "velocityX": 0.39482697538202266, + "velocityY": 0.07227048987661186, + "timestamp": 0.9429902574704897 + }, + { + "x": 2.35478925704956, + "y": 7.003068923950195, + "heading": -7.945307467511743e-18, + "angularVelocity": 9.541751832566194e-18, + "velocityX": -2.469200856331852e-16, + "velocityY": -5.7488464838897e-17, + "timestamp": 1.0058562746351891 + }, + { + "x": 2.415475621976421, + "y": 7.007915755074314, + "heading": -3.517124144434822e-19, + "angularVelocity": 7.839197585347938e-17, + "velocityX": 0.6264916515173242, + "velocityY": 0.05003593870277661, + "timestamp": 1.1027232716332853 + }, + { + "x": 2.536848350474334, + "y": 7.017609417214267, + "heading": 2.0883695559124673e-17, + "angularVelocity": 2.1922232683494886e-16, + "velocityX": 1.2529832890380508, + "velocityY": 0.1000718762876886, + "timestamp": 1.1995902686313813 + }, + { + "x": 2.7189074405095877, + "y": 7.03214991020763, + "heading": 4.970708271093658e-17, + "angularVelocity": 2.975563285402562e-16, + "velocityX": 1.8794749055638844, + "velocityY": 0.1501078121958038, + "timestamp": 1.2964572656294773 + }, + { + "x": 2.9616528886926634, + "y": 7.051537233783689, + "heading": 1.0287632521899536e-16, + "angularVelocity": 5.488891512697579e-16, + "velocityX": 2.505966487098253, + "velocityY": 0.20014374530925957, + "timestamp": 1.3933242626275733 + }, + { + "x": 3.2650846882445492, + "y": 7.075771387401028, + "heading": 1.9068454520432853e-16, + "angularVelocity": 9.064823379151022e-16, + "velocityX": 3.1324579986499463, + "velocityY": 0.25017967283341686, + "timestamp": 1.4901912596256692 + }, + { + "x": 3.629202818829201, + "y": 7.104852369435471, + "heading": 3.733614670935224e-16, + "angularVelocity": 1.8858530856090668e-15, + "velocityX": 3.7589493002638363, + "velocityY": 0.300215583590495, + "timestamp": 1.5870582566237652 + }, + { + "x": 4.024026376830976, + "y": 7.136385698409197, + "heading": 4.457439879437158e-16, + "angularVelocity": 7.472361390272464e-16, + "velocityX": 4.07593473770572, + "velocityY": 0.3255322240901749, + "timestamp": 1.6839252536218612 + }, + { + "x": 4.418849934832716, + "y": 7.167919027382921, + "heading": 4.558091019327557e-16, + "angularVelocity": 1.0390653460947907e-16, + "velocityX": 4.075934737705357, + "velocityY": 0.32553222409014865, + "timestamp": 1.7807922506199572 + }, + { + "x": 4.8136734928344564, + "y": 7.199452356356644, + "heading": 4.22612711995065e-16, + "angularVelocity": -3.427007234567253e-16, + "velocityX": 4.075934737705359, + "velocityY": 0.32553222409012916, + "timestamp": 1.8776592476180531 + }, + { + "x": 5.208497050836196, + "y": 7.230985685330376, + "heading": 4.132801040379339e-16, + "angularVelocity": -9.634455769312537e-17, + "velocityX": 4.07593473770535, + "velocityY": 0.3255322240902326, + "timestamp": 1.9745262446161491 + }, + { + "x": 5.6033206088379375, + "y": 7.262519014304082, + "heading": 4.974719423376743e-16, + "angularVelocity": 8.691488422559714e-16, + "velocityX": 4.0759347377053725, + "velocityY": 0.32553222408995947, + "timestamp": 2.071393241614245 + }, + { + "x": 5.998144166839676, + "y": 7.294052343277824, + "heading": 4.787346935143853e-16, + "angularVelocity": -1.9343274205936813e-16, + "velocityX": 4.075934737705342, + "velocityY": 0.3255322240903373, + "timestamp": 2.168260238612341 + }, + { + "x": 6.392967724841452, + "y": 7.325585672251541, + "heading": 4.705780860197851e-16, + "angularVelocity": -8.420419481072571e-17, + "velocityX": 4.075934737705728, + "velocityY": 0.32553222409008153, + "timestamp": 2.265127235610437 + }, + { + "x": 6.757085855426104, + "y": 7.3546666542859835, + "heading": 2.57250007761421e-16, + "angularVelocity": -2.20227825947358e-15, + "velocityX": 3.7589493002638363, + "velocityY": 0.3002155835905016, + "timestamp": 2.361994232608533 + }, + { + "x": 7.06051765497799, + "y": 7.378900807903323, + "heading": 1.3962913393362377e-16, + "angularVelocity": -1.2142512866829015e-15, + "velocityX": 3.132457998649947, + "velocityY": 0.25017967283342074, + "timestamp": 2.458861229606629 + }, + { + "x": 7.303263103161066, + "y": 7.398288131479383, + "heading": 7.249700096797418e-17, + "angularVelocity": -6.930341220083241e-16, + "velocityX": 2.5059664870982536, + "velocityY": 0.20014374530926216, + "timestamp": 2.555728226604725 + }, + { + "x": 7.485322193196319, + "y": 7.412828624472744, + "heading": 2.3826173239994654e-17, + "angularVelocity": -5.024500609510776e-16, + "velocityX": 1.8794749055638846, + "velocityY": 0.1501078121958055, + "timestamp": 2.652595223602821 + }, + { + "x": 7.606694921694233, + "y": 7.422522286612698, + "heading": 6.1848492309357054e-18, + "angularVelocity": -1.8211904002605062e-16, + "velocityX": 1.2529832890380512, + "velocityY": 0.10007187628768963, + "timestamp": 2.749462220600917 + }, + { + "x": 7.667381286621095, + "y": 7.427369117736816, + "heading": -2.892642650950664e-26, + "angularVelocity": -6.384888230976486e-17, + "velocityX": 0.6264916515173246, + "velocityY": 0.05003593870277713, + "timestamp": 2.846329217599013 + }, + { + "x": 7.667381286621094, + "y": 7.427369117736816, + "heading": -2.807242222383161e-26, + "angularVelocity": -9.314848061399685e-27, + "velocityX": 2.930418041341978e-17, + "velocityY": 1.8573166632838487e-18, + "timestamp": 2.943196214597109 + }, + { + "x": 7.6043184218722315, + "y": 7.4216272524122235, + "heading": -5.4231338022379995e-19, + "angularVelocity": -5.489423433560598e-18, + "velocityX": -0.6383377994270321, + "velocityY": -0.05812057048950791, + "timestamp": 3.041988523004143 + }, + { + "x": 7.478192693747755, + "y": 7.410143521888074, + "heading": 9.1638326322218e-18, + "angularVelocity": 9.824800347715444e-17, + "velocityX": -1.276675584953701, + "velocityY": -0.11624113971338963, + "timestamp": 3.140780831411177 + }, + { + "x": 7.289004104307538, + "y": 7.392917926351917, + "heading": 3.509462172627408e-17, + "angularVelocity": 2.624778291044605e-16, + "velocityX": -1.915013349629827, + "velocityY": -0.17436170703883222, + "timestamp": 3.2395731398182113 + }, + { + "x": 7.0367526569847, + "y": 7.369950466116338, + "heading": 8.808387501704071e-17, + "angularVelocity": 5.363702518284207e-16, + "velocityX": -2.5533510795550636, + "velocityY": -0.23248227120021098, + "timestamp": 3.3383654482252454 + }, + { + "x": 6.721438358645465, + "y": 7.341241141806506, + "heading": 1.8168852174846708e-16, + "angularVelocity": 9.474892406751412e-16, + "velocityX": -3.191688739978706, + "velocityY": -0.2906028290334789, + "timestamp": 3.4371577566322795 + }, + { + "x": 6.343061229887728, + "y": 7.306789955297856, + "heading": 3.542313739589584e-16, + "angularVelocity": 1.7465211410774172e-15, + "velocityX": -3.830026191905404, + "velocityY": -0.3487233678831281, + "timestamp": 3.5359500650393136 + }, + { + "x": 5.940772069277239, + "y": 7.270161581516117, + "heading": 3.929421150957015e-16, + "angularVelocity": 3.9183962579211066e-16, + "velocityX": -4.072069648914557, + "velocityY": -0.37076139197828895, + "timestamp": 3.6347423734463478 + }, + { + "x": 5.5384829086667535, + "y": 7.233533207734375, + "heading": 3.5944692196834336e-16, + "angularVelocity": -3.3904656795171214e-16, + "velocityX": -4.072069648914502, + "velocityY": -0.37076139197830654, + "timestamp": 3.733534681853382 + }, + { + "x": 5.136193748056268, + "y": 7.1969048339526385, + "heading": 2.816331867279604e-16, + "angularVelocity": -7.876497311722583e-16, + "velocityX": -4.072069648914507, + "velocityY": -0.3707613919782439, + "timestamp": 3.832326990260416 + }, + { + "x": 4.7339045874457835, + "y": 7.1602764601708975, + "heading": 3.7044270006052305e-16, + "angularVelocity": 8.989516982312421e-16, + "velocityX": -4.072069648914502, + "velocityY": -0.3707613919782984, + "timestamp": 3.93111929866745 + }, + { + "x": 4.3316154268352935, + "y": 7.123648086389155, + "heading": 3.627714094587869e-16, + "angularVelocity": -7.765068710911665e-17, + "velocityX": -4.072069648914555, + "velocityY": -0.37076139197830443, + "timestamp": 4.029911607074484 + }, + { + "x": 3.9532382980775562, + "y": 7.0891968998805055, + "heading": 2.0138393758473373e-16, + "angularVelocity": -1.6336036426615943e-15, + "velocityX": -3.8300261919054046, + "velocityY": -0.34872336788312547, + "timestamp": 4.128703915481519 + }, + { + "x": 3.6379239997383217, + "y": 7.060487575570674, + "heading": 1.144803164813082e-16, + "angularVelocity": -8.79659814093205e-16, + "velocityX": -3.191688739978706, + "velocityY": -0.29060282903347734, + "timestamp": 4.227496223888553 + }, + { + "x": 3.385672552415484, + "y": 7.037520115335095, + "heading": 6.046472103882109e-17, + "angularVelocity": -5.467591390886901e-16, + "velocityX": -2.553351079555064, + "velocityY": -0.23248227120021, + "timestamp": 4.326288532295588 + }, + { + "x": 3.196483962975267, + "y": 7.020294519798938, + "heading": 3.2682810287652467e-17, + "angularVelocity": -2.8121533703852023e-16, + "velocityX": -1.9150133496298272, + "velocityY": -0.17436170703883164, + "timestamp": 4.4250808407026225 + }, + { + "x": 3.07035823485079, + "y": 7.008810789274788, + "heading": 1.6547008414870972e-17, + "angularVelocity": -1.6333055858508895e-16, + "velocityX": -1.276675584953701, + "velocityY": -0.11624113971338933, + "timestamp": 4.523873149109657 + }, + { + "x": 3.0072953701019274, + "y": 7.003068923950195, + "heading": -5.572543390873087e-26, + "angularVelocity": -1.6749288756381388e-16, + "velocityX": -0.638337799427032, + "velocityY": -0.058120570489507827, + "timestamp": 4.622665457516692 + }, + { + "x": 3.007295370101928, + "y": 7.003068923950195, + "heading": -2.701724786958032e-26, + "angularVelocity": 1.8342141297599884e-26, + "velocityX": 1.650330335862223e-16, + "velocityY": -1.072378048093932e-16, + "timestamp": 4.721457765923726 + }, + { + "x": 3.036322603690972, + "y": 6.998184073615173, + "heading": -6.659182533814151e-19, + "angularVelocity": -9.886580304682387e-18, + "velocityX": 0.430953927728845, + "velocityY": -0.07252311632756721, + "timestamp": 4.788813535813145 + }, + { + "x": 3.094377070111199, + "y": 6.988414372048806, + "heading": -6.272481275693907e-18, + "angularVelocity": -8.323805132134451e-17, + "velocityX": 0.8619078442060418, + "velocityY": -0.145046245962409, + "timestamp": 4.856169305702564 + }, + { + "x": 3.1814587683881745, + "y": 6.973759818098456, + "heading": -1.1245385282688892e-17, + "angularVelocity": -7.383040860905551e-17, + "velocityX": 1.292861746216284, + "velocityY": -0.2175693927099312, + "timestamp": 4.923525075591983 + }, + { + "x": 3.2975676972225916, + "y": 6.954220410226885, + "heading": -1.425981430781261e-17, + "angularVelocity": -4.475383529525523e-17, + "velocityX": 1.7238156289363036, + "velocityY": -0.2900925622801421, + "timestamp": 4.990880845481402 + }, + { + "x": 3.442703854795298, + "y": 6.9297961462812125, + "heading": -2.2571965647820338e-17, + "angularVelocity": -1.2340667118105605e-16, + "velocityX": 2.154769484648219, + "velocityY": -0.3626157638131432, + "timestamp": 5.058236615370821 + }, + { + "x": 3.61686723837731, + "y": 6.900487023030509, + "heading": -2.846165657687679e-17, + "angularVelocity": -8.74415093652977e-17, + "velocityX": 2.5857232998441617, + "velocityY": -0.43513901331428134, + "timestamp": 5.12559238526024 + }, + { + "x": 3.820057843419671, + "y": 6.866293035085623, + "heading": -3.614720128617906e-17, + "angularVelocity": -1.141037318539728e-16, + "velocityX": 3.0166770475038507, + "velocityY": -0.5076623428256568, + "timestamp": 5.192948155149659 + }, + { + "x": 4.052275660822413, + "y": 6.827214171652438, + "heading": -4.8269174033446284e-17, + "angularVelocity": -1.7996931628267424e-16, + "velocityX": 3.4476306600605273, + "velocityY": -0.5801858325923079, + "timestamp": 5.260303925039078 + }, + { + "x": 4.313520663286318, + "y": 6.783250400223824, + "heading": -6.49281895371743e-17, + "angularVelocity": -2.4732870697850985e-16, + "velocityX": 3.878583867318316, + "velocityY": -0.6527098049772873, + "timestamp": 5.327659694928497 + }, + { + "x": 4.585110952119839, + "y": 6.737529090718732, + "heading": -9.015940488845162e-17, + "angularVelocity": -3.745962038515257e-16, + "velocityX": 4.032175554958435, + "velocityY": -0.6788031608898043, + "timestamp": 5.395015464817916 + }, + { + "x": 4.85667163042734, + "y": 6.691632237173498, + "heading": -4.751315953731664e-17, + "angularVelocity": 6.331491122848668e-16, + "velocityX": 4.031735941157495, + "velocityY": -0.6814093821593625, + "timestamp": 5.4623712347073345 + }, + { + "x": 5.127899646759034, + "y": 6.643808746337891, + "heading": 8.852905771561079e-23, + "angularVelocity": 7.054072252152007e-16, + "velocityX": 4.026797062478613, + "velocityY": -0.7100132759837658, + "timestamp": 5.5297270045967535 + }, + { + "x": 5.435225736063499, + "y": 6.550301165877784, + "heading": -3.054048980043161e-19, + "angularVelocity": -3.89086507141557e-18, + "velocityX": 3.9118505934900796, + "velocityY": -1.190226592691475, + "timestamp": 5.608289843836051 + }, + { + "x": 5.741994583635364, + "y": 6.454981350959253, + "heading": -3.5555327560181985e-17, + "angularVelocity": -4.486866408095523e-16, + "velocityX": 3.9047576505906223, + "velocityY": -1.213293916587032, + "timestamp": 5.686852683075348 + }, + { + "x": 6.048713801922417, + "y": 6.3595019612227075, + "heading": 7.714193179741933e-18, + "angularVelocity": 5.507616890435799e-16, + "velocityX": 3.9041259360904785, + "velocityY": -1.2153250908580402, + "timestamp": 5.765415522314646 + }, + { + "x": 6.35543181351245, + "y": 6.264018695159109, + "heading": 2.1299456161841412e-17, + "angularVelocity": 1.7292071017284973e-16, + "velocityX": 3.904110576449497, + "velocityY": -1.2153744313228805, + "timestamp": 5.843978361553943 + }, + { + "x": 6.661317138890106, + "y": 6.168794814888757, + "heading": -2.6772613029830932e-14, + "angularVelocity": -3.41050623150326e-13, + "velocityX": 3.8935115932605218, + "velocityY": -1.212072796659924, + "timestamp": 5.9225412007932405 + }, + { + "x": 6.928966825594151, + "y": 6.085473915495383, + "heading": -3.073303152925181e-14, + "angularVelocity": -5.041082116318115e-14, + "velocityX": 3.406822987759037, + "velocityY": -1.0605637499881233, + "timestamp": 6.001104040032538 + }, + { + "x": 7.158380853190387, + "y": 6.014056000162917, + "heading": -2.7188130909040473e-14, + "angularVelocity": 4.512183847842235e-14, + "velocityX": 2.920134122157474, + "velocityY": -0.9090546627891893, + "timestamp": 6.079666879271835 + }, + { + "x": 7.34955921485854, + "y": 5.954541069923522, + "heading": -2.0785853473910866e-14, + "angularVelocity": 8.149241754648641e-14, + "velocityX": 2.4334451697429427, + "velocityY": -0.7575455624521772, + "timestamp": 6.158229718511133 + }, + { + "x": 7.502501907186795, + "y": 5.906929125287755, + "heading": -1.3764492993566052e-14, + "angularVelocity": 8.937251498866303e-14, + "velocityX": 1.9467561739005792, + "velocityY": -0.6060364556164637, + "timestamp": 6.23679255775043 + }, + { + "x": 7.617208928127525, + "y": 5.871220166560173, + "heading": -7.419672395660612e-15, + "angularVelocity": 8.076106815549843e-14, + "velocityX": 1.4600671519946562, + "velocityY": -0.4545273449041617, + "timestamp": 6.315355396989728 + }, + { + "x": 7.693680276315422, + "y": 5.847414193943067, + "heading": -2.6268163161796944e-15, + "angularVelocity": 6.100663836778702e-14, + "velocityX": 0.973378112710156, + "velocityY": -0.3030182316169395, + "timestamp": 6.393918236229025 + }, + { + "x": 7.731915950775149, + "y": 5.8355112075805655, + "heading": 4.745265421419366e-24, + "angularVelocity": 3.343585339172883e-14, + "velocityX": 0.4866890610109727, + "velocityY": -0.15150911649515025, + "timestamp": 6.472481075468322 + }, + { + "x": 7.731915950775149, + "y": 5.8355112075805655, + "heading": 2.3401335118799086e-24, + "angularVelocity": -8.262952430355052e-25, + "velocityX": -7.030605668713669e-16, + "velocityY": 2.492642804337347e-16, + "timestamp": 6.55104391470762 + }, + { + "x": 7.699082568912615, + "y": 5.845746923563925, + "heading": -2.39940234007706e-18, + "angularVelocity": -3.295608955934343e-17, + "velocityX": -0.4509701424121041, + "velocityY": 0.14058869458014864, + "timestamp": 6.623850024856996 + }, + { + "x": 7.633415805912784, + "y": 5.8662183553377405, + "heading": -7.195977584044839e-18, + "angularVelocity": -6.588149336242636e-17, + "velocityX": -0.901940274862986, + "velocityY": 0.28117738651074886, + "timestamp": 6.696656135006371 + }, + { + "x": 7.534915662742616, + "y": 5.8969255026447325, + "heading": -1.2575357650155535e-17, + "angularVelocity": -7.388638205297987e-17, + "velocityX": -1.3529103940325595, + "velocityY": 0.42176607490759116, + "timestamp": 6.769462245155747 + }, + { + "x": 7.40358214075581, + "y": 5.93786836512457, + "heading": -1.2739401484069817e-17, + "angularVelocity": -2.253160375109903e-18, + "velocityX": -1.8038804946089275, + "velocityY": 0.5623547583552307, + "timestamp": 6.842268355305123 + }, + { + "x": 7.239415241982809, + "y": 5.98904694223645, + "heading": -1.5244617828039866e-17, + "angularVelocity": -3.4409424685999395e-17, + "velocityX": -2.254850567296915, + "velocityY": 0.7029434343748852, + "timestamp": 6.915074465454499 + }, + { + "x": 7.042414969807385, + "y": 6.050461233078238, + "heading": -2.121155671219581e-17, + "angularVelocity": -8.195656807520109e-17, + "velocityX": -2.705820593508446, + "velocityY": 0.8435320980036471, + "timestamp": 6.987880575603874 + }, + { + "x": 6.812581330995641, + "y": 6.122111235842821, + "heading": -3.0333248024884946e-17, + "angularVelocity": -1.2528744281701554e-16, + "velocityX": -3.156790526786791, + "velocityY": 0.9841207368114688, + "timestamp": 7.06068668575325 + }, + { + "x": 6.549914345820485, + "y": 6.2039969450909735, + "heading": -3.5945148234933864e-17, + "angularVelocity": -7.708007265759047e-17, + "velocityX": -3.6077601816144935, + "velocityY": 1.124709300910954, + "timestamp": 7.133492795902626 + }, + { + "x": 6.265706841223026, + "y": 6.29259789219859, + "heading": -5.062522731893286e-17, + "angularVelocity": -2.0163251482297616e-16, + "velocityX": -3.9036216055816446, + "velocityY": 1.21694383789813, + "timestamp": 7.206298906052002 + }, + { + "x": 5.981499386126825, + "y": 6.381198998076362, + "heading": -3.5793594630502314e-17, + "angularVelocity": 2.0371412039541545e-16, + "velocityX": -3.90362092567637, + "velocityY": 1.2169460186238399, + "timestamp": 7.2791050162013775 + }, + { + "x": 5.697290967881506, + "y": 6.46979701439404, + "heading": -7.376404563418145e-17, + "angularVelocity": -5.215283580351126e-16, + "velocityX": -3.90363415463632, + "velocityY": 1.216903583173247, + "timestamp": 7.351911126350753 + }, + { + "x": 5.41304748356572, + "y": 6.558282465917216, + "heading": -8.687949694144753e-18, + "angularVelocity": 8.938274014785796e-16, + "velocityX": -3.9041157910044126, + "velocityY": 1.2153574932329017, + "timestamp": 7.424717236500129 + }, + { + "x": 5.127899646759033, + "y": 6.643808746337891, + "heading": -1.0157183102941835e-28, + "angularVelocity": 1.1932995438677598e-16, + "velocityX": -3.9165371727956653, + "velocityY": 1.1747129498500006, + "timestamp": 7.497523346649505 + }, + { + "x": 4.835131855746803, + "y": 6.69650604863309, + "heading": -2.645085913591072e-17, + "angularVelocity": -3.635805599807214e-16, + "velocityX": -4.024242721584003, + "velocityY": 0.7243513177298383, + "timestamp": 7.570274373679255 + }, + { + "x": 4.54187305912769, + "y": 6.7463985666619255, + "heading": 4.8500280831595865e-17, + "angularVelocity": 1.0302416754610894e-15, + "velocityX": -4.030991844269019, + "velocityY": 0.6857981263744645, + "timestamp": 7.643025400709005 + }, + { + "x": 4.248595942277107, + "y": 6.796183281920838, + "heading": 1.565041210121607e-17, + "angularVelocity": -4.515382128821962e-16, + "velocityX": -4.031243665201547, + "velocityY": 0.6843163222774555, + "timestamp": 7.7157764277387555 + }, + { + "x": 3.9777514636703137, + "y": 6.84215783779885, + "heading": 1.1250247092738965e-17, + "angularVelocity": -6.048251553582641e-17, + "velocityX": -3.7228956025052042, + "velocityY": 0.6319437368109796, + "timestamp": 7.788527454768506 + }, + { + "x": 3.74076252273117, + "y": 6.882385596024865, + "heading": 7.740492042931928e-18, + "angularVelocity": -4.824337500544115e-17, + "velocityX": -3.2575339567678285, + "velocityY": 0.5529510698119757, + "timestamp": 7.861278481798256 + }, + { + "x": 3.5376291362506724, + "y": 6.916866540158376, + "heading": 4.67781503303015e-18, + "angularVelocity": -4.20980577468297e-17, + "velocityX": -2.792172080229617, + "velocityY": 0.4739581768297859, + "timestamp": 7.934029508828006 + }, + { + "x": 3.368351309817325, + "y": 6.9456006646442425, + "heading": 1.1688792842416151e-18, + "angularVelocity": -4.823211315826823e-17, + "velocityX": -2.326810126874584, + "velocityY": 0.3949652074893709, + "timestamp": 8.006780535857756 + }, + { + "x": 3.2329290462231546, + "y": 6.968587966690773, + "heading": -1.3220258587467443e-18, + "angularVelocity": -3.4238762839143694e-17, + "velocityX": -1.861448135141702, + "velocityY": 0.3159721997757365, + "timestamp": 8.079531562887505 + }, + { + "x": 3.131362347142629, + "y": 6.985828444618419, + "heading": -1.9579482778740535e-18, + "angularVelocity": -8.74107836798576e-18, + "velocityX": -1.396086120392406, + "velocityY": 0.2369791689758226, + "timestamp": 8.152282589917254 + }, + { + "x": 3.063651213691743, + "y": 6.997322097305583, + "heading": -1.6053614795746217e-18, + "angularVelocity": 4.846485264019486e-18, + "velocityX": -0.9307240903032074, + "velocityY": 0.1579861227590071, + "timestamp": 8.225033616947004 + }, + { + "x": 3.029795646667476, + "y": 7.003068923950196, + "heading": 1.137912307807085e-28, + "angularVelocity": 2.2066513092231192e-17, + "velocityX": -0.4653620492590992, + "velocityY": 0.07899306551733094, + "timestamp": 8.297784643976753 + }, + { + "x": 3.0297956466674782, + "y": 7.003068923950195, + "heading": 5.621189806598705e-29, + "angularVelocity": -1.879533730837788e-29, + "velocityX": -1.7673666788727504e-16, + "velocityY": 5.511519448895902e-17, + "timestamp": 8.370535671006502 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Skip.1.traj b/src/main/deploy/choreo/4NT-Skip.1.traj index e95d5ae..11863e4 100644 --- a/src/main/deploy/choreo/4NT-Skip.1.traj +++ b/src/main/deploy/choreo/4NT-Skip.1.traj @@ -170,186 +170,6 @@ "velocityX": 3.923327801499733, "velocityY": 1.1518285043541625, "timestamp": 0.9024323775185769 - }, - { - "x": 3.0930925857719416, - "y": 7.801578883603503, - "heading": -2.025054003603945e-14, - "angularVelocity": -2.4734392492358033e-13, - "velocityX": 4.027794432293094, - "velocityY": 0.704334060174232, - "timestamp": 0.9843043703007283 - }, - { - "x": 3.427244052945336, - "y": 7.821878655200531, - "heading": -2.0171148368757895e-14, - "angularVelocity": 9.697048353617892e-16, - "velocityX": 4.081389200608798, - "velocityY": 0.24794524851792668, - "timestamp": 1.0661763630828798 - }, - { - "x": 3.7615632056259036, - "y": 7.804558480607454, - "heading": -2.0136813522881885e-14, - "angularVelocity": 4.1937229947938353e-16, - "velocityX": 4.083437343098987, - "velocityY": -0.21155188733665228, - "timestamp": 1.1480483558650312 - }, - { - "x": 4.095052943678329, - "y": 7.7753372729513455, - "heading": -2.0212632475990418e-14, - "angularVelocity": -9.260670289323022e-16, - "velocityX": 4.073306716007144, - "velocityY": -0.3569133553871345, - "timestamp": 1.2299203486471826 - }, - { - "x": 4.428549167930835, - "y": 7.746190184376778, - "heading": -2.0152082358748023e-14, - "angularVelocity": 7.395705806686352e-16, - "velocityX": 4.0733859396813115, - "velocityY": -0.35600805091092386, - "timestamp": 1.311792341429334 - }, - { - "x": 4.762045392031515, - "y": 7.71704309406487, - "heading": -2.015572108540389e-14, - "angularVelocity": -4.4444095376460173e-17, - "velocityX": 4.0733859378268935, - "velocityY": -0.3560080721311419, - "timestamp": 1.3936643342114854 - }, - { - "x": 5.095541616206554, - "y": 7.68789600460369, - "heading": -2.0229754033283868e-14, - "angularVelocity": -9.04252423377198e-16, - "velocityX": 4.0733859387351234, - "velocityY": -0.3560080617401835, - "timestamp": 1.4755363269936368 - }, - { - "x": 5.42903784040285, - "y": 7.658748915385721, - "heading": -2.0309309367944147e-14, - "angularVelocity": -9.717038996712115e-16, - "velocityX": 4.073385938994756, - "velocityY": -0.35600805876956854, - "timestamp": 1.5574083197757882 - }, - { - "x": 5.762534064600043, - "y": 7.629601826178022, - "heading": -2.0390490605011424e-14, - "angularVelocity": -9.915629790920958e-16, - "velocityX": 4.07338593900572, - "velocityY": -0.3560080586441164, - "timestamp": 1.6392803125579396 - }, - { - "x": 6.09603028879551, - "y": 7.60045473695057, - "heading": -2.0426978309746098e-14, - "angularVelocity": -4.456677246361619e-16, - "velocityX": 4.073385938984632, - "velocityY": -0.3560080588853828, - "timestamp": 1.721152305340091 - }, - { - "x": 6.4295265129813, - "y": 7.57130764761672, - "heading": -2.0431228864965123e-14, - "angularVelocity": -5.1917085129884607e-17, - "velocityX": 4.073385938866449, - "velocityY": -0.3560080601849525, - "timestamp": 1.8030242981222424 - }, - { - "x": 6.729362912053891, - "y": 7.545102378587393, - "heading": -1.7809391327471565e-14, - "angularVelocity": 3.202361940795799e-14, - "velocityX": 3.662258470615334, - "velocityY": -0.32007611075321446, - "timestamp": 1.8848962909043938 - }, - { - "x": 6.991719787015596, - "y": 7.522172765937965, - "heading": -1.4158080705052285e-14, - "angularVelocity": 4.459779832223585e-14, - "velocityX": 3.204476476586995, - "velocityY": -0.2800666243759492, - "timestamp": 1.9667682836865452 - }, - { - "x": 7.216597118349246, - "y": 7.502518811371365, - "heading": -1.0522354480483052e-14, - "angularVelocity": 4.440744754194895e-14, - "velocityX": 2.746694244172289, - "velocityY": -0.24005711719874057, - "timestamp": 2.0486402764686966 - }, - { - "x": 7.403994899549076, - "y": 7.486140515455231, - "heading": -7.2265963678814e-15, - "angularVelocity": 4.025501274271611e-14, - "velocityX": 2.2889119322949267, - "velocityY": -0.2000476030883275, - "timestamp": 2.1305122692508482 - }, - { - "x": 7.553913127362196, - "y": 7.473037878473377, - "heading": -4.440408288437967e-15, - "angularVelocity": 3.4031028009627144e-14, - "velocityX": 1.8311295806861385, - "velocityY": -0.16003808551133375, - "timestamp": 2.212384262033 - }, - { - "x": 7.666351799836867, - "y": 7.463210900596095, - "heading": -2.2615780344771733e-15, - "angularVelocity": 2.6612644691277202e-14, - "velocityX": 1.3733472052384643, - "velocityY": -0.12002856585439871, - "timestamp": 2.2942562548151515 - }, - { - "x": 7.7413109156719315, - "y": 7.456659581936907, - "heading": -7.614087443589096e-16, - "angularVelocity": 1.8323351358986854e-14, - "velocityX": 0.9155648138981872, - "velocityY": -0.08001904481083937, - "timestamp": 2.376128247597303 - }, - { - "x": 7.778790473937988, - "y": 7.453383922576904, - "heading": 1.2753058277402247e-32, - "angularVelocity": 9.299990369536053e-15, - "velocityX": 0.4577824112060448, - "velocityY": -0.04000952277683576, - "timestamp": 2.4580002403794547 - }, - { - "x": 7.778790473937988, - "y": 7.453383922576904, - "heading": -1.9205360939372803e-33, - "angularVelocity": -1.7921313580146165e-31, - "velocityX": 0, - "velocityY": -2.7149722697711243e-33, - "timestamp": 2.5398722331616064 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Skip.2.traj b/src/main/deploy/choreo/4NT-Skip.2.traj index 4732e91..7cc4d83 100644 --- a/src/main/deploy/choreo/4NT-Skip.2.traj +++ b/src/main/deploy/choreo/4NT-Skip.2.traj @@ -1,5 +1,185 @@ { "samples": [ + { + "x": 2.763329029083252, + "y": 7.743913650512695, + "heading": 0, + "angularVelocity": -9.762167765450472e-7, + "velocityX": 3.923327801499733, + "velocityY": 1.1518285043541625, + "timestamp": 0 + }, + { + "x": 3.0930925857719416, + "y": 7.801578883603503, + "heading": -2.025054003603945e-14, + "angularVelocity": -2.4734392492358033e-13, + "velocityX": 4.027794432293094, + "velocityY": 0.704334060174232, + "timestamp": 0.0818719927821514 + }, + { + "x": 3.427244052945336, + "y": 7.821878655200531, + "heading": -2.0171148368757895e-14, + "angularVelocity": 9.697048353617892e-16, + "velocityX": 4.081389200608798, + "velocityY": 0.24794524851792668, + "timestamp": 0.16374398556430292 + }, + { + "x": 3.7615632056259036, + "y": 7.804558480607454, + "heading": -2.0136813522881885e-14, + "angularVelocity": 4.1937229947938353e-16, + "velocityX": 4.083437343098987, + "velocityY": -0.21155188733665228, + "timestamp": 0.24561597834645432 + }, + { + "x": 4.095052943678329, + "y": 7.7753372729513455, + "heading": -2.0212632475990418e-14, + "angularVelocity": -9.260670289323022e-16, + "velocityX": 4.073306716007144, + "velocityY": -0.3569133553871345, + "timestamp": 0.3274879711286057 + }, + { + "x": 4.428549167930835, + "y": 7.746190184376778, + "heading": -2.0152082358748023e-14, + "angularVelocity": 7.395705806686352e-16, + "velocityX": 4.0733859396813115, + "velocityY": -0.35600805091092386, + "timestamp": 0.40935996391075713 + }, + { + "x": 4.762045392031515, + "y": 7.71704309406487, + "heading": -2.015572108540389e-14, + "angularVelocity": -4.4444095376460173e-17, + "velocityX": 4.0733859378268935, + "velocityY": -0.3560080721311419, + "timestamp": 0.49123195669290853 + }, + { + "x": 5.095541616206554, + "y": 7.68789600460369, + "heading": -2.0229754033283868e-14, + "angularVelocity": -9.04252423377198e-16, + "velocityX": 4.0733859387351234, + "velocityY": -0.3560080617401835, + "timestamp": 0.5731039494750599 + }, + { + "x": 5.42903784040285, + "y": 7.658748915385721, + "heading": -2.0309309367944147e-14, + "angularVelocity": -9.717038996712115e-16, + "velocityX": 4.073385938994756, + "velocityY": -0.35600805876956854, + "timestamp": 0.6549759422572113 + }, + { + "x": 5.762534064600043, + "y": 7.629601826178022, + "heading": -2.0390490605011424e-14, + "angularVelocity": -9.915629790920958e-16, + "velocityX": 4.07338593900572, + "velocityY": -0.3560080586441164, + "timestamp": 0.7368479350393627 + }, + { + "x": 6.09603028879551, + "y": 7.60045473695057, + "heading": -2.0426978309746098e-14, + "angularVelocity": -4.456677246361619e-16, + "velocityX": 4.073385938984632, + "velocityY": -0.3560080588853828, + "timestamp": 0.8187199278215141 + }, + { + "x": 6.4295265129813, + "y": 7.57130764761672, + "heading": -2.0431228864965123e-14, + "angularVelocity": -5.1917085129884607e-17, + "velocityX": 4.073385938866449, + "velocityY": -0.3560080601849525, + "timestamp": 0.9005919206036656 + }, + { + "x": 6.729362912053891, + "y": 7.545102378587393, + "heading": -1.7809391327471565e-14, + "angularVelocity": 3.202361940795799e-14, + "velocityX": 3.662258470615334, + "velocityY": -0.32007611075321446, + "timestamp": 0.982463913385817 + }, + { + "x": 6.991719787015596, + "y": 7.522172765937965, + "heading": -1.4158080705052285e-14, + "angularVelocity": 4.459779832223585e-14, + "velocityX": 3.204476476586995, + "velocityY": -0.2800666243759492, + "timestamp": 1.0643359061679685 + }, + { + "x": 7.216597118349246, + "y": 7.502518811371365, + "heading": -1.0522354480483052e-14, + "angularVelocity": 4.440744754194895e-14, + "velocityX": 2.746694244172289, + "velocityY": -0.24005711719874057, + "timestamp": 1.1462078989501197 + }, + { + "x": 7.403994899549076, + "y": 7.486140515455231, + "heading": -7.2265963678814e-15, + "angularVelocity": 4.025501274271611e-14, + "velocityX": 2.2889119322949267, + "velocityY": -0.2000476030883275, + "timestamp": 1.2280798917322713 + }, + { + "x": 7.553913127362196, + "y": 7.473037878473377, + "heading": -4.440408288437967e-15, + "angularVelocity": 3.4031028009627144e-14, + "velocityX": 1.8311295806861385, + "velocityY": -0.16003808551133375, + "timestamp": 1.309951884514423 + }, + { + "x": 7.666351799836867, + "y": 7.463210900596095, + "heading": -2.2615780344771733e-15, + "angularVelocity": 2.6612644691277202e-14, + "velocityX": 1.3733472052384643, + "velocityY": -0.12002856585439871, + "timestamp": 1.3918238772965745 + }, + { + "x": 7.7413109156719315, + "y": 7.456659581936907, + "heading": -7.614087443589096e-16, + "angularVelocity": 1.8323351358986854e-14, + "velocityX": 0.9155648138981872, + "velocityY": -0.08001904481083937, + "timestamp": 1.4736958700787262 + }, + { + "x": 7.778790473937988, + "y": 7.453383922576904, + "heading": 1.2753058277402247e-32, + "angularVelocity": 9.299990369536053e-15, + "velocityX": 0.4577824112060448, + "velocityY": -0.04000952277683576, + "timestamp": 1.5555678628608778 + }, { "x": 7.778790473937988, "y": 7.453383922576904, @@ -7,151 +187,7 @@ "angularVelocity": -1.7921313580146165e-31, "velocityX": 0, "velocityY": -2.7149722697711243e-33, - "timestamp": 0 - }, - { - "x": 7.738439660582295, - "y": 7.44435478084036, - "heading": 0.006378483370257001, - "angularVelocity": 0.07429220524317, - "velocityX": -0.4699786349727835, - "velocityY": -0.10516525827894567, - "timestamp": 0.08585669720503297 - }, - { - "x": 7.657738063777793, - "y": 7.4262963371792745, - "heading": 0.019137718520693816, - "angularVelocity": 0.14861083137133396, - "velocityX": -0.9399569216106665, - "velocityY": -0.21033238231795398, - "timestamp": 0.17171339441006594 - }, - { - "x": 7.536685745097366, - "y": 7.399208360987091, - "heading": 0.038283130587402035, - "angularVelocity": 0.2229926457686509, - "velocityX": -1.4099344910897795, - "velocityY": -0.3155021923041767, - "timestamp": 0.2575700916150989 - }, - { - "x": 7.375282797714109, - "y": 7.363090535376708, - "heading": 0.06382240791742924, - "angularVelocity": 0.29746400876611084, - "velocityX": -1.8799109753524952, - "velocityY": -0.4206756931742923, - "timestamp": 0.3434267888201319 - }, - { - "x": 7.173529348282375, - "y": 7.317942425556285, - "heading": 0.09576422280862332, - "angularVelocity": 0.3720363807486605, - "velocityX": -2.349885984432058, - "velocityY": -0.5258542582019549, - "timestamp": 0.42928348602516486 - }, - { - "x": 6.931425567675906, - "y": 7.263763399408429, - "heading": 0.1341164540627939, - "angularVelocity": 0.4467005196179663, - "velocityX": -2.819859003291312, - "velocityY": -0.6310401857000439, - "timestamp": 0.5151401832301978 - }, - { - "x": 6.648971729733199, - "y": 7.200552321235508, - "heading": 0.17888339892270952, - "angularVelocity": 0.5214147098275685, - "velocityX": -3.2898288326673564, - "velocityY": -0.73623934102623, - "timestamp": 0.6009968804352308 - }, - { - "x": 6.32616907238056, - "y": 7.128303578050117, - "heading": 0.23005243262398928, - "angularVelocity": 0.5959818554292148, - "velocityX": -3.759784243525683, - "velocityY": -0.8415038725849725, - "timestamp": 0.6868535776402638 - }, - { - "x": 6.043717325555703, - "y": 7.065082680229889, - "heading": 0.2748002023996994, - "angularVelocity": 0.5211913715810514, - "velocityX": -3.2898044767589747, - "velocityY": -0.7363537135519165, - "timestamp": 0.7727102748452968 - }, - { - "x": 5.801616036461219, - "y": 7.010892017513263, - "heading": 0.3131359109312741, - "angularVelocity": 0.44650807426269673, - "velocityX": -2.8198299838663337, - "velocityY": -0.6311757204823929, - "timestamp": 0.8585669720503297 - }, - { - "x": 5.599865072469117, - "y": 6.965732394898873, - "heading": 0.345068551946627, - "angularVelocity": 0.3719295297266695, - "velocityX": -2.349857035733676, - "velocityY": -0.5259883513402064, - "timestamp": 0.9444236692553627 - }, - { - "x": 5.438464367683131, - "y": 6.929604278069296, - "heading": 0.37060660034883014, - "angularVelocity": 0.29744969505659097, - "velocityX": -1.8798848551155938, - "velocityY": -0.4207955582463213, - "timestamp": 1.0302803664603957 - }, - { - "x": 5.317413881627067, - "y": 6.902507972751596, - "heading": 0.38975689670647157, - "angularVelocity": 0.22304953464386104, - "velocityX": -1.4099131459364944, - "velocityY": -0.31559920425301746, - "timestamp": 1.1161370636654286 - }, - { - "x": 5.2367135843204595, - "y": 6.884443678448797, - "heading": 0.40252374557523257, - "angularVelocity": 0.14869951074722523, - "velocityX": -0.9399417859492977, - "velocityY": -0.21040052658513142, - "timestamp": 1.2019937608704616 - }, - { - "x": 5.19636344909668, - "y": 6.875411510467529, - "heading": 0.4089083102765838, - "angularVelocity": 0.07436303642224136, - "velocityX": -0.46997073655676014, - "velocityY": -0.10520050590460155, - "timestamp": 1.2878504580754946 - }, - { - "x": 5.19636344909668, - "y": 6.875411510467529, - "heading": 0.4089083102765838, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -2.0717354478922107e-39, - "timestamp": 1.3737071552805276 + "timestamp": 1.6374398556430294 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Skip.3.traj b/src/main/deploy/choreo/4NT-Skip.3.traj index 681a48f..4732e91 100644 --- a/src/main/deploy/choreo/4NT-Skip.3.traj +++ b/src/main/deploy/choreo/4NT-Skip.3.traj @@ -1,166 +1,157 @@ { "samples": [ + { + "x": 7.778790473937988, + "y": 7.453383922576904, + "heading": -1.9205360939372803e-33, + "angularVelocity": -1.7921313580146165e-31, + "velocityX": 0, + "velocityY": -2.7149722697711243e-33, + "timestamp": 0 + }, + { + "x": 7.738439660582295, + "y": 7.44435478084036, + "heading": 0.006378483370257001, + "angularVelocity": 0.07429220524317, + "velocityX": -0.4699786349727835, + "velocityY": -0.10516525827894567, + "timestamp": 0.08585669720503297 + }, + { + "x": 7.657738063777793, + "y": 7.4262963371792745, + "heading": 0.019137718520693816, + "angularVelocity": 0.14861083137133396, + "velocityX": -0.9399569216106665, + "velocityY": -0.21033238231795398, + "timestamp": 0.17171339441006594 + }, + { + "x": 7.536685745097366, + "y": 7.399208360987091, + "heading": 0.038283130587402035, + "angularVelocity": 0.2229926457686509, + "velocityX": -1.4099344910897795, + "velocityY": -0.3155021923041767, + "timestamp": 0.2575700916150989 + }, + { + "x": 7.375282797714109, + "y": 7.363090535376708, + "heading": 0.06382240791742924, + "angularVelocity": 0.29746400876611084, + "velocityX": -1.8799109753524952, + "velocityY": -0.4206756931742923, + "timestamp": 0.3434267888201319 + }, + { + "x": 7.173529348282375, + "y": 7.317942425556285, + "heading": 0.09576422280862332, + "angularVelocity": 0.3720363807486605, + "velocityX": -2.349885984432058, + "velocityY": -0.5258542582019549, + "timestamp": 0.42928348602516486 + }, + { + "x": 6.931425567675906, + "y": 7.263763399408429, + "heading": 0.1341164540627939, + "angularVelocity": 0.4467005196179663, + "velocityX": -2.819859003291312, + "velocityY": -0.6310401857000439, + "timestamp": 0.5151401832301978 + }, + { + "x": 6.648971729733199, + "y": 7.200552321235508, + "heading": 0.17888339892270952, + "angularVelocity": 0.5214147098275685, + "velocityX": -3.2898288326673564, + "velocityY": -0.73623934102623, + "timestamp": 0.6009968804352308 + }, + { + "x": 6.32616907238056, + "y": 7.128303578050117, + "heading": 0.23005243262398928, + "angularVelocity": 0.5959818554292148, + "velocityX": -3.759784243525683, + "velocityY": -0.8415038725849725, + "timestamp": 0.6868535776402638 + }, + { + "x": 6.043717325555703, + "y": 7.065082680229889, + "heading": 0.2748002023996994, + "angularVelocity": 0.5211913715810514, + "velocityX": -3.2898044767589747, + "velocityY": -0.7363537135519165, + "timestamp": 0.7727102748452968 + }, + { + "x": 5.801616036461219, + "y": 7.010892017513263, + "heading": 0.3131359109312741, + "angularVelocity": 0.44650807426269673, + "velocityX": -2.8198299838663337, + "velocityY": -0.6311757204823929, + "timestamp": 0.8585669720503297 + }, + { + "x": 5.599865072469117, + "y": 6.965732394898873, + "heading": 0.345068551946627, + "angularVelocity": 0.3719295297266695, + "velocityX": -2.349857035733676, + "velocityY": -0.5259883513402064, + "timestamp": 0.9444236692553627 + }, + { + "x": 5.438464367683131, + "y": 6.929604278069296, + "heading": 0.37060660034883014, + "angularVelocity": 0.29744969505659097, + "velocityX": -1.8798848551155938, + "velocityY": -0.4207955582463213, + "timestamp": 1.0302803664603957 + }, + { + "x": 5.317413881627067, + "y": 6.902507972751596, + "heading": 0.38975689670647157, + "angularVelocity": 0.22304953464386104, + "velocityX": -1.4099131459364944, + "velocityY": -0.31559920425301746, + "timestamp": 1.1161370636654286 + }, + { + "x": 5.2367135843204595, + "y": 6.884443678448797, + "heading": 0.40252374557523257, + "angularVelocity": 0.14869951074722523, + "velocityX": -0.9399417859492977, + "velocityY": -0.21040052658513142, + "timestamp": 1.2019937608704616 + }, { "x": 5.19636344909668, "y": 6.875411510467529, "heading": 0.4089083102765838, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": -2.0717354478922107e-39, - "timestamp": 0 + "angularVelocity": 0.07436303642224136, + "velocityX": -0.46997073655676014, + "velocityY": -0.10520050590460155, + "timestamp": 1.2878504580754946 }, { - "x": 5.232018908472479, - "y": 6.860290267658786, - "heading": 0.4034159167471209, - "angularVelocity": -0.06610093533675257, - "velocityX": 0.4291133186212821, - "velocityY": -0.181984099964854, - "timestamp": 0.08309101076228087 - }, - { - "x": 5.3033298450653605, - "y": 6.830047764220078, - "heading": 0.39242803594978326, - "angularVelocity": -0.13223910380357973, - "velocityX": 0.858226851962353, - "velocityY": -0.36396841440803684, - "timestamp": 0.16618202152456174 - }, - { - "x": 5.410296246847904, - "y": 6.784683995320563, - "heading": 0.37593441821627777, - "angularVelocity": -0.19850062698951831, - "velocityX": 1.2873402405534362, - "velocityY": -0.5459527869903824, - "timestamp": 0.24927303228684217 - }, - { - "x": 5.552918069928713, - "y": 6.72419896473783, - "heading": 0.3539176492261319, - "angularVelocity": -0.2649717314564233, - "velocityX": 1.7164531009117598, - "velocityY": -0.7279371141094613, - "timestamp": 0.3323640430491226 - }, - { - "x": 5.731195235870137, - "y": 6.64859267901311, - "heading": 0.3263531755047263, - "angularVelocity": -0.33173833689743104, - "velocityX": 2.1455650172732543, - "velocityY": -0.9099213625048691, - "timestamp": 0.415455053811403 - }, - { - "x": 5.945127628296537, - "y": 6.557865140131203, - "heading": 0.29320941514251314, - "angularVelocity": -0.39888503050030044, - "velocityX": 2.57467553305435, - "velocityY": -1.0919055870131655, - "timestamp": 0.49854606457368345 - }, - { - "x": 6.194715088245195, - "y": 6.452016336978496, - "heading": 0.25444802631157254, - "angularVelocity": -0.4664931678570496, - "velocityX": 3.003784135719744, - "velocityY": -1.2738899452738088, - "timestamp": 0.5816370753359639 - }, - { - "x": 6.47995740296407, - "y": 6.331046237930042, - "heading": 0.2100244721238566, - "angularVelocity": -0.5346373064928706, - "velocityX": 3.4328901779151324, - "velocityY": -1.455874684140549, - "timestamp": 0.6647280860982443 - }, - { - "x": 6.765189822342828, - "y": 6.210072243447842, - "heading": 0.16347583941488503, - "angularVelocity": -0.5602126184521319, - "velocityX": 3.4327710875342943, - "velocityY": -1.4559215656709357, - "timestamp": 0.7478190968605247 - }, - { - "x": 7.0147678146535055, - "y": 6.104219702621471, - "heading": 0.1226760205647685, - "angularVelocity": -0.49102566542177156, - "velocityX": 3.0036701927324945, - "velocityY": -1.2739349281621999, - "timestamp": 0.8309101076228051 - }, - { - "x": 7.22869154945277, - "y": 6.013488715324614, - "heading": 0.0876591508055317, - "angularVelocity": -0.421427895003207, - "velocityX": 2.574571338544547, - "velocityY": -1.091947088674045, - "timestamp": 0.9140011183850856 - }, - { - "x": 7.406961171745674, - "y": 5.937879380234634, - "heading": 0.058451527477084785, - "angularVelocity": -0.3515136361983664, - "velocityX": 2.1454742294918456, - "velocityY": -0.9099580616042243, - "timestamp": 0.997092129147366 - }, - { - "x": 7.5495767925597805, - "y": 5.877391789669617, - "heading": 0.035072287662159383, - "angularVelocity": -0.2813690626752902, - "velocityX": 1.7163784566554727, - "velocityY": -0.7279679234865629, - "timestamp": 1.0801831399096464 - }, - { - "x": 7.656538486904284, - "y": 5.832026023472794, - "heading": 0.017533973815731525, - "angularVelocity": -0.21107354075405452, - "velocityX": 1.287283586554457, - "velocityY": -0.54597682445592, - "timestamp": 1.1632741506719269 - }, - { - "x": 7.7278462932659, - "y": 5.801782144275187, - "heading": 0.005843001619637481, - "angularVelocity": -0.14070080612620453, - "velocityX": 0.8581891796409112, - "velocityY": -0.36398497166117527, - "timestamp": 1.2463651614342073 - }, - { - "x": 7.763500213623047, - "y": 5.786660194396973, - "heading": 0, - "angularVelocity": -0.07032050237484791, - "velocityX": 0.42909479653763305, - "velocityY": -0.18199260954325794, - "timestamp": 1.3294561721964877 - }, - { - "x": 7.763500213623047, - "y": 5.786660194396973, - "heading": 0, + "x": 5.19636344909668, + "y": 6.875411510467529, + "heading": 0.4089083102765838, "angularVelocity": 0, "velocityX": 0, - "velocityY": -4.001772889855341e-38, - "timestamp": 1.4125471829587681 + "velocityY": -2.0717354478922107e-39, + "timestamp": 1.3737071552805276 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Skip.4.traj b/src/main/deploy/choreo/4NT-Skip.4.traj index 8ec2fa8..681a48f 100644 --- a/src/main/deploy/choreo/4NT-Skip.4.traj +++ b/src/main/deploy/choreo/4NT-Skip.4.traj @@ -1,211 +1,166 @@ { "samples": [ { - "x": 7.763500213623047, - "y": 5.786660194396973, - "heading": 0, + "x": 5.19636344909668, + "y": 6.875411510467529, + "heading": 0.4089083102765838, "angularVelocity": 0, "velocityX": 0, - "velocityY": -4.001772889855341e-38, + "velocityY": -2.0717354478922107e-39, "timestamp": 0 }, { - "x": 7.714022075284393, - "y": 5.791512130406288, - "heading": 0.0034123876701157535, - "angularVelocity": 0.03625557509808546, - "velocityX": -0.5256900837968682, - "velocityY": 0.0515503358242044, - "timestamp": 0.09412035696258858 - }, - { - "x": 7.615065798767571, - "y": 5.801215987589317, - "heading": 0.010236903417415826, - "angularVelocity": 0.07250839210068752, - "velocityX": -1.0513801658886168, - "velocityY": 0.103100514024672, - "timestamp": 0.18824071392517716 - }, - { - "x": 7.466631382770048, - "y": 5.8157717439660175, - "heading": 0.020472681304287838, - "angularVelocity": 0.10875200878106045, - "velocityX": -1.5770702618193642, - "velocityY": 0.15465045869391852, - "timestamp": 0.28236107088776574 - }, - { - "x": 7.268718824587655, - "y": 5.83517937052045, - "heading": 0.03411811725863541, - "angularVelocity": 0.14497858268611724, - "velocityX": -2.1027603864811066, - "velocityY": 0.20620009507770087, - "timestamp": 0.3764814278503543 - }, - { - "x": 7.021328120470755, - "y": 5.8594388313479895, - "heading": 0.05117069065282414, - "angularVelocity": 0.1811783756936537, - "velocityX": -2.6284505509815856, - "velocityY": 0.25774934998581056, - "timestamp": 0.4706017848129429 - }, - { - "x": 6.724459267218483, - "y": 5.888550083771315, - "heading": 0.07162673839699378, - "angularVelocity": 0.21733924949201522, - "velocityX": -3.1541407494902667, - "velocityY": 0.3092981514604363, - "timestamp": 0.5647221417755315 - }, - { - "x": 6.378112271590919, - "y": 5.922513077815848, - "heading": 0.09548116624563086, - "angularVelocity": 0.25344599849019656, - "velocityX": -3.679830876175178, - "velocityY": 0.36084642197046873, - "timestamp": 0.6588424987381201 - }, - { - "x": 5.995099579809893, - "y": 5.9600738684572026, - "heading": 0.09548116733610806, - "angularVelocity": 1.1585986600028837e-8, - "velocityX": -4.06939268125882, - "velocityY": 0.3990719101956213, - "timestamp": 0.7529628557007086 - }, - { - "x": 5.61208688806035, - "y": 5.997634659420377, - "heading": 0.09548116842656575, - "angularVelocity": 1.1585779406391748e-8, - "velocityX": -4.069392680924314, - "velocityY": 0.3990719136148622, - "timestamp": 0.8470832126632972 - }, - { - "x": 5.22907419631129, - "y": 6.035195450388469, - "heading": 0.09548116951702341, - "angularVelocity": 1.158577911551983e-8, - "velocityX": -4.069392680919191, - "velocityY": 0.3990719136671072, - "timestamp": 0.9412035696258858 - }, - { - "x": 4.846061504562215, - "y": 6.072756241356416, - "heading": 0.09548117060748103, - "angularVelocity": 1.1585778663343773e-8, - "velocityX": -4.06939268091934, - "velocityY": 0.3990719136655779, - "timestamp": 1.0353239265884744 - }, - { - "x": 4.463048812813157, - "y": 6.110317032324537, - "heading": 0.0954811716979387, - "angularVelocity": 1.1585779124921689e-8, - "velocityX": -4.069392680919158, - "velocityY": 0.39907191366742895, - "timestamp": 1.129444283551063 - }, - { - "x": 4.080036121063514, - "y": 6.147877823286686, - "heading": 0.09548117278839631, - "angularVelocity": 1.1585778495719892e-8, - "velocityX": -4.0693926809253815, - "velocityY": 0.3990719136039721, - "timestamp": 1.2235646405136515 - }, - { - "x": 3.697023429276966, - "y": 6.185438613871755, - "heading": 0.0954811738788733, - "angularVelocity": 1.1585984482237455e-8, - "velocityX": -4.069392681317477, - "velocityY": 0.39907190959759625, - "timestamp": 1.3176849974762401 - }, - { - "x": 3.3506759158353345, - "y": 6.219399783259486, - "heading": 0.1191237323968371, - "angularVelocity": 0.25119495166557093, - "velocityX": -3.679836377791235, - "velocityY": 0.36082703555013734, - "timestamp": 1.4118053544388287 - }, - { - "x": 3.0538065721545458, - "y": 6.248509296515882, - "heading": 0.13937914766365392, - "angularVelocity": 0.21520759079640947, - "velocityX": -3.1541459601432313, - "velocityY": 0.3092796733438538, - "timestamp": 1.5059257114014173 - }, - { - "x": 2.806415429883936, - "y": 6.272767183419743, - "heading": 0.15625241023006078, - "angularVelocity": 0.17927325300214964, - "velocityX": -2.6284552062307136, - "velocityY": 0.25773262752820747, - "timestamp": 1.6000460683640059 - }, - { - "x": 2.608502504943331, - "y": 6.2921734683030435, - "heading": 0.16974760497245162, - "angularVelocity": 0.14338231576995536, - "velocityX": -2.102764283174911, - "velocityY": 0.2061858402323502, - "timestamp": 1.6941664253265944 - }, - { - "x": 2.460067807083176, - "y": 6.306728169431489, - "heading": 0.17986771256728493, - "angularVelocity": 0.10752304731330184, - "velocityX": -1.5770732565236112, - "velocityY": 0.1546392470040303, - "timestamp": 1.788286782289183 - }, - { - "x": 2.361111341639132, - "y": 6.3164312990743445, - "heading": 0.1866144786841838, - "angularVelocity": 0.0716823260623689, - "velocityX": -1.051382173182552, - "velocityY": 0.10309278413290339, - "timestamp": 1.8824071392517716 - }, - { - "x": 2.3116331100463867, - "y": 6.321282863616943, - "heading": 0.18998832965561516, - "angularVelocity": 0.035846134463476516, - "velocityX": -0.5256910745930495, - "velocityY": 0.05154638910397498, - "timestamp": 1.9765274962143602 - }, - { - "x": 2.3116331100463867, - "y": 6.321282863616943, - "heading": 0.18998832965561516, + "x": 5.232018908472479, + "y": 6.860290267658786, + "heading": 0.4034159167471209, + "angularVelocity": -0.06610093533675257, + "velocityX": 0.4291133186212821, + "velocityY": -0.181984099964854, + "timestamp": 0.08309101076228087 + }, + { + "x": 5.3033298450653605, + "y": 6.830047764220078, + "heading": 0.39242803594978326, + "angularVelocity": -0.13223910380357973, + "velocityX": 0.858226851962353, + "velocityY": -0.36396841440803684, + "timestamp": 0.16618202152456174 + }, + { + "x": 5.410296246847904, + "y": 6.784683995320563, + "heading": 0.37593441821627777, + "angularVelocity": -0.19850062698951831, + "velocityX": 1.2873402405534362, + "velocityY": -0.5459527869903824, + "timestamp": 0.24927303228684217 + }, + { + "x": 5.552918069928713, + "y": 6.72419896473783, + "heading": 0.3539176492261319, + "angularVelocity": -0.2649717314564233, + "velocityX": 1.7164531009117598, + "velocityY": -0.7279371141094613, + "timestamp": 0.3323640430491226 + }, + { + "x": 5.731195235870137, + "y": 6.64859267901311, + "heading": 0.3263531755047263, + "angularVelocity": -0.33173833689743104, + "velocityX": 2.1455650172732543, + "velocityY": -0.9099213625048691, + "timestamp": 0.415455053811403 + }, + { + "x": 5.945127628296537, + "y": 6.557865140131203, + "heading": 0.29320941514251314, + "angularVelocity": -0.39888503050030044, + "velocityX": 2.57467553305435, + "velocityY": -1.0919055870131655, + "timestamp": 0.49854606457368345 + }, + { + "x": 6.194715088245195, + "y": 6.452016336978496, + "heading": 0.25444802631157254, + "angularVelocity": -0.4664931678570496, + "velocityX": 3.003784135719744, + "velocityY": -1.2738899452738088, + "timestamp": 0.5816370753359639 + }, + { + "x": 6.47995740296407, + "y": 6.331046237930042, + "heading": 0.2100244721238566, + "angularVelocity": -0.5346373064928706, + "velocityX": 3.4328901779151324, + "velocityY": -1.455874684140549, + "timestamp": 0.6647280860982443 + }, + { + "x": 6.765189822342828, + "y": 6.210072243447842, + "heading": 0.16347583941488503, + "angularVelocity": -0.5602126184521319, + "velocityX": 3.4327710875342943, + "velocityY": -1.4559215656709357, + "timestamp": 0.7478190968605247 + }, + { + "x": 7.0147678146535055, + "y": 6.104219702621471, + "heading": 0.1226760205647685, + "angularVelocity": -0.49102566542177156, + "velocityX": 3.0036701927324945, + "velocityY": -1.2739349281621999, + "timestamp": 0.8309101076228051 + }, + { + "x": 7.22869154945277, + "y": 6.013488715324614, + "heading": 0.0876591508055317, + "angularVelocity": -0.421427895003207, + "velocityX": 2.574571338544547, + "velocityY": -1.091947088674045, + "timestamp": 0.9140011183850856 + }, + { + "x": 7.406961171745674, + "y": 5.937879380234634, + "heading": 0.058451527477084785, + "angularVelocity": -0.3515136361983664, + "velocityX": 2.1454742294918456, + "velocityY": -0.9099580616042243, + "timestamp": 0.997092129147366 + }, + { + "x": 7.5495767925597805, + "y": 5.877391789669617, + "heading": 0.035072287662159383, + "angularVelocity": -0.2813690626752902, + "velocityX": 1.7163784566554727, + "velocityY": -0.7279679234865629, + "timestamp": 1.0801831399096464 + }, + { + "x": 7.656538486904284, + "y": 5.832026023472794, + "heading": 0.017533973815731525, + "angularVelocity": -0.21107354075405452, + "velocityX": 1.287283586554457, + "velocityY": -0.54597682445592, + "timestamp": 1.1632741506719269 + }, + { + "x": 7.7278462932659, + "y": 5.801782144275187, + "heading": 0.005843001619637481, + "angularVelocity": -0.14070080612620453, + "velocityX": 0.8581891796409112, + "velocityY": -0.36398497166117527, + "timestamp": 1.2463651614342073 + }, + { + "x": 7.763500213623047, + "y": 5.786660194396973, + "heading": 0, + "angularVelocity": -0.07032050237484791, + "velocityX": 0.42909479653763305, + "velocityY": -0.18199260954325794, + "timestamp": 1.3294561721964877 + }, + { + "x": 7.763500213623047, + "y": 5.786660194396973, + "heading": 0, "angularVelocity": 0, - "velocityX": 5.252647651099752e-37, - "velocityY": 0, - "timestamp": 2.0706478531769488 + "velocityX": 0, + "velocityY": -4.001772889855341e-38, + "timestamp": 1.4125471829587681 } ] } \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Skip.5.traj b/src/main/deploy/choreo/4NT-Skip.5.traj new file mode 100644 index 0000000..8ec2fa8 --- /dev/null +++ b/src/main/deploy/choreo/4NT-Skip.5.traj @@ -0,0 +1,211 @@ +{ + "samples": [ + { + "x": 7.763500213623047, + "y": 5.786660194396973, + "heading": 0, + "angularVelocity": 0, + "velocityX": 0, + "velocityY": -4.001772889855341e-38, + "timestamp": 0 + }, + { + "x": 7.714022075284393, + "y": 5.791512130406288, + "heading": 0.0034123876701157535, + "angularVelocity": 0.03625557509808546, + "velocityX": -0.5256900837968682, + "velocityY": 0.0515503358242044, + "timestamp": 0.09412035696258858 + }, + { + "x": 7.615065798767571, + "y": 5.801215987589317, + "heading": 0.010236903417415826, + "angularVelocity": 0.07250839210068752, + "velocityX": -1.0513801658886168, + "velocityY": 0.103100514024672, + "timestamp": 0.18824071392517716 + }, + { + "x": 7.466631382770048, + "y": 5.8157717439660175, + "heading": 0.020472681304287838, + "angularVelocity": 0.10875200878106045, + "velocityX": -1.5770702618193642, + "velocityY": 0.15465045869391852, + "timestamp": 0.28236107088776574 + }, + { + "x": 7.268718824587655, + "y": 5.83517937052045, + "heading": 0.03411811725863541, + "angularVelocity": 0.14497858268611724, + "velocityX": -2.1027603864811066, + "velocityY": 0.20620009507770087, + "timestamp": 0.3764814278503543 + }, + { + "x": 7.021328120470755, + "y": 5.8594388313479895, + "heading": 0.05117069065282414, + "angularVelocity": 0.1811783756936537, + "velocityX": -2.6284505509815856, + "velocityY": 0.25774934998581056, + "timestamp": 0.4706017848129429 + }, + { + "x": 6.724459267218483, + "y": 5.888550083771315, + "heading": 0.07162673839699378, + "angularVelocity": 0.21733924949201522, + "velocityX": -3.1541407494902667, + "velocityY": 0.3092981514604363, + "timestamp": 0.5647221417755315 + }, + { + "x": 6.378112271590919, + "y": 5.922513077815848, + "heading": 0.09548116624563086, + "angularVelocity": 0.25344599849019656, + "velocityX": -3.679830876175178, + "velocityY": 0.36084642197046873, + "timestamp": 0.6588424987381201 + }, + { + "x": 5.995099579809893, + "y": 5.9600738684572026, + "heading": 0.09548116733610806, + "angularVelocity": 1.1585986600028837e-8, + "velocityX": -4.06939268125882, + "velocityY": 0.3990719101956213, + "timestamp": 0.7529628557007086 + }, + { + "x": 5.61208688806035, + "y": 5.997634659420377, + "heading": 0.09548116842656575, + "angularVelocity": 1.1585779406391748e-8, + "velocityX": -4.069392680924314, + "velocityY": 0.3990719136148622, + "timestamp": 0.8470832126632972 + }, + { + "x": 5.22907419631129, + "y": 6.035195450388469, + "heading": 0.09548116951702341, + "angularVelocity": 1.158577911551983e-8, + "velocityX": -4.069392680919191, + "velocityY": 0.3990719136671072, + "timestamp": 0.9412035696258858 + }, + { + "x": 4.846061504562215, + "y": 6.072756241356416, + "heading": 0.09548117060748103, + "angularVelocity": 1.1585778663343773e-8, + "velocityX": -4.06939268091934, + "velocityY": 0.3990719136655779, + "timestamp": 1.0353239265884744 + }, + { + "x": 4.463048812813157, + "y": 6.110317032324537, + "heading": 0.0954811716979387, + "angularVelocity": 1.1585779124921689e-8, + "velocityX": -4.069392680919158, + "velocityY": 0.39907191366742895, + "timestamp": 1.129444283551063 + }, + { + "x": 4.080036121063514, + "y": 6.147877823286686, + "heading": 0.09548117278839631, + "angularVelocity": 1.1585778495719892e-8, + "velocityX": -4.0693926809253815, + "velocityY": 0.3990719136039721, + "timestamp": 1.2235646405136515 + }, + { + "x": 3.697023429276966, + "y": 6.185438613871755, + "heading": 0.0954811738788733, + "angularVelocity": 1.1585984482237455e-8, + "velocityX": -4.069392681317477, + "velocityY": 0.39907190959759625, + "timestamp": 1.3176849974762401 + }, + { + "x": 3.3506759158353345, + "y": 6.219399783259486, + "heading": 0.1191237323968371, + "angularVelocity": 0.25119495166557093, + "velocityX": -3.679836377791235, + "velocityY": 0.36082703555013734, + "timestamp": 1.4118053544388287 + }, + { + "x": 3.0538065721545458, + "y": 6.248509296515882, + "heading": 0.13937914766365392, + "angularVelocity": 0.21520759079640947, + "velocityX": -3.1541459601432313, + "velocityY": 0.3092796733438538, + "timestamp": 1.5059257114014173 + }, + { + "x": 2.806415429883936, + "y": 6.272767183419743, + "heading": 0.15625241023006078, + "angularVelocity": 0.17927325300214964, + "velocityX": -2.6284552062307136, + "velocityY": 0.25773262752820747, + "timestamp": 1.6000460683640059 + }, + { + "x": 2.608502504943331, + "y": 6.2921734683030435, + "heading": 0.16974760497245162, + "angularVelocity": 0.14338231576995536, + "velocityX": -2.102764283174911, + "velocityY": 0.2061858402323502, + "timestamp": 1.6941664253265944 + }, + { + "x": 2.460067807083176, + "y": 6.306728169431489, + "heading": 0.17986771256728493, + "angularVelocity": 0.10752304731330184, + "velocityX": -1.5770732565236112, + "velocityY": 0.1546392470040303, + "timestamp": 1.788286782289183 + }, + { + "x": 2.361111341639132, + "y": 6.3164312990743445, + "heading": 0.1866144786841838, + "angularVelocity": 0.0716823260623689, + "velocityX": -1.051382173182552, + "velocityY": 0.10309278413290339, + "timestamp": 1.8824071392517716 + }, + { + "x": 2.3116331100463867, + "y": 6.321282863616943, + "heading": 0.18998832965561516, + "angularVelocity": 0.035846134463476516, + "velocityX": -0.5256910745930495, + "velocityY": 0.05154638910397498, + "timestamp": 1.9765274962143602 + }, + { + "x": 2.3116331100463867, + "y": 6.321282863616943, + "heading": 0.18998832965561516, + "angularVelocity": 0, + "velocityX": 5.252647651099752e-37, + "velocityY": 0, + "timestamp": 2.0706478531769488 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Sweep.1.traj b/src/main/deploy/choreo/4NT-Sweep.1.traj new file mode 100644 index 0000000..2dfe056 --- /dev/null +++ b/src/main/deploy/choreo/4NT-Sweep.1.traj @@ -0,0 +1,139 @@ +{ + "samples": [ + { + "x": 0.7823778390884399, + "y": 6.68265962600708, + "heading": 1.0516504568237233, + "angularVelocity": -1.305568560622299e-21, + "velocityX": 1.822131686996459e-19, + "velocityY": 9.010541927457129e-21, + "timestamp": 0 + }, + { + "x": 0.8145359389114578, + "y": 6.684261194082208, + "heading": 1.040561037250096, + "angularVelocity": -0.14616328569842307, + "velocityX": 0.42385748872992884, + "velocityY": 0.021109351177139767, + "timestamp": 0.07587007585822825 + }, + { + "x": 0.8788523386671915, + "y": 6.687464759358471, + "heading": 1.0183708391377924, + "angularVelocity": -0.29247628740702164, + "velocityX": 0.847717614991134, + "velocityY": 0.04222435841831154, + "timestamp": 0.1517401517164565 + }, + { + "x": 0.9753269412813598, + "y": 6.692270939975434, + "heading": 0.9850418553098709, + "angularVelocity": -0.4392902399386075, + "velocityX": 1.271576461771859, + "velocityY": 0.06334751300295363, + "timestamp": 0.22761022757468474 + }, + { + "x": 1.1039593198138598, + "y": 6.698680457702348, + "heading": 0.9405064375600677, + "angularVelocity": -0.5869958247178071, + "velocityX": 1.6954296812996996, + "velocityY": 0.08448018081451894, + "timestamp": 0.303480303432913 + }, + { + "x": 1.2647486754140431, + "y": 6.7066940112498585, + "heading": 0.8846637199397575, + "angularVelocity": -0.7360308657745194, + "velocityX": 2.119272371634848, + "velocityY": 0.10562205793077338, + "timestamp": 0.37935037929114124 + }, + { + "x": 1.4576937911793324, + "y": 6.71631209605501, + "heading": 0.8173761180571837, + "angularVelocity": -0.8868793278697673, + "velocityX": 2.5430990226743524, + "velocityY": 0.1267704651188646, + "timestamp": 0.4552204551493695 + }, + { + "x": 1.6827929759984845, + "y": 6.727534757965461, + "heading": 0.7384664173131257, + "angularVelocity": -1.040063554061942, + "velocityX": 2.9669033841454855, + "velocityY": 0.147919476598685, + "timestamp": 0.5310905310075977 + }, + { + "x": 1.875690822215849, + "y": 6.7371431845238074, + "heading": 0.6658970717001427, + "angularVelocity": -0.9564949657963566, + "velocityX": 2.542475989846324, + "velocityY": 0.1266431653014338, + "timestamp": 0.606960606865826 + }, + { + "x": 2.0364373939926854, + "y": 6.745148361535974, + "heading": 0.6052481909952064, + "angularVelocity": -0.7993781476937682, + "velocityX": 2.1187084625723753, + "velocityY": 0.10551165161776474, + "timestamp": 0.6828306827240542 + }, + { + "x": 2.165033879772669, + "y": 6.751551231704223, + "heading": 0.5566362685774989, + "angularVelocity": -0.6407258971052618, + "velocityX": 1.6949565994935976, + "velocityY": 0.08439256315249083, + "timestamp": 0.7587007585822825 + }, + { + "x": 2.2614810401187784, + "y": 6.756352675949364, + "heading": 0.5201406088325774, + "angularVelocity": -0.4810283808483043, + "velocityX": 1.2712147609596647, + "velocityY": 0.06328508559965212, + "timestamp": 0.8345708344405107 + }, + { + "x": 2.32577924734246, + "y": 6.759553371141778, + "heading": 0.4958065317915587, + "angularVelocity": -0.32073352722738296, + "velocityX": 0.8474778296495941, + "velocityY": 0.04218652948753564, + "timestamp": 0.910440910298739 + }, + { + "x": 2.357928514480591, + "y": 6.761153697967529, + "heading": 0.48364772207543033, + "angularVelocity": -0.1602583044578536, + "velocityX": 0.4237410701711366, + "velocityY": 0.021092990980281293, + "timestamp": 0.9863109861569672 + }, + { + "x": 2.357928514480591, + "y": 6.761153697967529, + "heading": 0.48364772207543033, + "angularVelocity": 1.1064970967631094e-21, + "velocityX": -1.8225634104070948e-19, + "velocityY": -9.012713579557001e-21, + "timestamp": 1.0621810620151955 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Sweep.2.traj b/src/main/deploy/choreo/4NT-Sweep.2.traj new file mode 100644 index 0000000..ccb846b --- /dev/null +++ b/src/main/deploy/choreo/4NT-Sweep.2.traj @@ -0,0 +1,157 @@ +{ + "samples": [ + { + "x": 2.357928514480591, + "y": 6.761153697967529, + "heading": 0.48364772207543033, + "angularVelocity": 1.1064970967631094e-21, + "velocityX": -1.8225634104070948e-19, + "velocityY": -9.012713579557001e-21, + "timestamp": 0 + }, + { + "x": 2.352635709829095, + "y": 6.749572893800152, + "heading": 0.46948048681204285, + "angularVelocity": -0.29258597212150655, + "velocityX": -0.10930858176748662, + "velocityY": -0.23917022497801835, + "timestamp": 0.048420760437222876 + }, + { + "x": 2.3424174496797017, + "y": 6.726214578494324, + "heading": 0.4417934028197234, + "angularVelocity": -0.5718019242637775, + "velocityX": -0.2110305591470682, + "velocityY": -0.4824029010471924, + "timestamp": 0.09684152087444575 + }, + { + "x": 2.327759577018962, + "y": 6.690845088074086, + "heading": 0.40137593071489563, + "angularVelocity": -0.8347137000714533, + "velocityX": -0.30271876212567034, + "velocityY": -0.7304612753055373, + "timestamp": 0.14526228131166863 + }, + { + "x": 2.3093233873244357, + "y": 6.64319111632961, + "heading": 0.3491865327700304, + "angularVelocity": -1.0778310268903832, + "velocityX": -0.38074969347969373, + "velocityY": -0.9841640510016261, + "timestamp": 0.1936830417488915 + }, + { + "x": 2.288046078154732, + "y": 6.582940438286813, + "heading": 0.2865195619339567, + "angularVelocity": -1.2942169901961953, + "velocityX": -0.4394253410639758, + "velocityY": -1.2443149900735475, + "timestamp": 0.24210380218611438 + }, + { + "x": 2.2653395566100536, + "y": 6.509769721522354, + "heading": 0.21545532238914708, + "angularVelocity": -1.4676398904751573, + "velocityX": -0.4689418617065527, + "velocityY": -1.5111434868794307, + "timestamp": 0.29052456262333726 + }, + { + "x": 2.243480972819474, + "y": 6.423521182774951, + "heading": 0.13956927847959602, + "angularVelocity": -1.5672212337089715, + "velocityX": -0.4514299980670268, + "velocityY": -1.7812305707016642, + "timestamp": 0.33894532306056013 + }, + { + "x": 2.2261544695623017, + "y": 6.324920657670576, + "heading": 0.06468345124510128, + "angularVelocity": -1.5465644603327438, + "velocityX": -0.35783211789157504, + "velocityY": -2.0363274804865874, + "timestamp": 0.387366083497783 + }, + { + "x": 2.2180731296539307, + "y": 6.21727180480957, + "heading": -6.945757678881964e-28, + "angularVelocity": -1.335861945600024, + "velocityX": -0.16689824437698478, + "velocityY": -2.223196246588727, + "timestamp": 0.4357868439350059 + }, + { + "x": 2.2352227511494878, + "y": 6.037365041726398, + "heading": -0.04735082051046012, + "angularVelocity": -0.6135224632970496, + "velocityX": 0.2222068785955233, + "velocityY": -2.3310438818310217, + "timestamp": 0.5129654695651065 + }, + { + "x": 2.2764918846129354, + "y": 5.877967183230441, + "heading": -0.04961558249433564, + "angularVelocity": -0.029344419719651398, + "velocityX": 0.5347223162698054, + "velocityY": -2.0653109224814865, + "timestamp": 0.590144095195207 + }, + { + "x": 2.319236637610397, + "y": 5.75175219753006, + "heading": -0.03647975719615842, + "angularVelocity": 0.1702003008078197, + "velocityX": 0.5538418525658589, + "velocityY": -1.6353619239775097, + "timestamp": 0.6673227208253076 + }, + { + "x": 2.3550484780260685, + "y": 5.658236249707955, + "heading": -0.020643050195192494, + "angularVelocity": 0.20519550421728966, + "velocityX": 0.4640124143608037, + "velocityY": -1.2116819528544773, + "timestamp": 0.7445013464554082 + }, + { + "x": 2.3804879836257373, + "y": 5.596496310620883, + "heading": -0.007498878272735541, + "angularVelocity": 0.17030844764525793, + "velocityX": 0.32961853611639047, + "velocityY": -0.7999616290522918, + "timestamp": 0.8216799720855088 + }, + { + "x": 2.3937551975250244, + "y": 5.565863609313965, + "heading": 2.4640341825929873e-23, + "angularVelocity": 0.09716263034633371, + "velocityX": 0.1719026970352361, + "velocityY": -0.3969065405975678, + "timestamp": 0.8988585977156094 + }, + { + "x": 2.3937551975250244, + "y": 5.565863609313965, + "heading": 2.4913308967565905e-23, + "angularVelocity": 3.537030839600793e-24, + "velocityX": -2.1338399459553326e-23, + "velocityY": -4.571258849613942e-23, + "timestamp": 0.9760372233457102 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Sweep.3.traj b/src/main/deploy/choreo/4NT-Sweep.3.traj new file mode 100644 index 0000000..5c8a9ae --- /dev/null +++ b/src/main/deploy/choreo/4NT-Sweep.3.traj @@ -0,0 +1,175 @@ +{ + "samples": [ + { + "x": 2.3937551975250244, + "y": 5.565863609313965, + "heading": 2.4913308967565905e-23, + "angularVelocity": 3.537030839600793e-24, + "velocityX": -2.1338399459553326e-23, + "velocityY": -4.571258849613942e-23, + "timestamp": 0 + }, + { + "x": 2.3837015705887254, + "y": 5.555193042220565, + "heading": -0.00946464391198462, + "angularVelocity": -0.18411639657129, + "velocityX": -0.1955739255694025, + "velocityY": -0.20757530667595975, + "timestamp": 0.05140576335535618 + }, + { + "x": 2.3641582098837484, + "y": 5.5333437080108325, + "heading": -0.028253803373095267, + "angularVelocity": -0.3655068660536286, + "velocityX": -0.38017839692173266, + "velocityY": -0.42503666483256275, + "timestamp": 0.10281152671071236 + }, + { + "x": 2.3359331924350997, + "y": 5.499671258409721, + "heading": -0.056164170438548724, + "angularVelocity": -0.5429423715102839, + "velocityX": -0.549063287972907, + "velocityY": -0.6550325761790774, + "timestamp": 0.15421729006606855 + }, + { + "x": 2.300248237017326, + "y": 5.453359822013785, + "heading": -0.09289041649224676, + "angularVelocity": -0.7144382975079676, + "velocityX": -0.6941819961137773, + "velocityY": -0.9008996924293653, + "timestamp": 0.20562305342142473 + }, + { + "x": 2.25906191216666, + "y": 5.393424337289651, + "heading": -0.13788233192980848, + "angularVelocity": -0.8752309566252937, + "velocityX": -0.8012005301031048, + "velocityY": -1.1659292813106188, + "timestamp": 0.2570288167767809 + }, + { + "x": 2.215650672687085, + "y": 5.318928923927703, + "heading": -0.18994798908185376, + "angularVelocity": -1.0128369613369528, + "velocityX": -0.8444819538907223, + "velocityY": -1.44916461695119, + "timestamp": 0.3084345801321371 + }, + { + "x": 2.1751906895479802, + "y": 5.229944253664982, + "heading": -0.24676651261993965, + "angularVelocity": -1.105294811893225, + "velocityX": -0.7870709527142773, + "velocityY": -1.7310251702244053, + "timestamp": 0.3598403434874933 + }, + { + "x": 2.143630529202551, + "y": 5.129096717824508, + "heading": -0.3045822393435162, + "angularVelocity": -1.1246934769533508, + "velocityX": -0.6139420618513388, + "velocityY": -1.9617943447962864, + "timestamp": 0.41124610684284946 + }, + { + "x": 2.1248362064361572, + "y": 5.020731449127197, + "heading": -0.35924157629701275, + "angularVelocity": -1.063292000464021, + "velocityX": -0.3656073082014722, + "velocityY": -2.1080373410313484, + "timestamp": 0.46265187019820564 + }, + { + "x": 2.1236034975576716, + "y": 4.879581187775217, + "heading": -0.41834324922038263, + "angularVelocity": -0.9214808419098464, + "velocityX": -0.019219720170182603, + "velocityY": -2.200737394270732, + "timestamp": 0.5267895822757316 + }, + { + "x": 2.145285525498013, + "y": 4.739114283134644, + "heading": -0.4654704427453386, + "angularVelocity": -0.734781332205791, + "velocityX": 0.33805427786593684, + "velocityY": -2.190082871537182, + "timestamp": 0.5909272943532575 + }, + { + "x": 2.1854463786837415, + "y": 4.612083049790391, + "heading": -0.49888391882293226, + "angularVelocity": -0.520964577551588, + "velocityX": 0.6261659776261528, + "velocityY": -1.9806012598439038, + "timestamp": 0.6550650064307835 + }, + { + "x": 2.23085672934491, + "y": 4.507371852155421, + "heading": -0.5212416835862062, + "angularVelocity": -0.3485899954811167, + "velocityX": 0.7080132606894286, + "velocityY": -1.6325995150622288, + "timestamp": 0.7192027185083094 + }, + { + "x": 2.2725752093899105, + "y": 4.425381093870058, + "heading": -0.5360315135324354, + "angularVelocity": -0.23059491003283958, + "velocityX": 0.6504516405975544, + "velocityY": -1.278354896511699, + "timestamp": 0.7833404305858354 + }, + { + "x": 2.306393030453338, + "y": 4.365049187904869, + "heading": -0.5454513613849756, + "angularVelocity": -0.1468690969386946, + "velocityX": 0.5272689026161482, + "velocityY": -0.9406619601937548, + "timestamp": 0.8474781426633613 + }, + { + "x": 2.330102947478555, + "y": 4.325455758257715, + "heading": -0.5508966378561475, + "angularVelocity": -0.08489976169698503, + "velocityX": 0.36967201132085403, + "velocityY": -0.6173190212849581, + "timestamp": 0.9116158547408872 + }, + { + "x": 2.3423891067504883, + "y": 4.305914878845215, + "heading": -0.5532943925729576, + "angularVelocity": -0.037384475360018043, + "velocityX": 0.19155905120349095, + "velocityY": -0.30467066534709186, + "timestamp": 0.9757535668184132 + }, + { + "x": 2.3423891067504883, + "y": 4.305914878845215, + "heading": -0.5532943925729576, + "angularVelocity": -2.509202302365357e-24, + "velocityX": 1.871020058653073e-23, + "velocityY": -1.3414864759418838e-22, + "timestamp": 1.0398912788959391 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT-Sweep.traj b/src/main/deploy/choreo/4NT-Sweep.traj new file mode 100644 index 0000000..06dcb7c --- /dev/null +++ b/src/main/deploy/choreo/4NT-Sweep.traj @@ -0,0 +1,445 @@ +{ + "samples": [ + { + "x": 0.7823778390884399, + "y": 6.68265962600708, + "heading": 1.0516504568237233, + "angularVelocity": -1.305568560622299e-21, + "velocityX": 1.822131686996459e-19, + "velocityY": 9.010541927457129e-21, + "timestamp": 0 + }, + { + "x": 0.8145359389114578, + "y": 6.684261194082208, + "heading": 1.040561037250096, + "angularVelocity": -0.14616328569842307, + "velocityX": 0.42385748872992884, + "velocityY": 0.021109351177139767, + "timestamp": 0.07587007585822825 + }, + { + "x": 0.8788523386671915, + "y": 6.687464759358471, + "heading": 1.0183708391377924, + "angularVelocity": -0.29247628740702164, + "velocityX": 0.847717614991134, + "velocityY": 0.04222435841831154, + "timestamp": 0.1517401517164565 + }, + { + "x": 0.9753269412813598, + "y": 6.692270939975434, + "heading": 0.9850418553098709, + "angularVelocity": -0.4392902399386075, + "velocityX": 1.271576461771859, + "velocityY": 0.06334751300295363, + "timestamp": 0.22761022757468474 + }, + { + "x": 1.1039593198138598, + "y": 6.698680457702348, + "heading": 0.9405064375600677, + "angularVelocity": -0.5869958247178071, + "velocityX": 1.6954296812996996, + "velocityY": 0.08448018081451894, + "timestamp": 0.303480303432913 + }, + { + "x": 1.2647486754140431, + "y": 6.7066940112498585, + "heading": 0.8846637199397575, + "angularVelocity": -0.7360308657745194, + "velocityX": 2.119272371634848, + "velocityY": 0.10562205793077338, + "timestamp": 0.37935037929114124 + }, + { + "x": 1.4576937911793324, + "y": 6.71631209605501, + "heading": 0.8173761180571837, + "angularVelocity": -0.8868793278697673, + "velocityX": 2.5430990226743524, + "velocityY": 0.1267704651188646, + "timestamp": 0.4552204551493695 + }, + { + "x": 1.6827929759984845, + "y": 6.727534757965461, + "heading": 0.7384664173131257, + "angularVelocity": -1.040063554061942, + "velocityX": 2.9669033841454855, + "velocityY": 0.147919476598685, + "timestamp": 0.5310905310075977 + }, + { + "x": 1.875690822215849, + "y": 6.7371431845238074, + "heading": 0.6658970717001427, + "angularVelocity": -0.9564949657963566, + "velocityX": 2.542475989846324, + "velocityY": 0.1266431653014338, + "timestamp": 0.606960606865826 + }, + { + "x": 2.0364373939926854, + "y": 6.745148361535974, + "heading": 0.6052481909952064, + "angularVelocity": -0.7993781476937682, + "velocityX": 2.1187084625723753, + "velocityY": 0.10551165161776474, + "timestamp": 0.6828306827240542 + }, + { + "x": 2.165033879772669, + "y": 6.751551231704223, + "heading": 0.5566362685774989, + "angularVelocity": -0.6407258971052618, + "velocityX": 1.6949565994935976, + "velocityY": 0.08439256315249083, + "timestamp": 0.7587007585822825 + }, + { + "x": 2.2614810401187784, + "y": 6.756352675949364, + "heading": 0.5201406088325774, + "angularVelocity": -0.4810283808483043, + "velocityX": 1.2712147609596647, + "velocityY": 0.06328508559965212, + "timestamp": 0.8345708344405107 + }, + { + "x": 2.32577924734246, + "y": 6.759553371141778, + "heading": 0.4958065317915587, + "angularVelocity": -0.32073352722738296, + "velocityX": 0.8474778296495941, + "velocityY": 0.04218652948753564, + "timestamp": 0.910440910298739 + }, + { + "x": 2.357928514480591, + "y": 6.761153697967529, + "heading": 0.48364772207543033, + "angularVelocity": -0.1602583044578536, + "velocityX": 0.4237410701711366, + "velocityY": 0.021092990980281293, + "timestamp": 0.9863109861569672 + }, + { + "x": 2.357928514480591, + "y": 6.761153697967529, + "heading": 0.48364772207543033, + "angularVelocity": 1.1064970967631094e-21, + "velocityX": -1.8225634104070948e-19, + "velocityY": -9.012713579557001e-21, + "timestamp": 1.0621810620151955 + }, + { + "x": 2.352635709829095, + "y": 6.749572893800152, + "heading": 0.46948048681204285, + "angularVelocity": -0.29258597212150655, + "velocityX": -0.10930858176748662, + "velocityY": -0.23917022497801835, + "timestamp": 1.1106018224524183 + }, + { + "x": 2.3424174496797017, + "y": 6.726214578494324, + "heading": 0.4417934028197234, + "angularVelocity": -0.5718019242637775, + "velocityX": -0.2110305591470682, + "velocityY": -0.4824029010471924, + "timestamp": 1.1590225828896412 + }, + { + "x": 2.327759577018962, + "y": 6.690845088074086, + "heading": 0.40137593071489563, + "angularVelocity": -0.8347137000714533, + "velocityX": -0.30271876212567034, + "velocityY": -0.7304612753055373, + "timestamp": 1.207443343326864 + }, + { + "x": 2.3093233873244357, + "y": 6.64319111632961, + "heading": 0.3491865327700304, + "angularVelocity": -1.0778310268903832, + "velocityX": -0.38074969347969373, + "velocityY": -0.9841640510016261, + "timestamp": 1.255864103764087 + }, + { + "x": 2.288046078154732, + "y": 6.582940438286813, + "heading": 0.2865195619339567, + "angularVelocity": -1.2942169901961953, + "velocityX": -0.4394253410639758, + "velocityY": -1.2443149900735475, + "timestamp": 1.3042848642013098 + }, + { + "x": 2.2653395566100536, + "y": 6.509769721522354, + "heading": 0.21545532238914708, + "angularVelocity": -1.4676398904751573, + "velocityX": -0.4689418617065527, + "velocityY": -1.5111434868794307, + "timestamp": 1.3527056246385327 + }, + { + "x": 2.243480972819474, + "y": 6.423521182774951, + "heading": 0.13956927847959602, + "angularVelocity": -1.5672212337089715, + "velocityX": -0.4514299980670268, + "velocityY": -1.7812305707016642, + "timestamp": 1.4011263850757556 + }, + { + "x": 2.2261544695623017, + "y": 6.324920657670576, + "heading": 0.06468345124510128, + "angularVelocity": -1.5465644603327438, + "velocityX": -0.35783211789157504, + "velocityY": -2.0363274804865874, + "timestamp": 1.4495471455129785 + }, + { + "x": 2.2180731296539307, + "y": 6.21727180480957, + "heading": -6.945757678881964e-28, + "angularVelocity": -1.335861945600024, + "velocityX": -0.16689824437698478, + "velocityY": -2.223196246588727, + "timestamp": 1.4979679059502014 + }, + { + "x": 2.2352227511494878, + "y": 6.037365041726398, + "heading": -0.04735082051046012, + "angularVelocity": -0.6135224632970496, + "velocityX": 0.2222068785955233, + "velocityY": -2.3310438818310217, + "timestamp": 1.575146531580302 + }, + { + "x": 2.2764918846129354, + "y": 5.877967183230441, + "heading": -0.04961558249433564, + "angularVelocity": -0.029344419719651398, + "velocityX": 0.5347223162698054, + "velocityY": -2.0653109224814865, + "timestamp": 1.6523251572104025 + }, + { + "x": 2.319236637610397, + "y": 5.75175219753006, + "heading": -0.03647975719615842, + "angularVelocity": 0.1702003008078197, + "velocityX": 0.5538418525658589, + "velocityY": -1.6353619239775097, + "timestamp": 1.729503782840503 + }, + { + "x": 2.3550484780260685, + "y": 5.658236249707955, + "heading": -0.020643050195192494, + "angularVelocity": 0.20519550421728966, + "velocityX": 0.4640124143608037, + "velocityY": -1.2116819528544773, + "timestamp": 1.8066824084706037 + }, + { + "x": 2.3804879836257373, + "y": 5.596496310620883, + "heading": -0.007498878272735541, + "angularVelocity": 0.17030844764525793, + "velocityX": 0.32961853611639047, + "velocityY": -0.7999616290522918, + "timestamp": 1.8838610341007043 + }, + { + "x": 2.3937551975250244, + "y": 5.565863609313965, + "heading": 2.4640341825929873e-23, + "angularVelocity": 0.09716263034633371, + "velocityX": 0.1719026970352361, + "velocityY": -0.3969065405975678, + "timestamp": 1.9610396597308049 + }, + { + "x": 2.3937551975250244, + "y": 5.565863609313965, + "heading": 2.4913308967565905e-23, + "angularVelocity": 3.537030839600793e-24, + "velocityX": -2.1338399459553326e-23, + "velocityY": -4.571258849613942e-23, + "timestamp": 2.0382182853609057 + }, + { + "x": 2.3837015705887254, + "y": 5.555193042220565, + "heading": -0.00946464391198462, + "angularVelocity": -0.18411639657129, + "velocityX": -0.1955739255694025, + "velocityY": -0.20757530667595975, + "timestamp": 2.089624048716262 + }, + { + "x": 2.3641582098837484, + "y": 5.5333437080108325, + "heading": -0.028253803373095267, + "angularVelocity": -0.3655068660536286, + "velocityX": -0.38017839692173266, + "velocityY": -0.42503666483256275, + "timestamp": 2.141029812071618 + }, + { + "x": 2.3359331924350997, + "y": 5.499671258409721, + "heading": -0.056164170438548724, + "angularVelocity": -0.5429423715102839, + "velocityX": -0.549063287972907, + "velocityY": -0.6550325761790774, + "timestamp": 2.192435575426974 + }, + { + "x": 2.300248237017326, + "y": 5.453359822013785, + "heading": -0.09289041649224676, + "angularVelocity": -0.7144382975079676, + "velocityX": -0.6941819961137773, + "velocityY": -0.9008996924293653, + "timestamp": 2.2438413387823304 + }, + { + "x": 2.25906191216666, + "y": 5.393424337289651, + "heading": -0.13788233192980848, + "angularVelocity": -0.8752309566252937, + "velocityX": -0.8012005301031048, + "velocityY": -1.1659292813106188, + "timestamp": 2.2952471021376866 + }, + { + "x": 2.215650672687085, + "y": 5.318928923927703, + "heading": -0.18994798908185376, + "angularVelocity": -1.0128369613369528, + "velocityX": -0.8444819538907223, + "velocityY": -1.44916461695119, + "timestamp": 2.3466528654930427 + }, + { + "x": 2.1751906895479802, + "y": 5.229944253664982, + "heading": -0.24676651261993965, + "angularVelocity": -1.105294811893225, + "velocityX": -0.7870709527142773, + "velocityY": -1.7310251702244053, + "timestamp": 2.398058628848399 + }, + { + "x": 2.143630529202551, + "y": 5.129096717824508, + "heading": -0.3045822393435162, + "angularVelocity": -1.1246934769533508, + "velocityX": -0.6139420618513388, + "velocityY": -1.9617943447962864, + "timestamp": 2.449464392203755 + }, + { + "x": 2.1248362064361572, + "y": 5.020731449127197, + "heading": -0.35924157629701275, + "angularVelocity": -1.063292000464021, + "velocityX": -0.3656073082014722, + "velocityY": -2.1080373410313484, + "timestamp": 2.5008701555591113 + }, + { + "x": 2.1236034975576716, + "y": 4.879581187775217, + "heading": -0.41834324922038263, + "angularVelocity": -0.9214808419098464, + "velocityX": -0.019219720170182603, + "velocityY": -2.200737394270732, + "timestamp": 2.5650078676366372 + }, + { + "x": 2.145285525498013, + "y": 4.739114283134644, + "heading": -0.4654704427453386, + "angularVelocity": -0.734781332205791, + "velocityX": 0.33805427786593684, + "velocityY": -2.190082871537182, + "timestamp": 2.629145579714163 + }, + { + "x": 2.1854463786837415, + "y": 4.612083049790391, + "heading": -0.49888391882293226, + "angularVelocity": -0.520964577551588, + "velocityX": 0.6261659776261528, + "velocityY": -1.9806012598439038, + "timestamp": 2.693283291791689 + }, + { + "x": 2.23085672934491, + "y": 4.507371852155421, + "heading": -0.5212416835862062, + "angularVelocity": -0.3485899954811167, + "velocityX": 0.7080132606894286, + "velocityY": -1.6325995150622288, + "timestamp": 2.757421003869215 + }, + { + "x": 2.2725752093899105, + "y": 4.425381093870058, + "heading": -0.5360315135324354, + "angularVelocity": -0.23059491003283958, + "velocityX": 0.6504516405975544, + "velocityY": -1.278354896511699, + "timestamp": 2.821558715946741 + }, + { + "x": 2.306393030453338, + "y": 4.365049187904869, + "heading": -0.5454513613849756, + "angularVelocity": -0.1468690969386946, + "velocityX": 0.5272689026161482, + "velocityY": -0.9406619601937548, + "timestamp": 2.885696428024267 + }, + { + "x": 2.330102947478555, + "y": 4.325455758257715, + "heading": -0.5508966378561475, + "angularVelocity": -0.08489976169698503, + "velocityX": 0.36967201132085403, + "velocityY": -0.6173190212849581, + "timestamp": 2.949834140101793 + }, + { + "x": 2.3423891067504883, + "y": 4.305914878845215, + "heading": -0.5532943925729576, + "angularVelocity": -0.037384475360018043, + "velocityX": 0.19155905120349095, + "velocityY": -0.30467066534709186, + "timestamp": 3.013971852179319 + }, + { + "x": 2.3423891067504883, + "y": 4.305914878845215, + "heading": -0.5532943925729576, + "angularVelocity": -2.509202302365357e-24, + "velocityX": 1.871020058653073e-23, + "velocityY": -1.3414864759418838e-22, + "timestamp": 3.078109564256845 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT.1.traj b/src/main/deploy/choreo/4NT.1.traj deleted file mode 100644 index c33d538..0000000 --- a/src/main/deploy/choreo/4NT.1.traj +++ /dev/null @@ -1,139 +0,0 @@ -{ - "samples": [ - { - "x": 0.7644821405410767, - "y": 6.707253456115723, - "heading": 1.0390723224103011, - "angularVelocity": -9.206160444329012e-19, - "velocityX": 9.05046145899678e-17, - "velocityY": 2.68861825331568e-18, - "timestamp": 0 - }, - { - "x": 0.7980550173951986, - "y": 6.708255980327082, - "heading": 1.0294370798834647, - "angularVelocity": -0.12440616498554202, - "velocityX": 0.4334787469355561, - "velocityY": 0.012944167424232692, - "timestamp": 0.07744987981870564 - }, - { - "x": 0.8652008914326297, - "y": 6.71026127914523, - "heading": 1.0101571456310194, - "angularVelocity": -0.24893433401802018, - "velocityX": 0.866959047510542, - "velocityY": 0.025891567848019475, - "timestamp": 0.1548997596374113 - }, - { - "x": 0.9659196942242209, - "y": 6.713269722660067, - "heading": 0.9812027432997417, - "angularVelocity": -0.37384696269450945, - "velocityX": 1.3004384645573803, - "velocityY": 0.038843746715683435, - "timestamp": 0.23234963945611692 - }, - { - "x": 1.1002111527349556, - "y": 6.717281753096185, - "heading": 0.9425220110355934, - "angularVelocity": -0.49942920963456483, - "velocityX": 1.7339143562918844, - "velocityY": 0.0518016353996883, - "timestamp": 0.3097995192748226 - }, - { - "x": 1.268074768329642, - "y": 6.722297816740347, - "heading": 0.8940388796768679, - "angularVelocity": -0.6259936293279543, - "velocityX": 2.167383809860272, - "velocityY": 0.06476528634910197, - "timestamp": 0.38724939909352823 - }, - { - "x": 1.469509791267818, - "y": 6.728318270649073, - "heading": 0.8356508730520583, - "angularVelocity": -0.7538811778854153, - "velocityX": 2.600843583097795, - "velocityY": 0.07773354746086393, - "timestamp": 0.4646992789122339 - }, - { - "x": 1.7045151768946503, - "y": 6.7353432595041784, - "heading": 0.7672266029593876, - "angularVelocity": -0.883465155179555, - "velocityX": 3.0342898681951693, - "velocityY": 0.09070367664288498, - "timestamp": 0.5421491587309395 - }, - { - "x": 1.9059214136950664, - "y": 6.741362767222263, - "heading": 0.7049265752872264, - "angularVelocity": -0.8043915344735578, - "velocityX": 2.6004719086958845, - "velocityY": 0.07772133064860018, - "timestamp": 0.6195990385496452 - }, - { - "x": 2.0737589181433664, - "y": 6.746378213892937, - "heading": 0.6528779373682576, - "angularVelocity": -0.6720299378230677, - "velocityX": 2.1670466738124516, - "velocityY": 0.0647573202491049, - "timestamp": 0.6970489183683508 - }, - { - "x": 2.208028432585087, - "y": 6.750389994490007, - "heading": 0.6111683036443215, - "angularVelocity": -0.5385371006587716, - "velocityX": 1.7336310237797354, - "velocityY": 0.051798409583853885, - "timestamp": 0.7744987981870565 - }, - { - "x": 2.308730438118727, - "y": 6.753398497687527, - "heading": 0.5798578891124007, - "angularVelocity": -0.4042667929919595, - "velocityX": 1.300221585486807, - "velocityY": 0.03884451731317741, - "timestamp": 0.8519486780057621 - }, - { - "x": 2.3758651742265764, - "y": 6.755404033176191, - "heading": 0.5589815311679106, - "angularVelocity": -0.26954667965085594, - "velocityX": 0.8668152392876279, - "velocityY": 0.02589462363734593, - "timestamp": 0.9293985578244678 - }, - { - "x": 2.4094326496124268, - "y": 6.756406784057617, - "heading": 0.5485498201613986, - "angularVelocity": -0.13468982819509207, - "velocityX": 0.43340900546811284, - "velocityY": 0.012947094091989499, - "timestamp": 1.0068484376431734 - }, - { - "x": 2.4094326496124268, - "y": 6.756406784057617, - "heading": 0.5485498201613986, - "angularVelocity": 7.885113031928889e-19, - "velocityX": -9.054050236249424e-17, - "velocityY": -2.6979151059026156e-18, - "timestamp": 1.084298317461879 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT.2.traj b/src/main/deploy/choreo/4NT.2.traj deleted file mode 100644 index 87cf0e9..0000000 --- a/src/main/deploy/choreo/4NT.2.traj +++ /dev/null @@ -1,229 +0,0 @@ -{ - "samples": [ - { - "x": 2.4094326496124268, - "y": 6.756406784057617, - "heading": 0.5485498201613986, - "angularVelocity": 7.885113031928889e-19, - "velocityX": -9.054050236249424e-17, - "velocityY": -2.6979151059026156e-18, - "timestamp": 0 - }, - { - "x": 2.449918473004672, - "y": 6.762011948706376, - "heading": 0.5401868465452816, - "angularVelocity": -0.09794900486670763, - "velocityX": 0.4741789576900468, - "velocityY": 0.0656489335805002, - "timestamp": 0.0853808941448424 - }, - { - "x": 2.5308915856088263, - "y": 6.7732229923798455, - "heading": 0.5235987360078687, - "angularVelocity": -0.1942836357425878, - "velocityX": 0.9483750833856259, - "velocityY": 0.13130623409085962, - "timestamp": 0.1707617882896848 - }, - { - "x": 2.652353832669549, - "y": 6.790040853204755, - "heading": 0.4989623873518077, - "angularVelocity": -0.2885463885429357, - "velocityX": 1.42259282099659, - "velocityY": 0.19697452214987327, - "timestamp": 0.2561426824345272 - }, - { - "x": 2.814307686694331, - "y": 6.812466902815374, - "heading": 0.4665261355043061, - "angularVelocity": -0.3799005875070355, - "velocityX": 1.8968395171645707, - "velocityY": 0.26265887509418356, - "timestamp": 0.3415235765793696 - }, - { - "x": 3.0167566936149885, - "y": 6.840503375672428, - "heading": 0.42667323499598936, - "angularVelocity": -0.4667660242666151, - "velocityX": 2.3711277440737293, - "velocityY": 0.32836939853888514, - "timestamp": 0.426904470724212 - }, - { - "x": 3.259706353846804, - "y": 6.874154379047141, - "heading": 0.3800673035647531, - "angularVelocity": -0.5458590226540936, - "velocityX": 2.8454803930686077, - "velocityY": 0.39412802725662344, - "timestamp": 0.5122853648690544 - }, - { - "x": 3.54316599484672, - "y": 6.913428880352818, - "heading": 0.3280843834092549, - "angularVelocity": -0.6088355091165131, - "velocityX": 3.3199422873113495, - "velocityY": 0.45999168431113957, - "timestamp": 0.5976662590138968 - }, - { - "x": 3.867149061852821, - "y": 6.958354860932515, - "heading": 0.274992628968794, - "angularVelocity": -0.621822422594857, - "velocityX": 3.7945616551694545, - "velocityY": 0.526183065071723, - "timestamp": 0.6830471531587392 - }, - { - "x": 4.212980616322228, - "y": 7.006124001931196, - "heading": 0.27499262530079555, - "angularVelocity": -4.296041295056196e-8, - "velocityX": 4.050455994086094, - "velocityY": 0.5594827915205997, - "timestamp": 0.7684280473035816 - }, - { - "x": 4.558812171652906, - "y": 7.053893136695068, - "heading": 0.27499262163285115, - "angularVelocity": -4.295977880952904e-8, - "velocityX": 4.050456004173501, - "velocityY": 0.5594827184971202, - "timestamp": 0.853808941448424 - }, - { - "x": 4.904643726983597, - "y": 7.1016622714588475, - "heading": 0.27499261796490687, - "angularVelocity": -4.295977836262281e-8, - "velocityX": 4.050456004173649, - "velocityY": 0.5594827184960538, - "timestamp": 0.9391898355932664 - }, - { - "x": 5.250475282314288, - "y": 7.149431406222625, - "heading": 0.27499261429696265, - "angularVelocity": -4.2959777948006654e-8, - "velocityX": 4.050456004173652, - "velocityY": 0.5594827184960296, - "timestamp": 1.0245707297381088 - }, - { - "x": 5.59630683764498, - "y": 7.1972005409864055, - "heading": 0.2749926106290183, - "angularVelocity": -4.2959778676517735e-8, - "velocityX": 4.05045600417365, - "velocityY": 0.5594827184960434, - "timestamp": 1.1099516238829512 - }, - { - "x": 5.942138392975663, - "y": 7.244969675750248, - "heading": 0.27499260696107397, - "angularVelocity": -4.2959778796089525e-8, - "velocityX": 4.0504560041735465, - "velocityY": 0.5594827184967942, - "timestamp": 1.1953325180277936 - }, - { - "x": 6.287969947721882, - "y": 7.292738814744901, - "heading": 0.2749926032930733, - "angularVelocity": -4.296043929094884e-8, - "velocityX": 4.05045599732818, - "velocityY": 0.5594827680489721, - "timestamp": 1.280713412172636 - }, - { - "x": 6.611957814067862, - "y": 7.337606380569157, - "heading": 0.21950497451670065, - "angularVelocity": -0.6498834350720434, - "velocityX": 3.7946178661043106, - "velocityY": 0.5254988984788682, - "timestamp": 1.3660943063174784 - }, - { - "x": 6.8954258290534405, - "y": 7.376830372069738, - "heading": 0.1669734117826824, - "angularVelocity": -0.6152613328797233, - "velocityX": 3.320040365291738, - "velocityY": 0.45940010225285227, - "timestamp": 1.4514752004623208 - }, - { - "x": 7.138387345763902, - "y": 7.410436361963956, - "heading": 0.12043561907247709, - "angularVelocity": -0.5450609668160352, - "velocityX": 2.845619258780494, - "velocityY": 0.393600819373116, - "timestamp": 1.5368560946071632 - }, - { - "x": 7.340849161615168, - "y": 7.438433533551551, - "heading": 0.08088918506026732, - "angularVelocity": -0.4631766205812063, - "velocityX": 2.3712777651145713, - "velocityY": 0.3279090933400053, - "timestamp": 1.6222369887520056 - }, - { - "x": 7.502815057013489, - "y": 7.460826777716827, - "heading": 0.04882589115457254, - "angularVelocity": -0.3755324212381965, - "velocityX": 1.8969805484065148, - "velocityY": 0.26227465043042614, - "timestamp": 1.707617882896848 - }, - { - "x": 7.62428740604637, - "y": 7.477619165692711, - "heading": 0.024536835467999913, - "angularVelocity": -0.28447881613148784, - "velocityX": 1.4227111375387078, - "velocityY": 0.19667617848317423, - "timestamp": 1.7929987770416904 - }, - { - "x": 7.705267842650796, - "y": 7.488812781962917, - "heading": 0.00821504269256905, - "angularVelocity": -0.1911644629504835, - "velocityX": 0.9484608637038761, - "velocityY": 0.1311021204722608, - "timestamp": 1.8783796711865328 - }, - { - "x": 7.745757579803467, - "y": 7.494409084320068, - "heading": -3.4489084618021245e-23, - "angularVelocity": -0.09621640502654895, - "velocityX": 0.47422479652159877, - "velocityY": 0.06554513645240315, - "timestamp": 1.9637605653313752 - }, - { - "x": 7.745757579803467, - "y": 7.494409084320068, - "heading": -1.6876431270400293e-23, - "angularVelocity": 9.102523324796537e-24, - "velocityX": 6.60258980472981e-23, - "velocityY": 9.24836920818837e-24, - "timestamp": 2.0491414594762176 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT.3.traj b/src/main/deploy/choreo/4NT.3.traj deleted file mode 100644 index 66f743d..0000000 --- a/src/main/deploy/choreo/4NT.3.traj +++ /dev/null @@ -1,184 +0,0 @@ -{ - "samples": [ - { - "x": 7.745757579803467, - "y": 7.494409084320068, - "heading": -1.6876431270400293e-23, - "angularVelocity": 9.102523324796537e-24, - "velocityX": 6.60258980472981e-23, - "velocityY": 9.24836920818837e-24, - "timestamp": 0 - }, - { - "x": 7.691102928405584, - "y": 7.488096388275378, - "heading": 6.192852550462252e-16, - "angularVelocity": 6.254986697847185e-15, - "velocityX": -0.552030111111553, - "velocityY": -0.06376032432436478, - "timestamp": 0.09900664890875532 - }, - { - "x": 7.581793626590513, - "y": 7.475470996299268, - "heading": 1.8570099149900534e-15, - "angularVelocity": 1.2501429687662208e-14, - "velocityX": -1.1040602123177685, - "velocityY": -0.1275206475046479, - "timestamp": 0.19801329781751065 - }, - { - "x": 7.417829675731226, - "y": 7.456532908550319, - "heading": 3.711983243078296e-15, - "angularVelocity": 1.8735846011693857e-14, - "velocityX": -1.6560902996565103, - "velocityY": -0.1912809690832165, - "timestamp": 0.29701994672626597 - }, - { - "x": 7.19921107788718, - "y": 7.431282125266402, - "heading": 6.179661952041429e-15, - "angularVelocity": 2.4924373627041796e-14, - "velocityX": -2.20812036619404, - "velocityY": -0.25504128825921324, - "timestamp": 0.3960265956350213 - }, - { - "x": 6.9259378364908075, - "y": 7.399718646843969, - "heading": 9.255687598717214e-15, - "angularVelocity": 3.106887952051271e-14, - "velocityX": -2.7601503980628785, - "velocityY": -0.31880160343092295, - "timestamp": 0.4950332445437766 - }, - { - "x": 6.598009958406971, - "y": 7.36184247407592, - "heading": 1.2963393641775597e-14, - "angularVelocity": 3.744906108755832e-14, - "velocityX": -3.312180360594323, - "velocityY": -0.38256191059405736, - "timestamp": 0.5940398934525319 - }, - { - "x": 6.215427464230262, - "y": 7.317653609340963, - "heading": 1.7282751287499044e-14, - "angularVelocity": 4.362694519339008e-14, - "velocityX": -3.8642101151135524, - "velocityY": -0.4463221937314619, - "timestamp": 0.6930465423612873 - }, - { - "x": 5.813271436087006, - "y": 7.27120397141853, - "heading": 1.7340324515409316e-14, - "angularVelocity": 5.81508702141454e-16, - "velocityX": -4.061909301807436, - "velocityY": -0.4691567529494063, - "timestamp": 0.7920531912700426 - }, - { - "x": 5.411115407943726, - "y": 7.224754333496093, - "heading": 1.7396219297650977e-14, - "angularVelocity": 5.645558440545595e-16, - "velocityX": -4.061909301807676, - "velocityY": -0.469156752949434, - "timestamp": 0.8910598401787975 - }, - { - "x": 5.008959379800445, - "y": 7.178304695573657, - "heading": 1.7361996647802476e-14, - "angularVelocity": -3.456601170296343e-16, - "velocityX": -4.061909301807676, - "velocityY": -0.46915675294943404, - "timestamp": 0.9900664890875528 - }, - { - "x": 4.606803351657189, - "y": 7.131855057651224, - "heading": 1.7439558668245237e-14, - "angularVelocity": 7.834021381153129e-16, - "velocityX": -4.061909301807435, - "velocityY": -0.46915675294940606, - "timestamp": 1.0890731379963081 - }, - { - "x": 4.22422085748048, - "y": 7.087666192916267, - "heading": 2.176954052963867e-14, - "angularVelocity": 4.37342532963012e-14, - "velocityX": -3.864210115113553, - "velocityY": -0.44632219373146187, - "timestamp": 1.1880797869050634 - }, - { - "x": 3.896292979396644, - "y": 7.049790020148219, - "heading": 2.5486765461328337e-14, - "angularVelocity": 3.7545205020679964e-14, - "velocityX": -3.3121803605943234, - "velocityY": -0.3825619105940573, - "timestamp": 1.2870864358138188 - }, - { - "x": 3.6230197380002713, - "y": 7.018226541725785, - "heading": 2.859002831120057e-14, - "angularVelocity": 3.134398430889467e-14, - "velocityX": -2.7601503980628785, - "velocityY": -0.3188016034309229, - "timestamp": 1.386093084722574 - }, - { - "x": 3.4044011401562257, - "y": 6.992975758441869, - "heading": 3.1065991495946517e-14, - "angularVelocity": 2.5008049580870388e-14, - "velocityX": -2.20812036619404, - "velocityY": -0.2550412882592132, - "timestamp": 1.4850997336313294 - }, - { - "x": 3.240437189296938, - "y": 6.97403767069292, - "heading": 3.291706550760512e-14, - "angularVelocity": 1.8696461622134015e-14, - "velocityX": -1.6560902996565106, - "velocityY": -0.19128096908321646, - "timestamp": 1.5841063825400847 - }, - { - "x": 3.131127887481867, - "y": 6.96141227871681, - "heading": 3.415005257379642e-14, - "angularVelocity": 1.2453578419037865e-14, - "velocityX": -1.1040602123177685, - "velocityY": -0.12752064750464787, - "timestamp": 1.68311303144884 - }, - { - "x": 3.0764732360839844, - "y": 6.955099582672119, - "heading": 3.4758328772534545e-14, - "angularVelocity": 6.143791406360488e-15, - "velocityX": -0.552030111111553, - "velocityY": -0.06376032432436476, - "timestamp": 1.7821196803575954 - }, - { - "x": 3.0764732360839844, - "y": 6.955099582672119, - "heading": 3.475832877253451e-14, - "angularVelocity": -5.200407773157229e-29, - "velocityX": 6.353773828235079e-22, - "velocityY": -9.018528098234072e-21, - "timestamp": 1.8811263292663507 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT.4.traj b/src/main/deploy/choreo/4NT.4.traj deleted file mode 100644 index 94f9b1a..0000000 --- a/src/main/deploy/choreo/4NT.4.traj +++ /dev/null @@ -1,265 +0,0 @@ -{ - "samples": [ - { - "x": 3.0764732360839844, - "y": 6.955099582672119, - "heading": 3.475832877253451e-14, - "angularVelocity": -5.200407773157229e-29, - "velocityX": 6.353773828235079e-22, - "velocityY": -9.018528098234072e-21, - "timestamp": 0 - }, - { - "x": 3.1002974851018976, - "y": 6.954611325658584, - "heading": 3.504479981929914e-14, - "angularVelocity": 4.396565873129549e-15, - "velocityX": 0.3656386268981033, - "velocityY": -0.007493441823419454, - "timestamp": 0.06515791074927257 - }, - { - "x": 3.1479459827206875, - "y": 6.953634810583847, - "heading": 3.561530727564337e-14, - "angularVelocity": 8.755766564394683e-15, - "velocityX": 0.7312772473958112, - "velocityY": -0.014986899725712866, - "timestamp": 0.13031582149854515 - }, - { - "x": 3.219418728430643, - "y": 6.952170036167429, - "heading": 3.6469746902170535e-14, - "angularVelocity": 1.311336745917341e-14, - "velocityX": 1.096915860070811, - "velocityY": -0.022480377279967864, - "timestamp": 0.19547373224781772 - }, - { - "x": 3.314715721594625, - "y": 6.950217000808728, - "heading": 3.7610775824707776e-14, - "angularVelocity": 1.7511748142569967e-14, - "velocityX": 1.4625544629674223, - "velocityY": -0.029973879399181655, - "timestamp": 0.2606316429970903 - }, - { - "x": 3.433836961393453, - "y": 6.947775702449826, - "heading": 3.903558275776818e-14, - "angularVelocity": 2.1866983098078997e-14, - "velocityX": 1.8281930532918131, - "velocityY": -0.03746741310192467, - "timestamp": 0.32578955374636287 - }, - { - "x": 3.576782446734888, - "y": 6.944846138346835, - "heading": 4.0741719020202745e-14, - "angularVelocity": 2.6184637334365043e-14, - "velocityX": 2.1938316268532327, - "velocityY": -0.04496098891605818, - "timestamp": 0.39094746449563544 - }, - { - "x": 3.7435521760897914, - "y": 6.941428304658305, - "heading": 4.2729089755284945e-14, - "angularVelocity": 3.0500835773105754e-14, - "velocityX": 2.5594701769464736, - "velocityY": -0.05245462368617194, - "timestamp": 0.456105375244908 - }, - { - "x": 3.934146147164455, - "y": 6.937522195622059, - "heading": 4.5003079615010204e-14, - "angularVelocity": 3.48996742463965e-14, - "velocityX": 2.925108691837403, - "velocityY": -0.05994834689032576, - "timestamp": 0.5212632859941806 - }, - { - "x": 4.148564356136022, - "y": 6.933127801634456, - "heading": 4.756109468648397e-14, - "angularVelocity": 3.925870308084403e-14, - "velocityX": 3.2907471480577066, - "velocityY": -0.06744221748471975, - "timestamp": 0.5864211967434532 - }, - { - "x": 4.386806795358758, - "y": 6.928245103488179, - "heading": 5.0403457079820235e-14, - "angularVelocity": 4.362267544571434e-14, - "velocityX": 3.6563854869363728, - "velocityY": -0.07493638286018017, - "timestamp": 0.6515791074927257 - }, - { - "x": 4.648873441895276, - "y": 6.922874043561024, - "heading": 5.3527133351455455e-14, - "angularVelocity": 4.7940092549239784e-14, - "velocityX": 4.022023473787371, - "velocityY": -0.08243143258264855, - "timestamp": 0.7167370182419983 - }, - { - "x": 4.914409944960004, - "y": 6.90113291291604, - "heading": 5.351906829106693e-14, - "angularVelocity": -1.237771484037243e-16, - "velocityX": 4.075276509194888, - "velocityY": -0.33366832046908873, - "timestamp": 0.7818949289912709 - }, - { - "x": 5.176941871643066, - "y": 6.855753421783447, - "heading": 5.3521968753757984e-14, - "angularVelocity": 4.45143599254819e-17, - "velocityX": 4.0291642820360565, - "velocityY": -0.6964540546306409, - "timestamp": 0.8470528397405435 - }, - { - "x": 5.463817637938302, - "y": 6.77599939155945, - "heading": 5.354091246992271e-14, - "angularVelocity": 2.601435934873782e-16, - "velocityX": 3.9395064875027384, - "velocityY": -1.0952180573822985, - "timestamp": 0.9198730691792534 - }, - { - "x": 5.741298450592208, - "y": 6.668004902444624, - "heading": 5.354780772967558e-14, - "angularVelocity": 9.468879466669219e-17, - "velocityX": 3.810490776981841, - "velocityY": -1.4830286851227334, - "timestamp": 0.9926932986179633 - }, - { - "x": 6.014319653285819, - "y": 6.549186410545976, - "heading": 5.3548296476122484e-14, - "angularVelocity": 6.711685074013839e-18, - "velocityX": 3.7492494159662573, - "velocityY": -1.6316687384053477, - "timestamp": 1.0655135280566732 - }, - { - "x": 6.287340700556493, - "y": 6.430367561517044, - "heading": 5.354604536453578e-14, - "angularVelocity": -3.0913272385962676e-17, - "velocityX": 3.749247281628875, - "velocityY": -1.6316736426783327, - "timestamp": 1.1383337574953831 - }, - { - "x": 6.560248685316027, - "y": 6.311597917328536, - "heading": 3.587309338739589e-14, - "angularVelocity": -2.4269289060693314e-13, - "velocityX": 3.747694656595805, - "velocityY": -1.6309979397754133, - "timestamp": 1.211153986934093 - }, - { - "x": 6.80586589819516, - "y": 6.204705225984495, - "heading": 2.502296079067399e-14, - "angularVelocity": -1.489988795744515e-13, - "velocityX": 3.3729255561582545, - "velocityY": -1.467898304742482, - "timestamp": 1.283974216372803 - }, - { - "x": 7.024192320386008, - "y": 6.1096894956701115, - "heading": 1.7418086030342693e-14, - "angularVelocity": -1.0443354571867126e-13, - "velocityX": 2.9981561974424364, - "velocityY": -1.304798557306861, - "timestamp": 1.3567944458115129 - }, - { - "x": 7.215227945620654, - "y": 6.026550729113183, - "heading": 1.1866391794709625e-14, - "angularVelocity": -7.623835132661919e-14, - "velocityX": 2.623386752652774, - "velocityY": -1.141698772411907, - "timestamp": 1.4296146752502228 - }, - { - "x": 7.378972770765402, - "y": 5.955288927677494, - "heading": 7.783117662427377e-15, - "angularVelocity": -5.6073348899830995e-14, - "velocityX": 2.248617264829748, - "velocityY": -0.9785989687888353, - "timestamp": 1.5024349046889327 - }, - { - "x": 7.515426793940112, - "y": 5.895904092181282, - "heading": 4.802760291325747e-15, - "angularVelocity": -4.0927602042373195e-14, - "velocityX": 1.8738477511878469, - "velocityY": -0.8154991539293902, - "timestamp": 1.5752551341276426 - }, - { - "x": 7.624590013891394, - "y": 5.848396223170021, - "heading": 2.683796378124848e-15, - "angularVelocity": -2.909856134117839e-14, - "velocityX": 1.499078220333837, - "velocityY": -0.6523993315792364, - "timestamp": 1.6480753635663525 - }, - { - "x": 7.706462429723989, - "y": 5.8127653210333285, - "heading": 1.2533736642541387e-15, - "angularVelocity": -1.9643205258982397e-14, - "velocityX": 1.124308677185698, - "velocityY": -0.48929950387867815, - "timestamp": 1.7208955930050625 - }, - { - "x": 7.76104404076646, - "y": 5.789011386063414, - "heading": 3.9416699766786147e-16, - "angularVelocity": -1.1799010703604574e-14, - "velocityX": 0.7495391248170898, - "velocityY": -0.32619967216537205, - "timestamp": 1.7937158224437724 - }, - { - "x": 7.788334846496582, - "y": 5.777134418487549, - "heading": 9.668206947791558e-29, - "angularVelocity": -5.412877722384226e-15, - "velocityX": 0.37476956527708116, - "velocityY": -0.1630998373310729, - "timestamp": 1.8665360518824823 - }, - { - "x": 7.788334846496582, - "y": 5.777134418487549, - "heading": 6.841375823115053e-29, - "angularVelocity": 8.529174821322553e-29, - "velocityX": -8.917160283036205e-22, - "velocityY": -7.355274998038012e-21, - "timestamp": 1.9393562813211922 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT.5.traj b/src/main/deploy/choreo/4NT.5.traj deleted file mode 100644 index 6727f53..0000000 --- a/src/main/deploy/choreo/4NT.5.traj +++ /dev/null @@ -1,265 +0,0 @@ -{ - "samples": [ - { - "x": 7.788334846496582, - "y": 5.777134418487549, - "heading": 6.841375823115053e-29, - "angularVelocity": 8.529174821322553e-29, - "velocityX": -8.917160283036205e-22, - "velocityY": -7.355274998038012e-21, - "timestamp": 0 - }, - { - "x": 7.764426386423816, - "y": 5.787558027926257, - "heading": 9.971110849937308e-19, - "angularVelocity": 1.462722327199216e-17, - "velocityX": -0.3507276058737257, - "velocityY": 0.15291020717662415, - "timestamp": 0.06816817288506982 - }, - { - "x": 7.716609466703687, - "y": 5.808405246618205, - "heading": 5.480572379025607e-18, - "angularVelocity": 6.577059504431801e-17, - "velocityX": -0.7014552055069206, - "velocityY": 0.3058204116325032, - "timestamp": 0.13633634577013964 - }, - { - "x": 7.644884087867956, - "y": 5.839676074331557, - "heading": 1.0960016322262206e-17, - "angularVelocity": 8.038126461333601e-17, - "velocityX": -1.0521827973394524, - "velocityY": 0.4587306126874509, - "timestamp": 0.20450451865520947 - }, - { - "x": 7.5492502506003065, - "y": 5.88137051076824, - "heading": 1.743462997857394e-17, - "angularVelocity": 9.498000873247208e-17, - "velocityX": -1.4029103791425612, - "velocityY": 0.611640809369773, - "timestamp": 0.2726726915402793 - }, - { - "x": 7.429707955812324, - "y": 5.933488555530821, - "heading": 2.450986571187997e-17, - "angularVelocity": 1.0379089586106166e-16, - "velocityX": -1.753637947573107, - "velocityY": 0.7645510002219281, - "timestamp": 0.3408408644253491 - }, - { - "x": 7.286257204780225, - "y": 5.996030208062897, - "heading": 3.2577664546725194e-17, - "angularVelocity": 1.1835140202508417e-16, - "velocityX": -2.104365497282067, - "velocityY": 0.9174611829118506, - "timestamp": 0.40900903731041893 - }, - { - "x": 7.118897999418333, - "y": 6.068995467529859, - "heading": 3.9164721464432884e-17, - "angularVelocity": 9.662950668074327e-17, - "velocityX": -2.455093018908655, - "velocityY": 1.070371353358427, - "timestamp": 0.47717721019548875 - }, - { - "x": 6.927630342917185, - "y": 6.1523843325407, - "heading": 4.673832569637582e-17, - "angularVelocity": 1.1110176363638367e-16, - "velocityX": -2.8058204937313085, - "velocityY": 1.2232815033994346, - "timestamp": 0.5453453830805586 - }, - { - "x": 6.712454241657855, - "y": 6.246196800313401, - "heading": 5.529092126185662e-17, - "angularVelocity": 1.254631773564182e-16, - "velocityX": -3.156547874946174, - "velocityY": 1.37619161262934, - "timestamp": 0.6135135559656284 - }, - { - "x": 6.473369714783506, - "y": 6.350432862501926, - "heading": 6.480025692264811e-17, - "angularVelocity": 1.3949817426000514e-16, - "velocityX": -3.507274975338426, - "velocityY": 1.5291015994262664, - "timestamp": 0.6816817288506982 - }, - { - "x": 6.217863323353405, - "y": 6.461828528523879, - "heading": 7.581887128261551e-17, - "angularVelocity": 1.6163869286199958e-16, - "velocityX": -3.7481772008306624, - "velocityY": 1.6341301417857155, - "timestamp": 0.749849901735768 - }, - { - "x": 5.962356931918229, - "y": 6.573224194534609, - "heading": 7.994866139472889e-17, - "angularVelocity": 6.058237939191866e-17, - "velocityX": -3.7481772009051104, - "velocityY": 1.6341301416210885, - "timestamp": 0.8180180746208379 - }, - { - "x": 5.706850260692564, - "y": 6.684619218792329, - "heading": 8.350055926035511e-17, - "angularVelocity": 5.210492984138971e-17, - "velocityX": -3.7481813053203648, - "velocityY": 1.6341207273595257, - "timestamp": 0.8861862475059077 - }, - { - "x": 5.445903710147097, - "y": 6.782595788135376, - "heading": 7.181307218009646e-17, - "angularVelocity": -1.714507897986972e-16, - "velocityX": -3.8279821726396834, - "velocityY": 1.4372773274741362, - "timestamp": 0.9543544203909775 - }, - { - "x": 5.176941871643066, - "y": 6.855753421783447, - "heading": 6.07611657923737e-17, - "angularVelocity": -1.621270736768902e-16, - "velocityX": -3.9455632609883127, - "velocityY": 1.0731934061283093, - "timestamp": 1.0225225932760473 - }, - { - "x": 4.892034856694491, - "y": 6.904225124198338, - "heading": 4.348929794316467e-17, - "angularVelocity": -2.443700947187874e-16, - "velocityX": -4.030991600719193, - "velocityY": 0.6857992785549824, - "timestamp": 1.0932017321007832 - }, - { - "x": 4.6037716259633, - "y": 6.92486061131334, - "heading": -4.780536109631277e-18, - "angularVelocity": -6.829431548752389e-16, - "velocityX": -4.078476839481642, - "velocityY": 0.2919600812648877, - "timestamp": 1.1638808709255182 - }, - { - "x": 4.323500629705874, - "y": 6.932939131074572, - "heading": -5.6528899699478266e-18, - "angularVelocity": -1.2342451744525731e-17, - "velocityX": -3.9653991392342633, - "velocityY": 0.11429850300331837, - "timestamp": 1.2345600097502532 - }, - { - "x": 4.071256703765887, - "y": 6.940209763425182, - "heading": -5.97740235993501e-18, - "angularVelocity": -4.591346121302763e-18, - "velocityX": -3.568859639977803, - "velocityY": 0.10286815135991294, - "timestamp": 1.3052391485749881 - }, - { - "x": 3.8470398688675607, - "y": 6.946672533421318, - "heading": -6.017364404100811e-18, - "angularVelocity": -5.6540083456305495e-19, - "velocityX": -3.172319847505807, - "velocityY": 0.09143815422202402, - "timestamp": 1.375918287399723 - }, - { - "x": 3.6508501319189572, - "y": 6.952327449414996, - "heading": -6.01163708431682e-18, - "angularVelocity": 8.103267776729523e-20, - "velocityX": -2.775779957295443, - "velocityY": 0.08000827525220806, - "timestamp": 1.446597426224458 - }, - { - "x": 3.482687496374107, - "y": 6.95717451558222, - "heading": -6.001606208178643e-18, - "angularVelocity": 1.4192131369018897e-19, - "velocityX": -2.3792400182159317, - "velocityY": 0.06857845536633275, - "timestamp": 1.517276565049193 - }, - { - "x": 3.342551964305426, - "y": 6.961213734428586, - "heading": -5.990861342748415e-18, - "angularVelocity": 1.5202315284148297e-19, - "velocityX": -1.9827000498149467, - "velocityY": 0.05714867093078821, - "timestamp": 1.587955703873928 - }, - { - "x": 3.2304435370945255, - "y": 6.964445107624494, - "heading": -5.979520825641484e-18, - "angularVelocity": 1.6045069862761274e-19, - "velocityX": -1.5861600618663185, - "velocityY": 0.04571891012878258, - "timestamp": 1.658634842698663 - }, - { - "x": 3.1463622157282694, - "y": 6.966868636363084, - "heading": -5.967668668372028e-18, - "angularVelocity": 1.6768961196496423e-19, - "velocityX": -1.1896200599550915, - "velocityY": 0.03428916620786852, - "timestamp": 1.729313981523398 - }, - { - "x": 3.0903080009468056, - "y": 6.968484321539212, - "heading": -5.968425491191159e-18, - "angularVelocity": -1.070786702703606e-20, - "velocityX": -0.7930800475719175, - "velocityY": 0.02285943494776875, - "timestamp": 1.799993120348133 - }, - { - "x": 3.0622808933258057, - "y": 6.969292163848877, - "heading": -5.968784000627222e-18, - "angularVelocity": -5.072351465622455e-21, - "velocityX": -0.396540027043897, - "velocityY": 0.011429713534966315, - "timestamp": 1.870672259172868 - }, - { - "x": 3.0622808933258057, - "y": 6.969292163848877, - "heading": -5.968784000628849e-18, - "angularVelocity": -4.846357394740608e-33, - "velocityX": -1.592220183778683e-21, - "velocityY": 4.017271607125232e-21, - "timestamp": 1.941351397997603 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/4NT.traj b/src/main/deploy/choreo/4NT.traj deleted file mode 100644 index b6eb8e5..0000000 --- a/src/main/deploy/choreo/4NT.traj +++ /dev/null @@ -1,1030 +0,0 @@ -{ - "samples": [ - { - "x": 0.7644821405410767, - "y": 6.707253456115723, - "heading": 1.0390723224103011, - "angularVelocity": -9.206160444329012e-19, - "velocityX": 9.05046145899678e-17, - "velocityY": 2.68861825331568e-18, - "timestamp": 0 - }, - { - "x": 0.7980550173951986, - "y": 6.708255980327082, - "heading": 1.0294370798834647, - "angularVelocity": -0.12440616498554202, - "velocityX": 0.4334787469355561, - "velocityY": 0.012944167424232692, - "timestamp": 0.07744987981870564 - }, - { - "x": 0.8652008914326297, - "y": 6.71026127914523, - "heading": 1.0101571456310194, - "angularVelocity": -0.24893433401802018, - "velocityX": 0.866959047510542, - "velocityY": 0.025891567848019475, - "timestamp": 0.1548997596374113 - }, - { - "x": 0.9659196942242209, - "y": 6.713269722660067, - "heading": 0.9812027432997417, - "angularVelocity": -0.37384696269450945, - "velocityX": 1.3004384645573803, - "velocityY": 0.038843746715683435, - "timestamp": 0.23234963945611692 - }, - { - "x": 1.1002111527349556, - "y": 6.717281753096185, - "heading": 0.9425220110355934, - "angularVelocity": -0.49942920963456483, - "velocityX": 1.7339143562918844, - "velocityY": 0.0518016353996883, - "timestamp": 0.3097995192748226 - }, - { - "x": 1.268074768329642, - "y": 6.722297816740347, - "heading": 0.8940388796768679, - "angularVelocity": -0.6259936293279543, - "velocityX": 2.167383809860272, - "velocityY": 0.06476528634910197, - "timestamp": 0.38724939909352823 - }, - { - "x": 1.469509791267818, - "y": 6.728318270649073, - "heading": 0.8356508730520583, - "angularVelocity": -0.7538811778854153, - "velocityX": 2.600843583097795, - "velocityY": 0.07773354746086393, - "timestamp": 0.4646992789122339 - }, - { - "x": 1.7045151768946503, - "y": 6.7353432595041784, - "heading": 0.7672266029593876, - "angularVelocity": -0.883465155179555, - "velocityX": 3.0342898681951693, - "velocityY": 0.09070367664288498, - "timestamp": 0.5421491587309395 - }, - { - "x": 1.9059214136950664, - "y": 6.741362767222263, - "heading": 0.7049265752872264, - "angularVelocity": -0.8043915344735578, - "velocityX": 2.6004719086958845, - "velocityY": 0.07772133064860018, - "timestamp": 0.6195990385496452 - }, - { - "x": 2.0737589181433664, - "y": 6.746378213892937, - "heading": 0.6528779373682576, - "angularVelocity": -0.6720299378230677, - "velocityX": 2.1670466738124516, - "velocityY": 0.0647573202491049, - "timestamp": 0.6970489183683508 - }, - { - "x": 2.208028432585087, - "y": 6.750389994490007, - "heading": 0.6111683036443215, - "angularVelocity": -0.5385371006587716, - "velocityX": 1.7336310237797354, - "velocityY": 0.051798409583853885, - "timestamp": 0.7744987981870565 - }, - { - "x": 2.308730438118727, - "y": 6.753398497687527, - "heading": 0.5798578891124007, - "angularVelocity": -0.4042667929919595, - "velocityX": 1.300221585486807, - "velocityY": 0.03884451731317741, - "timestamp": 0.8519486780057621 - }, - { - "x": 2.3758651742265764, - "y": 6.755404033176191, - "heading": 0.5589815311679106, - "angularVelocity": -0.26954667965085594, - "velocityX": 0.8668152392876279, - "velocityY": 0.02589462363734593, - "timestamp": 0.9293985578244678 - }, - { - "x": 2.4094326496124268, - "y": 6.756406784057617, - "heading": 0.5485498201613986, - "angularVelocity": -0.13468982819509207, - "velocityX": 0.43340900546811284, - "velocityY": 0.012947094091989499, - "timestamp": 1.0068484376431734 - }, - { - "x": 2.4094326496124268, - "y": 6.756406784057617, - "heading": 0.5485498201613986, - "angularVelocity": 7.885113031928889e-19, - "velocityX": -9.054050236249424e-17, - "velocityY": -2.6979151059026156e-18, - "timestamp": 1.084298317461879 - }, - { - "x": 2.449918473004672, - "y": 6.762011948706376, - "heading": 0.5401868465452816, - "angularVelocity": -0.09794900486670763, - "velocityX": 0.4741789576900468, - "velocityY": 0.0656489335805002, - "timestamp": 1.1696792116067214 - }, - { - "x": 2.5308915856088263, - "y": 6.7732229923798455, - "heading": 0.5235987360078687, - "angularVelocity": -0.1942836357425878, - "velocityX": 0.9483750833856259, - "velocityY": 0.13130623409085962, - "timestamp": 1.2550601057515638 - }, - { - "x": 2.652353832669549, - "y": 6.790040853204755, - "heading": 0.4989623873518077, - "angularVelocity": -0.2885463885429357, - "velocityX": 1.42259282099659, - "velocityY": 0.19697452214987327, - "timestamp": 1.3404409998964062 - }, - { - "x": 2.814307686694331, - "y": 6.812466902815374, - "heading": 0.4665261355043061, - "angularVelocity": -0.3799005875070355, - "velocityX": 1.8968395171645707, - "velocityY": 0.26265887509418356, - "timestamp": 1.4258218940412486 - }, - { - "x": 3.0167566936149885, - "y": 6.840503375672428, - "heading": 0.42667323499598936, - "angularVelocity": -0.4667660242666151, - "velocityX": 2.3711277440737293, - "velocityY": 0.32836939853888514, - "timestamp": 1.511202788186091 - }, - { - "x": 3.259706353846804, - "y": 6.874154379047141, - "heading": 0.3800673035647531, - "angularVelocity": -0.5458590226540936, - "velocityX": 2.8454803930686077, - "velocityY": 0.39412802725662344, - "timestamp": 1.5965836823309334 - }, - { - "x": 3.54316599484672, - "y": 6.913428880352818, - "heading": 0.3280843834092549, - "angularVelocity": -0.6088355091165131, - "velocityX": 3.3199422873113495, - "velocityY": 0.45999168431113957, - "timestamp": 1.6819645764757758 - }, - { - "x": 3.867149061852821, - "y": 6.958354860932515, - "heading": 0.274992628968794, - "angularVelocity": -0.621822422594857, - "velocityX": 3.7945616551694545, - "velocityY": 0.526183065071723, - "timestamp": 1.7673454706206182 - }, - { - "x": 4.212980616322228, - "y": 7.006124001931196, - "heading": 0.27499262530079555, - "angularVelocity": -4.296041295056196e-8, - "velocityX": 4.050455994086094, - "velocityY": 0.5594827915205997, - "timestamp": 1.8527263647654606 - }, - { - "x": 4.558812171652906, - "y": 7.053893136695068, - "heading": 0.27499262163285115, - "angularVelocity": -4.295977880952904e-8, - "velocityX": 4.050456004173501, - "velocityY": 0.5594827184971202, - "timestamp": 1.938107258910303 - }, - { - "x": 4.904643726983597, - "y": 7.1016622714588475, - "heading": 0.27499261796490687, - "angularVelocity": -4.295977836262281e-8, - "velocityX": 4.050456004173649, - "velocityY": 0.5594827184960538, - "timestamp": 2.0234881530551454 - }, - { - "x": 5.250475282314288, - "y": 7.149431406222625, - "heading": 0.27499261429696265, - "angularVelocity": -4.2959777948006654e-8, - "velocityX": 4.050456004173652, - "velocityY": 0.5594827184960296, - "timestamp": 2.108869047199988 - }, - { - "x": 5.59630683764498, - "y": 7.1972005409864055, - "heading": 0.2749926106290183, - "angularVelocity": -4.2959778676517735e-8, - "velocityX": 4.05045600417365, - "velocityY": 0.5594827184960434, - "timestamp": 2.19424994134483 - }, - { - "x": 5.942138392975663, - "y": 7.244969675750248, - "heading": 0.27499260696107397, - "angularVelocity": -4.2959778796089525e-8, - "velocityX": 4.0504560041735465, - "velocityY": 0.5594827184967942, - "timestamp": 2.2796308354896726 - }, - { - "x": 6.287969947721882, - "y": 7.292738814744901, - "heading": 0.2749926032930733, - "angularVelocity": -4.296043929094884e-8, - "velocityX": 4.05045599732818, - "velocityY": 0.5594827680489721, - "timestamp": 2.365011729634515 - }, - { - "x": 6.611957814067862, - "y": 7.337606380569157, - "heading": 0.21950497451670065, - "angularVelocity": -0.6498834350720434, - "velocityX": 3.7946178661043106, - "velocityY": 0.5254988984788682, - "timestamp": 2.4503926237793574 - }, - { - "x": 6.8954258290534405, - "y": 7.376830372069738, - "heading": 0.1669734117826824, - "angularVelocity": -0.6152613328797233, - "velocityX": 3.320040365291738, - "velocityY": 0.45940010225285227, - "timestamp": 2.5357735179242 - }, - { - "x": 7.138387345763902, - "y": 7.410436361963956, - "heading": 0.12043561907247709, - "angularVelocity": -0.5450609668160352, - "velocityX": 2.845619258780494, - "velocityY": 0.393600819373116, - "timestamp": 2.621154412069042 - }, - { - "x": 7.340849161615168, - "y": 7.438433533551551, - "heading": 0.08088918506026732, - "angularVelocity": -0.4631766205812063, - "velocityX": 2.3712777651145713, - "velocityY": 0.3279090933400053, - "timestamp": 2.7065353062138846 - }, - { - "x": 7.502815057013489, - "y": 7.460826777716827, - "heading": 0.04882589115457254, - "angularVelocity": -0.3755324212381965, - "velocityX": 1.8969805484065148, - "velocityY": 0.26227465043042614, - "timestamp": 2.791916200358727 - }, - { - "x": 7.62428740604637, - "y": 7.477619165692711, - "heading": 0.024536835467999913, - "angularVelocity": -0.28447881613148784, - "velocityX": 1.4227111375387078, - "velocityY": 0.19667617848317423, - "timestamp": 2.8772970945035694 - }, - { - "x": 7.705267842650796, - "y": 7.488812781962917, - "heading": 0.00821504269256905, - "angularVelocity": -0.1911644629504835, - "velocityX": 0.9484608637038761, - "velocityY": 0.1311021204722608, - "timestamp": 2.962677988648412 - }, - { - "x": 7.745757579803467, - "y": 7.494409084320068, - "heading": -3.4489084618021245e-23, - "angularVelocity": -0.09621640502654895, - "velocityX": 0.47422479652159877, - "velocityY": 0.06554513645240315, - "timestamp": 3.048058882793254 - }, - { - "x": 7.745757579803467, - "y": 7.494409084320068, - "heading": -1.6876431270400293e-23, - "angularVelocity": 9.102523324796537e-24, - "velocityX": 6.60258980472981e-23, - "velocityY": 9.24836920818837e-24, - "timestamp": 3.1334397769380966 - }, - { - "x": 7.691102928405584, - "y": 7.488096388275378, - "heading": 6.192852550462252e-16, - "angularVelocity": 6.254986697847185e-15, - "velocityX": -0.552030111111553, - "velocityY": -0.06376032432436478, - "timestamp": 3.232446425846852 - }, - { - "x": 7.581793626590513, - "y": 7.475470996299268, - "heading": 1.8570099149900534e-15, - "angularVelocity": 1.2501429687662208e-14, - "velocityX": -1.1040602123177685, - "velocityY": -0.1275206475046479, - "timestamp": 3.3314530747556073 - }, - { - "x": 7.417829675731226, - "y": 7.456532908550319, - "heading": 3.711983243078296e-15, - "angularVelocity": 1.8735846011693857e-14, - "velocityX": -1.6560902996565103, - "velocityY": -0.1912809690832165, - "timestamp": 3.4304597236643626 - }, - { - "x": 7.19921107788718, - "y": 7.431282125266402, - "heading": 6.179661952041429e-15, - "angularVelocity": 2.4924373627041796e-14, - "velocityX": -2.20812036619404, - "velocityY": -0.25504128825921324, - "timestamp": 3.529466372573118 - }, - { - "x": 6.9259378364908075, - "y": 7.399718646843969, - "heading": 9.255687598717214e-15, - "angularVelocity": 3.106887952051271e-14, - "velocityX": -2.7601503980628785, - "velocityY": -0.31880160343092295, - "timestamp": 3.6284730214818732 - }, - { - "x": 6.598009958406971, - "y": 7.36184247407592, - "heading": 1.2963393641775597e-14, - "angularVelocity": 3.744906108755832e-14, - "velocityX": -3.312180360594323, - "velocityY": -0.38256191059405736, - "timestamp": 3.7274796703906286 - }, - { - "x": 6.215427464230262, - "y": 7.317653609340963, - "heading": 1.7282751287499044e-14, - "angularVelocity": 4.362694519339008e-14, - "velocityX": -3.8642101151135524, - "velocityY": -0.4463221937314619, - "timestamp": 3.826486319299384 - }, - { - "x": 5.813271436087006, - "y": 7.27120397141853, - "heading": 1.7340324515409316e-14, - "angularVelocity": 5.81508702141454e-16, - "velocityX": -4.061909301807436, - "velocityY": -0.4691567529494063, - "timestamp": 3.925492968208139 - }, - { - "x": 5.411115407943726, - "y": 7.224754333496093, - "heading": 1.7396219297650977e-14, - "angularVelocity": 5.645558440545595e-16, - "velocityX": -4.061909301807676, - "velocityY": -0.469156752949434, - "timestamp": 4.024499617116894 - }, - { - "x": 5.008959379800445, - "y": 7.178304695573657, - "heading": 1.7361996647802476e-14, - "angularVelocity": -3.456601170296343e-16, - "velocityX": -4.061909301807676, - "velocityY": -0.46915675294943404, - "timestamp": 4.123506266025649 - }, - { - "x": 4.606803351657189, - "y": 7.131855057651224, - "heading": 1.7439558668245237e-14, - "angularVelocity": 7.834021381153129e-16, - "velocityX": -4.061909301807435, - "velocityY": -0.46915675294940606, - "timestamp": 4.222512914934405 - }, - { - "x": 4.22422085748048, - "y": 7.087666192916267, - "heading": 2.176954052963867e-14, - "angularVelocity": 4.37342532963012e-14, - "velocityX": -3.864210115113553, - "velocityY": -0.44632219373146187, - "timestamp": 4.32151956384316 - }, - { - "x": 3.896292979396644, - "y": 7.049790020148219, - "heading": 2.5486765461328337e-14, - "angularVelocity": 3.7545205020679964e-14, - "velocityX": -3.3121803605943234, - "velocityY": -0.3825619105940573, - "timestamp": 4.420526212751915 - }, - { - "x": 3.6230197380002713, - "y": 7.018226541725785, - "heading": 2.859002831120057e-14, - "angularVelocity": 3.134398430889467e-14, - "velocityX": -2.7601503980628785, - "velocityY": -0.3188016034309229, - "timestamp": 4.519532861660671 - }, - { - "x": 3.4044011401562257, - "y": 6.992975758441869, - "heading": 3.1065991495946517e-14, - "angularVelocity": 2.5008049580870388e-14, - "velocityX": -2.20812036619404, - "velocityY": -0.2550412882592132, - "timestamp": 4.618539510569426 - }, - { - "x": 3.240437189296938, - "y": 6.97403767069292, - "heading": 3.291706550760512e-14, - "angularVelocity": 1.8696461622134015e-14, - "velocityX": -1.6560902996565106, - "velocityY": -0.19128096908321646, - "timestamp": 4.717546159478181 - }, - { - "x": 3.131127887481867, - "y": 6.96141227871681, - "heading": 3.415005257379642e-14, - "angularVelocity": 1.2453578419037865e-14, - "velocityX": -1.1040602123177685, - "velocityY": -0.12752064750464787, - "timestamp": 4.816552808386937 - }, - { - "x": 3.0764732360839844, - "y": 6.955099582672119, - "heading": 3.4758328772534545e-14, - "angularVelocity": 6.143791406360488e-15, - "velocityX": -0.552030111111553, - "velocityY": -0.06376032432436476, - "timestamp": 4.915559457295692 - }, - { - "x": 3.0764732360839844, - "y": 6.955099582672119, - "heading": 3.475832877253451e-14, - "angularVelocity": -5.200407773157229e-29, - "velocityX": 6.353773828235079e-22, - "velocityY": -9.018528098234072e-21, - "timestamp": 5.014566106204447 - }, - { - "x": 3.1002974851018976, - "y": 6.954611325658584, - "heading": 3.504479981929914e-14, - "angularVelocity": 4.396565873129549e-15, - "velocityX": 0.3656386268981033, - "velocityY": -0.007493441823419454, - "timestamp": 5.07972401695372 - }, - { - "x": 3.1479459827206875, - "y": 6.953634810583847, - "heading": 3.561530727564337e-14, - "angularVelocity": 8.755766564394683e-15, - "velocityX": 0.7312772473958112, - "velocityY": -0.014986899725712866, - "timestamp": 5.1448819277029925 - }, - { - "x": 3.219418728430643, - "y": 6.952170036167429, - "heading": 3.6469746902170535e-14, - "angularVelocity": 1.311336745917341e-14, - "velocityX": 1.096915860070811, - "velocityY": -0.022480377279967864, - "timestamp": 5.210039838452265 - }, - { - "x": 3.314715721594625, - "y": 6.950217000808728, - "heading": 3.7610775824707776e-14, - "angularVelocity": 1.7511748142569967e-14, - "velocityX": 1.4625544629674223, - "velocityY": -0.029973879399181655, - "timestamp": 5.275197749201538 - }, - { - "x": 3.433836961393453, - "y": 6.947775702449826, - "heading": 3.903558275776818e-14, - "angularVelocity": 2.1866983098078997e-14, - "velocityX": 1.8281930532918131, - "velocityY": -0.03746741310192467, - "timestamp": 5.34035565995081 - }, - { - "x": 3.576782446734888, - "y": 6.944846138346835, - "heading": 4.0741719020202745e-14, - "angularVelocity": 2.6184637334365043e-14, - "velocityX": 2.1938316268532327, - "velocityY": -0.04496098891605818, - "timestamp": 5.405513570700083 - }, - { - "x": 3.7435521760897914, - "y": 6.941428304658305, - "heading": 4.2729089755284945e-14, - "angularVelocity": 3.0500835773105754e-14, - "velocityX": 2.5594701769464736, - "velocityY": -0.05245462368617194, - "timestamp": 5.470671481449355 - }, - { - "x": 3.934146147164455, - "y": 6.937522195622059, - "heading": 4.5003079615010204e-14, - "angularVelocity": 3.48996742463965e-14, - "velocityX": 2.925108691837403, - "velocityY": -0.05994834689032576, - "timestamp": 5.535829392198628 - }, - { - "x": 4.148564356136022, - "y": 6.933127801634456, - "heading": 4.756109468648397e-14, - "angularVelocity": 3.925870308084403e-14, - "velocityX": 3.2907471480577066, - "velocityY": -0.06744221748471975, - "timestamp": 5.6009873029479005 - }, - { - "x": 4.386806795358758, - "y": 6.928245103488179, - "heading": 5.0403457079820235e-14, - "angularVelocity": 4.362267544571434e-14, - "velocityX": 3.6563854869363728, - "velocityY": -0.07493638286018017, - "timestamp": 5.666145213697173 - }, - { - "x": 4.648873441895276, - "y": 6.922874043561024, - "heading": 5.3527133351455455e-14, - "angularVelocity": 4.7940092549239784e-14, - "velocityX": 4.022023473787371, - "velocityY": -0.08243143258264855, - "timestamp": 5.731303124446446 - }, - { - "x": 4.914409944960004, - "y": 6.90113291291604, - "heading": 5.351906829106693e-14, - "angularVelocity": -1.237771484037243e-16, - "velocityX": 4.075276509194888, - "velocityY": -0.33366832046908873, - "timestamp": 5.796461035195718 - }, - { - "x": 5.176941871643066, - "y": 6.855753421783447, - "heading": 5.3521968753757984e-14, - "angularVelocity": 4.45143599254819e-17, - "velocityX": 4.0291642820360565, - "velocityY": -0.6964540546306409, - "timestamp": 5.861618945944991 - }, - { - "x": 5.463817637938302, - "y": 6.77599939155945, - "heading": 5.354091246992271e-14, - "angularVelocity": 2.601435934873782e-16, - "velocityX": 3.9395064875027384, - "velocityY": -1.0952180573822985, - "timestamp": 5.934439175383701 - }, - { - "x": 5.741298450592208, - "y": 6.668004902444624, - "heading": 5.354780772967558e-14, - "angularVelocity": 9.468879466669219e-17, - "velocityX": 3.810490776981841, - "velocityY": -1.4830286851227334, - "timestamp": 6.007259404822411 - }, - { - "x": 6.014319653285819, - "y": 6.549186410545976, - "heading": 5.3548296476122484e-14, - "angularVelocity": 6.711685074013839e-18, - "velocityX": 3.7492494159662573, - "velocityY": -1.6316687384053477, - "timestamp": 6.0800796342611205 - }, - { - "x": 6.287340700556493, - "y": 6.430367561517044, - "heading": 5.354604536453578e-14, - "angularVelocity": -3.0913272385962676e-17, - "velocityX": 3.749247281628875, - "velocityY": -1.6316736426783327, - "timestamp": 6.15289986369983 - }, - { - "x": 6.560248685316027, - "y": 6.311597917328536, - "heading": 3.587309338739589e-14, - "angularVelocity": -2.4269289060693314e-13, - "velocityX": 3.747694656595805, - "velocityY": -1.6309979397754133, - "timestamp": 6.22572009313854 - }, - { - "x": 6.80586589819516, - "y": 6.204705225984495, - "heading": 2.502296079067399e-14, - "angularVelocity": -1.489988795744515e-13, - "velocityX": 3.3729255561582545, - "velocityY": -1.467898304742482, - "timestamp": 6.29854032257725 - }, - { - "x": 7.024192320386008, - "y": 6.1096894956701115, - "heading": 1.7418086030342693e-14, - "angularVelocity": -1.0443354571867126e-13, - "velocityX": 2.9981561974424364, - "velocityY": -1.304798557306861, - "timestamp": 6.37136055201596 - }, - { - "x": 7.215227945620654, - "y": 6.026550729113183, - "heading": 1.1866391794709625e-14, - "angularVelocity": -7.623835132661919e-14, - "velocityX": 2.623386752652774, - "velocityY": -1.141698772411907, - "timestamp": 6.44418078145467 - }, - { - "x": 7.378972770765402, - "y": 5.955288927677494, - "heading": 7.783117662427377e-15, - "angularVelocity": -5.6073348899830995e-14, - "velocityX": 2.248617264829748, - "velocityY": -0.9785989687888353, - "timestamp": 6.51700101089338 - }, - { - "x": 7.515426793940112, - "y": 5.895904092181282, - "heading": 4.802760291325747e-15, - "angularVelocity": -4.0927602042373195e-14, - "velocityX": 1.8738477511878469, - "velocityY": -0.8154991539293902, - "timestamp": 6.58982124033209 - }, - { - "x": 7.624590013891394, - "y": 5.848396223170021, - "heading": 2.683796378124848e-15, - "angularVelocity": -2.909856134117839e-14, - "velocityX": 1.499078220333837, - "velocityY": -0.6523993315792364, - "timestamp": 6.6626414697708 - }, - { - "x": 7.706462429723989, - "y": 5.8127653210333285, - "heading": 1.2533736642541387e-15, - "angularVelocity": -1.9643205258982397e-14, - "velocityX": 1.124308677185698, - "velocityY": -0.48929950387867815, - "timestamp": 6.73546169920951 - }, - { - "x": 7.76104404076646, - "y": 5.789011386063414, - "heading": 3.9416699766786147e-16, - "angularVelocity": -1.1799010703604574e-14, - "velocityX": 0.7495391248170898, - "velocityY": -0.32619967216537205, - "timestamp": 6.80828192864822 - }, - { - "x": 7.788334846496582, - "y": 5.777134418487549, - "heading": 9.668206947791558e-29, - "angularVelocity": -5.412877722384226e-15, - "velocityX": 0.37476956527708116, - "velocityY": -0.1630998373310729, - "timestamp": 6.88110215808693 - }, - { - "x": 7.788334846496582, - "y": 5.777134418487549, - "heading": 6.841375823115053e-29, - "angularVelocity": 8.529174821322553e-29, - "velocityX": -8.917160283036205e-22, - "velocityY": -7.355274998038012e-21, - "timestamp": 6.9539223875256395 - }, - { - "x": 7.764426386423816, - "y": 5.787558027926257, - "heading": 9.971110849937308e-19, - "angularVelocity": 1.462722327199216e-17, - "velocityX": -0.3507276058737257, - "velocityY": 0.15291020717662415, - "timestamp": 7.022090560410709 - }, - { - "x": 7.716609466703687, - "y": 5.808405246618205, - "heading": 5.480572379025607e-18, - "angularVelocity": 6.577059504431801e-17, - "velocityX": -0.7014552055069206, - "velocityY": 0.3058204116325032, - "timestamp": 7.090258733295779 - }, - { - "x": 7.644884087867956, - "y": 5.839676074331557, - "heading": 1.0960016322262206e-17, - "angularVelocity": 8.038126461333601e-17, - "velocityX": -1.0521827973394524, - "velocityY": 0.4587306126874509, - "timestamp": 7.158426906180849 - }, - { - "x": 7.5492502506003065, - "y": 5.88137051076824, - "heading": 1.743462997857394e-17, - "angularVelocity": 9.498000873247208e-17, - "velocityX": -1.4029103791425612, - "velocityY": 0.611640809369773, - "timestamp": 7.226595079065919 - }, - { - "x": 7.429707955812324, - "y": 5.933488555530821, - "heading": 2.450986571187997e-17, - "angularVelocity": 1.0379089586106166e-16, - "velocityX": -1.753637947573107, - "velocityY": 0.7645510002219281, - "timestamp": 7.294763251950989 - }, - { - "x": 7.286257204780225, - "y": 5.996030208062897, - "heading": 3.2577664546725194e-17, - "angularVelocity": 1.1835140202508417e-16, - "velocityX": -2.104365497282067, - "velocityY": 0.9174611829118506, - "timestamp": 7.3629314248360584 - }, - { - "x": 7.118897999418333, - "y": 6.068995467529859, - "heading": 3.9164721464432884e-17, - "angularVelocity": 9.662950668074327e-17, - "velocityX": -2.455093018908655, - "velocityY": 1.070371353358427, - "timestamp": 7.431099597721128 - }, - { - "x": 6.927630342917185, - "y": 6.1523843325407, - "heading": 4.673832569637582e-17, - "angularVelocity": 1.1110176363638367e-16, - "velocityX": -2.8058204937313085, - "velocityY": 1.2232815033994346, - "timestamp": 7.499267770606198 - }, - { - "x": 6.712454241657855, - "y": 6.246196800313401, - "heading": 5.529092126185662e-17, - "angularVelocity": 1.254631773564182e-16, - "velocityX": -3.156547874946174, - "velocityY": 1.37619161262934, - "timestamp": 7.567435943491268 - }, - { - "x": 6.473369714783506, - "y": 6.350432862501926, - "heading": 6.480025692264811e-17, - "angularVelocity": 1.3949817426000514e-16, - "velocityX": -3.507274975338426, - "velocityY": 1.5291015994262664, - "timestamp": 7.635604116376338 - }, - { - "x": 6.217863323353405, - "y": 6.461828528523879, - "heading": 7.581887128261551e-17, - "angularVelocity": 1.6163869286199958e-16, - "velocityX": -3.7481772008306624, - "velocityY": 1.6341301417857155, - "timestamp": 7.703772289261408 - }, - { - "x": 5.962356931918229, - "y": 6.573224194534609, - "heading": 7.994866139472889e-17, - "angularVelocity": 6.058237939191866e-17, - "velocityX": -3.7481772009051104, - "velocityY": 1.6341301416210885, - "timestamp": 7.771940462146477 - }, - { - "x": 5.706850260692564, - "y": 6.684619218792329, - "heading": 8.350055926035511e-17, - "angularVelocity": 5.210492984138971e-17, - "velocityX": -3.7481813053203648, - "velocityY": 1.6341207273595257, - "timestamp": 7.840108635031547 - }, - { - "x": 5.445903710147097, - "y": 6.782595788135376, - "heading": 7.181307218009646e-17, - "angularVelocity": -1.714507897986972e-16, - "velocityX": -3.8279821726396834, - "velocityY": 1.4372773274741362, - "timestamp": 7.908276807916617 - }, - { - "x": 5.176941871643066, - "y": 6.855753421783447, - "heading": 6.07611657923737e-17, - "angularVelocity": -1.621270736768902e-16, - "velocityX": -3.9455632609883127, - "velocityY": 1.0731934061283093, - "timestamp": 7.976444980801687 - }, - { - "x": 4.892034856694491, - "y": 6.904225124198338, - "heading": 4.348929794316467e-17, - "angularVelocity": -2.443700947187874e-16, - "velocityX": -4.030991600719193, - "velocityY": 0.6857992785549824, - "timestamp": 8.047124119626423 - }, - { - "x": 4.6037716259633, - "y": 6.92486061131334, - "heading": -4.780536109631277e-18, - "angularVelocity": -6.829431548752389e-16, - "velocityX": -4.078476839481642, - "velocityY": 0.2919600812648877, - "timestamp": 8.117803258451158 - }, - { - "x": 4.323500629705874, - "y": 6.932939131074572, - "heading": -5.6528899699478266e-18, - "angularVelocity": -1.2342451744525731e-17, - "velocityX": -3.9653991392342633, - "velocityY": 0.11429850300331837, - "timestamp": 8.188482397275893 - }, - { - "x": 4.071256703765887, - "y": 6.940209763425182, - "heading": -5.97740235993501e-18, - "angularVelocity": -4.591346121302763e-18, - "velocityX": -3.568859639977803, - "velocityY": 0.10286815135991294, - "timestamp": 8.259161536100628 - }, - { - "x": 3.8470398688675607, - "y": 6.946672533421318, - "heading": -6.017364404100811e-18, - "angularVelocity": -5.6540083456305495e-19, - "velocityX": -3.172319847505807, - "velocityY": 0.09143815422202402, - "timestamp": 8.329840674925363 - }, - { - "x": 3.6508501319189572, - "y": 6.952327449414996, - "heading": -6.01163708431682e-18, - "angularVelocity": 8.103267776729523e-20, - "velocityX": -2.775779957295443, - "velocityY": 0.08000827525220806, - "timestamp": 8.400519813750098 - }, - { - "x": 3.482687496374107, - "y": 6.95717451558222, - "heading": -6.001606208178643e-18, - "angularVelocity": 1.4192131369018897e-19, - "velocityX": -2.3792400182159317, - "velocityY": 0.06857845536633275, - "timestamp": 8.471198952574833 - }, - { - "x": 3.342551964305426, - "y": 6.961213734428586, - "heading": -5.990861342748415e-18, - "angularVelocity": 1.5202315284148297e-19, - "velocityX": -1.9827000498149467, - "velocityY": 0.05714867093078821, - "timestamp": 8.541878091399568 - }, - { - "x": 3.2304435370945255, - "y": 6.964445107624494, - "heading": -5.979520825641484e-18, - "angularVelocity": 1.6045069862761274e-19, - "velocityX": -1.5861600618663185, - "velocityY": 0.04571891012878258, - "timestamp": 8.612557230224303 - }, - { - "x": 3.1463622157282694, - "y": 6.966868636363084, - "heading": -5.967668668372028e-18, - "angularVelocity": 1.6768961196496423e-19, - "velocityX": -1.1896200599550915, - "velocityY": 0.03428916620786852, - "timestamp": 8.683236369049038 - }, - { - "x": 3.0903080009468056, - "y": 6.968484321539212, - "heading": -5.968425491191159e-18, - "angularVelocity": -1.070786702703606e-20, - "velocityX": -0.7930800475719175, - "velocityY": 0.02285943494776875, - "timestamp": 8.753915507873772 - }, - { - "x": 3.0622808933258057, - "y": 6.969292163848877, - "heading": -5.968784000627222e-18, - "angularVelocity": -5.072351465622455e-21, - "velocityX": -0.396540027043897, - "velocityY": 0.011429713534966315, - "timestamp": 8.824594646698507 - }, - { - "x": 3.0622808933258057, - "y": 6.969292163848877, - "heading": -5.968784000628849e-18, - "angularVelocity": -4.846357394740608e-33, - "velocityX": -1.592220183778683e-21, - "velocityY": 4.017271607125232e-21, - "timestamp": 8.895273785523242 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/New Path.1.traj b/src/main/deploy/choreo/New Path.1.traj new file mode 100644 index 0000000..083d6aa --- /dev/null +++ b/src/main/deploy/choreo/New Path.1.traj @@ -0,0 +1,103 @@ +{ + "samples": [ + { + "x": 1.3308136463165283, + "y": 5.578441619873047, + "heading": -1.1461213177572739e-35, + "angularVelocity": -9.909156311978142e-36, + "velocityX": 2.9222166777723766e-37, + "velocityY": 2.0060798561156742e-35, + "timestamp": 0 + }, + { + "x": 1.3943953952794308, + "y": 5.577873916574882, + "heading": -1.0470024289152453e-19, + "angularVelocity": -1.0576278444103841e-18, + "velocityX": 0.642270043962327, + "velocityY": -0.005734645998538515, + "timestamp": 0.09899535181595946 + }, + { + "x": 1.5215588911614684, + "y": 5.5767385099968, + "heading": -1.8496185268666512e-19, + "angularVelocity": -8.10761391189343e-19, + "velocityX": 1.2845400672795728, + "velocityY": -0.011469291812742989, + "timestamp": 0.19799070363191892 + }, + { + "x": 1.7123041302838606, + "y": 5.575035400171647, + "heading": -2.686268786519769e-19, + "angularVelocity": -8.451409538681279e-19, + "velocityX": 1.9268100534356736, + "velocityY": -0.0172039372951462, + "timestamp": 0.29698605544787837 + }, + { + "x": 1.9666311040627873, + "y": 5.572764587176066, + "heading": -1.6212239705976632e-19, + "angularVelocity": 1.0758533310717686e-18, + "velocityX": 2.5690799528824466, + "velocityY": -0.022938582003346558, + "timestamp": 0.39598140726383785 + }, + { + "x": 2.2845397695791814, + "y": 5.56992607139327, + "heading": -1.9153900489002526e-19, + "angularVelocity": -2.9715139630070603e-19, + "velocityX": 3.211349418782938, + "velocityY": -0.028673222840535854, + "timestamp": 0.4949767590797973 + }, + { + "x": 2.5388667433581076, + "y": 5.567655258397689, + "heading": -4.152427192621361e-20, + "angularVelocity": 1.5153714573576932e-18, + "velocityX": 2.5690799528824466, + "velocityY": -0.02293858200334656, + "timestamp": 0.5939721108957567 + }, + { + "x": 2.7296119824804994, + "y": 5.565952148572536, + "heading": -1.102082856978159e-19, + "angularVelocity": -6.938104712714696e-19, + "velocityX": 1.9268100534356736, + "velocityY": -0.0172039372951462, + "timestamp": 0.6929674627117162 + }, + { + "x": 2.8567754783625374, + "y": 5.564816741994454, + "heading": 9.435864642184072e-20, + "angularVelocity": 2.0664296369926834e-18, + "velocityX": 1.2845400672795726, + "velocityY": -0.011469291812742989, + "timestamp": 0.7919628145276756 + }, + { + "x": 2.9203572273254395, + "y": 5.564249038696289, + "heading": 4.6611799040550797e-35, + "angularVelocity": -9.531623683821139e-19, + "velocityX": 0.642270043962327, + "velocityY": -0.005734645998538515, + "timestamp": 0.890958166343635 + }, + { + "x": 2.9203572273254395, + "y": 5.564249038696289, + "heading": 4.6611799040550797e-35, + "angularVelocity": 0, + "velocityX": -3.5881132169082085e-37, + "velocityY": -9.426787938112893e-35, + "timestamp": 0.9899535181595944 + } + ] +} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottom-Straight.1.traj b/src/main/deploy/choreo/NoTeamBottom-Straight.1.traj deleted file mode 100644 index f62fd46..0000000 --- a/src/main/deploy/choreo/NoTeamBottom-Straight.1.traj +++ /dev/null @@ -1,184 +0,0 @@ -{ - "samples": [ - { - "x": 0.7265444397926331, - "y": 4.389017105102539, - "heading": -1.051650239140039, - "angularVelocity": -3.9411935078304533e-19, - "velocityX": -9.649679705969973e-18, - "velocityY": 2.5413372154671343e-18, - "timestamp": 0 - }, - { - "x": 0.776139548616391, - "y": 4.375870910257796, - "heading": -1.051650255054925, - "angularVelocity": -1.6645605511844412e-7, - "velocityX": 0.5187223048695124, - "velocityY": -0.13749792372392594, - "timestamp": 0.09561013351109666 - }, - { - "x": 0.8753297592770444, - "y": 4.349578522401735, - "heading": -1.051650302930334, - "angularVelocity": -5.007357093189477e-7, - "velocityX": 1.037444536662436, - "velocityY": -0.27499582827179037, - "timestamp": 0.19122026702219333 - }, - { - "x": 1.0139574723685933, - "y": 4.312832420562262, - "heading": -1.0367199784583665, - "angularVelocity": 0.15615838953127859, - "velocityX": 1.4499269899613692, - "velocityY": -0.3843327112937186, - "timestamp": 0.28683040053329 - }, - { - "x": 1.1525851854374713, - "y": 4.2760863186372635, - "heading": -0.9797252599514273, - "angularVelocity": 0.5961158761515635, - "velocityX": 1.4499269897242488, - "velocityY": -0.3843327121882254, - "timestamp": 0.38244053404438666 - }, - { - "x": 1.2912128985023568, - "y": 4.239340216697203, - "heading": -0.8972351135862031, - "angularVelocity": 0.8627761863301555, - "velocityX": 1.4499269896824911, - "velocityY": -0.38433271234576016, - "timestamp": 0.4780506675554833 - }, - { - "x": 1.4298406115670086, - "y": 4.202594114756262, - "heading": -0.7996636520135696, - "angularVelocity": 1.0205138094625397, - "velocityX": 1.4499269896800473, - "velocityY": -0.38433271235497996, - "timestamp": 0.57366080106658 - }, - { - "x": 1.5684683246315605, - "y": 4.165848012814943, - "heading": -0.6935028756739687, - "angularVelocity": 1.1103506756141064, - "velocityX": 1.449926989679002, - "velocityY": -0.38433271235892325, - "timestamp": 0.6692709345776767 - }, - { - "x": 1.7070960376964128, - "y": 4.129101910874758, - "heading": -0.5829458891996031, - "angularVelocity": 1.156331263375335, - "velocityX": 1.4499269896821445, - "velocityY": -0.3843327123470672, - "timestamp": 0.7648810680887734 - }, - { - "x": 1.845723750761851, - "y": 4.092355808936783, - "heading": -0.4710059950722154, - "angularVelocity": 1.1707952914259698, - "velocityX": 1.4499269896882703, - "velocityY": -0.3843327123239573, - "timestamp": 0.8604912015998701 - }, - { - "x": 1.984351463828032, - "y": 4.055609707001611, - "heading": -0.36033688653760365, - "angularVelocity": 1.1575039639681362, - "velocityX": 1.449926989696042, - "velocityY": -0.3843327122946371, - "timestamp": 0.9561013351109668 - }, - { - "x": 2.1229791768949604, - "y": 4.018863605069257, - "heading": -0.2539613845394898, - "angularVelocity": 1.1125965218505633, - "velocityX": 1.4499269897038571, - "velocityY": -0.3843327122651551, - "timestamp": 1.0517114686220634 - }, - { - "x": 2.2616068899624193, - "y": 3.982117503138906, - "heading": -0.15608476800741816, - "angularVelocity": 1.0237054686332996, - "velocityX": 1.449926989709406, - "velocityY": -0.3843327122442218, - "timestamp": 1.14732160213316 - }, - { - "x": 2.4002346030301833, - "y": 3.945371401209705, - "heading": -0.07319927069686108, - "angularVelocity": 0.8669112181600921, - "velocityX": 1.4499269897125988, - "velocityY": -0.3843327122321768, - "timestamp": 1.2429317356442566 - }, - { - "x": 2.538862316095791, - "y": 3.908625299272368, - "heading": -0.01568345993735033, - "angularVelocity": 0.6015660542177947, - "velocityX": 1.4499269896900406, - "velocityY": -0.3843327123172788, - "timestamp": 1.3385418691553532 - }, - { - "x": 2.677490029150183, - "y": 3.8718791972927162, - "heading": 6.907865962288835e-8, - "angularVelocity": 0.16403626310369768, - "velocityX": 1.4499269895727396, - "velocityY": -0.3843327127598549, - "timestamp": 1.4341520026664498 - }, - { - "x": 2.776680239832323, - "y": 3.8455868095177137, - "heading": 1.7239404214233085e-8, - "angularVelocity": -5.42194141971473e-7, - "velocityX": 1.0374445368871719, - "velocityY": -0.27499582742399126, - "timestamp": 1.5297621361775464 - }, - { - "x": 2.8262753486633296, - "y": 3.8324406147003174, - "heading": 1.0613888575539939e-18, - "angularVelocity": -1.8030937935116867e-7, - "velocityX": 0.5187223049453299, - "velocityY": -0.13749792343790435, - "timestamp": 1.625372269688643 - }, - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 5.127273269042298e-19, - "angularVelocity": -4.403694489230173e-19, - "velocityX": -8.07004841092386e-18, - "velocityY": 1.7417348388539475e-18, - "timestamp": 1.7209824031997396 - }, - { - "x": 2.795280387972076, - "y": 3.848734116088756, - "heading": -0.0007551310869653908, - "angularVelocity": -0.009560276519822239, - "velocityX": -0.3924092121809185, - "velocityY": 0.20628256661445335, - "timestamp": 1.799968725969767 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottom-Straight.2.traj b/src/main/deploy/choreo/NoTeamBottom-Straight.2.traj deleted file mode 100644 index dfad8b1..0000000 --- a/src/main/deploy/choreo/NoTeamBottom-Straight.2.traj +++ /dev/null @@ -1,202 +0,0 @@ -{ - "samples": [ - { - "x": 2.795280387972076, - "y": 3.848734116088756, - "heading": -0.0007551310869653908, - "angularVelocity": -0.009560276519822239, - "velocityX": -0.3924092121809185, - "velocityY": 0.20628256661445335, - "timestamp": 0 - }, - { - "x": 2.7336960485405553, - "y": 3.882070373799011, - "heading": -0.002264872044319623, - "angularVelocity": -0.019113954219009816, - "velocityX": -0.7796835866232954, - "velocityY": 0.4220510151778421, - "timestamp": 0.07898632277002737 - }, - { - "x": 2.6421816952334147, - "y": 3.9335824329962463, - "heading": -0.004527426530412457, - "angularVelocity": -0.028644889478908624, - "velocityX": -1.1586101251173504, - "velocityY": 0.6521642911167751, - "timestamp": 0.15797264554005475 - }, - { - "x": 2.521976425090861, - "y": 4.005169271138778, - "heading": -0.007536715216345028, - "angularVelocity": -0.038098857883209325, - "velocityX": -1.521849174983587, - "velocityY": 0.9063194187550713, - "timestamp": 0.23695896831008212 - }, - { - "x": 2.3760898363674414, - "y": 4.10056022967408, - "heading": -0.011268245061689747, - "angularVelocity": -0.0472427341149841, - "velocityX": -1.8469854476980263, - "velocityY": 1.2076895744727607, - "timestamp": 0.31594529108010927 - }, - { - "x": 2.2166317486226847, - "y": 4.228231039414653, - "heading": -0.015572186616028692, - "angularVelocity": -0.054489706615031094, - "velocityX": -2.0188063218112653, - "velocityY": 1.6163660398812645, - "timestamp": 0.3949316138501364 - }, - { - "x": 2.084696959264052, - "y": 4.377550389705627, - "heading": -0.01967245587357254, - "angularVelocity": -0.05191112984816348, - "velocityX": -1.6703498116093853, - "velocityY": 1.8904456500111395, - "timestamp": 0.47391793662016357 - }, - { - "x": 1.9877043893162503, - "y": 4.529153861531996, - "heading": -0.02316087350132265, - "angularVelocity": -0.04416483139627623, - "velocityX": -1.227966647215625, - "velocityY": 1.9193635873867676, - "timestamp": 0.5529042593901907 - }, - { - "x": 1.9255292584302608, - "y": 4.67702675102025, - "heading": -0.025960074267663252, - "angularVelocity": -0.03543905663883883, - "velocityX": -0.787163254415777, - "velocityY": 1.872132849111024, - "timestamp": 0.6318905821602179 - }, - { - "x": 1.8977891338477046, - "y": 4.818543542345504, - "heading": -0.028044646496215683, - "angularVelocity": -0.02639155939214653, - "velocityX": -0.35120162085938406, - "velocityY": 1.791661978457797, - "timestamp": 0.710876904930245 - }, - { - "x": 1.90418541431427, - "y": 4.952258110046387, - "heading": -0.029402891635568452, - "angularVelocity": -0.017195953574233005, - "velocityX": 0.08097959548247807, - "velocityY": 1.6928825524667095, - "timestamp": 0.7898632277002722 - }, - { - "x": 1.9499952520887203, - "y": 5.085555153658078, - "heading": -0.0300168588401326, - "angularVelocity": -0.007252882671074071, - "velocityX": 0.541158185794122, - "velocityY": 1.5746570998086957, - "timestamp": 0.8745147011086003 - }, - { - "x": 2.0338114612800804, - "y": 5.20569356907258, - "heading": -0.02977603194516244, - "angularVelocity": 0.002844922660808227, - "velocityX": 0.9901329039727554, - "velocityY": 1.4192123370965932, - "timestamp": 0.9591661745169284 - }, - { - "x": 2.1527419399014183, - "y": 5.306220242191226, - "heading": -0.028678425606030473, - "angularVelocity": 0.0129661811536051, - "velocityX": 1.4049428064607963, - "velocityY": 1.187536011732969, - "timestamp": 1.0438176479252566 - }, - { - "x": 2.2919822627653264, - "y": 5.372031605779915, - "heading": -0.0268917327794857, - "angularVelocity": 0.02110645869005037, - "velocityX": 1.64486591027498, - "velocityY": 0.7774390797811452, - "timestamp": 1.1284691213335847 - }, - { - "x": 2.4072021963771473, - "y": 5.405583228268601, - "heading": -0.02516240946692918, - "angularVelocity": 0.02042874439071947, - "velocityX": 1.3611096059254855, - "velocityY": 0.3963501299835196, - "timestamp": 1.2131205947419128 - }, - { - "x": 2.4857544102266513, - "y": 5.422607976222576, - "heading": -0.023906284272725863, - "angularVelocity": 0.014838787130664898, - "velocityX": 0.9279485717938638, - "velocityY": 0.20111579005663074, - "timestamp": 1.297772068150241 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": 0.007737795266915266, - "velocityX": 0.4679917078538006, - "velocityY": 0.0820204075679476, - "timestamp": 1.382423541558569 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": -1.0584672558388422e-21, - "velocityX": -1.3848067340945103e-20, - "velocityY": -9.014000859371455e-19, - "timestamp": 1.4670750149668972 - }, - { - "x": 2.4932837154220553, - "y": 5.448396033127147, - "heading": -0.022770666908350497, - "angularVelocity": 0.005902466019987162, - "velocityX": -0.3940722117463989, - "velocityY": 0.2314420795890136, - "timestamp": 1.5484988795806995 - }, - { - "x": 2.4295351524707884, - "y": 5.486791799190017, - "heading": -0.021801144362707133, - "angularVelocity": 0.011907105493479883, - "velocityX": -0.7829223441263743, - "velocityY": 0.471554208891744, - "timestamp": 1.629922744194502 - }, - { - "x": 2.334824983937302, - "y": 5.545829177644723, - "heading": -0.020330384697378846, - "angularVelocity": 0.018063004897939713, - "velocityX": -1.1631745678334153, - "velocityY": 0.7250623479335421, - "timestamp": 1.7113466088083042 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottom-Straight.3.traj b/src/main/deploy/choreo/NoTeamBottom-Straight.3.traj deleted file mode 100644 index e1642da..0000000 --- a/src/main/deploy/choreo/NoTeamBottom-Straight.3.traj +++ /dev/null @@ -1,175 +0,0 @@ -{ - "samples": [ - { - "x": 2.334824983937302, - "y": 5.545829177644723, - "heading": -0.020330384697378846, - "angularVelocity": 0.018063004897939713, - "velocityX": -1.1631745678334153, - "velocityY": 0.7250623479335421, - "timestamp": 0 - }, - { - "x": 2.210503958944244, - "y": 5.62740303484446, - "heading": -0.018338525194008283, - "angularVelocity": 0.02446284652316683, - "velocityX": -1.5268376855203283, - "velocityY": 1.0018421206933206, - "timestamp": 0.08142386461380235 - }, - { - "x": 2.060080687733814, - "y": 5.735497929201447, - "heading": -0.01579076657886024, - "angularVelocity": 0.031290072354489634, - "velocityX": -1.8474101164799204, - "velocityY": 1.3275578955836365, - "timestamp": 0.1628477292276047 - }, - { - "x": 1.9009764416054544, - "y": 5.879777468130423, - "heading": -0.012665610982838561, - "angularVelocity": 0.03838132236592367, - "velocityX": -1.9540247430283388, - "velocityY": 1.771956411222945, - "timestamp": 0.24427159384140706 - }, - { - "x": 1.7772040404414515, - "y": 6.035734732004305, - "heading": -0.009508151620445738, - "angularVelocity": 0.03877805821878904, - "velocityX": -1.5200998104310322, - "velocityY": 1.9153753584848283, - "timestamp": 0.3256954584552094 - }, - { - "x": 1.6904452898858147, - "y": 6.187858419199818, - "heading": -0.006606347045565794, - "angularVelocity": 0.03563825653134196, - "velocityX": -1.065519930393113, - "velocityY": 1.8682936251803464, - "timestamp": 0.4071193230690118 - }, - { - "x": 1.6399494780145882, - "y": 6.33163320865491, - "heading": -0.004035525936270969, - "angularVelocity": 0.0315733123389361, - "velocityX": -0.6201598525288734, - "velocityY": 1.7657573751506779, - "timestamp": 0.48854318768281413 - }, - { - "x": 1.6251865832225818, - "y": 6.465022310901009, - "heading": -0.0018276262461566339, - "angularVelocity": 0.027116124990953467, - "velocityX": -0.18130918818491096, - "velocityY": 1.6382064751995045, - "timestamp": 0.5699670522966165 - }, - { - "x": 1.645802617073059, - "y": 6.586877346038818, - "heading": 4.952395911421774e-21, - "angularVelocity": 0.022445830283604976, - "velocityX": 0.25319399844579077, - "velocityY": 1.4965518489668062, - "timestamp": 0.6513909169104184 - }, - { - "x": 1.7070719230542575, - "y": 6.702201095157488, - "heading": 0.0014972380423929784, - "angularVelocity": 0.017357118177337608, - "velocityX": 0.710280232299891, - "velocityY": 1.3369203062107229, - "timestamp": 0.7376516675198057 - }, - { - "x": 1.8073044591521912, - "y": 6.802488503929648, - "heading": 0.0025384256366347965, - "angularVelocity": 0.012070235731620374, - "velocityX": 1.1619715269093187, - "velocityY": 1.162607652538175, - "timestamp": 0.823912418129193 - }, - { - "x": 1.9455905219304854, - "y": 6.885566837368984, - "heading": 0.003095480223723526, - "angularVelocity": 0.006457798977558507, - "velocityX": 1.603116849799888, - "velocityY": 0.9631070081402183, - "timestamp": 0.9101731687385803 - }, - { - "x": 2.1195534915718537, - "y": 6.946933930333879, - "heading": 0.0031159494279625953, - "angularVelocity": 0.0002372945296031513, - "velocityX": 2.016710594475592, - "velocityY": 0.7114138531298224, - "timestamp": 0.9964339193479677 - }, - { - "x": 2.3175113214846284, - "y": 6.974118315265626, - "heading": 0.0025073631392465968, - "angularVelocity": -0.007055193519841369, - "velocityX": 2.2948772009784966, - "velocityY": 0.31514199377705937, - "timestamp": 1.082694669957355 - }, - { - "x": 2.4838287511855577, - "y": 6.97404258239362, - "heading": 0.0016205368299748227, - "angularVelocity": -0.010280762722406305, - "velocityX": 1.928077700761749, - "velocityY": -0.0008779528519129953, - "timestamp": 1.1689554205667423 - }, - { - "x": 2.6088542637061045, - "y": 6.9677049199263745, - "heading": 0.0008452961970897526, - "angularVelocity": -0.008987176988472742, - "velocityX": 1.4493905007469428, - "velocityY": -0.07347098677525214, - "timestamp": 1.2552161711761296 - }, - { - "x": 2.692116063326133, - "y": 6.961208581732824, - "heading": 0.0002899594520839453, - "angularVelocity": -0.006437884450142658, - "velocityX": 0.9652338871598958, - "velocityY": -0.07531047605842896, - "timestamp": 1.341476921785517 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 6.368402820587344e-22, - "angularVelocity": -0.003361429735256571, - "velocityX": 0.4819485634738517, - "velocityY": -0.04622118029966558, - "timestamp": 1.4277376723949042 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 3.1853758732676014e-22, - "angularVelocity": 5.381619199616603e-24, - "velocityX": -3.3871462049455533e-19, - "velocityY": -1.593318872358821e-19, - "timestamp": 1.5139984230042915 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottom-Straight.traj b/src/main/deploy/choreo/NoTeamBottom-Straight.traj deleted file mode 100644 index c32cf19..0000000 --- a/src/main/deploy/choreo/NoTeamBottom-Straight.traj +++ /dev/null @@ -1,535 +0,0 @@ -{ - "samples": [ - { - "x": 0.7265444397926331, - "y": 4.389017105102539, - "heading": -1.051650239140039, - "angularVelocity": -3.9411935078304533e-19, - "velocityX": -9.649679705969973e-18, - "velocityY": 2.5413372154671343e-18, - "timestamp": 0 - }, - { - "x": 0.776139548616391, - "y": 4.375870910257796, - "heading": -1.051650255054925, - "angularVelocity": -1.6645605511844412e-7, - "velocityX": 0.5187223048695124, - "velocityY": -0.13749792372392594, - "timestamp": 0.09561013351109666 - }, - { - "x": 0.8753297592770444, - "y": 4.349578522401735, - "heading": -1.051650302930334, - "angularVelocity": -5.007357093189477e-7, - "velocityX": 1.037444536662436, - "velocityY": -0.27499582827179037, - "timestamp": 0.19122026702219333 - }, - { - "x": 1.0139574723685933, - "y": 4.312832420562262, - "heading": -1.0367199784583665, - "angularVelocity": 0.15615838953127859, - "velocityX": 1.4499269899613692, - "velocityY": -0.3843327112937186, - "timestamp": 0.28683040053329 - }, - { - "x": 1.1525851854374713, - "y": 4.2760863186372635, - "heading": -0.9797252599514273, - "angularVelocity": 0.5961158761515635, - "velocityX": 1.4499269897242488, - "velocityY": -0.3843327121882254, - "timestamp": 0.38244053404438666 - }, - { - "x": 1.2912128985023568, - "y": 4.239340216697203, - "heading": -0.8972351135862031, - "angularVelocity": 0.8627761863301555, - "velocityX": 1.4499269896824911, - "velocityY": -0.38433271234576016, - "timestamp": 0.4780506675554833 - }, - { - "x": 1.4298406115670086, - "y": 4.202594114756262, - "heading": -0.7996636520135696, - "angularVelocity": 1.0205138094625397, - "velocityX": 1.4499269896800473, - "velocityY": -0.38433271235497996, - "timestamp": 0.57366080106658 - }, - { - "x": 1.5684683246315605, - "y": 4.165848012814943, - "heading": -0.6935028756739687, - "angularVelocity": 1.1103506756141064, - "velocityX": 1.449926989679002, - "velocityY": -0.38433271235892325, - "timestamp": 0.6692709345776767 - }, - { - "x": 1.7070960376964128, - "y": 4.129101910874758, - "heading": -0.5829458891996031, - "angularVelocity": 1.156331263375335, - "velocityX": 1.4499269896821445, - "velocityY": -0.3843327123470672, - "timestamp": 0.7648810680887734 - }, - { - "x": 1.845723750761851, - "y": 4.092355808936783, - "heading": -0.4710059950722154, - "angularVelocity": 1.1707952914259698, - "velocityX": 1.4499269896882703, - "velocityY": -0.3843327123239573, - "timestamp": 0.8604912015998701 - }, - { - "x": 1.984351463828032, - "y": 4.055609707001611, - "heading": -0.36033688653760365, - "angularVelocity": 1.1575039639681362, - "velocityX": 1.449926989696042, - "velocityY": -0.3843327122946371, - "timestamp": 0.9561013351109668 - }, - { - "x": 2.1229791768949604, - "y": 4.018863605069257, - "heading": -0.2539613845394898, - "angularVelocity": 1.1125965218505633, - "velocityX": 1.4499269897038571, - "velocityY": -0.3843327122651551, - "timestamp": 1.0517114686220634 - }, - { - "x": 2.2616068899624193, - "y": 3.982117503138906, - "heading": -0.15608476800741816, - "angularVelocity": 1.0237054686332996, - "velocityX": 1.449926989709406, - "velocityY": -0.3843327122442218, - "timestamp": 1.14732160213316 - }, - { - "x": 2.4002346030301833, - "y": 3.945371401209705, - "heading": -0.07319927069686108, - "angularVelocity": 0.8669112181600921, - "velocityX": 1.4499269897125988, - "velocityY": -0.3843327122321768, - "timestamp": 1.2429317356442566 - }, - { - "x": 2.538862316095791, - "y": 3.908625299272368, - "heading": -0.01568345993735033, - "angularVelocity": 0.6015660542177947, - "velocityX": 1.4499269896900406, - "velocityY": -0.3843327123172788, - "timestamp": 1.3385418691553532 - }, - { - "x": 2.677490029150183, - "y": 3.8718791972927162, - "heading": 6.907865962288835e-8, - "angularVelocity": 0.16403626310369768, - "velocityX": 1.4499269895727396, - "velocityY": -0.3843327127598549, - "timestamp": 1.4341520026664498 - }, - { - "x": 2.776680239832323, - "y": 3.8455868095177137, - "heading": 1.7239404214233085e-8, - "angularVelocity": -5.42194141971473e-7, - "velocityX": 1.0374445368871719, - "velocityY": -0.27499582742399126, - "timestamp": 1.5297621361775464 - }, - { - "x": 2.8262753486633296, - "y": 3.8324406147003174, - "heading": 1.0613888575539939e-18, - "angularVelocity": -1.8030937935116867e-7, - "velocityX": 0.5187223049453299, - "velocityY": -0.13749792343790435, - "timestamp": 1.625372269688643 - }, - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 5.127273269042298e-19, - "angularVelocity": -4.403694489230173e-19, - "velocityX": -8.07004841092386e-18, - "velocityY": 1.7417348388539475e-18, - "timestamp": 1.7209824031997396 - }, - { - "x": 2.795280387972076, - "y": 3.848734116088756, - "heading": -0.0007551310869653908, - "angularVelocity": -0.009560276519822239, - "velocityX": -0.3924092121809185, - "velocityY": 0.20628256661445335, - "timestamp": 1.799968725969767 - }, - { - "x": 2.7336960485405553, - "y": 3.882070373799011, - "heading": -0.002264872044319623, - "angularVelocity": -0.019113954219009816, - "velocityX": -0.7796835866232954, - "velocityY": 0.4220510151778421, - "timestamp": 1.8789550487397944 - }, - { - "x": 2.6421816952334147, - "y": 3.9335824329962463, - "heading": -0.004527426530412457, - "angularVelocity": -0.028644889478908624, - "velocityX": -1.1586101251173504, - "velocityY": 0.6521642911167751, - "timestamp": 1.9579413715098217 - }, - { - "x": 2.521976425090861, - "y": 4.005169271138778, - "heading": -0.007536715216345028, - "angularVelocity": -0.038098857883209325, - "velocityX": -1.521849174983587, - "velocityY": 0.9063194187550713, - "timestamp": 2.036927694279849 - }, - { - "x": 2.3760898363674414, - "y": 4.10056022967408, - "heading": -0.011268245061689747, - "angularVelocity": -0.0472427341149841, - "velocityX": -1.8469854476980263, - "velocityY": 1.2076895744727607, - "timestamp": 2.1159140170498763 - }, - { - "x": 2.2166317486226847, - "y": 4.228231039414653, - "heading": -0.015572186616028692, - "angularVelocity": -0.054489706615031094, - "velocityX": -2.0188063218112653, - "velocityY": 1.6163660398812645, - "timestamp": 2.1949003398199034 - }, - { - "x": 2.084696959264052, - "y": 4.377550389705627, - "heading": -0.01967245587357254, - "angularVelocity": -0.05191112984816348, - "velocityX": -1.6703498116093853, - "velocityY": 1.8904456500111395, - "timestamp": 2.2738866625899306 - }, - { - "x": 1.9877043893162503, - "y": 4.529153861531996, - "heading": -0.02316087350132265, - "angularVelocity": -0.04416483139627623, - "velocityX": -1.227966647215625, - "velocityY": 1.9193635873867676, - "timestamp": 2.3528729853599577 - }, - { - "x": 1.9255292584302608, - "y": 4.67702675102025, - "heading": -0.025960074267663252, - "angularVelocity": -0.03543905663883883, - "velocityX": -0.787163254415777, - "velocityY": 1.872132849111024, - "timestamp": 2.431859308129985 - }, - { - "x": 1.8977891338477046, - "y": 4.818543542345504, - "heading": -0.028044646496215683, - "angularVelocity": -0.02639155939214653, - "velocityX": -0.35120162085938406, - "velocityY": 1.791661978457797, - "timestamp": 2.510845630900012 - }, - { - "x": 1.90418541431427, - "y": 4.952258110046387, - "heading": -0.029402891635568452, - "angularVelocity": -0.017195953574233005, - "velocityX": 0.08097959548247807, - "velocityY": 1.6928825524667095, - "timestamp": 2.589831953670039 - }, - { - "x": 1.9499952520887203, - "y": 5.085555153658078, - "heading": -0.0300168588401326, - "angularVelocity": -0.007252882671074071, - "velocityX": 0.541158185794122, - "velocityY": 1.5746570998086957, - "timestamp": 2.6744834270783673 - }, - { - "x": 2.0338114612800804, - "y": 5.20569356907258, - "heading": -0.02977603194516244, - "angularVelocity": 0.002844922660808227, - "velocityX": 0.9901329039727554, - "velocityY": 1.4192123370965932, - "timestamp": 2.7591349004866954 - }, - { - "x": 2.1527419399014183, - "y": 5.306220242191226, - "heading": -0.028678425606030473, - "angularVelocity": 0.0129661811536051, - "velocityX": 1.4049428064607963, - "velocityY": 1.187536011732969, - "timestamp": 2.8437863738950235 - }, - { - "x": 2.2919822627653264, - "y": 5.372031605779915, - "heading": -0.0268917327794857, - "angularVelocity": 0.02110645869005037, - "velocityX": 1.64486591027498, - "velocityY": 0.7774390797811452, - "timestamp": 2.9284378473033517 - }, - { - "x": 2.4072021963771473, - "y": 5.405583228268601, - "heading": -0.02516240946692918, - "angularVelocity": 0.02042874439071947, - "velocityX": 1.3611096059254855, - "velocityY": 0.3963501299835196, - "timestamp": 3.01308932071168 - }, - { - "x": 2.4857544102266513, - "y": 5.422607976222576, - "heading": -0.023906284272725863, - "angularVelocity": 0.014838787130664898, - "velocityX": 0.9279485717938638, - "velocityY": 0.20111579005663074, - "timestamp": 3.097740794120008 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": 0.007737795266915266, - "velocityX": 0.4679917078538006, - "velocityY": 0.0820204075679476, - "timestamp": 3.182392267528336 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": -1.0584672558388422e-21, - "velocityX": -1.3848067340945103e-20, - "velocityY": -9.014000859371455e-19, - "timestamp": 3.267043740936664 - }, - { - "x": 2.4932837154220553, - "y": 5.448396033127147, - "heading": -0.022770666908350497, - "angularVelocity": 0.005902466019987162, - "velocityX": -0.3940722117463989, - "velocityY": 0.2314420795890136, - "timestamp": 3.3484676055504665 - }, - { - "x": 2.4295351524707884, - "y": 5.486791799190017, - "heading": -0.021801144362707133, - "angularVelocity": 0.011907105493479883, - "velocityX": -0.7829223441263743, - "velocityY": 0.471554208891744, - "timestamp": 3.429891470164269 - }, - { - "x": 2.334824983937302, - "y": 5.545829177644723, - "heading": -0.020330384697378846, - "angularVelocity": 0.018063004897939713, - "velocityX": -1.1631745678334153, - "velocityY": 0.7250623479335421, - "timestamp": 3.5113153347780712 - }, - { - "x": 2.210503958944244, - "y": 5.62740303484446, - "heading": -0.018338525194008283, - "angularVelocity": 0.02446284652316683, - "velocityX": -1.5268376855203283, - "velocityY": 1.0018421206933206, - "timestamp": 3.5927391993918736 - }, - { - "x": 2.060080687733814, - "y": 5.735497929201447, - "heading": -0.01579076657886024, - "angularVelocity": 0.031290072354489634, - "velocityX": -1.8474101164799204, - "velocityY": 1.3275578955836365, - "timestamp": 3.674163064005676 - }, - { - "x": 1.9009764416054544, - "y": 5.879777468130423, - "heading": -0.012665610982838561, - "angularVelocity": 0.03838132236592367, - "velocityX": -1.9540247430283388, - "velocityY": 1.771956411222945, - "timestamp": 3.7555869286194783 - }, - { - "x": 1.7772040404414515, - "y": 6.035734732004305, - "heading": -0.009508151620445738, - "angularVelocity": 0.03877805821878904, - "velocityX": -1.5200998104310322, - "velocityY": 1.9153753584848283, - "timestamp": 3.8370107932332806 - }, - { - "x": 1.6904452898858147, - "y": 6.187858419199818, - "heading": -0.006606347045565794, - "angularVelocity": 0.03563825653134196, - "velocityX": -1.065519930393113, - "velocityY": 1.8682936251803464, - "timestamp": 3.918434657847083 - }, - { - "x": 1.6399494780145882, - "y": 6.33163320865491, - "heading": -0.004035525936270969, - "angularVelocity": 0.0315733123389361, - "velocityX": -0.6201598525288734, - "velocityY": 1.7657573751506779, - "timestamp": 3.9998585224608854 - }, - { - "x": 1.6251865832225818, - "y": 6.465022310901009, - "heading": -0.0018276262461566339, - "angularVelocity": 0.027116124990953467, - "velocityX": -0.18130918818491096, - "velocityY": 1.6382064751995045, - "timestamp": 4.081282387074688 - }, - { - "x": 1.645802617073059, - "y": 6.586877346038818, - "heading": 4.952395911421774e-21, - "angularVelocity": 0.022445830283604976, - "velocityX": 0.25319399844579077, - "velocityY": 1.4965518489668062, - "timestamp": 4.16270625168849 - }, - { - "x": 1.7070719230542575, - "y": 6.702201095157488, - "heading": 0.0014972380423929784, - "angularVelocity": 0.017357118177337608, - "velocityX": 0.710280232299891, - "velocityY": 1.3369203062107229, - "timestamp": 4.248967002297877 - }, - { - "x": 1.8073044591521912, - "y": 6.802488503929648, - "heading": 0.0025384256366347965, - "angularVelocity": 0.012070235731620374, - "velocityX": 1.1619715269093187, - "velocityY": 1.162607652538175, - "timestamp": 4.335227752907264 - }, - { - "x": 1.9455905219304854, - "y": 6.885566837368984, - "heading": 0.003095480223723526, - "angularVelocity": 0.006457798977558507, - "velocityX": 1.603116849799888, - "velocityY": 0.9631070081402183, - "timestamp": 4.421488503516652 - }, - { - "x": 2.1195534915718537, - "y": 6.946933930333879, - "heading": 0.0031159494279625953, - "angularVelocity": 0.0002372945296031513, - "velocityX": 2.016710594475592, - "velocityY": 0.7114138531298224, - "timestamp": 4.507749254126039 - }, - { - "x": 2.3175113214846284, - "y": 6.974118315265626, - "heading": 0.0025073631392465968, - "angularVelocity": -0.007055193519841369, - "velocityX": 2.2948772009784966, - "velocityY": 0.31514199377705937, - "timestamp": 4.594010004735426 - }, - { - "x": 2.4838287511855577, - "y": 6.97404258239362, - "heading": 0.0016205368299748227, - "angularVelocity": -0.010280762722406305, - "velocityX": 1.928077700761749, - "velocityY": -0.0008779528519129953, - "timestamp": 4.6802707553448135 - }, - { - "x": 2.6088542637061045, - "y": 6.9677049199263745, - "heading": 0.0008452961970897526, - "angularVelocity": -0.008987176988472742, - "velocityX": 1.4493905007469428, - "velocityY": -0.07347098677525214, - "timestamp": 4.766531505954201 - }, - { - "x": 2.692116063326133, - "y": 6.961208581732824, - "heading": 0.0002899594520839453, - "angularVelocity": -0.006437884450142658, - "velocityX": 0.9652338871598958, - "velocityY": -0.07531047605842896, - "timestamp": 4.852792256563588 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 6.368402820587344e-22, - "angularVelocity": -0.003361429735256571, - "velocityX": 0.4819485634738517, - "velocityY": -0.04622118029966558, - "timestamp": 4.9390530071729755 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 3.1853758732676014e-22, - "angularVelocity": 5.381619199616603e-24, - "velocityX": -3.3871462049455533e-19, - "velocityY": -1.593318872358821e-19, - "timestamp": 5.025313757782363 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottom.1.traj b/src/main/deploy/choreo/NoTeamBottom.1.traj deleted file mode 100644 index e4b5b6a..0000000 --- a/src/main/deploy/choreo/NoTeamBottom.1.traj +++ /dev/null @@ -1,175 +0,0 @@ -{ - "samples": [ - { - "x": 0.7265444397926332, - "y": 4.389017105102539, - "heading": -1.051650239140039, - "angularVelocity": -1.9490781819297303e-18, - "velocityX": -1.0487369107199747e-16, - "velocityY": 2.7065156705910276e-17, - "timestamp": 0 - }, - { - "x": 0.7771701484751878, - "y": 4.375943969063242, - "heading": -1.0516503761135536, - "angularVelocity": -0.000001419156261399036, - "velocityX": 0.5245232431272688, - "velocityY": -0.13544825132569957, - "timestamp": 0.09651756970905265 - }, - { - "x": 0.8784215587828176, - "y": 4.349797698833336, - "heading": -1.0516507145054637, - "angularVelocity": -0.000003506013550567298, - "velocityX": 1.0490464131333472, - "velocityY": -0.27089648349747875, - "timestamp": 0.1930351394181053 - }, - { - "x": 1.0185995630841687, - "y": 4.313599368591188, - "heading": -1.034957242895387, - "angularVelocity": 0.172957852755694, - "velocityX": 1.4523573762156516, - "velocityY": -0.37504394641583094, - "timestamp": 0.2895527091271579 - }, - { - "x": 1.158777567423705, - "y": 4.277401038497066, - "heading": -0.9771905498626422, - "angularVelocity": 0.5985096102904218, - "velocityX": 1.4523573766112823, - "velocityY": -0.3750439448821489, - "timestamp": 0.3860702788362106 - }, - { - "x": 1.29895557174175, - "y": 4.241202708319719, - "heading": -0.8946611027304613, - "angularVelocity": 0.8550717489153622, - "velocityX": 1.452357376388613, - "velocityY": -0.3750439457444395, - "timestamp": 0.48258784854526326 - }, - { - "x": 1.4391335760555888, - "y": 4.205004378126083, - "heading": -0.7975443374250958, - "angularVelocity": 1.0062081504757965, - "velocityX": 1.4523573763450335, - "velocityY": -0.3750439459132019, - "timestamp": 0.5791054182543159 - }, - { - "x": 1.5793115803675255, - "y": 4.168806047925082, - "heading": -0.692145515107339, - "angularVelocity": 1.0920169523069798, - "velocityX": 1.452357376325328, - "velocityY": -0.3750439459895117, - "timestamp": 0.6756229879633686 - }, - { - "x": 1.71948958467887, - "y": 4.132607717721787, - "heading": -0.5825155459732, - "angularVelocity": 1.1358550517238752, - "velocityX": 1.45235737631919, - "velocityY": -0.37504394601328, - "timestamp": 0.7721405576724213 - }, - { - "x": 1.859667588990117, - "y": 4.096409387518115, - "heading": -0.4715528544072786, - "angularVelocity": 1.1496631328411298, - "velocityX": 1.4523573763181812, - "velocityY": -0.375043946017187, - "timestamp": 0.868658127381474 - }, - { - "x": 1.999845593301567, - "y": 4.060211057315229, - "heading": -0.36180710006295524, - "angularVelocity": 1.1370546800457813, - "velocityX": 1.4523573763202846, - "velocityY": -0.3750439460090416, - "timestamp": 0.9651756970905266 - }, - { - "x": 2.1400235976133968, - "y": 4.0240127271138135, - "heading": -0.2561858402181512, - "angularVelocity": 1.0943215847973948, - "velocityX": 1.4523573763242201, - "velocityY": -0.3750439459938017, - "timestamp": 1.0616932667995793 - }, - { - "x": 2.2802016019249636, - "y": 3.987814396911379, - "heading": -0.15875132018300867, - "angularVelocity": 1.0095003461945151, - "velocityX": 1.4523573763214923, - "velocityY": -0.3750439460043652, - "timestamp": 1.1582108365086319 - }, - { - "x": 2.4203796062348975, - "y": 3.9516160667026234, - "heading": -0.07580749268653832, - "angularVelocity": 0.859365064272749, - "velocityX": 1.4523573763045803, - "velocityY": -0.37504394606985697, - "timestamp": 1.2547284062176844 - }, - { - "x": 2.560557610534093, - "y": 3.91541773645228, - "heading": -0.017491102880811727, - "angularVelocity": 0.6042049129657698, - "velocityX": 1.4523573761933126, - "velocityY": -0.3750439465007366, - "timestamp": 1.351245975926737 - }, - { - "x": 2.7007356148590005, - "y": 3.879219406301364, - "heading": 5.051589087929001e-7, - "angularVelocity": 0.1812271909917254, - "velocityX": 1.4523573764597182, - "velocityY": -0.37504394547059267, - "timestamp": 1.4477635456357896 - }, - { - "x": 2.8019870251576924, - "y": 3.8530731360366373, - "heading": 1.4580072575536665e-7, - "angularVelocity": -0.0000037232410744759824, - "velocityX": 1.0490464130407438, - "velocityY": -0.2708964838582552, - "timestamp": 1.5442811153448421 - }, - { - "x": 2.852612733840942, - "y": 3.84, - "heading": 4.9602240502975986e-18, - "angularVelocity": -0.0000015106133003901624, - "velocityX": 0.5245232431344686, - "velocityY": -0.13544825129813873, - "timestamp": 1.6407986850538947 - }, - { - "x": 2.8526127338409424, - "y": 3.84, - "heading": 2.3793426412723873e-18, - "angularVelocity": -2.1367846851313794e-18, - "velocityX": -8.310450142659232e-17, - "velocityY": 2.0891089221855998e-17, - "timestamp": 1.7373162547629473 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottom.2.traj b/src/main/deploy/choreo/NoTeamBottom.2.traj deleted file mode 100644 index 3ea3a59..0000000 --- a/src/main/deploy/choreo/NoTeamBottom.2.traj +++ /dev/null @@ -1,184 +0,0 @@ -{ - "samples": [ - { - "x": 2.8526127338409424, - "y": 3.84, - "heading": 2.3793426412723873e-18, - "angularVelocity": -2.1367846851313794e-18, - "velocityX": -8.310450142659232e-17, - "velocityY": 2.0891089221855998e-17, - "timestamp": 0 - }, - { - "x": 2.8209562343759993, - "y": 3.856054932580381, - "heading": -0.0007560219648679484, - "angularVelocity": -0.009506842825878051, - "velocityX": -0.3980748957251135, - "velocityY": 0.20188794468215046, - "timestamp": 0.07952397853996551 - }, - { - "x": 2.7580363510582124, - "y": 3.8889174673683593, - "heading": -0.002267772169913451, - "angularVelocity": -0.019009992115595162, - "velocityX": -0.7912064319841098, - "velocityY": 0.4132405771356513, - "timestamp": 0.15904795707993102 - }, - { - "x": 2.6644940143973495, - "y": 3.9397288138759325, - "heading": -0.004533824337803499, - "angularVelocity": -0.028495206219482882, - "velocityX": -1.1762783801599213, - "velocityY": 0.6389437178628763, - "timestamp": 0.23857193561989654 - }, - { - "x": 2.541539594192653, - "y": 4.010410332743005, - "heading": -0.007548770346538153, - "angularVelocity": -0.037912414143357574, - "velocityX": -1.546130141651605, - "velocityY": 0.8888076296578984, - "timestamp": 0.3180959141598618 - }, - { - "x": 2.3921429241592778, - "y": 4.104771009771783, - "heading": -0.011289431433418899, - "angularVelocity": -0.04703815321564728, - "velocityX": -1.8786367681327063, - "velocityY": 1.1865688659095006, - "timestamp": 0.39761989269982734 - }, - { - "x": 2.2285981442653044, - "y": 4.231685243413465, - "heading": -0.015605626770518823, - "angularVelocity": -0.05427539487263933, - "velocityX": -2.056546753527704, - "velocityY": 1.5959240970055453, - "timestamp": 0.47714387123979285 - }, - { - "x": 2.0932777236393827, - "y": 4.3801240033728215, - "heading": -0.01970246217760219, - "angularVelocity": -0.05151698245359373, - "velocityX": -1.7016304152578978, - "velocityY": 1.8665912179526678, - "timestamp": 0.5566678497797584 - }, - { - "x": 1.9933780770975498, - "y": 4.530859168690553, - "heading": -0.02318390168855428, - "angularVelocity": -0.04377848763190915, - "velocityX": -1.2562204303149604, - "velocityY": 1.895468110187389, - "timestamp": 0.6361918283197239 - }, - { - "x": 1.9287898956537786, - "y": 4.677988681014765, - "heading": -0.025975407403759726, - "angularVelocity": -0.035102691873024824, - "velocityX": -0.812184986586304, - "velocityY": 1.850127659926749, - "timestamp": 0.7157158068596894 - }, - { - "x": 1.8991532059051248, - "y": 4.818930146657707, - "heading": -0.028052234646139453, - "angularVelocity": -0.02611573616548874, - "velocityX": -0.37267614489081113, - "velocityY": 1.7723140646454278, - "timestamp": 0.7952397853996549 - }, - { - "x": 1.90418541431427, - "y": 4.952258110046387, - "heading": -0.029402891635568452, - "angularVelocity": -0.016984273350335675, - "velocityX": 0.06327913292990113, - "velocityY": 1.6765756170219066, - "timestamp": 0.8747637639396204 - }, - { - "x": 1.9488424853011135, - "y": 5.08490987258151, - "heading": -0.030009855294308496, - "angularVelocity": -0.007148386818663905, - "velocityX": 0.5259392601282725, - "velocityY": 1.5622782305402025, - "timestamp": 0.9596729433303202 - }, - { - "x": 2.031885999740562, - "y": 5.204759123935114, - "heading": -0.029768448326245058, - "angularVelocity": 0.002843119787468783, - "velocityX": 0.9780275234710838, - "velocityY": 1.411499348052011, - "timestamp": 1.04458212272102 - }, - { - "x": 2.150567006153512, - "y": 5.305440928887294, - "heading": -0.028675027108268608, - "angularVelocity": 0.012877538398353833, - "velocityX": 1.397740588998668, - "velocityY": 1.1857587798476337, - "timestamp": 1.1294913021117197 - }, - { - "x": 2.290536940712368, - "y": 5.371710342064258, - "heading": -0.026887862525856107, - "angularVelocity": 0.021047954947121472, - "velocityX": 1.6484664622042853, - "velocityY": 0.7804740742109001, - "timestamp": 1.2144004815024194 - }, - { - "x": 2.406483690373778, - "y": 5.405416808831692, - "heading": -0.025158746552686077, - "angularVelocity": 0.02036429966203896, - "velocityX": 1.3655384552463332, - "velocityY": 0.39697082234581244, - "timestamp": 1.2993096608931192 - }, - { - "x": 2.485516159596883, - "y": 5.4225466650859895, - "heading": -0.023904634648769633, - "angularVelocity": 0.014770039151430181, - "velocityX": 0.9307882821413964, - "velocityY": 0.20174327884476004, - "timestamp": 1.384218840283819 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": 0.0076948823555783585, - "velocityX": 0.4693772631942069, - "velocityY": 0.08249354824799049, - "timestamp": 1.4691280196745187 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": -8.395893895847363e-23, - "velocityX": 1.4623773604187483e-19, - "velocityY": -1.3212521106474066e-18, - "timestamp": 1.5540371990652184 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottom.3.traj b/src/main/deploy/choreo/NoTeamBottom.3.traj deleted file mode 100644 index 30b5952..0000000 --- a/src/main/deploy/choreo/NoTeamBottom.3.traj +++ /dev/null @@ -1,202 +0,0 @@ -{ - "samples": [ - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": -8.395893895847363e-23, - "velocityX": 1.4623773604187483e-19, - "velocityY": -1.3212521106474066e-18, - "timestamp": 0 - }, - { - "x": 2.4932837152722374, - "y": 5.448396032908335, - "heading": -0.022770666861651127, - "angularVelocity": 0.005902466591950069, - "velocityX": -0.394072213481467, - "velocityY": 0.23144207684009668, - "timestamp": 0.08142386463547746 - }, - { - "x": 2.429535152050195, - "y": 5.486791798588922, - "heading": -0.021801144225693077, - "angularVelocity": 0.01190710659950195, - "velocityX": -0.7829223472434627, - "velocityY": 0.47155420407122456, - "timestamp": 0.16284772927095492 - }, - { - "x": 2.334824983189393, - "y": 5.545829176600406, - "heading": -0.020330384435373747, - "angularVelocity": 0.018063006428197637, - "velocityX": -1.1631745715436788, - "velocityY": 0.7250623422971361, - "timestamp": 0.24427159390643238 - }, - { - "x": 2.2105039579858143, - "y": 5.627403033507208, - "heading": -0.018338524798568293, - "angularVelocity": 0.02446284815542353, - "velocityX": -1.5268376876993603, - "velocityY": 1.0018421168289628, - "timestamp": 0.32569545854190984 - }, - { - "x": 2.060080687324347, - "y": 5.7354979281499965, - "heading": -0.01579076613135179, - "angularVelocity": 0.031290072985634236, - "velocityX": -1.8474101092460986, - "velocityY": 1.3275578987402952, - "timestamp": 0.4071193231773873 - }, - { - "x": 1.90097644380825, - "y": 5.879777464179869, - "heading": -0.012665610755061086, - "angularVelocity": 0.03838131965709989, - "velocityX": -1.9540247104258965, - "velocityY": 1.77195637514615, - "timestamp": 0.48854318781286477 - }, - { - "x": 1.7772040422925621, - "y": 6.035734728989453, - "heading": -0.009508151519862748, - "angularVelocity": 0.03877805664633832, - "velocityX": -1.5200998143455622, - "velocityY": 1.9153753694667015, - "timestamp": 0.5699670524483422 - }, - { - "x": 1.6904452913414765, - "y": 6.187858417258873, - "heading": -0.00660634699813739, - "angularVelocity": 0.03563825586904154, - "velocityX": -1.0655199349661346, - "velocityY": 1.8682936378720785, - "timestamp": 0.6513909170838197 - }, - { - "x": 1.6399494790279758, - "y": 6.331633207559586, - "heading": -0.004035525910181172, - "angularVelocity": 0.03157331206846292, - "velocityX": -0.6201598577955342, - "velocityY": 1.7657573850660415, - "timestamp": 0.7328147817192971 - }, - { - "x": 1.6251865837463155, - "y": 6.465022310434175, - "heading": -0.0018276262317727312, - "angularVelocity": 0.027116124839970116, - "velocityX": -0.18130919415028465, - "velocityY": 1.6382064824821505, - "timestamp": 0.814238646354775 - }, - { - "x": 1.645802617073059, - "y": 6.586877346038818, - "heading": 3.400199739559167e-21, - "angularVelocity": 0.022445830100975162, - "velocityX": 0.25319399194619935, - "velocityY": 1.4965518543018048, - "timestamp": 0.895662510990253 - }, - { - "x": 1.7070719226916355, - "y": 6.702201095743444, - "heading": 0.001497238017935697, - "angularVelocity": 0.017357117852509537, - "velocityX": 0.710280226406013, - "velocityY": 1.3369203098224118, - "timestamp": 0.9819232618048948 - }, - { - "x": 1.8073044586022589, - "y": 6.8024885050101265, - "heading": 0.002538425575927766, - "angularVelocity": 0.012070235282665064, - "velocityX": 1.1619715219730005, - "velocityY": 1.1626076555046687, - "timestamp": 1.0681840126195365 - }, - { - "x": 1.9455905214116462, - "y": 6.885566838960804, - "heading": 0.003095480119106647, - "angularVelocity": 0.006457798453156142, - "velocityX": 1.6031168463457748, - "velocityY": 0.9631070117763845, - "timestamp": 1.1544447634341783 - }, - { - "x": 2.119553491307074, - "y": 6.946933932487094, - "heading": 0.003115949285592413, - "angularVelocity": 0.00023729409137361653, - "velocityX": 2.016710592622141, - "velocityY": 0.711413857945154, - "timestamp": 1.2407055142488201 - }, - { - "x": 2.317511319909413, - "y": 6.974118317411219, - "heading": 0.002507363017790665, - "angularVelocity": -0.007055193260599903, - "velocityX": 2.2948771803263415, - "velocityY": 0.3151419929388198, - "timestamp": 1.326966265063462 - }, - { - "x": 2.4838287501732563, - "y": 6.974042584379811, - "heading": 0.0016205367707247194, - "angularVelocity": -0.01028076197680645, - "velocityX": 1.9280777026996743, - "velocityY": -0.0008779546977302001, - "timestamp": 1.4132270158781037 - }, - { - "x": 2.6088542632161906, - "y": 6.967704921114638, - "heading": 0.0008452961733031628, - "angularVelocity": -0.008987176555968111, - "velocityX": 1.4493905033540737, - "velocityY": -0.07347099585060349, - "timestamp": 1.4994877666927455 - }, - { - "x": 2.6921160631697596, - "y": 6.961208582170421, - "heading": 0.00028995944566665723, - "angularVelocity": -0.006437884233465802, - "velocityX": 0.9652338887298002, - "velocityY": -0.07531048458152965, - "timestamp": 1.5857485175073873 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 1.3621691331615836e-21, - "angularVelocity": -0.003361429652864086, - "velocityX": 0.48194856413987436, - "velocityY": -0.04622118526264293, - "timestamp": 1.6720092683220291 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 6.82640346993216e-22, - "angularVelocity": 4.0562645330660325e-23, - "velocityX": -4.3269580997377834e-19, - "velocityY": -2.905097183597039e-19, - "timestamp": 1.758270019136671 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottom.traj b/src/main/deploy/choreo/NoTeamBottom.traj deleted file mode 100644 index 4ec8aa1..0000000 --- a/src/main/deploy/choreo/NoTeamBottom.traj +++ /dev/null @@ -1,535 +0,0 @@ -{ - "samples": [ - { - "x": 0.7265444397926332, - "y": 4.389017105102539, - "heading": -1.051650239140039, - "angularVelocity": -1.9490781819297303e-18, - "velocityX": -1.0487369107199747e-16, - "velocityY": 2.7065156705910276e-17, - "timestamp": 0 - }, - { - "x": 0.7771701484751878, - "y": 4.375943969063242, - "heading": -1.0516503761135536, - "angularVelocity": -0.000001419156261399036, - "velocityX": 0.5245232431272688, - "velocityY": -0.13544825132569957, - "timestamp": 0.09651756970905265 - }, - { - "x": 0.8784215587828176, - "y": 4.349797698833336, - "heading": -1.0516507145054637, - "angularVelocity": -0.000003506013550567298, - "velocityX": 1.0490464131333472, - "velocityY": -0.27089648349747875, - "timestamp": 0.1930351394181053 - }, - { - "x": 1.0185995630841687, - "y": 4.313599368591188, - "heading": -1.034957242895387, - "angularVelocity": 0.172957852755694, - "velocityX": 1.4523573762156516, - "velocityY": -0.37504394641583094, - "timestamp": 0.2895527091271579 - }, - { - "x": 1.158777567423705, - "y": 4.277401038497066, - "heading": -0.9771905498626422, - "angularVelocity": 0.5985096102904218, - "velocityX": 1.4523573766112823, - "velocityY": -0.3750439448821489, - "timestamp": 0.3860702788362106 - }, - { - "x": 1.29895557174175, - "y": 4.241202708319719, - "heading": -0.8946611027304613, - "angularVelocity": 0.8550717489153622, - "velocityX": 1.452357376388613, - "velocityY": -0.3750439457444395, - "timestamp": 0.48258784854526326 - }, - { - "x": 1.4391335760555888, - "y": 4.205004378126083, - "heading": -0.7975443374250958, - "angularVelocity": 1.0062081504757965, - "velocityX": 1.4523573763450335, - "velocityY": -0.3750439459132019, - "timestamp": 0.5791054182543159 - }, - { - "x": 1.5793115803675255, - "y": 4.168806047925082, - "heading": -0.692145515107339, - "angularVelocity": 1.0920169523069798, - "velocityX": 1.452357376325328, - "velocityY": -0.3750439459895117, - "timestamp": 0.6756229879633686 - }, - { - "x": 1.71948958467887, - "y": 4.132607717721787, - "heading": -0.5825155459732, - "angularVelocity": 1.1358550517238752, - "velocityX": 1.45235737631919, - "velocityY": -0.37504394601328, - "timestamp": 0.7721405576724213 - }, - { - "x": 1.859667588990117, - "y": 4.096409387518115, - "heading": -0.4715528544072786, - "angularVelocity": 1.1496631328411298, - "velocityX": 1.4523573763181812, - "velocityY": -0.375043946017187, - "timestamp": 0.868658127381474 - }, - { - "x": 1.999845593301567, - "y": 4.060211057315229, - "heading": -0.36180710006295524, - "angularVelocity": 1.1370546800457813, - "velocityX": 1.4523573763202846, - "velocityY": -0.3750439460090416, - "timestamp": 0.9651756970905266 - }, - { - "x": 2.1400235976133968, - "y": 4.0240127271138135, - "heading": -0.2561858402181512, - "angularVelocity": 1.0943215847973948, - "velocityX": 1.4523573763242201, - "velocityY": -0.3750439459938017, - "timestamp": 1.0616932667995793 - }, - { - "x": 2.2802016019249636, - "y": 3.987814396911379, - "heading": -0.15875132018300867, - "angularVelocity": 1.0095003461945151, - "velocityX": 1.4523573763214923, - "velocityY": -0.3750439460043652, - "timestamp": 1.1582108365086319 - }, - { - "x": 2.4203796062348975, - "y": 3.9516160667026234, - "heading": -0.07580749268653832, - "angularVelocity": 0.859365064272749, - "velocityX": 1.4523573763045803, - "velocityY": -0.37504394606985697, - "timestamp": 1.2547284062176844 - }, - { - "x": 2.560557610534093, - "y": 3.91541773645228, - "heading": -0.017491102880811727, - "angularVelocity": 0.6042049129657698, - "velocityX": 1.4523573761933126, - "velocityY": -0.3750439465007366, - "timestamp": 1.351245975926737 - }, - { - "x": 2.7007356148590005, - "y": 3.879219406301364, - "heading": 5.051589087929001e-7, - "angularVelocity": 0.1812271909917254, - "velocityX": 1.4523573764597182, - "velocityY": -0.37504394547059267, - "timestamp": 1.4477635456357896 - }, - { - "x": 2.8019870251576924, - "y": 3.8530731360366373, - "heading": 1.4580072575536665e-7, - "angularVelocity": -0.0000037232410744759824, - "velocityX": 1.0490464130407438, - "velocityY": -0.2708964838582552, - "timestamp": 1.5442811153448421 - }, - { - "x": 2.852612733840942, - "y": 3.84, - "heading": 4.9602240502975986e-18, - "angularVelocity": -0.0000015106133003901624, - "velocityX": 0.5245232431344686, - "velocityY": -0.13544825129813873, - "timestamp": 1.6407986850538947 - }, - { - "x": 2.8526127338409424, - "y": 3.84, - "heading": 2.3793426412723873e-18, - "angularVelocity": -2.1367846851313794e-18, - "velocityX": -8.310450142659232e-17, - "velocityY": 2.0891089221855998e-17, - "timestamp": 1.7373162547629473 - }, - { - "x": 2.8209562343759993, - "y": 3.856054932580381, - "heading": -0.0007560219648679484, - "angularVelocity": -0.009506842825878051, - "velocityX": -0.3980748957251135, - "velocityY": 0.20188794468215046, - "timestamp": 1.8168402333029128 - }, - { - "x": 2.7580363510582124, - "y": 3.8889174673683593, - "heading": -0.002267772169913451, - "angularVelocity": -0.019009992115595162, - "velocityX": -0.7912064319841098, - "velocityY": 0.4132405771356513, - "timestamp": 1.8963642118428783 - }, - { - "x": 2.6644940143973495, - "y": 3.9397288138759325, - "heading": -0.004533824337803499, - "angularVelocity": -0.028495206219482882, - "velocityX": -1.1762783801599213, - "velocityY": 0.6389437178628763, - "timestamp": 1.9758881903828438 - }, - { - "x": 2.541539594192653, - "y": 4.010410332743005, - "heading": -0.007548770346538153, - "angularVelocity": -0.037912414143357574, - "velocityX": -1.546130141651605, - "velocityY": 0.8888076296578984, - "timestamp": 2.055412168922809 - }, - { - "x": 2.3921429241592778, - "y": 4.104771009771783, - "heading": -0.011289431433418899, - "angularVelocity": -0.04703815321564728, - "velocityX": -1.8786367681327063, - "velocityY": 1.1865688659095006, - "timestamp": 2.1349361474627746 - }, - { - "x": 2.2285981442653044, - "y": 4.231685243413465, - "heading": -0.015605626770518823, - "angularVelocity": -0.05427539487263933, - "velocityX": -2.056546753527704, - "velocityY": 1.5959240970055453, - "timestamp": 2.21446012600274 - }, - { - "x": 2.0932777236393827, - "y": 4.3801240033728215, - "heading": -0.01970246217760219, - "angularVelocity": -0.05151698245359373, - "velocityX": -1.7016304152578978, - "velocityY": 1.8665912179526678, - "timestamp": 2.2939841045427056 - }, - { - "x": 1.9933780770975498, - "y": 4.530859168690553, - "heading": -0.02318390168855428, - "angularVelocity": -0.04377848763190915, - "velocityX": -1.2562204303149604, - "velocityY": 1.895468110187389, - "timestamp": 2.373508083082671 - }, - { - "x": 1.9287898956537786, - "y": 4.677988681014765, - "heading": -0.025975407403759726, - "angularVelocity": -0.035102691873024824, - "velocityX": -0.812184986586304, - "velocityY": 1.850127659926749, - "timestamp": 2.4530320616226367 - }, - { - "x": 1.8991532059051248, - "y": 4.818930146657707, - "heading": -0.028052234646139453, - "angularVelocity": -0.02611573616548874, - "velocityX": -0.37267614489081113, - "velocityY": 1.7723140646454278, - "timestamp": 2.532556040162602 - }, - { - "x": 1.90418541431427, - "y": 4.952258110046387, - "heading": -0.029402891635568452, - "angularVelocity": -0.016984273350335675, - "velocityX": 0.06327913292990113, - "velocityY": 1.6765756170219066, - "timestamp": 2.6120800187025677 - }, - { - "x": 1.9488424853011135, - "y": 5.08490987258151, - "heading": -0.030009855294308496, - "angularVelocity": -0.007148386818663905, - "velocityX": 0.5259392601282725, - "velocityY": 1.5622782305402025, - "timestamp": 2.6969891980932674 - }, - { - "x": 2.031885999740562, - "y": 5.204759123935114, - "heading": -0.029768448326245058, - "angularVelocity": 0.002843119787468783, - "velocityX": 0.9780275234710838, - "velocityY": 1.411499348052011, - "timestamp": 2.781898377483967 - }, - { - "x": 2.150567006153512, - "y": 5.305440928887294, - "heading": -0.028675027108268608, - "angularVelocity": 0.012877538398353833, - "velocityX": 1.397740588998668, - "velocityY": 1.1857587798476337, - "timestamp": 2.866807556874667 - }, - { - "x": 2.290536940712368, - "y": 5.371710342064258, - "heading": -0.026887862525856107, - "angularVelocity": 0.021047954947121472, - "velocityX": 1.6484664622042853, - "velocityY": 0.7804740742109001, - "timestamp": 2.9517167362653667 - }, - { - "x": 2.406483690373778, - "y": 5.405416808831692, - "heading": -0.025158746552686077, - "angularVelocity": 0.02036429966203896, - "velocityX": 1.3655384552463332, - "velocityY": 0.39697082234581244, - "timestamp": 3.0366259156560664 - }, - { - "x": 2.485516159596883, - "y": 5.4225466650859895, - "heading": -0.023904634648769633, - "angularVelocity": 0.014770039151430181, - "velocityX": 0.9307882821413964, - "velocityY": 0.20174327884476004, - "timestamp": 3.121535095046766 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": 0.0076948823555783585, - "velocityX": 0.4693772631942069, - "velocityY": 0.08249354824799049, - "timestamp": 3.206444274437466 - }, - { - "x": 2.5253705978393555, - "y": 5.429551124572754, - "heading": -0.023251268502449497, - "angularVelocity": -8.395893895847363e-23, - "velocityX": 1.4623773604187483e-19, - "velocityY": -1.3212521106474066e-18, - "timestamp": 3.2913534538281657 - }, - { - "x": 2.4932837152722374, - "y": 5.448396032908335, - "heading": -0.022770666861651127, - "angularVelocity": 0.005902466591950069, - "velocityX": -0.394072213481467, - "velocityY": 0.23144207684009668, - "timestamp": 3.372777318463643 - }, - { - "x": 2.429535152050195, - "y": 5.486791798588922, - "heading": -0.021801144225693077, - "angularVelocity": 0.01190710659950195, - "velocityX": -0.7829223472434627, - "velocityY": 0.47155420407122456, - "timestamp": 3.4542011830991206 - }, - { - "x": 2.334824983189393, - "y": 5.545829176600406, - "heading": -0.020330384435373747, - "angularVelocity": 0.018063006428197637, - "velocityX": -1.1631745715436788, - "velocityY": 0.7250623422971361, - "timestamp": 3.535625047734598 - }, - { - "x": 2.2105039579858143, - "y": 5.627403033507208, - "heading": -0.018338524798568293, - "angularVelocity": 0.02446284815542353, - "velocityX": -1.5268376876993603, - "velocityY": 1.0018421168289628, - "timestamp": 3.6170489123700755 - }, - { - "x": 2.060080687324347, - "y": 5.7354979281499965, - "heading": -0.01579076613135179, - "angularVelocity": 0.031290072985634236, - "velocityX": -1.8474101092460986, - "velocityY": 1.3275578987402952, - "timestamp": 3.698472777005553 - }, - { - "x": 1.90097644380825, - "y": 5.879777464179869, - "heading": -0.012665610755061086, - "angularVelocity": 0.03838131965709989, - "velocityX": -1.9540247104258965, - "velocityY": 1.77195637514615, - "timestamp": 3.7798966416410305 - }, - { - "x": 1.7772040422925621, - "y": 6.035734728989453, - "heading": -0.009508151519862748, - "angularVelocity": 0.03877805664633832, - "velocityX": -1.5200998143455622, - "velocityY": 1.9153753694667015, - "timestamp": 3.861320506276508 - }, - { - "x": 1.6904452913414765, - "y": 6.187858417258873, - "heading": -0.00660634699813739, - "angularVelocity": 0.03563825586904154, - "velocityX": -1.0655199349661346, - "velocityY": 1.8682936378720785, - "timestamp": 3.9427443709119854 - }, - { - "x": 1.6399494790279758, - "y": 6.331633207559586, - "heading": -0.004035525910181172, - "angularVelocity": 0.03157331206846292, - "velocityX": -0.6201598577955342, - "velocityY": 1.7657573850660415, - "timestamp": 4.024168235547463 - }, - { - "x": 1.6251865837463155, - "y": 6.465022310434175, - "heading": -0.0018276262317727312, - "angularVelocity": 0.027116124839970116, - "velocityX": -0.18130919415028465, - "velocityY": 1.6382064824821505, - "timestamp": 4.105592100182941 - }, - { - "x": 1.645802617073059, - "y": 6.586877346038818, - "heading": 3.400199739559167e-21, - "angularVelocity": 0.022445830100975162, - "velocityX": 0.25319399194619935, - "velocityY": 1.4965518543018048, - "timestamp": 4.187015964818419 - }, - { - "x": 1.7070719226916355, - "y": 6.702201095743444, - "heading": 0.001497238017935697, - "angularVelocity": 0.017357117852509537, - "velocityX": 0.710280226406013, - "velocityY": 1.3369203098224118, - "timestamp": 4.27327671563306 - }, - { - "x": 1.8073044586022589, - "y": 6.8024885050101265, - "heading": 0.002538425575927766, - "angularVelocity": 0.012070235282665064, - "velocityX": 1.1619715219730005, - "velocityY": 1.1626076555046687, - "timestamp": 4.359537466447702 - }, - { - "x": 1.9455905214116462, - "y": 6.885566838960804, - "heading": 0.003095480119106647, - "angularVelocity": 0.006457798453156142, - "velocityX": 1.6031168463457748, - "velocityY": 0.9631070117763845, - "timestamp": 4.445798217262344 - }, - { - "x": 2.119553491307074, - "y": 6.946933932487094, - "heading": 0.003115949285592413, - "angularVelocity": 0.00023729409137361653, - "velocityX": 2.016710592622141, - "velocityY": 0.711413857945154, - "timestamp": 4.532058968076986 - }, - { - "x": 2.317511319909413, - "y": 6.974118317411219, - "heading": 0.002507363017790665, - "angularVelocity": -0.007055193260599903, - "velocityX": 2.2948771803263415, - "velocityY": 0.3151419929388198, - "timestamp": 4.618319718891628 - }, - { - "x": 2.4838287501732563, - "y": 6.974042584379811, - "heading": 0.0016205367707247194, - "angularVelocity": -0.01028076197680645, - "velocityX": 1.9280777026996743, - "velocityY": -0.0008779546977302001, - "timestamp": 4.704580469706269 - }, - { - "x": 2.6088542632161906, - "y": 6.967704921114638, - "heading": 0.0008452961733031628, - "angularVelocity": -0.008987176555968111, - "velocityX": 1.4493905033540737, - "velocityY": -0.07347099585060349, - "timestamp": 4.790841220520911 - }, - { - "x": 2.6921160631697596, - "y": 6.961208582170421, - "heading": 0.00028995944566665723, - "angularVelocity": -0.006437884233465802, - "velocityX": 0.9652338887298002, - "velocityY": -0.07531048458152965, - "timestamp": 4.877101971335553 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 1.3621691331615836e-21, - "angularVelocity": -0.003361429652864086, - "velocityX": 0.48194856413987436, - "velocityY": -0.04622118526264293, - "timestamp": 4.963362722150195 - }, - { - "x": 2.733689308166504, - "y": 6.957221508026123, - "heading": 6.82640346993216e-22, - "angularVelocity": 4.0562645330660325e-23, - "velocityX": -4.3269580997377834e-19, - "velocityY": -2.905097183597039e-19, - "timestamp": 5.049623472964837 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottomStraight.1.traj b/src/main/deploy/choreo/NoTeamBottomStraight.1.traj deleted file mode 100644 index 74dabe5..0000000 --- a/src/main/deploy/choreo/NoTeamBottomStraight.1.traj +++ /dev/null @@ -1,346 +0,0 @@ -{ - "samples": [ - { - "x": 0.727, - "y": 4.389017105102539, - "heading": -1.052, - "angularVelocity": -1.9035720406977927e-18, - "velocityX": -7.789821171233355e-19, - "velocityY": 6.642220463091642e-19, - "timestamp": 0 - }, - { - "x": 0.7318438956432248, - "y": 4.379292392557921, - "heading": -1.0218708963854122, - "angularVelocity": 0.6376018021899323, - "velocityX": 0.10250808093224481, - "velocityY": -0.20579750143065442, - "timestamp": 0.047253793058779206 - }, - { - "x": 0.7416803389522922, - "y": 4.359743623946586, - "heading": -0.963247947139427, - "angularVelocity": 1.2405977478477537, - "velocityX": 0.20816198388205162, - "velocityY": -0.41369734249730467, - "timestamp": 0.09450758611755841 - }, - { - "x": 0.756679070865091, - "y": 4.330228659181428, - "heading": -0.8784548898183928, - "angularVelocity": 1.7944180103290261, - "velocityX": 0.31740799927196917, - "velocityY": -0.6246051979032636, - "timestamp": 0.14176137917633763 - }, - { - "x": 0.7769909237075252, - "y": 4.290540404198398, - "heading": -0.7706337125535929, - "angularVelocity": 2.2817465072207144, - "velocityX": 0.4298459769603722, - "velocityY": -0.8398956446450863, - "timestamp": 0.18901517223511682 - }, - { - "x": 0.8027070398966437, - "y": 4.240415566240092, - "heading": -0.6435269412573291, - "angularVelocity": 2.689874464429873, - "velocityX": 0.544212739856253, - "velocityY": -1.0607579775861957, - "timestamp": 0.23626896529389602 - }, - { - "x": 0.8338645488878723, - "y": 4.179590349127875, - "heading": -0.5009199000549501, - "angularVelocity": 3.0178961723768816, - "velocityX": 0.659365248255766, - "velocityY": -1.28720285028876, - "timestamp": 0.2835227583526752 - }, - { - "x": 0.8705177044270638, - "y": 4.107851215829094, - "heading": -0.34654429878237253, - "angularVelocity": 3.266946233936165, - "velocityX": 0.7756658919126009, - "velocityY": -1.518166662505675, - "timestamp": 0.3307765514114544 - }, - { - "x": 0.9127860516643015, - "y": 4.025035475201868, - "heading": -0.18511651921150357, - "angularVelocity": 3.416186704209506, - "velocityX": 0.8944963885684252, - "velocityY": -1.752573397107217, - "timestamp": 0.3780303444702336 - }, - { - "x": 0.9608190780560075, - "y": 3.9310914505496757, - "heading": -0.02385519507028553, - "angularVelocity": 3.412664120753743, - "velocityX": 1.0164903869612656, - "velocityY": -1.9880737306174552, - "timestamp": 0.4252841375290128 - }, - { - "x": 1.0146390571858634, - "y": 3.8264366161374967, - "heading": 0.1250934065508822, - "angularVelocity": 3.152098318031103, - "velocityX": 1.138955746111406, - "velocityY": -2.2147393391679286, - "timestamp": 0.472537930587792 - }, - { - "x": 1.0745072439716545, - "y": 3.7116431971477404, - "heading": 0.2562841881014644, - "angularVelocity": 2.776301605828625, - "velocityX": 1.266949866041881, - "velocityY": -2.4292953339632333, - "timestamp": 0.5197917236465712 - }, - { - "x": 1.1413904234655279, - "y": 3.5874142558818387, - "heading": 0.36909871419180623, - "angularVelocity": 2.3874173645703185, - "velocityX": 1.4154034028692064, - "velocityY": -2.6289728977179108, - "timestamp": 0.5670455167053504 - }, - { - "x": 1.2171004406920223, - "y": 3.4555713477306926, - "heading": 0.46254553428918027, - "angularVelocity": 1.9775517275647088, - "velocityX": 1.6021997881168686, - "velocityY": -2.790102119149382, - "timestamp": 0.6142993097641296 - }, - { - "x": 1.304824543975774, - "y": 3.3227207721324725, - "heading": 0.5413110165917999, - "angularVelocity": 1.6668605249242734, - "velocityX": 1.856445749754547, - "velocityY": -2.811426702465267, - "timestamp": 0.6615531028229088 - }, - { - "x": 1.4008888245081486, - "y": 3.1991200061349105, - "heading": 0.6262952838301894, - "angularVelocity": 1.798464456232692, - "velocityX": 2.0329432689748255, - "velocityY": -2.615679250209918, - "timestamp": 0.708806895881688 - }, - { - "x": 1.5007168068199321, - "y": 3.0869110822536374, - "heading": 0.7270728004213672, - "angularVelocity": 2.1326862896661085, - "velocityX": 2.1125919391826877, - "velocityY": -2.3746014154185615, - "timestamp": 0.7560606889404672 - }, - { - "x": 1.601781282376718, - "y": 2.9867599379462364, - "heading": 0.8415198083717181, - "angularVelocity": 2.4219644718888445, - "velocityX": 2.1387590077916334, - "velocityY": -2.119430797498578, - "timestamp": 0.8033144819992464 - }, - { - "x": 1.7021006490957822, - "y": 2.8990265047996244, - "heading": 0.9624877761029027, - "angularVelocity": 2.5599631246683616, - "velocityX": 2.122990774397232, - "velocityY": -1.856643191319687, - "timestamp": 0.8505682750580256 - }, - { - "x": 1.8004198198487906, - "y": 2.8236557821766897, - "heading": 1.0813753656601117, - "angularVelocity": 2.515937491183494, - "velocityX": 2.080661982641449, - "velocityY": -1.5950195263516973, - "timestamp": 0.8978220681168048 - }, - { - "x": 1.8960745245056285, - "y": 2.7604196812117996, - "heading": 1.1931387342522881, - "angularVelocity": 2.365172430775102, - "velocityX": 2.0242756922783456, - "velocityY": -1.3382227514779625, - "timestamp": 0.945075861175584 - }, - { - "x": 1.9886778939409326, - "y": 2.709098454682459, - "heading": 1.294430400917861, - "angularVelocity": 2.143566899266176, - "velocityX": 1.9597023527849797, - "velocityY": -1.0860763381577074, - "timestamp": 0.9923296542343631 - }, - { - "x": 2.0779902618648065, - "y": 2.6695027774627897, - "heading": 1.3827319365638684, - "angularVelocity": 1.868665559528068, - "velocityX": 1.8900571180133192, - "velocityY": -0.8379364841763153, - "timestamp": 1.0395834472931424 - }, - { - "x": 2.163854051486685, - "y": 2.641470045467505, - "heading": 1.4560082555180538, - "angularVelocity": 1.550697080825592, - "velocityX": 1.81707719240807, - "velocityY": -0.5932377102598947, - "timestamp": 1.0868372403519218 - }, - { - "x": 2.246158483677488, - "y": 2.624860911919073, - "heading": 1.5125976444748226, - "angularVelocity": 1.1975628895309458, - "velocityX": 1.7417529231658644, - "velocityY": -0.35148783776513787, - "timestamp": 1.134091033410701 - }, - { - "x": 2.3248213255730343, - "y": 2.6195588118421886, - "heading": 1.5511852292241632, - "angularVelocity": 0.8166029063813219, - "velocityX": 1.6646884155457526, - "velocityY": -0.11220475085013962, - "timestamp": 1.1813448264694804 - }, - { - "x": 2.3997801498313853, - "y": 2.6254709122263833, - "heading": 1.5707963267948966, - "angularVelocity": 0.41501636802656, - "velocityX": 1.5863028003933837, - "velocityY": 0.12511377397451495, - "timestamp": 1.2285986195282597 - }, - { - "x": 2.470987558364868, - "y": 2.642529249191284, - "heading": 1.5707963267948966, - "angularVelocity": 7.2320671802386e-18, - "velocityX": 1.5069141316318813, - "velocityY": 0.360994025255961, - "timestamp": 1.275852412587039 - }, - { - "x": 2.5896469460709723, - "y": 2.7162173203691675, - "heading": 1.5707635810166263, - "angularVelocity": -0.0003685297147391131, - "velocityX": 1.3354243695676342, - "velocityY": 0.8293051894141662, - "timestamp": 1.3647076037982955 - }, - { - "x": 2.691674793744484, - "y": 2.8309800714405235, - "heading": 1.570698306188048, - "angularVelocity": -0.0007346203152394417, - "velocityX": 1.1482485860723253, - "velocityY": 1.2915705825054542, - "timestamp": 1.453562795009552 - }, - { - "x": 2.7746449871658987, - "y": 2.9857496097500946, - "heading": 1.5706009997851365, - "angularVelocity": -0.0010951121885513847, - "velocityX": 0.933768666640422, - "velocityY": 1.7418176270826997, - "timestamp": 1.5424179862208085 - }, - { - "x": 2.8334018266497893, - "y": 3.177633163967886, - "heading": 1.5704732520807607, - "angularVelocity": -0.0014377067072201901, - "velocityX": 0.6612651290591912, - "velocityY": 2.159508652247243, - "timestamp": 1.631273177432065 - }, - { - "x": 2.8534735329147445, - "y": 3.39113142463186, - "heading": 1.5703246879122892, - "angularVelocity": -0.0016719807413194167, - "velocityX": 0.2258923310089341, - "velocityY": 2.4027663184739962, - "timestamp": 1.7201283686433215 - }, - { - "x": 2.8490781305616832, - "y": 3.5676824837315517, - "heading": 1.5701962202028332, - "angularVelocity": -0.0014458098362590802, - "velocityX": -0.04946702936704393, - "velocityY": 1.9869526663887853, - "timestamp": 1.808983559854578 - }, - { - "x": 2.839727568645754, - "y": 3.7001973861849375, - "heading": 1.570098429466109, - "angularVelocity": -0.0011005630103461549, - "velocityX": -0.10523371553720058, - "velocityY": 1.4913580247475502, - "timestamp": 1.8978387510658346 - }, - { - "x": 2.831251803689392, - "y": 3.7884068493950025, - "heading": 1.5700328717959458, - "angularVelocity": -0.0007378034898061426, - "velocityX": -0.09538851743856928, - "velocityY": 0.9927328050011545, - "timestamp": 1.986693942277091 - }, - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 1.57, - "angularVelocity": -0.00036994795124125425, - "velocityX": -0.056006350987752376, - "velocityY": 0.4955677288524759, - "timestamp": 2.0755491334883476 - }, - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 1.57, - "angularVelocity": 3.0178106927934417e-22, - "velocityX": -8.449689256106595e-19, - "velocityY": -6.586266614449837e-18, - "timestamp": 2.1644043246996043 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottomStraight.2.traj b/src/main/deploy/choreo/NoTeamBottomStraight.2.traj deleted file mode 100644 index e0013be..0000000 --- a/src/main/deploy/choreo/NoTeamBottomStraight.2.traj +++ /dev/null @@ -1,112 +0,0 @@ -{ - "samples": [ - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 1.57, - "angularVelocity": 3.0178106927934417e-22, - "velocityX": -8.449689256106595e-19, - "velocityY": -6.586266614449837e-18, - "timestamp": 0 - }, - { - "x": 2.82627534866333, - "y": 3.887160790032957, - "heading": 1.57, - "angularVelocity": 8.611213547003838e-17, - "velocityX": 1.0232237644061418e-17, - "velocityY": 0.5541939799877377, - "timestamp": 0.09873830699830144 - }, - { - "x": 2.82627534866333, - "y": 3.996601139183229, - "heading": 1.57, - "angularVelocity": 1.801778145303611e-16, - "velocityX": 2.3075955144475316e-17, - "velocityY": 1.1083879446318137, - "timestamp": 0.19747661399660288 - }, - { - "x": 2.82627534866333, - "y": 4.1607616596261225, - "heading": 1.57, - "angularVelocity": 2.8227476617608174e-16, - "velocityX": 3.770139097941117e-17, - "velocityY": 1.6625818837031228, - "timestamp": 0.2962149209949043 - }, - { - "x": 2.82627534866333, - "y": 4.3796423463116145, - "heading": 1.57, - "angularVelocity": 3.924863437180408e-16, - "velocityX": 5.411880274246253e-17, - "velocityY": 2.2167757716289116, - "timestamp": 0.39495322799320576 - }, - { - "x": 2.82627534866333, - "y": 4.653243184089661, - "heading": 1.57, - "angularVelocity": 5.109198296204701e-16, - "velocityX": 7.231247688509119e-17, - "velocityY": 2.7709695061183575, - "timestamp": 0.4936915349915072 - }, - { - "x": 2.82627534866333, - "y": 4.926844021867707, - "heading": 1.57, - "angularVelocity": 5.107227261231941e-16, - "velocityX": 1.0354556057725255e-16, - "velocityY": 2.7709695061183575, - "timestamp": 0.5924298419898086 - }, - { - "x": 2.82627534866333, - "y": 5.145724708553199, - "heading": 1.57, - "angularVelocity": 3.9232798727341053e-16, - "velocityX": 7.910713017021751e-17, - "velocityY": 2.2167757716289116, - "timestamp": 0.6911681489881101 - }, - { - "x": 2.82627534866333, - "y": 5.309885228996092, - "heading": 1.57, - "angularVelocity": 2.8215477627973073e-16, - "velocityX": 5.657236997399948e-17, - "velocityY": 1.662581883703123, - "timestamp": 0.7899064559864115 - }, - { - "x": 2.82627534866333, - "y": 5.419325578146364, - "heading": 1.57, - "angularVelocity": 1.8009528804964001e-16, - "velocityX": 3.589318347482986e-17, - "velocityY": 1.108387944631814, - "timestamp": 0.888644762984713 - }, - { - "x": 2.82627534866333, - "y": 5.474045753479004, - "heading": 1.57, - "angularVelocity": 8.606670402357876e-17, - "velocityX": 1.7043045035067325e-17, - "velocityY": 0.5541939799877377, - "timestamp": 0.9873830699830144 - }, - { - "x": 2.82627534866333, - "y": 5.474045753479004, - "heading": 1.57, - "angularVelocity": -1.8688348541769264e-27, - "velocityX": -6.335524016517998e-27, - "velocityY": 3.200026210806558e-22, - "timestamp": 1.0861213769813158 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottomStraight.3.traj b/src/main/deploy/choreo/NoTeamBottomStraight.3.traj deleted file mode 100644 index 24d695c..0000000 --- a/src/main/deploy/choreo/NoTeamBottomStraight.3.traj +++ /dev/null @@ -1,112 +0,0 @@ -{ - "samples": [ - { - "x": 2.82627534866333, - "y": 5.474045753479004, - "heading": 1.57, - "angularVelocity": -1.8688348541769264e-27, - "velocityX": -6.335524016517998e-27, - "velocityY": 3.200026210806558e-22, - "timestamp": 0 - }, - { - "x": 2.82627534866333, - "y": 5.523484949137364, - "heading": 1.57, - "angularVelocity": 2.815530703859563e-18, - "velocityX": -4.120948964782728e-17, - "velocityY": 0.5267733078959163, - "timestamp": 0.09385288684393389 - }, - { - "x": 2.82627534866333, - "y": 5.622363339014038, - "heading": 1.57, - "angularVelocity": 5.079322160604046e-18, - "velocityX": -8.241891103838567e-17, - "velocityY": 1.053546600448171, - "timestamp": 0.18770577368786778 - }, - { - "x": 2.82627534866333, - "y": 5.770680920708947, - "heading": 1.57, - "angularVelocity": 6.7861542129190236e-18, - "velocityX": -1.236282186820289e-16, - "velocityY": 1.5803198674276584, - "timestamp": 0.2815586605318017 - }, - { - "x": 2.82627534866333, - "y": 5.968437689421936, - "heading": 1.57, - "angularVelocity": 7.930330289030281e-18, - "velocityX": -1.6483729882150294e-16, - "velocityY": 2.107093083261625, - "timestamp": 0.37541154737573557 - }, - { - "x": 2.82627534866333, - "y": 6.2156336307525635, - "heading": 1.57, - "angularVelocity": 8.504428120601007e-18, - "velocityX": -2.0604569642935152e-16, - "velocityY": 2.6338661456592565, - "timestamp": 0.46926443421966946 - }, - { - "x": 2.82627534866333, - "y": 6.462829572083191, - "heading": 1.57, - "angularVelocity": 8.962407688809337e-18, - "velocityX": -2.0605181411254769e-16, - "velocityY": 2.6338661456592574, - "timestamp": 0.5631173210636033 - }, - { - "x": 2.82627534866333, - "y": 6.66058634079618, - "heading": 1.57, - "angularVelocity": 8.29359895225545e-18, - "velocityX": -1.6484073568780158e-16, - "velocityY": 2.1070930832616255, - "timestamp": 0.6569702079075372 - }, - { - "x": 2.82627534866333, - "y": 6.808903922491089, - "heading": 1.57, - "angularVelocity": 7.056973102062562e-18, - "velocityX": -1.2363031580105674e-16, - "velocityY": 1.5803198674276586, - "timestamp": 0.7508230947514711 - }, - { - "x": 2.82627534866333, - "y": 6.907782312367763, - "heading": 1.57, - "angularVelocity": 5.259040639378902e-18, - "velocityX": -8.242011541755058e-17, - "velocityY": 1.0535466004481713, - "timestamp": 0.8446759815954055 - }, - { - "x": 2.82627534866333, - "y": 6.957221508026123, - "heading": 1.57, - "angularVelocity": 2.9050319144623277e-18, - "velocityX": -4.1210024782203125e-17, - "velocityY": 0.5267733078959164, - "timestamp": 0.9385288684393398 - }, - { - "x": 2.82627534866333, - "y": 6.957221508026123, - "heading": 1.57, - "angularVelocity": 7.725181741580017e-29, - "velocityX": 2.7049094499726637e-34, - "velocityY": 9.029888043887834e-23, - "timestamp": 1.0323817552832741 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamBottomStraight.traj b/src/main/deploy/choreo/NoTeamBottomStraight.traj deleted file mode 100644 index 5304de9..0000000 --- a/src/main/deploy/choreo/NoTeamBottomStraight.traj +++ /dev/null @@ -1,544 +0,0 @@ -{ - "samples": [ - { - "x": 0.727, - "y": 4.389017105102539, - "heading": -1.052, - "angularVelocity": -1.9035720406977927e-18, - "velocityX": -7.789821171233355e-19, - "velocityY": 6.642220463091642e-19, - "timestamp": 0 - }, - { - "x": 0.7318438956432248, - "y": 4.379292392557921, - "heading": -1.0218708963854122, - "angularVelocity": 0.6376018021899323, - "velocityX": 0.10250808093224481, - "velocityY": -0.20579750143065442, - "timestamp": 0.047253793058779206 - }, - { - "x": 0.7416803389522922, - "y": 4.359743623946586, - "heading": -0.963247947139427, - "angularVelocity": 1.2405977478477537, - "velocityX": 0.20816198388205162, - "velocityY": -0.41369734249730467, - "timestamp": 0.09450758611755841 - }, - { - "x": 0.756679070865091, - "y": 4.330228659181428, - "heading": -0.8784548898183928, - "angularVelocity": 1.7944180103290261, - "velocityX": 0.31740799927196917, - "velocityY": -0.6246051979032636, - "timestamp": 0.14176137917633763 - }, - { - "x": 0.7769909237075252, - "y": 4.290540404198398, - "heading": -0.7706337125535929, - "angularVelocity": 2.2817465072207144, - "velocityX": 0.4298459769603722, - "velocityY": -0.8398956446450863, - "timestamp": 0.18901517223511682 - }, - { - "x": 0.8027070398966437, - "y": 4.240415566240092, - "heading": -0.6435269412573291, - "angularVelocity": 2.689874464429873, - "velocityX": 0.544212739856253, - "velocityY": -1.0607579775861957, - "timestamp": 0.23626896529389602 - }, - { - "x": 0.8338645488878723, - "y": 4.179590349127875, - "heading": -0.5009199000549501, - "angularVelocity": 3.0178961723768816, - "velocityX": 0.659365248255766, - "velocityY": -1.28720285028876, - "timestamp": 0.2835227583526752 - }, - { - "x": 0.8705177044270638, - "y": 4.107851215829094, - "heading": -0.34654429878237253, - "angularVelocity": 3.266946233936165, - "velocityX": 0.7756658919126009, - "velocityY": -1.518166662505675, - "timestamp": 0.3307765514114544 - }, - { - "x": 0.9127860516643015, - "y": 4.025035475201868, - "heading": -0.18511651921150357, - "angularVelocity": 3.416186704209506, - "velocityX": 0.8944963885684252, - "velocityY": -1.752573397107217, - "timestamp": 0.3780303444702336 - }, - { - "x": 0.9608190780560075, - "y": 3.9310914505496757, - "heading": -0.02385519507028553, - "angularVelocity": 3.412664120753743, - "velocityX": 1.0164903869612656, - "velocityY": -1.9880737306174552, - "timestamp": 0.4252841375290128 - }, - { - "x": 1.0146390571858634, - "y": 3.8264366161374967, - "heading": 0.1250934065508822, - "angularVelocity": 3.152098318031103, - "velocityX": 1.138955746111406, - "velocityY": -2.2147393391679286, - "timestamp": 0.472537930587792 - }, - { - "x": 1.0745072439716545, - "y": 3.7116431971477404, - "heading": 0.2562841881014644, - "angularVelocity": 2.776301605828625, - "velocityX": 1.266949866041881, - "velocityY": -2.4292953339632333, - "timestamp": 0.5197917236465712 - }, - { - "x": 1.1413904234655279, - "y": 3.5874142558818387, - "heading": 0.36909871419180623, - "angularVelocity": 2.3874173645703185, - "velocityX": 1.4154034028692064, - "velocityY": -2.6289728977179108, - "timestamp": 0.5670455167053504 - }, - { - "x": 1.2171004406920223, - "y": 3.4555713477306926, - "heading": 0.46254553428918027, - "angularVelocity": 1.9775517275647088, - "velocityX": 1.6021997881168686, - "velocityY": -2.790102119149382, - "timestamp": 0.6142993097641296 - }, - { - "x": 1.304824543975774, - "y": 3.3227207721324725, - "heading": 0.5413110165917999, - "angularVelocity": 1.6668605249242734, - "velocityX": 1.856445749754547, - "velocityY": -2.811426702465267, - "timestamp": 0.6615531028229088 - }, - { - "x": 1.4008888245081486, - "y": 3.1991200061349105, - "heading": 0.6262952838301894, - "angularVelocity": 1.798464456232692, - "velocityX": 2.0329432689748255, - "velocityY": -2.615679250209918, - "timestamp": 0.708806895881688 - }, - { - "x": 1.5007168068199321, - "y": 3.0869110822536374, - "heading": 0.7270728004213672, - "angularVelocity": 2.1326862896661085, - "velocityX": 2.1125919391826877, - "velocityY": -2.3746014154185615, - "timestamp": 0.7560606889404672 - }, - { - "x": 1.601781282376718, - "y": 2.9867599379462364, - "heading": 0.8415198083717181, - "angularVelocity": 2.4219644718888445, - "velocityX": 2.1387590077916334, - "velocityY": -2.119430797498578, - "timestamp": 0.8033144819992464 - }, - { - "x": 1.7021006490957822, - "y": 2.8990265047996244, - "heading": 0.9624877761029027, - "angularVelocity": 2.5599631246683616, - "velocityX": 2.122990774397232, - "velocityY": -1.856643191319687, - "timestamp": 0.8505682750580256 - }, - { - "x": 1.8004198198487906, - "y": 2.8236557821766897, - "heading": 1.0813753656601117, - "angularVelocity": 2.515937491183494, - "velocityX": 2.080661982641449, - "velocityY": -1.5950195263516973, - "timestamp": 0.8978220681168048 - }, - { - "x": 1.8960745245056285, - "y": 2.7604196812117996, - "heading": 1.1931387342522881, - "angularVelocity": 2.365172430775102, - "velocityX": 2.0242756922783456, - "velocityY": -1.3382227514779625, - "timestamp": 0.945075861175584 - }, - { - "x": 1.9886778939409326, - "y": 2.709098454682459, - "heading": 1.294430400917861, - "angularVelocity": 2.143566899266176, - "velocityX": 1.9597023527849797, - "velocityY": -1.0860763381577074, - "timestamp": 0.9923296542343631 - }, - { - "x": 2.0779902618648065, - "y": 2.6695027774627897, - "heading": 1.3827319365638684, - "angularVelocity": 1.868665559528068, - "velocityX": 1.8900571180133192, - "velocityY": -0.8379364841763153, - "timestamp": 1.0395834472931424 - }, - { - "x": 2.163854051486685, - "y": 2.641470045467505, - "heading": 1.4560082555180538, - "angularVelocity": 1.550697080825592, - "velocityX": 1.81707719240807, - "velocityY": -0.5932377102598947, - "timestamp": 1.0868372403519218 - }, - { - "x": 2.246158483677488, - "y": 2.624860911919073, - "heading": 1.5125976444748226, - "angularVelocity": 1.1975628895309458, - "velocityX": 1.7417529231658644, - "velocityY": -0.35148783776513787, - "timestamp": 1.134091033410701 - }, - { - "x": 2.3248213255730343, - "y": 2.6195588118421886, - "heading": 1.5511852292241632, - "angularVelocity": 0.8166029063813219, - "velocityX": 1.6646884155457526, - "velocityY": -0.11220475085013962, - "timestamp": 1.1813448264694804 - }, - { - "x": 2.3997801498313853, - "y": 2.6254709122263833, - "heading": 1.5707963267948966, - "angularVelocity": 0.41501636802656, - "velocityX": 1.5863028003933837, - "velocityY": 0.12511377397451495, - "timestamp": 1.2285986195282597 - }, - { - "x": 2.470987558364868, - "y": 2.642529249191284, - "heading": 1.5707963267948966, - "angularVelocity": 7.2320671802386e-18, - "velocityX": 1.5069141316318813, - "velocityY": 0.360994025255961, - "timestamp": 1.275852412587039 - }, - { - "x": 2.5896469460709723, - "y": 2.7162173203691675, - "heading": 1.5707635810166263, - "angularVelocity": -0.0003685297147391131, - "velocityX": 1.3354243695676342, - "velocityY": 0.8293051894141662, - "timestamp": 1.3647076037982955 - }, - { - "x": 2.691674793744484, - "y": 2.8309800714405235, - "heading": 1.570698306188048, - "angularVelocity": -0.0007346203152394417, - "velocityX": 1.1482485860723253, - "velocityY": 1.2915705825054542, - "timestamp": 1.453562795009552 - }, - { - "x": 2.7746449871658987, - "y": 2.9857496097500946, - "heading": 1.5706009997851365, - "angularVelocity": -0.0010951121885513847, - "velocityX": 0.933768666640422, - "velocityY": 1.7418176270826997, - "timestamp": 1.5424179862208085 - }, - { - "x": 2.8334018266497893, - "y": 3.177633163967886, - "heading": 1.5704732520807607, - "angularVelocity": -0.0014377067072201901, - "velocityX": 0.6612651290591912, - "velocityY": 2.159508652247243, - "timestamp": 1.631273177432065 - }, - { - "x": 2.8534735329147445, - "y": 3.39113142463186, - "heading": 1.5703246879122892, - "angularVelocity": -0.0016719807413194167, - "velocityX": 0.2258923310089341, - "velocityY": 2.4027663184739962, - "timestamp": 1.7201283686433215 - }, - { - "x": 2.8490781305616832, - "y": 3.5676824837315517, - "heading": 1.5701962202028332, - "angularVelocity": -0.0014458098362590802, - "velocityX": -0.04946702936704393, - "velocityY": 1.9869526663887853, - "timestamp": 1.808983559854578 - }, - { - "x": 2.839727568645754, - "y": 3.7001973861849375, - "heading": 1.570098429466109, - "angularVelocity": -0.0011005630103461549, - "velocityX": -0.10523371553720058, - "velocityY": 1.4913580247475502, - "timestamp": 1.8978387510658346 - }, - { - "x": 2.831251803689392, - "y": 3.7884068493950025, - "heading": 1.5700328717959458, - "angularVelocity": -0.0007378034898061426, - "velocityX": -0.09538851743856928, - "velocityY": 0.9927328050011545, - "timestamp": 1.986693942277091 - }, - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 1.57, - "angularVelocity": -0.00036994795124125425, - "velocityX": -0.056006350987752376, - "velocityY": 0.4955677288524759, - "timestamp": 2.0755491334883476 - }, - { - "x": 2.82627534866333, - "y": 3.8324406147003174, - "heading": 1.57, - "angularVelocity": 3.0178106927934417e-22, - "velocityX": -8.449689256106595e-19, - "velocityY": -6.586266614449837e-18, - "timestamp": 2.1644043246996043 - }, - { - "x": 2.82627534866333, - "y": 3.887160790032957, - "heading": 1.57, - "angularVelocity": 8.611213547003838e-17, - "velocityX": 1.0232237644061418e-17, - "velocityY": 0.5541939799877377, - "timestamp": 2.2631426316979057 - }, - { - "x": 2.82627534866333, - "y": 3.996601139183229, - "heading": 1.57, - "angularVelocity": 1.801778145303611e-16, - "velocityX": 2.3075955144475316e-17, - "velocityY": 1.1083879446318137, - "timestamp": 2.361880938696207 - }, - { - "x": 2.82627534866333, - "y": 4.1607616596261225, - "heading": 1.57, - "angularVelocity": 2.8227476617608174e-16, - "velocityX": 3.770139097941117e-17, - "velocityY": 1.6625818837031228, - "timestamp": 2.4606192456945086 - }, - { - "x": 2.82627534866333, - "y": 4.3796423463116145, - "heading": 1.57, - "angularVelocity": 3.924863437180408e-16, - "velocityX": 5.411880274246253e-17, - "velocityY": 2.2167757716289116, - "timestamp": 2.55935755269281 - }, - { - "x": 2.82627534866333, - "y": 4.653243184089661, - "heading": 1.57, - "angularVelocity": 5.109198296204701e-16, - "velocityX": 7.231247688509119e-17, - "velocityY": 2.7709695061183575, - "timestamp": 2.6580958596911115 - }, - { - "x": 2.82627534866333, - "y": 4.926844021867707, - "heading": 1.57, - "angularVelocity": 5.107227261231941e-16, - "velocityX": 1.0354556057725255e-16, - "velocityY": 2.7709695061183575, - "timestamp": 2.756834166689413 - }, - { - "x": 2.82627534866333, - "y": 5.145724708553199, - "heading": 1.57, - "angularVelocity": 3.9232798727341053e-16, - "velocityX": 7.910713017021751e-17, - "velocityY": 2.2167757716289116, - "timestamp": 2.8555724736877144 - }, - { - "x": 2.82627534866333, - "y": 5.309885228996092, - "heading": 1.57, - "angularVelocity": 2.8215477627973073e-16, - "velocityX": 5.657236997399948e-17, - "velocityY": 1.662581883703123, - "timestamp": 2.954310780686016 - }, - { - "x": 2.82627534866333, - "y": 5.419325578146364, - "heading": 1.57, - "angularVelocity": 1.8009528804964001e-16, - "velocityX": 3.589318347482986e-17, - "velocityY": 1.108387944631814, - "timestamp": 3.0530490876843173 - }, - { - "x": 2.82627534866333, - "y": 5.474045753479004, - "heading": 1.57, - "angularVelocity": 8.606670402357876e-17, - "velocityX": 1.7043045035067325e-17, - "velocityY": 0.5541939799877377, - "timestamp": 3.1517873946826187 - }, - { - "x": 2.82627534866333, - "y": 5.474045753479004, - "heading": 1.57, - "angularVelocity": -1.8688348541769264e-27, - "velocityX": -6.335524016517998e-27, - "velocityY": 3.200026210806558e-22, - "timestamp": 3.25052570168092 - }, - { - "x": 2.82627534866333, - "y": 5.523484949137364, - "heading": 1.57, - "angularVelocity": 2.815530703859563e-18, - "velocityX": -4.120948964782728e-17, - "velocityY": 0.5267733078959163, - "timestamp": 3.344378588524854 - }, - { - "x": 2.82627534866333, - "y": 5.622363339014038, - "heading": 1.57, - "angularVelocity": 5.079322160604046e-18, - "velocityX": -8.241891103838567e-17, - "velocityY": 1.053546600448171, - "timestamp": 3.438231475368788 - }, - { - "x": 2.82627534866333, - "y": 5.770680920708947, - "heading": 1.57, - "angularVelocity": 6.7861542129190236e-18, - "velocityX": -1.236282186820289e-16, - "velocityY": 1.5803198674276584, - "timestamp": 3.532084362212722 - }, - { - "x": 2.82627534866333, - "y": 5.968437689421936, - "heading": 1.57, - "angularVelocity": 7.930330289030281e-18, - "velocityX": -1.6483729882150294e-16, - "velocityY": 2.107093083261625, - "timestamp": 3.6259372490566557 - }, - { - "x": 2.82627534866333, - "y": 6.2156336307525635, - "heading": 1.57, - "angularVelocity": 8.504428120601007e-18, - "velocityX": -2.0604569642935152e-16, - "velocityY": 2.6338661456592565, - "timestamp": 3.7197901359005896 - }, - { - "x": 2.82627534866333, - "y": 6.462829572083191, - "heading": 1.57, - "angularVelocity": 8.962407688809337e-18, - "velocityX": -2.0605181411254769e-16, - "velocityY": 2.6338661456592574, - "timestamp": 3.8136430227445235 - }, - { - "x": 2.82627534866333, - "y": 6.66058634079618, - "heading": 1.57, - "angularVelocity": 8.29359895225545e-18, - "velocityX": -1.6484073568780158e-16, - "velocityY": 2.1070930832616255, - "timestamp": 3.9074959095884574 - }, - { - "x": 2.82627534866333, - "y": 6.808903922491089, - "heading": 1.57, - "angularVelocity": 7.056973102062562e-18, - "velocityX": -1.2363031580105674e-16, - "velocityY": 1.5803198674276586, - "timestamp": 4.001348796432391 - }, - { - "x": 2.82627534866333, - "y": 6.907782312367763, - "heading": 1.57, - "angularVelocity": 5.259040639378902e-18, - "velocityX": -8.242011541755058e-17, - "velocityY": 1.0535466004481713, - "timestamp": 4.095201683276326 - }, - { - "x": 2.82627534866333, - "y": 6.957221508026123, - "heading": 1.57, - "angularVelocity": 2.9050319144623277e-18, - "velocityX": -4.1210024782203125e-17, - "velocityY": 0.5267733078959164, - "timestamp": 4.18905457012026 - }, - { - "x": 2.82627534866333, - "y": 6.957221508026123, - "heading": 1.57, - "angularVelocity": 7.725181741580017e-29, - "velocityX": 2.7049094499726637e-34, - "velocityY": 9.029888043887834e-23, - "timestamp": 4.282907456964194 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamMiddle.1.traj b/src/main/deploy/choreo/NoTeamMiddle.1.traj deleted file mode 100644 index c22df79..0000000 --- a/src/main/deploy/choreo/NoTeamMiddle.1.traj +++ /dev/null @@ -1,112 +0,0 @@ -{ - "samples": [ - { - "x": 1.345005989074707, - "y": 5.5500569343566895, - "heading": -2.7546975510839134e-33, - "angularVelocity": 7.98318564036553e-35, - "velocityX": 7.982434630916693e-33, - "velocityY": 1.9344790398404236e-33, - "timestamp": 0 - }, - { - "x": 1.374336856748517, - "y": 5.501802917788148, - "heading": 9.87730102393334e-22, - "angularVelocity": 9.847392319149301e-21, - "velocityX": 0.2924205298932618, - "velocityY": -0.48107902061998664, - "timestamp": 0.10030372246612224 - }, - { - "x": 1.4329985912967438, - "y": 5.405294885966198, - "heading": 3.2109950624668562e-21, - "angularVelocity": 2.2165328394625732e-20, - "velocityX": 0.5848410518167925, - "velocityY": -0.9621580281284781, - "timestamp": 0.20060744493224447 - }, - { - "x": 1.520991191387064, - "y": 5.260532841082725, - "heading": 7.246933022052211e-21, - "angularVelocity": 4.023743732214471e-20, - "velocityX": 0.8772615604574385, - "velocityY": -1.4432370137844779, - "timestamp": 0.3009111673983667 - }, - { - "x": 1.6383146543548324, - "y": 5.067516787521501, - "heading": 1.409551317097851e-20, - "angularVelocity": 6.827869140851397e-20, - "velocityX": 1.1696820425323176, - "velocityY": -1.9243159557354987, - "timestamp": 0.40121488986448894 - }, - { - "x": 1.7849689722061157, - "y": 4.826246738433838, - "heading": 2.72851162530444e-20, - "angularVelocity": 1.3149785356894001e-19, - "velocityX": 1.462102444909918, - "velocityY": -2.405394766571619, - "timestamp": 0.5015186123306112 - }, - { - "x": 1.9316232900573989, - "y": 4.584976689346176, - "heading": 4.737044334212545e-20, - "angularVelocity": 2.0024494806875148e-19, - "velocityX": 1.4621024449099178, - "velocityY": -2.405394766571619, - "timestamp": 0.6018223347967335 - }, - { - "x": 2.0489467530251675, - "y": 4.391960635784951, - "heading": 6.089560135631662e-20, - "angularVelocity": 1.3484202907628645e-19, - "velocityX": 1.1696820425323173, - "velocityY": -1.9243159557354987, - "timestamp": 0.7021260572628558 - }, - { - "x": 2.1369393531154874, - "y": 4.247198590901478, - "heading": 6.930746734079673e-20, - "angularVelocity": 8.386394336827385e-20, - "velocityX": 0.8772615604574384, - "velocityY": -1.4432370137844779, - "timestamp": 0.8024297797289781 - }, - { - "x": 2.195601087663714, - "y": 4.1506905590795276, - "heading": 7.420098024062153e-20, - "angularVelocity": 4.8786950395290725e-20, - "velocityX": 0.5848410518167924, - "velocityY": -0.9621580281284781, - "timestamp": 0.9027335021951004 - }, - { - "x": 2.2249319553375244, - "y": 4.102436542510986, - "heading": 7.6367465447036e-20, - "angularVelocity": 2.1599249454848124e-20, - "velocityX": 0.2924205298932618, - "velocityY": -0.48107902061998664, - "timestamp": 1.0030372246612227 - }, - { - "x": 2.2249319553375244, - "y": 4.102436542510986, - "heading": 7.636746544703908e-20, - "angularVelocity": 8.194514664647686e-33, - "velocityX": 1.446672081349358e-33, - "velocityY": -1.672389052514602e-33, - "timestamp": 1.103340947127345 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamMiddle.2.traj b/src/main/deploy/choreo/NoTeamMiddle.2.traj deleted file mode 100644 index a70c872..0000000 --- a/src/main/deploy/choreo/NoTeamMiddle.2.traj +++ /dev/null @@ -1,166 +0,0 @@ -{ - "samples": [ - { - "x": 2.2249319553375244, - "y": 4.102436542510986, - "heading": 7.636746544703908e-20, - "angularVelocity": 8.194514664647686e-33, - "velocityX": 1.446672081349358e-33, - "velocityY": -1.672389052514602e-33, - "timestamp": 0 - }, - { - "x": 2.197352071426116, - "y": 4.119337343550709, - "heading": 2.1521498385967074e-19, - "angularVelocity": 1.8289989887525516e-18, - "velocityX": -0.36330198842015404, - "velocityY": 0.22262945860641717, - "timestamp": 0.07591448654421451 - }, - { - "x": 2.143014042988731, - "y": 4.154411991095742, - "heading": 5.261862325416936e-19, - "angularVelocity": 4.096336081347265e-18, - "velocityX": -0.7157794369818592, - "velocityY": 0.46202838406349855, - "timestamp": 0.15182897308842902 - }, - { - "x": 2.0634115959086574, - "y": 4.209685549993584, - "heading": -6.256825300150602e-18, - "angularVelocity": -8.935068455939364e-17, - "velocityX": -1.0485804581410318, - "velocityY": 0.7281029143976387, - "timestamp": 0.22774345963264353 - }, - { - "x": 1.9618797741940481, - "y": 4.28873702220571, - "heading": -1.2720644596750681e-17, - "angularVelocity": -8.514605574453078e-17, - "velocityX": -1.3374498904826855, - "velocityY": 1.041322622475801, - "timestamp": 0.30365794617685804 - }, - { - "x": 1.8487346991478477, - "y": 4.397978202555833, - "heading": -1.8686559268739726e-17, - "angularVelocity": -7.858729930366567e-17, - "velocityX": -1.490427982810653, - "velocityY": 1.4390030852213924, - "timestamp": 0.37957243272107255 - }, - { - "x": 1.7533063097381842, - "y": 4.534282361069246, - "heading": -2.3214525333326314e-17, - "angularVelocity": -5.964561163868586e-17, - "velocityX": -1.2570511078156814, - "velocityY": 1.7954960208289874, - "timestamp": 0.45548691926528706 - }, - { - "x": 1.6889882259887987, - "y": 4.679442887556022, - "heading": -2.7498023126090817e-17, - "angularVelocity": -5.642529870698493e-17, - "velocityX": -0.8472438750135669, - "velocityY": 1.9121584442546633, - "timestamp": 0.5314014058095016 - }, - { - "x": 1.657008982479021, - "y": 4.825300663776631, - "heading": -3.2126097337513165e-17, - "angularVelocity": -6.096430842947865e-17, - "velocityX": -0.4212535046410702, - "velocityY": 1.9213431172409716, - "timestamp": 0.6073158923537161 - }, - { - "x": 1.6572377681732178, - "y": 4.968170166015625, - "heading": -3.528590547494103e-17, - "angularVelocity": -4.1623256892899375e-17, - "velocityX": 0.0030137290603089984, - "velocityY": 1.8819794316303808, - "timestamp": 0.6832303788979306 - }, - { - "x": 1.6963109232900544, - "y": 5.11955499843328, - "heading": -3.8568694444513565e-17, - "angularVelocity": -3.923406280814191e-17, - "velocityX": 0.46698055196147537, - "velocityY": 1.8092670630156014, - "timestamp": 0.7669022897574218 - }, - { - "x": 1.7732918382344491, - "y": 5.260591607649996, - "heading": -4.157623585429151e-17, - "angularVelocity": -3.594445751060187e-17, - "velocityX": 0.9200329495721387, - "velocityY": 1.6855908723485022, - "timestamp": 0.850574200616913 - }, - { - "x": 1.884554817294881, - "y": 5.382423433953372, - "heading": -4.416508678675616e-17, - "angularVelocity": -3.094050334766252e-17, - "velocityX": 1.3297530547291259, - "velocityY": 1.4560660208653102, - "timestamp": 0.9342461114764042 - }, - { - "x": 2.010708721210131, - "y": 5.467891221596337, - "heading": -4.614895263674143e-17, - "angularVelocity": -2.3710060738734744e-17, - "velocityX": 1.5077210812969026, - "velocityY": 1.0214633174386198, - "timestamp": 1.0179180223358955 - }, - { - "x": 2.1156467514770063, - "y": 5.520283792104612, - "heading": -4.311417988982476e-17, - "angularVelocity": 3.626990802127755e-17, - "velocityX": 1.2541607952888236, - "velocityY": 0.6261667741311343, - "timestamp": 1.1015899331953867 - }, - { - "x": 2.188091221601493, - "y": 5.550579929073903, - "heading": -3.975437161737579e-17, - "angularVelocity": 4.0154551314033223e-17, - "velocityX": 0.8658158918605504, - "velocityY": 0.36208252755416254, - "timestamp": 1.1852618440548779 - }, - { - "x": 2.2249319553375244, - "y": 5.564249038696289, - "heading": -3.992691037709511e-17, - "angularVelocity": -2.0620870119604985e-18, - "velocityX": 0.44029989703351463, - "velocityY": 0.1633655725317422, - "timestamp": 1.268933754914369 - }, - { - "x": 2.2249319553375244, - "y": 5.564249038696289, - "heading": -3.992691037709511e-17, - "angularVelocity": 1.948197557554799e-34, - "velocityX": -1.3588263959389525e-32, - "velocityY": -1.5731738564130587e-32, - "timestamp": 1.3526056657738603 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamMiddle.3.traj b/src/main/deploy/choreo/NoTeamMiddle.3.traj deleted file mode 100644 index dfc578f..0000000 --- a/src/main/deploy/choreo/NoTeamMiddle.3.traj +++ /dev/null @@ -1,166 +0,0 @@ -{ - "samples": [ - { - "x": 2.2249319553375244, - "y": 5.564249038696289, - "heading": -3.992691037709511e-17, - "angularVelocity": 1.948197557554799e-34, - "velocityX": -1.3588263959389525e-32, - "velocityY": -1.5731738564130587e-32, - "timestamp": 0 - }, - { - "x": 2.198199915755316, - "y": 5.579643402434152, - "heading": -4.443401956416108e-17, - "angularVelocity": -6.079583697798712e-17, - "velocityX": -0.3605851761095616, - "velocityY": 0.20765266871766613, - "timestamp": 0.07413515960535877 - }, - { - "x": 2.145519284812898, - "y": 5.611718860578594, - "heading": -4.847236948556383e-17, - "angularVelocity": -5.447280155428928e-17, - "velocityX": -0.7106025160376092, - "velocityY": 0.4326618883022348, - "timestamp": 0.14827031921071754 - }, - { - "x": 2.068317300626264, - "y": 5.662510392673879, - "heading": -5.190015426747772e-17, - "angularVelocity": -4.6236964337837673e-17, - "velocityX": -1.0413680175182847, - "velocityY": 0.6851206953038543, - "timestamp": 0.2224054788160763 - }, - { - "x": 1.9697712674085048, - "y": 5.73557342108719, - "heading": -5.447049142281068e-17, - "angularVelocity": -3.467095832077673e-17, - "velocityX": -1.3292752553895588, - "velocityY": 0.9855381549354427, - "timestamp": 0.2965406384214351 - }, - { - "x": 1.8594813582153729, - "y": 5.837161331961853, - "heading": -5.868151668893362e-17, - "angularVelocity": -5.68019968220714e-17, - "velocityX": -1.487686946116183, - "velocityY": 1.3703067669300684, - "timestamp": 0.37067579802679385 - }, - { - "x": 1.7641319442068013, - "y": 5.965737551430191, - "heading": -6.066243047179228e-17, - "angularVelocity": -2.6720298755315395e-17, - "velocityX": -1.2861564541864055, - "velocityY": 1.7343487240438071, - "timestamp": 0.4448109576321526 - }, - { - "x": 1.6977316821706918, - "y": 6.104969019593263, - "heading": -6.137469251207977e-17, - "angularVelocity": -9.60761370564525e-18, - "velocityX": -0.8956649232236864, - "velocityY": 1.8780760560068654, - "timestamp": 0.5189461172375114 - }, - { - "x": 1.66207189361609, - "y": 6.246771743474824, - "heading": -6.43119401475664e-17, - "angularVelocity": -3.96201692050749e-17, - "velocityX": -0.4810104779490378, - "velocityY": 1.9127594064195064, - "timestamp": 0.5930812768428702 - }, - { - "x": 1.6572377681732178, - "y": 6.3874053955078125, - "heading": -6.711231297016692e-17, - "angularVelocity": -3.7773881886241834e-17, - "velocityX": -0.06520692028729415, - "velocityY": 1.8969899408272504, - "timestamp": 0.6672164364482289 - }, - { - "x": 1.692247730583365, - "y": 6.5448538320481955, - "heading": -6.973008277469343e-17, - "angularVelocity": -3.070599085824093e-17, - "velocityX": 0.4106608874835372, - "velocityY": 1.8468433049167998, - "timestamp": 0.7524691680260585 - }, - { - "x": 1.7671066950619185, - "y": 6.6935741603960714, - "heading": -7.151388543134576e-17, - "angularVelocity": -2.0923698913909792e-17, - "velocityX": 0.8780828847719975, - "velocityY": 1.7444640845566861, - "timestamp": 0.837721899603888 - }, - { - "x": 1.8784928172798085, - "y": 6.82413163710067, - "heading": -7.201746198476356e-17, - "angularVelocity": -5.906865781623406e-18, - "velocityX": 1.3065402146815994, - "velocityY": 1.5314169327865985, - "timestamp": 0.9229746311817175 - }, - { - "x": 2.006539662386896, - "y": 6.917452813996992, - "heading": -7.066227191561186e-17, - "angularVelocity": 1.5896150266967084e-17, - "velocityX": 1.501967652381787, - "velocityY": 1.0946414873654486, - "timestamp": 1.008227362759547 - }, - { - "x": 2.1133877261411924, - "y": 6.975920959825139, - "heading": -7.201772249126913e-17, - "angularVelocity": -1.5899201837052936e-17, - "velocityX": 1.253309562952265, - "velocityY": 0.6858213777557197, - "timestamp": 1.0934800943373766 - }, - { - "x": 2.1872956347822887, - "y": 7.010325255827941, - "heading": -7.084833328584506e-17, - "angularVelocity": 1.371673720081464e-17, - "velocityX": 0.8669271620185445, - "velocityY": 0.40355652383283236, - "timestamp": 1.1787328259152061 - }, - { - "x": 2.2249319553375244, - "y": 7.026061534881592, - "heading": -6.901099450925116e-17, - "angularVelocity": 2.155167137746115e-17, - "velocityX": 0.44146762055215316, - "velocityY": 0.18458386918999145, - "timestamp": 1.2639855574930356 - }, - { - "x": 2.2249319553375244, - "y": 7.026061534881592, - "heading": -6.901099450925116e-17, - "angularVelocity": 9.740777663850368e-35, - "velocityX": 0, - "velocityY": -6.61490476729539e-33, - "timestamp": 1.3492382890708652 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamMiddle.traj b/src/main/deploy/choreo/NoTeamMiddle.traj deleted file mode 100644 index a35fdd9..0000000 --- a/src/main/deploy/choreo/NoTeamMiddle.traj +++ /dev/null @@ -1,418 +0,0 @@ -{ - "samples": [ - { - "x": 1.345005989074707, - "y": 5.5500569343566895, - "heading": -2.7546975510839134e-33, - "angularVelocity": 7.98318564036553e-35, - "velocityX": 7.982434630916693e-33, - "velocityY": 1.9344790398404236e-33, - "timestamp": 0 - }, - { - "x": 1.374336856748517, - "y": 5.501802917788148, - "heading": 9.87730102393334e-22, - "angularVelocity": 9.847392319149301e-21, - "velocityX": 0.2924205298932618, - "velocityY": -0.48107902061998664, - "timestamp": 0.10030372246612224 - }, - { - "x": 1.4329985912967438, - "y": 5.405294885966198, - "heading": 3.2109950624668562e-21, - "angularVelocity": 2.2165328394625732e-20, - "velocityX": 0.5848410518167925, - "velocityY": -0.9621580281284781, - "timestamp": 0.20060744493224447 - }, - { - "x": 1.520991191387064, - "y": 5.260532841082725, - "heading": 7.246933022052211e-21, - "angularVelocity": 4.023743732214471e-20, - "velocityX": 0.8772615604574385, - "velocityY": -1.4432370137844779, - "timestamp": 0.3009111673983667 - }, - { - "x": 1.6383146543548324, - "y": 5.067516787521501, - "heading": 1.409551317097851e-20, - "angularVelocity": 6.827869140851397e-20, - "velocityX": 1.1696820425323176, - "velocityY": -1.9243159557354987, - "timestamp": 0.40121488986448894 - }, - { - "x": 1.7849689722061157, - "y": 4.826246738433838, - "heading": 2.72851162530444e-20, - "angularVelocity": 1.3149785356894001e-19, - "velocityX": 1.462102444909918, - "velocityY": -2.405394766571619, - "timestamp": 0.5015186123306112 - }, - { - "x": 1.9316232900573989, - "y": 4.584976689346176, - "heading": 4.737044334212545e-20, - "angularVelocity": 2.0024494806875148e-19, - "velocityX": 1.4621024449099178, - "velocityY": -2.405394766571619, - "timestamp": 0.6018223347967335 - }, - { - "x": 2.0489467530251675, - "y": 4.391960635784951, - "heading": 6.089560135631662e-20, - "angularVelocity": 1.3484202907628645e-19, - "velocityX": 1.1696820425323173, - "velocityY": -1.9243159557354987, - "timestamp": 0.7021260572628558 - }, - { - "x": 2.1369393531154874, - "y": 4.247198590901478, - "heading": 6.930746734079673e-20, - "angularVelocity": 8.386394336827385e-20, - "velocityX": 0.8772615604574384, - "velocityY": -1.4432370137844779, - "timestamp": 0.8024297797289781 - }, - { - "x": 2.195601087663714, - "y": 4.1506905590795276, - "heading": 7.420098024062153e-20, - "angularVelocity": 4.8786950395290725e-20, - "velocityX": 0.5848410518167924, - "velocityY": -0.9621580281284781, - "timestamp": 0.9027335021951004 - }, - { - "x": 2.2249319553375244, - "y": 4.102436542510986, - "heading": 7.6367465447036e-20, - "angularVelocity": 2.1599249454848124e-20, - "velocityX": 0.2924205298932618, - "velocityY": -0.48107902061998664, - "timestamp": 1.0030372246612227 - }, - { - "x": 2.2249319553375244, - "y": 4.102436542510986, - "heading": 7.636746544703908e-20, - "angularVelocity": 8.194514664647686e-33, - "velocityX": 1.446672081349358e-33, - "velocityY": -1.672389052514602e-33, - "timestamp": 1.103340947127345 - }, - { - "x": 2.197352071426116, - "y": 4.119337343550709, - "heading": 2.1521498385967074e-19, - "angularVelocity": 1.8289989887525516e-18, - "velocityX": -0.36330198842015404, - "velocityY": 0.22262945860641717, - "timestamp": 1.1792554336715595 - }, - { - "x": 2.143014042988731, - "y": 4.154411991095742, - "heading": 5.261862325416936e-19, - "angularVelocity": 4.096336081347265e-18, - "velocityX": -0.7157794369818592, - "velocityY": 0.46202838406349855, - "timestamp": 1.255169920215774 - }, - { - "x": 2.0634115959086574, - "y": 4.209685549993584, - "heading": -6.256825300150602e-18, - "angularVelocity": -8.935068455939364e-17, - "velocityX": -1.0485804581410318, - "velocityY": 0.7281029143976387, - "timestamp": 1.3310844067599885 - }, - { - "x": 1.9618797741940481, - "y": 4.28873702220571, - "heading": -1.2720644596750681e-17, - "angularVelocity": -8.514605574453078e-17, - "velocityX": -1.3374498904826855, - "velocityY": 1.041322622475801, - "timestamp": 1.406998893304203 - }, - { - "x": 1.8487346991478477, - "y": 4.397978202555833, - "heading": -1.8686559268739726e-17, - "angularVelocity": -7.858729930366567e-17, - "velocityX": -1.490427982810653, - "velocityY": 1.4390030852213924, - "timestamp": 1.4829133798484175 - }, - { - "x": 1.7533063097381842, - "y": 4.534282361069246, - "heading": -2.3214525333326314e-17, - "angularVelocity": -5.964561163868586e-17, - "velocityX": -1.2570511078156814, - "velocityY": 1.7954960208289874, - "timestamp": 1.558827866392632 - }, - { - "x": 1.6889882259887987, - "y": 4.679442887556022, - "heading": -2.7498023126090817e-17, - "angularVelocity": -5.642529870698493e-17, - "velocityX": -0.8472438750135669, - "velocityY": 1.9121584442546633, - "timestamp": 1.6347423529368466 - }, - { - "x": 1.657008982479021, - "y": 4.825300663776631, - "heading": -3.2126097337513165e-17, - "angularVelocity": -6.096430842947865e-17, - "velocityX": -0.4212535046410702, - "velocityY": 1.9213431172409716, - "timestamp": 1.710656839481061 - }, - { - "x": 1.6572377681732178, - "y": 4.968170166015625, - "heading": -3.528590547494103e-17, - "angularVelocity": -4.1623256892899375e-17, - "velocityX": 0.0030137290603089984, - "velocityY": 1.8819794316303808, - "timestamp": 1.7865713260252756 - }, - { - "x": 1.6963109232900544, - "y": 5.11955499843328, - "heading": -3.8568694444513565e-17, - "angularVelocity": -3.923406280814191e-17, - "velocityX": 0.46698055196147537, - "velocityY": 1.8092670630156014, - "timestamp": 1.8702432368847668 - }, - { - "x": 1.7732918382344491, - "y": 5.260591607649996, - "heading": -4.157623585429151e-17, - "angularVelocity": -3.594445751060187e-17, - "velocityX": 0.9200329495721387, - "velocityY": 1.6855908723485022, - "timestamp": 1.953915147744258 - }, - { - "x": 1.884554817294881, - "y": 5.382423433953372, - "heading": -4.416508678675616e-17, - "angularVelocity": -3.094050334766252e-17, - "velocityX": 1.3297530547291259, - "velocityY": 1.4560660208653102, - "timestamp": 2.0375870586037492 - }, - { - "x": 2.010708721210131, - "y": 5.467891221596337, - "heading": -4.614895263674143e-17, - "angularVelocity": -2.3710060738734744e-17, - "velocityX": 1.5077210812969026, - "velocityY": 1.0214633174386198, - "timestamp": 2.1212589694632404 - }, - { - "x": 2.1156467514770063, - "y": 5.520283792104612, - "heading": -4.311417988982476e-17, - "angularVelocity": 3.626990802127755e-17, - "velocityX": 1.2541607952888236, - "velocityY": 0.6261667741311343, - "timestamp": 2.2049308803227317 - }, - { - "x": 2.188091221601493, - "y": 5.550579929073903, - "heading": -3.975437161737579e-17, - "angularVelocity": 4.0154551314033223e-17, - "velocityX": 0.8658158918605504, - "velocityY": 0.36208252755416254, - "timestamp": 2.288602791182223 - }, - { - "x": 2.2249319553375244, - "y": 5.564249038696289, - "heading": -3.992691037709511e-17, - "angularVelocity": -2.0620870119604985e-18, - "velocityX": 0.44029989703351463, - "velocityY": 0.1633655725317422, - "timestamp": 2.372274702041714 - }, - { - "x": 2.2249319553375244, - "y": 5.564249038696289, - "heading": -3.992691037709511e-17, - "angularVelocity": 1.948197557554799e-34, - "velocityX": -1.3588263959389525e-32, - "velocityY": -1.5731738564130587e-32, - "timestamp": 2.4559466129012053 - }, - { - "x": 2.198199915755316, - "y": 5.579643402434152, - "heading": -4.443401956416108e-17, - "angularVelocity": -6.079583697798712e-17, - "velocityX": -0.3605851761095616, - "velocityY": 0.20765266871766613, - "timestamp": 2.530081772506564 - }, - { - "x": 2.145519284812898, - "y": 5.611718860578594, - "heading": -4.847236948556383e-17, - "angularVelocity": -5.447280155428928e-17, - "velocityX": -0.7106025160376092, - "velocityY": 0.4326618883022348, - "timestamp": 2.604216932111923 - }, - { - "x": 2.068317300626264, - "y": 5.662510392673879, - "heading": -5.190015426747772e-17, - "angularVelocity": -4.6236964337837673e-17, - "velocityX": -1.0413680175182847, - "velocityY": 0.6851206953038543, - "timestamp": 2.6783520917172816 - }, - { - "x": 1.9697712674085048, - "y": 5.73557342108719, - "heading": -5.447049142281068e-17, - "angularVelocity": -3.467095832077673e-17, - "velocityX": -1.3292752553895588, - "velocityY": 0.9855381549354427, - "timestamp": 2.7524872513226404 - }, - { - "x": 1.8594813582153729, - "y": 5.837161331961853, - "heading": -5.868151668893362e-17, - "angularVelocity": -5.68019968220714e-17, - "velocityX": -1.487686946116183, - "velocityY": 1.3703067669300684, - "timestamp": 2.826622410927999 - }, - { - "x": 1.7641319442068013, - "y": 5.965737551430191, - "heading": -6.066243047179228e-17, - "angularVelocity": -2.6720298755315395e-17, - "velocityX": -1.2861564541864055, - "velocityY": 1.7343487240438071, - "timestamp": 2.900757570533358 - }, - { - "x": 1.6977316821706918, - "y": 6.104969019593263, - "heading": -6.137469251207977e-17, - "angularVelocity": -9.60761370564525e-18, - "velocityX": -0.8956649232236864, - "velocityY": 1.8780760560068654, - "timestamp": 2.9748927301387167 - }, - { - "x": 1.66207189361609, - "y": 6.246771743474824, - "heading": -6.43119401475664e-17, - "angularVelocity": -3.96201692050749e-17, - "velocityX": -0.4810104779490378, - "velocityY": 1.9127594064195064, - "timestamp": 3.0490278897440755 - }, - { - "x": 1.6572377681732178, - "y": 6.3874053955078125, - "heading": -6.711231297016692e-17, - "angularVelocity": -3.7773881886241834e-17, - "velocityX": -0.06520692028729415, - "velocityY": 1.8969899408272504, - "timestamp": 3.1231630493494342 - }, - { - "x": 1.692247730583365, - "y": 6.5448538320481955, - "heading": -6.973008277469343e-17, - "angularVelocity": -3.070599085824093e-17, - "velocityX": 0.4106608874835372, - "velocityY": 1.8468433049167998, - "timestamp": 3.2084157809272638 - }, - { - "x": 1.7671066950619185, - "y": 6.6935741603960714, - "heading": -7.151388543134576e-17, - "angularVelocity": -2.0923698913909792e-17, - "velocityX": 0.8780828847719975, - "velocityY": 1.7444640845566861, - "timestamp": 3.2936685125050933 - }, - { - "x": 1.8784928172798085, - "y": 6.82413163710067, - "heading": -7.201746198476356e-17, - "angularVelocity": -5.906865781623406e-18, - "velocityX": 1.3065402146815994, - "velocityY": 1.5314169327865985, - "timestamp": 3.378921244082923 - }, - { - "x": 2.006539662386896, - "y": 6.917452813996992, - "heading": -7.066227191561186e-17, - "angularVelocity": 1.5896150266967084e-17, - "velocityX": 1.501967652381787, - "velocityY": 1.0946414873654486, - "timestamp": 3.4641739756607524 - }, - { - "x": 2.1133877261411924, - "y": 6.975920959825139, - "heading": -7.201772249126913e-17, - "angularVelocity": -1.5899201837052936e-17, - "velocityX": 1.253309562952265, - "velocityY": 0.6858213777557197, - "timestamp": 3.549426707238582 - }, - { - "x": 2.1872956347822887, - "y": 7.010325255827941, - "heading": -7.084833328584506e-17, - "angularVelocity": 1.371673720081464e-17, - "velocityX": 0.8669271620185445, - "velocityY": 0.40355652383283236, - "timestamp": 3.6346794388164114 - }, - { - "x": 2.2249319553375244, - "y": 7.026061534881592, - "heading": -6.901099450925116e-17, - "angularVelocity": 2.155167137746115e-17, - "velocityX": 0.44146762055215316, - "velocityY": 0.18458386918999145, - "timestamp": 3.719932170394241 - }, - { - "x": 2.2249319553375244, - "y": 7.026061534881592, - "heading": -6.901099450925116e-17, - "angularVelocity": 9.740777663850368e-35, - "velocityX": 0, - "velocityY": -6.61490476729539e-33, - "timestamp": 3.8051849019720705 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamTop.1.traj b/src/main/deploy/choreo/NoTeamTop.1.traj deleted file mode 100644 index 8e5c03b..0000000 --- a/src/main/deploy/choreo/NoTeamTop.1.traj +++ /dev/null @@ -1,94 +0,0 @@ -{ - "samples": [ - { - "x": 2.633068561553955, - "y": 7.049606800079346, - "heading": 3.803214140819317e-31, - "angularVelocity": 3.020347919142555e-31, - "velocityX": 7.840324925442265e-34, - "velocityY": 5.62397113618381e-32, - "timestamp": 0 - }, - { - "x": 2.609144744005489, - "y": 7.015299626278446, - "heading": 0.018678562047563966, - "angularVelocity": 0.2627050620237914, - "velocityX": -0.33647707767394147, - "velocityY": -0.4825140285572393, - "timestamp": 0.0711008836436976 - }, - { - "x": 2.5612954442990494, - "y": 6.946685760529339, - "heading": 0.056012535459477056, - "angularVelocity": 0.5250845207353814, - "velocityX": -0.6729775672862703, - "velocityY": -0.9650212800862848, - "timestamp": 0.1422017672873952 - }, - { - "x": 2.4895180668345147, - "y": 6.843766221469334, - "heading": 0.11197590720911785, - "angularVelocity": 0.7870981186406304, - "velocityX": -1.009514562775715, - "velocityY": -1.4475142049676655, - "timestamp": 0.2133026509310928 - }, - { - "x": 2.393809314296558, - "y": 6.706543018483128, - "heading": 0.1865585662991481, - "angularVelocity": 1.0489695101931586, - "velocityX": -1.346097933431799, - "velocityY": -1.929978868812128, - "timestamp": 0.2844035345747904 - }, - { - "x": 2.298062994828891, - "y": 6.56935427735818, - "heading": 0.26139370468794165, - "angularVelocity": 1.0525205110503135, - "velocityX": -1.3466262943717173, - "velocityY": -1.9294941791782931, - "timestamp": 0.3555044182184881 - }, - { - "x": 2.22625248415356, - "y": 6.466464285312162, - "heading": 0.3175528585708617, - "angularVelocity": 0.7898517009204281, - "velocityX": -1.0099805655748195, - "velocityY": -1.447098640315394, - "timestamp": 0.4266053018621858 - }, - { - "x": 2.178378576021294, - "y": 6.397871445224483, - "heading": 0.355003104186284, - "angularVelocity": 0.5267198337941039, - "velocityX": -0.6733236730526801, - "velocityY": -0.9647255641914598, - "timestamp": 0.4977061855058835 - }, - { - "x": 2.154441595077514, - "y": 6.363574981689453, - "heading": 0.3737268100062681, - "angularVelocity": 0.2633399876408391, - "velocityX": -0.3366622145476251, - "velocityY": -0.48236339377858967, - "timestamp": 0.5688070691495812 - }, - { - "x": 2.154441595077514, - "y": 6.363574981689453, - "heading": 0.3737268100062681, - "angularVelocity": 3.1895226258919956e-35, - "velocityX": -6.441615179673898e-36, - "velocityY": -6.456245804980884e-36, - "timestamp": 0.639907952793279 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamTop.2.traj b/src/main/deploy/choreo/NoTeamTop.2.traj deleted file mode 100644 index 3ed7277..0000000 --- a/src/main/deploy/choreo/NoTeamTop.2.traj +++ /dev/null @@ -1,94 +0,0 @@ -{ - "samples": [ - { - "x": 2.154441595077514, - "y": 6.363574981689453, - "heading": 0.3737268100062681, - "angularVelocity": 3.1895226258919956e-35, - "velocityX": -6.441615179673898e-36, - "velocityY": -6.456245804980884e-36, - "timestamp": 0 - }, - { - "x": 2.1664141619492328, - "y": 6.323693868020063, - "heading": 0.35495101918303457, - "angularVelocity": -0.2646577261921422, - "velocityX": 0.16876159064530338, - "velocityY": -0.5621518135305482, - "timestamp": 0.07094367163723891 - }, - { - "x": 2.1903575818390344, - "y": 6.243931303899423, - "heading": 0.3173922807828279, - "angularVelocity": -0.5294163317661137, - "velocityX": 0.3374990233411272, - "velocityY": -1.1243083742337978, - "timestamp": 0.14188734327447783 - }, - { - "x": 2.226269330136065, - "y": 6.124286501815425, - "heading": 0.26105173416926286, - "angularVelocity": -0.7941588772238201, - "velocityX": 0.5062008699050788, - "velocityY": -1.6864760354635386, - "timestamp": 0.21283101491171674 - }, - { - "x": 2.2741463498064425, - "y": 5.964757852535621, - "heading": 0.18595590358446726, - "angularVelocity": -1.0585275451880791, - "velocityX": 0.674859625467234, - "velocityY": -2.248666379934951, - "timestamp": 0.28377468654895566 - }, - { - "x": 2.321990419372695, - "y": 5.805201381630825, - "heading": 0.11153311841802548, - "angularVelocity": -1.0490405056421792, - "velocityX": 0.6743951710153469, - "velocityY": -2.2490585449350284, - "timestamp": 0.35471835818619457 - }, - { - "x": 2.3578729357424715, - "y": 5.685532745421478, - "heading": 0.055757885713101384, - "angularVelocity": -0.7861903876377221, - "velocityX": 0.5057888257215818, - "velocityY": -1.6868119939049238, - "timestamp": 0.4256620298234335 - }, - { - "x": 2.381794458514866, - "y": 5.6057532815472815, - "heading": 0.018587215775969886, - "angularVelocity": -0.5239462390274757, - "velocityX": 0.33719036836314187, - "velocityY": -1.1245465879203078, - "timestamp": 0.4966057014606724 - }, - { - "x": 2.3937551975250244, - "y": 5.565863609313965, - "heading": -5.628092987966886e-31, - "angularVelocity": -0.26199963079178046, - "velocityX": 0.16859486877586724, - "velocityY": -0.5622724523941706, - "timestamp": 0.5675493730979113 - }, - { - "x": 2.3937551975250244, - "y": 5.565863609313965, - "heading": -5.174700324190939e-31, - "angularVelocity": -3.008085461468343e-31, - "velocityX": 1.034031579268112e-34, - "velocityY": 1.7448699710998943e-32, - "timestamp": 0.63849304473515 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamTop.3.traj b/src/main/deploy/choreo/NoTeamTop.3.traj deleted file mode 100644 index d55b55b..0000000 --- a/src/main/deploy/choreo/NoTeamTop.3.traj +++ /dev/null @@ -1,85 +0,0 @@ -{ - "samples": [ - { - "x": 2.3937551975250244, - "y": 5.565863609313965, - "heading": -5.174700324190939e-31, - "angularVelocity": -3.008085461468343e-31, - "velocityX": 1.034031579268112e-34, - "velocityY": 1.7448699710998943e-32, - "timestamp": 0 - }, - { - "x": 2.394754254757926, - "y": 5.5120190095853445, - "heading": -0.017115260393375086, - "angularVelocity": -0.21254841017002704, - "velocityX": 0.012406941036337042, - "velocityY": -0.6686771808034808, - "timestamp": 0.08052405745911706 - }, - { - "x": 2.396751769962143, - "y": 5.4043296750978795, - "heading": -0.051339403058144904, - "angularVelocity": -0.4250176126823665, - "velocityX": 0.02480644005340539, - "velocityY": -1.337356038499925, - "timestamp": 0.16104811491823412 - }, - { - "x": 2.3997467282028473, - "y": 5.242795162936243, - "heading": -0.10265156806320024, - "angularVelocity": -0.6372277630336057, - "velocityX": 0.03719333495117604, - "velocityY": -2.006040396606318, - "timestamp": 0.24157217237735118 - }, - { - "x": 2.403732200664872, - "y": 5.027411442372448, - "heading": -0.17081671584355101, - "angularVelocity": -0.8465190395423907, - "velocityX": 0.04949418332587571, - "velocityY": -2.674774810908508, - "timestamp": 0.32209622983646824 - }, - { - "x": 2.4067209273092627, - "y": 4.865873194470122, - "heading": -0.22191798416318598, - "angularVelocity": -0.6346087111367488, - "velocityX": 0.037115946944272976, - "velocityY": -2.0060867894584153, - "timestamp": 0.4026202872955853 - }, - { - "x": 2.4087132882328275, - "y": 4.758180884503706, - "heading": -0.2559781943076682, - "angularVelocity": -0.42298179226466653, - "velocityX": 0.024742430851509695, - "velocityY": -1.337392989928425, - "timestamp": 0.48314434475470236 - }, - { - "x": 2.4097094535827637, - "y": 4.704334735870361, - "heading": -0.2730086143988003, - "angularVelocity": -0.2114948082413471, - "velocityX": 0.012371027757036695, - "velocityY": -0.6686964161074856, - "timestamp": 0.5636684022138194 - }, - { - "x": 2.4097094535827637, - "y": 4.704334735870361, - "heading": -0.2730086143988003, - "angularVelocity": 8.309717858612118e-31, - "velocityX": 6.470358508109623e-32, - "velocityY": -3.482387330812075e-33, - "timestamp": 0.6441924596729365 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamTop.4.traj b/src/main/deploy/choreo/NoTeamTop.4.traj deleted file mode 100644 index e26d19d..0000000 --- a/src/main/deploy/choreo/NoTeamTop.4.traj +++ /dev/null @@ -1,76 +0,0 @@ -{ - "samples": [ - { - "x": 2.4097094535827637, - "y": 4.704334735870361, - "heading": -0.2730086143988003, - "angularVelocity": 8.309717858612118e-31, - "velocityX": 6.470358508109623e-32, - "velocityY": -3.482387330812075e-33, - "timestamp": 0 - }, - { - "x": 2.426997620799911, - "y": 4.656468232431784, - "heading": -0.25045854641235116, - "angularVelocity": 0.2875344989961346, - "velocityX": 0.22044033314361022, - "velocityY": -0.6103427756039931, - "timestamp": 0.07842560828414635 - }, - { - "x": 2.4615718445380113, - "y": 4.5607357498021335, - "heading": -0.20530912855642702, - "angularVelocity": 0.5756973881839936, - "velocityX": 0.4408537529327513, - "velocityY": -1.2206788665610262, - "timestamp": 0.1568512165682927 - }, - { - "x": 2.5134294609331516, - "y": 4.41713890238669, - "heading": -0.1374653380839415, - "angularVelocity": 0.8650719064451283, - "velocityX": 0.6612332059606509, - "velocityY": -1.8309943723378186, - "timestamp": 0.23527682485243906 - }, - { - "x": 2.5652719402701414, - "y": 4.273560233385289, - "heading": -0.06875061341189337, - "angularVelocity": 0.8761771336613093, - "velocityX": 0.6610401942839578, - "velocityY": -1.8307625805233965, - "timestamp": 0.3137024331365854 - }, - { - "x": 2.599833500729999, - "y": 4.1778417623198285, - "heading": -0.02291575248408611, - "angularVelocity": 0.5844374296944103, - "velocityX": 0.44069228426812324, - "velocityY": -1.2205002059870416, - "timestamp": 0.39212804142073177 - }, - { - "x": 2.617114305496216, - "y": 4.1299824714660645, - "heading": -1.023528578657426e-34, - "angularVelocity": 0.2921973190320597, - "velocityX": 0.22034645499472394, - "velocityY": -0.6102508083885511, - "timestamp": 0.4705536497048781 - }, - { - "x": 2.617114305496216, - "y": 4.1299824714660645, - "heading": -1.8621051633589405e-34, - "angularVelocity": 8.300885938147206e-34, - "velocityX": 2.197325936488708e-35, - "velocityY": -1.68400260681791e-35, - "timestamp": 0.5489792579890245 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamTop.5.traj b/src/main/deploy/choreo/NoTeamTop.5.traj deleted file mode 100644 index 0b1120e..0000000 --- a/src/main/deploy/choreo/NoTeamTop.5.traj +++ /dev/null @@ -1,94 +0,0 @@ -{ - "samples": [ - { - "x": 2.617114305496216, - "y": 4.1299824714660645, - "heading": -1.8621051633589405e-34, - "angularVelocity": 8.300885938147206e-34, - "velocityX": 2.197325936488708e-35, - "velocityY": -1.68400260681791e-35, - "timestamp": 0 - }, - { - "x": 2.590026298993156, - "y": 4.134571386049753, - "heading": -0.03154497655949952, - "angularVelocity": -0.5370590154132491, - "velocityX": -0.461178282209251, - "velocityY": 0.07812711299635268, - "timestamp": 0.05873651806259428 - }, - { - "x": 2.5358680596167886, - "y": 4.143794140189051, - "heading": -0.09482876594361848, - "angularVelocity": -1.0774181288151696, - "velocityX": -0.9220539651099411, - "velocityY": 0.15701908188479646, - "timestamp": 0.11747303612518856 - }, - { - "x": 2.454656525048389, - "y": 4.157724678166265, - "heading": -0.1899314404675443, - "angularVelocity": -1.6191404880789242, - "velocityX": -1.382641280878349, - "velocityY": 0.23716996575057447, - "timestamp": 0.17620955418778284 - }, - { - "x": 2.346386675307616, - "y": 4.176461325890866, - "heading": -0.3164723678981996, - "angularVelocity": -2.154382513716643, - "velocityX": -1.8433140627333764, - "velocityY": 0.31899486627099644, - "timestamp": 0.2349460722503771 - }, - { - "x": 2.2377299397341766, - "y": 4.196152369291608, - "heading": -0.43354708531926917, - "angularVelocity": -1.9932185509584521, - "velocityX": -1.8499008650401365, - "velocityY": 0.3352436278186754, - "timestamp": 0.2936825903129714 - }, - { - "x": 2.1562058087373717, - "y": 4.21092916440678, - "heading": -0.5207849832743855, - "angularVelocity": -1.4852412235629702, - "velocityX": -1.387963292443145, - "velocityY": 0.2515776488389278, - "timestamp": 0.35241910837556567 - }, - { - "x": 2.101847539092883, - "y": 4.220781491832273, - "heading": -0.5787788056200436, - "angularVelocity": -0.9873554691113143, - "velocityX": -0.925459517136503, - "velocityY": 0.1677376826286145, - "timestamp": 0.41115562643815995 - }, - { - "x": 2.0746705532073975, - "y": 4.2257080078125, - "heading": -0.6078018818929833, - "angularVelocity": -0.49412320018715394, - "velocityX": -0.4626931725255473, - "velocityY": 0.08387483873280513, - "timestamp": 0.4698921445007542 - }, - { - "x": 2.0746705532073975, - "y": 4.2257080078125, - "heading": -0.6078018818929833, - "angularVelocity": 9.526213155938014e-34, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0.5286286625633485 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/NoTeamTop.traj b/src/main/deploy/choreo/NoTeamTop.traj deleted file mode 100644 index c662977..0000000 --- a/src/main/deploy/choreo/NoTeamTop.traj +++ /dev/null @@ -1,526 +0,0 @@ -{ - "samples": [ - { - "x": 0.7823778390884399, - "y": 6.68265962600708, - "heading": 1.0516504568237233, - "angularVelocity": 0.000011116709994913391, - "velocityX": 3.4550282315085217, - "velocityY": 0.6846486983804677, - "timestamp": 0 - }, - { - "x": 0.9751074188860586, - "y": 6.720850881415975, - "heading": 1.0516503577916685, - "angularVelocity": -0.00000190674765800839, - "velocityX": 3.710785114782621, - "velocityY": 0.7353284443156448, - "timestamp": 0.05193768268333393 - }, - { - "x": 1.167836999199548, - "y": 6.7590421342229945, - "heading": 1.051650258760242, - "angularVelocity": -0.0000019067355590266187, - "velocityX": 3.7107851247151196, - "velocityY": 0.7353283942195143, - "timestamp": 0.10387536536666786 - }, - { - "x": 1.3605665805829503, - "y": 6.797233381630862, - "heading": 1.0516501597288161, - "angularVelocity": -0.000001906735551697809, - "velocityX": 3.710785145315053, - "velocityY": 0.7353282902651058, - "timestamp": 0.1558130480500018 - }, - { - "x": 1.5532961655237487, - "y": 6.835424611086715, - "heading": 1.051650060697391, - "angularVelocity": -0.0000019067355329513512, - "velocityX": 3.710785213808596, - "velocityY": 0.7353279446198315, - "timestamp": 0.20775073073333572 - }, - { - "x": 1.7460254429890028, - "y": 6.873617391792323, - "heading": 1.0516499614844557, - "angularVelocity": -0.000001910230304114969, - "velocityX": 3.710779293722669, - "velocityY": 0.7353578121394269, - "timestamp": 0.25968841341666965 - }, - { - "x": 1.9209155764266739, - "y": 6.9133027716288264, - "heading": 1.0096996919934733, - "angularVelocity": -0.8077039121432283, - "velocityX": 3.3673072112974913, - "velocityY": 0.7640960818076123, - "timestamp": 0.3116260961000036 - }, - { - "x": 2.078239262369381, - "y": 6.947237798484431, - "heading": 0.9225295067368948, - "angularVelocity": -1.6783610810682208, - "velocityX": 3.029085585160111, - "velocityY": 0.653379687008914, - "timestamp": 0.3635637787833375 - }, - { - "x": 2.2178882418919197, - "y": 6.9754482977346735, - "heading": 0.7884918073872876, - "angularVelocity": -2.580740849891981, - "velocityX": 2.6887795586488554, - "velocityY": 0.5431605299420711, - "timestamp": 0.41550146146667144 - }, - { - "x": 2.3397525319384047, - "y": 6.998192697437133, - "heading": 0.60541415124654, - "angularVelocity": -3.5249484898465573, - "velocityX": 2.346355935621853, - "velocityY": 0.4379171061815228, - "timestamp": 0.46743914415000537 - }, - { - "x": 2.4395876082580847, - "y": 7.016459363314808, - "heading": 0.4250239365373652, - "angularVelocity": -3.473204913839163, - "velocityX": 1.922208908094313, - "velocityY": 0.3517035211032972, - "timestamp": 0.5193768268333393 - }, - { - "x": 2.5180690988084318, - "y": 7.03039819123932, - "heading": 0.265125600134493, - "angularVelocity": -3.0786575014865125, - "velocityX": 1.5110703153402525, - "velocityY": 0.26837600763782044, - "timestamp": 0.5713145095166732 - }, - { - "x": 2.5760585000102645, - "y": 7.040303686655802, - "heading": 0.1368038723234758, - "angularVelocity": -2.470686429993418, - "velocityX": 1.1165188396139325, - "velocityY": 0.1907188558426073, - "timestamp": 0.6232521922000072 - }, - { - "x": 2.6142221606288607, - "y": 7.046583625856348, - "heading": 0.04689928938368535, - "angularVelocity": -1.7310087453832381, - "velocityX": 0.7347971385493195, - "velocityY": 0.12091296484741729, - "timestamp": 0.6751898748833411 - }, - { - "x": 2.633068561553955, - "y": 7.049606800079346, - "heading": 4.3474215162415935e-31, - "angularVelocity": -0.9029915652885839, - "velocityX": 0.36286564881998395, - "velocityY": 0.05820772253990189, - "timestamp": 0.7271275575666751 - }, - { - "x": 2.633068561553955, - "y": 7.049606800079346, - "heading": 3.803214140819317e-31, - "angularVelocity": 3.020347919142555e-31, - "velocityX": 7.840324925442265e-34, - "velocityY": 5.62397113618381e-32, - "timestamp": 0.7790652402500091 - }, - { - "x": 2.609144744005489, - "y": 7.015299626278446, - "heading": 0.018678562047563966, - "angularVelocity": 0.2627050620237914, - "velocityX": -0.33647707767394147, - "velocityY": -0.4825140285572393, - "timestamp": 0.8501661238937067 - }, - { - "x": 2.5612954442990494, - "y": 6.946685760529339, - "heading": 0.056012535459477056, - "angularVelocity": 0.5250845207353814, - "velocityX": -0.6729775672862703, - "velocityY": -0.9650212800862848, - "timestamp": 0.9212670075374043 - }, - { - "x": 2.4895180668345147, - "y": 6.843766221469334, - "heading": 0.11197590720911785, - "angularVelocity": 0.7870981186406304, - "velocityX": -1.009514562775715, - "velocityY": -1.4475142049676655, - "timestamp": 0.9923678911811019 - }, - { - "x": 2.393809314296558, - "y": 6.706543018483128, - "heading": 0.1865585662991481, - "angularVelocity": 1.0489695101931586, - "velocityX": -1.346097933431799, - "velocityY": -1.929978868812128, - "timestamp": 1.0634687748247995 - }, - { - "x": 2.298062994828891, - "y": 6.56935427735818, - "heading": 0.26139370468794165, - "angularVelocity": 1.0525205110503135, - "velocityX": -1.3466262943717173, - "velocityY": -1.9294941791782931, - "timestamp": 1.1345696584684972 - }, - { - "x": 2.22625248415356, - "y": 6.466464285312162, - "heading": 0.3175528585708617, - "angularVelocity": 0.7898517009204281, - "velocityX": -1.0099805655748195, - "velocityY": -1.447098640315394, - "timestamp": 1.205670542112195 - }, - { - "x": 2.178378576021294, - "y": 6.397871445224483, - "heading": 0.355003104186284, - "angularVelocity": 0.5267198337941039, - "velocityX": -0.6733236730526801, - "velocityY": -0.9647255641914598, - "timestamp": 1.2767714257558926 - }, - { - "x": 2.154441595077514, - "y": 6.363574981689453, - "heading": 0.3737268100062681, - "angularVelocity": 0.2633399876408391, - "velocityX": -0.3366622145476251, - "velocityY": -0.48236339377858967, - "timestamp": 1.3478723093995904 - }, - { - "x": 2.154441595077514, - "y": 6.363574981689453, - "heading": 0.3737268100062681, - "angularVelocity": 3.1895226258919956e-35, - "velocityX": -6.441615179673898e-36, - "velocityY": -6.456245804980884e-36, - "timestamp": 1.418973193043288 - }, - { - "x": 2.1664141619492328, - "y": 6.323693868020063, - "heading": 0.35495101918303457, - "angularVelocity": -0.2646577261921422, - "velocityX": 0.16876159064530338, - "velocityY": -0.5621518135305482, - "timestamp": 1.489916864680527 - }, - { - "x": 2.1903575818390344, - "y": 6.243931303899423, - "heading": 0.3173922807828279, - "angularVelocity": -0.5294163317661137, - "velocityX": 0.3374990233411272, - "velocityY": -1.1243083742337978, - "timestamp": 1.560860536317766 - }, - { - "x": 2.226269330136065, - "y": 6.124286501815425, - "heading": 0.26105173416926286, - "angularVelocity": -0.7941588772238201, - "velocityX": 0.5062008699050788, - "velocityY": -1.6864760354635386, - "timestamp": 1.6318042079550048 - }, - { - "x": 2.2741463498064425, - "y": 5.964757852535621, - "heading": 0.18595590358446726, - "angularVelocity": -1.0585275451880791, - "velocityX": 0.674859625467234, - "velocityY": -2.248666379934951, - "timestamp": 1.7027478795922437 - }, - { - "x": 2.321990419372695, - "y": 5.805201381630825, - "heading": 0.11153311841802548, - "angularVelocity": -1.0490405056421792, - "velocityX": 0.6743951710153469, - "velocityY": -2.2490585449350284, - "timestamp": 1.7736915512294826 - }, - { - "x": 2.3578729357424715, - "y": 5.685532745421478, - "heading": 0.055757885713101384, - "angularVelocity": -0.7861903876377221, - "velocityX": 0.5057888257215818, - "velocityY": -1.6868119939049238, - "timestamp": 1.8446352228667215 - }, - { - "x": 2.381794458514866, - "y": 5.6057532815472815, - "heading": 0.018587215775969886, - "angularVelocity": -0.5239462390274757, - "velocityX": 0.33719036836314187, - "velocityY": -1.1245465879203078, - "timestamp": 1.9155788945039605 - }, - { - "x": 2.3937551975250244, - "y": 5.565863609313965, - "heading": -5.628092987966886e-31, - "angularVelocity": -0.26199963079178046, - "velocityX": 0.16859486877586724, - "velocityY": -0.5622724523941706, - "timestamp": 1.9865225661411994 - }, - { - "x": 2.3937551975250244, - "y": 5.565863609313965, - "heading": -5.174700324190939e-31, - "angularVelocity": -3.008085461468343e-31, - "velocityX": 1.034031579268112e-34, - "velocityY": 1.7448699710998943e-32, - "timestamp": 2.057466237778438 - }, - { - "x": 2.394754254757926, - "y": 5.5120190095853445, - "heading": -0.017115260393375086, - "angularVelocity": -0.21254841017002704, - "velocityX": 0.012406941036337042, - "velocityY": -0.6686771808034808, - "timestamp": 2.137990295237555 - }, - { - "x": 2.396751769962143, - "y": 5.4043296750978795, - "heading": -0.051339403058144904, - "angularVelocity": -0.4250176126823665, - "velocityX": 0.02480644005340539, - "velocityY": -1.337356038499925, - "timestamp": 2.218514352696672 - }, - { - "x": 2.3997467282028473, - "y": 5.242795162936243, - "heading": -0.10265156806320024, - "angularVelocity": -0.6372277630336057, - "velocityX": 0.03719333495117604, - "velocityY": -2.006040396606318, - "timestamp": 2.2990384101557892 - }, - { - "x": 2.403732200664872, - "y": 5.027411442372448, - "heading": -0.17081671584355101, - "angularVelocity": -0.8465190395423907, - "velocityX": 0.04949418332587571, - "velocityY": -2.674774810908508, - "timestamp": 2.3795624676149063 - }, - { - "x": 2.4067209273092627, - "y": 4.865873194470122, - "heading": -0.22191798416318598, - "angularVelocity": -0.6346087111367488, - "velocityX": 0.037115946944272976, - "velocityY": -2.0060867894584153, - "timestamp": 2.4600865250740234 - }, - { - "x": 2.4087132882328275, - "y": 4.758180884503706, - "heading": -0.2559781943076682, - "angularVelocity": -0.42298179226466653, - "velocityX": 0.024742430851509695, - "velocityY": -1.337392989928425, - "timestamp": 2.5406105825331404 - }, - { - "x": 2.4097094535827637, - "y": 4.704334735870361, - "heading": -0.2730086143988003, - "angularVelocity": -0.2114948082413471, - "velocityX": 0.012371027757036695, - "velocityY": -0.6686964161074856, - "timestamp": 2.6211346399922575 - }, - { - "x": 2.4097094535827637, - "y": 4.704334735870361, - "heading": -0.2730086143988003, - "angularVelocity": 8.309717858612118e-31, - "velocityX": 6.470358508109623e-32, - "velocityY": -3.482387330812075e-33, - "timestamp": 2.7016586974513745 - }, - { - "x": 2.426997620799911, - "y": 4.656468232431784, - "heading": -0.25045854641235116, - "angularVelocity": 0.2875344989961346, - "velocityX": 0.22044033314361022, - "velocityY": -0.6103427756039931, - "timestamp": 2.780084305735521 - }, - { - "x": 2.4615718445380113, - "y": 4.5607357498021335, - "heading": -0.20530912855642702, - "angularVelocity": 0.5756973881839936, - "velocityX": 0.4408537529327513, - "velocityY": -1.2206788665610262, - "timestamp": 2.8585099140196673 - }, - { - "x": 2.5134294609331516, - "y": 4.41713890238669, - "heading": -0.1374653380839415, - "angularVelocity": 0.8650719064451283, - "velocityX": 0.6612332059606509, - "velocityY": -1.8309943723378186, - "timestamp": 2.9369355223038136 - }, - { - "x": 2.5652719402701414, - "y": 4.273560233385289, - "heading": -0.06875061341189337, - "angularVelocity": 0.8761771336613093, - "velocityX": 0.6610401942839578, - "velocityY": -1.8307625805233965, - "timestamp": 3.01536113058796 - }, - { - "x": 2.599833500729999, - "y": 4.1778417623198285, - "heading": -0.02291575248408611, - "angularVelocity": 0.5844374296944103, - "velocityX": 0.44069228426812324, - "velocityY": -1.2205002059870416, - "timestamp": 3.0937867388721063 - }, - { - "x": 2.617114305496216, - "y": 4.1299824714660645, - "heading": -1.023528578657426e-34, - "angularVelocity": 0.2921973190320597, - "velocityX": 0.22034645499472394, - "velocityY": -0.6102508083885511, - "timestamp": 3.1722123471562527 - }, - { - "x": 2.617114305496216, - "y": 4.1299824714660645, - "heading": -1.8621051633589405e-34, - "angularVelocity": 8.300885938147206e-34, - "velocityX": 2.197325936488708e-35, - "velocityY": -1.68400260681791e-35, - "timestamp": 3.250637955440399 - }, - { - "x": 2.590026298993156, - "y": 4.134571386049753, - "heading": -0.03154497655949952, - "angularVelocity": -0.5370590154132491, - "velocityX": -0.461178282209251, - "velocityY": 0.07812711299635268, - "timestamp": 3.3093744735029933 - }, - { - "x": 2.5358680596167886, - "y": 4.143794140189051, - "heading": -0.09482876594361848, - "angularVelocity": -1.0774181288151696, - "velocityX": -0.9220539651099411, - "velocityY": 0.15701908188479646, - "timestamp": 3.3681109915655876 - }, - { - "x": 2.454656525048389, - "y": 4.157724678166265, - "heading": -0.1899314404675443, - "angularVelocity": -1.6191404880789242, - "velocityX": -1.382641280878349, - "velocityY": 0.23716996575057447, - "timestamp": 3.426847509628182 - }, - { - "x": 2.346386675307616, - "y": 4.176461325890866, - "heading": -0.3164723678981996, - "angularVelocity": -2.154382513716643, - "velocityX": -1.8433140627333764, - "velocityY": 0.31899486627099644, - "timestamp": 3.485584027690776 - }, - { - "x": 2.2377299397341766, - "y": 4.196152369291608, - "heading": -0.43354708531926917, - "angularVelocity": -1.9932185509584521, - "velocityX": -1.8499008650401365, - "velocityY": 0.3352436278186754, - "timestamp": 3.5443205457533704 - }, - { - "x": 2.1562058087373717, - "y": 4.21092916440678, - "heading": -0.5207849832743855, - "angularVelocity": -1.4852412235629702, - "velocityX": -1.387963292443145, - "velocityY": 0.2515776488389278, - "timestamp": 3.6030570638159647 - }, - { - "x": 2.101847539092883, - "y": 4.220781491832273, - "heading": -0.5787788056200436, - "angularVelocity": -0.9873554691113143, - "velocityX": -0.925459517136503, - "velocityY": 0.1677376826286145, - "timestamp": 3.661793581878559 - }, - { - "x": 2.0746705532073975, - "y": 4.2257080078125, - "heading": -0.6078018818929833, - "angularVelocity": -0.49412320018715394, - "velocityX": -0.4626931725255473, - "velocityY": 0.08387483873280513, - "timestamp": 3.7205300999411532 - }, - { - "x": 2.0746705532073975, - "y": 4.2257080078125, - "heading": -0.6078018818929833, - "angularVelocity": 9.526213155938014e-34, - "velocityX": 0, - "velocityY": 0, - "timestamp": 3.7792666180037475 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SpeakerLineSweep.1.traj b/src/main/deploy/choreo/SpeakerLineSweep.1.traj deleted file mode 100644 index 9cb2517..0000000 --- a/src/main/deploy/choreo/SpeakerLineSweep.1.traj +++ /dev/null @@ -1,112 +0,0 @@ -{ - "samples": [ - { - "x": 1.3384926319122314, - "y": 4.117048263549805, - "heading": 5.0534082968991327e-20, - "angularVelocity": -4.6025172384168114e-20, - "velocityX": -3.7103990119463343e-19, - "velocityY": -2.821089137271851e-19, - "timestamp": 0 - }, - { - "x": 1.3653357299793498, - "y": 4.133349259334737, - "heading": -0.019009255341576676, - "angularVelocity": -0.2529021169326957, - "velocityX": 0.35712479022564303, - "velocityY": 0.2168710066776479, - "timestamp": 0.07516447696100347 - }, - { - "x": 1.4190230645876685, - "y": 4.165950829153562, - "heading": -0.05692830651267439, - "angularVelocity": -0.504481008904919, - "velocityX": 0.7142647268891719, - "velocityY": 0.43373640231328936, - "timestamp": 0.15032895392200693 - }, - { - "x": 1.4995568270554427, - "y": 4.214854288391961, - "heading": -0.11356410091898407, - "angularVelocity": -0.7534914988591354, - "velocityX": 1.0714338171947415, - "velocityY": 0.6506192980464744, - "timestamp": 0.22549343088301038 - }, - { - "x": 1.6069393550259177, - "y": 4.280063365399126, - "heading": -0.18864668681443278, - "angularVelocity": -0.9989105084094765, - "velocityX": 1.4286340078729833, - "velocityY": 0.8675517963225728, - "timestamp": 0.30065790784401386 - }, - { - "x": 1.7411719508325003, - "y": 4.36158463237266, - "heading": -0.28186205887156096, - "angularVelocity": -1.240151941794118, - "velocityX": 1.7858515249992974, - "velocityY": 1.0845717321471864, - "timestamp": 0.37582238480501734 - }, - { - "x": 1.8753985919559228, - "y": 4.44324068709112, - "heading": -0.3699075178015094, - "angularVelocity": -1.171370606032797, - "velocityX": 1.7857723029598294, - "velocityY": 1.0863649694632398, - "timestamp": 0.4509868617660208 - }, - { - "x": 1.982777264301858, - "y": 4.508571282122505, - "heading": -0.44023155047465523, - "angularVelocity": -0.9356019693934848, - "velocityX": 1.428582712038893, - "velocityY": 0.8691684911913804, - "timestamp": 0.5261513387270242 - }, - { - "x": 2.0633094933142337, - "y": 4.557571817735121, - "heading": -0.4929376794671588, - "angularVelocity": -0.7012106133572711, - "velocityX": 1.0714134158634099, - "velocityY": 0.6519108173670729, - "timestamp": 0.6013158156880277 - }, - { - "x": 2.1169967366564135, - "y": 4.590239479611671, - "heading": -0.5280752869185609, - "angularVelocity": -0.4674762450569844, - "velocityX": 0.7142635126701379, - "velocityY": 0.43461570142366257, - "timestamp": 0.6764802926490312 - }, - { - "x": 2.143840074539185, - "y": 4.606573104858398, - "heading": -0.545654759034219, - "angularVelocity": -0.23388005646309096, - "velocityX": 0.3571279807707339, - "velocityY": 0.217305114159198, - "timestamp": 0.7516447696100347 - }, - { - "x": 2.143840074539185, - "y": 4.606573104858398, - "heading": -0.545654759034219, - "angularVelocity": 1.5683301444144844e-20, - "velocityX": -3.0142842793093394e-19, - "velocityY": -5.3980366625836286e-20, - "timestamp": 0.8268092465710382 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SpeakerLineSweep.2.traj b/src/main/deploy/choreo/SpeakerLineSweep.2.traj deleted file mode 100644 index 1894cf8..0000000 --- a/src/main/deploy/choreo/SpeakerLineSweep.2.traj +++ /dev/null @@ -1,58 +0,0 @@ -{ - "samples": [ - { - "x": 2.143840074539185, - "y": 4.606573104858398, - "heading": -0.545654759034219, - "angularVelocity": 1.5683301444144844e-20, - "velocityX": -3.0142842793093394e-19, - "velocityY": -5.3980366625836286e-20, - "timestamp": 0 - }, - { - "x": 2.1859497246952704, - "y": 4.577622730310198, - "heading": -0.545654759034219, - "angularVelocity": -1.2223454048168632e-16, - "velocityX": 0.44131994440041794, - "velocityY": -0.30340735766328875, - "timestamp": 0.0954175098823048 - }, - { - "x": 2.270169019699097, - "y": 4.519721984863281, - "heading": -0.545654759034219, - "angularVelocity": -4.2630447189396326e-16, - "velocityX": 0.8826398331680338, - "velocityY": -0.6068146770790392, - "timestamp": 0.1908350197646096 - }, - { - "x": 2.354388314702924, - "y": 4.4618212394163645, - "heading": -0.545654759034219, - "angularVelocity": -2.8775665677216486e-16, - "velocityX": 0.8826398331680338, - "velocityY": -0.6068146770790392, - "timestamp": 0.2862525296469145 - }, - { - "x": 2.396497964859009, - "y": 4.432870864868164, - "heading": -0.545654759034219, - "angularVelocity": -2.2425802645674346e-16, - "velocityX": 0.44131994440041783, - "velocityY": -0.30340735766328875, - "timestamp": 0.3816700395292194 - }, - { - "x": 2.396497964859009, - "y": 4.432870864868164, - "heading": -0.545654759034219, - "angularVelocity": -4.500777153356061e-21, - "velocityX": 6.959937778265549e-19, - "velocityY": -9.390851389278111e-19, - "timestamp": 0.4770875494115243 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SpeakerLineSweep.3.traj b/src/main/deploy/choreo/SpeakerLineSweep.3.traj deleted file mode 100644 index 19c416c..0000000 --- a/src/main/deploy/choreo/SpeakerLineSweep.3.traj +++ /dev/null @@ -1,166 +0,0 @@ -{ - "samples": [ - { - "x": 2.396497964859009, - "y": 4.432870864868164, - "heading": -0.545654759034219, - "angularVelocity": -4.500777153356061e-21, - "velocityX": 6.959937778265549e-19, - "velocityY": -9.390851389278111e-19, - "timestamp": 0 - }, - { - "x": 2.3892073841825243, - "y": 4.443517909183797, - "heading": -0.5399198559541022, - "angularVelocity": 0.11930046625875322, - "velocityX": -0.15166248877285388, - "velocityY": 0.22148540845205233, - "timestamp": 0.04807108689481798 - }, - { - "x": 2.3749151619024427, - "y": 4.465002653365564, - "heading": -0.5284131755570481, - "angularVelocity": 0.2393680097608593, - "velocityX": -0.29731431517980145, - "velocityY": 0.446936933811727, - "timestamp": 0.09614217378963597 - }, - { - "x": 2.3540076462609654, - "y": 4.497563533418931, - "heading": -0.5111034409840843, - "angularVelocity": 0.3600861909121844, - "velocityX": -0.43492912251441324, - "velocityY": 0.677348530200942, - "timestamp": 0.14421326068445395 - }, - { - "x": 2.327025085213186, - "y": 4.541504166789359, - "heading": -0.48796944493464006, - "angularVelocity": 0.4812455374695909, - "velocityX": -0.5613054081098445, - "velocityY": 0.9140761361725198, - "timestamp": 0.19228434757927193 - }, - { - "x": 2.2947682894540304, - "y": 4.597215511178205, - "heading": -0.45900953597015726, - "angularVelocity": 0.6024392381193364, - "velocityX": -0.6710228089856828, - "velocityY": 1.1589366496069795, - "timestamp": 0.2403554344740899 - }, - { - "x": 2.2585182251752487, - "y": 4.665190312064463, - "heading": -0.4242633451549343, - "angularVelocity": 0.7228085125524426, - "velocityX": -0.7540928782845764, - "velocityY": 1.4140475133210455, - "timestamp": 0.2884265213689079 - }, - { - "x": 2.2205222312578843, - "y": 4.7459443413027, - "heading": -0.38387247213865705, - "angularVelocity": 0.8402321566943997, - "velocityX": -0.7904126237149689, - "velocityY": 1.679887734074554, - "timestamp": 0.3364976082637259 - }, - { - "x": 2.184971991211838, - "y": 4.839372484112632, - "heading": -0.33834514086205514, - "angularVelocity": 0.9470834594653016, - "velocityX": -0.7395347669968644, - "velocityY": 1.9435413019546706, - "timestamp": 0.38456869515854386 - }, - { - "x": 2.158144285848682, - "y": 4.942362627890074, - "heading": -0.2893597447607032, - "angularVelocity": 1.0190199403756723, - "velocityX": -0.558084018816885, - "velocityY": 2.142455068735783, - "timestamp": 0.43263978205336184 - }, - { - "x": 2.143840074539185, - "y": 5.048724174499512, - "heading": -0.23976704076559605, - "angularVelocity": 1.0316534781836302, - "velocityX": -0.29756371726721886, - "velocityY": 2.2125887613517055, - "timestamp": 0.4807108689481798 - }, - { - "x": 2.150831153056428, - "y": 5.203120058815637, - "heading": -0.17022330405415592, - "angularVelocity": 0.9811154477697007, - "velocityX": 0.0986293727398952, - "velocityY": 2.1782002857159504, - "timestamp": 0.5515931864873937 - }, - { - "x": 2.177641559660979, - "y": 5.337627387175908, - "heading": -0.11192179088651392, - "angularVelocity": 0.8225113849499632, - "velocityX": 0.3782382903848818, - "velocityY": 1.8976147088567974, - "timestamp": 0.6224755040266077 - }, - { - "x": 2.2094472529458686, - "y": 5.444555699782394, - "heading": -0.06628298178054384, - "angularVelocity": 0.6438673380102371, - "velocityX": 0.44871124970334597, - "velocityY": 1.508532964477786, - "timestamp": 0.6933578215658216 - }, - { - "x": 2.237923805238022, - "y": 5.523641889302388, - "heading": -0.032778902048380895, - "angularVelocity": 0.4726719003456341, - "velocityX": 0.4017440919083876, - "velocityY": 1.1157393305635117, - "timestamp": 0.7642401391050355 - }, - { - "x": 2.2589370922392824, - "y": 5.575682196671565, - "heading": -0.010823932954501845, - "angularVelocity": 0.3097383078894531, - "velocityX": 0.2964531596986178, - "velocityY": 0.7341789768708907, - "timestamp": 0.8351224566442497 - }, - { - "x": 2.270169258117676, - "y": 5.601413726806641, - "heading": 1.5872539686819988e-19, - "angularVelocity": 0.15270286483668877, - "velocityX": 0.1584621703738651, - "velocityY": 0.36301761889825573, - "timestamp": 0.9060047741834638 - }, - { - "x": 2.270169258117676, - "y": 5.601413726806641, - "heading": 2.0912492620225186e-20, - "angularVelocity": -6.859631204245618e-21, - "velocityX": -6.421349100095649e-19, - "velocityY": -5.26277378439996e-19, - "timestamp": 0.976887091722678 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SpeakerLineSweep.4.traj b/src/main/deploy/choreo/SpeakerLineSweep.4.traj deleted file mode 100644 index f4944d2..0000000 --- a/src/main/deploy/choreo/SpeakerLineSweep.4.traj +++ /dev/null @@ -1,166 +0,0 @@ -{ - "samples": [ - { - "x": 2.270169258117676, - "y": 5.601413726806641, - "heading": 2.0912492620225186e-20, - "angularVelocity": -6.859631204245618e-21, - "velocityX": -6.421349100095649e-19, - "velocityY": -5.26277378439996e-19, - "timestamp": 0 - }, - { - "x": 2.258307261050276, - "y": 5.622913942657856, - "heading": 0.011289477414845548, - "angularVelocity": 0.17015589193737768, - "velocityX": -0.17878495319082285, - "velocityY": 0.3240529451078872, - "timestamp": 0.06634784894195978 - }, - { - "x": 2.2359428908787105, - "y": 5.6665943436646735, - "heading": 0.0343229879295691, - "angularVelocity": 0.3471628829274178, - "velocityX": -0.3370775470223548, - "velocityY": 0.6583544410765856, - "timestamp": 0.13269569788391955 - }, - { - "x": 2.2053270603401187, - "y": 5.733363241767063, - "heading": 0.06978539081037693, - "angularVelocity": 0.5344921266675867, - "velocityX": -0.461444206961015, - "velocityY": 1.0063460860773108, - "timestamp": 0.19904354682587933 - }, - { - "x": 2.1706747853331265, - "y": 5.824280847900589, - "heading": 0.1187288687566709, - "angularVelocity": 0.7376799508467724, - "velocityX": -0.522281815606492, - "velocityY": 1.3703173137241953, - "timestamp": 0.2653913957678391 - }, - { - "x": 2.1408548318247003, - "y": 5.939174457686258, - "heading": 0.18203167256068778, - "angularVelocity": 0.9541048400739172, - "velocityX": -0.44944868573678187, - "velocityY": 1.7316855273812526, - "timestamp": 0.3317392447097989 - }, - { - "x": 2.13002082306829, - "y": 6.069625216883796, - "heading": 0.25771635797715586, - "angularVelocity": 1.1407255340361668, - "velocityX": -0.1632910324777442, - "velocityY": 1.9661641074702068, - "timestamp": 0.39808709365175865 - }, - { - "x": 2.143840074539185, - "y": 6.201476573944092, - "heading": 0.3371459606364029, - "angularVelocity": 1.197169221397527, - "velocityX": 0.2082848455717636, - "velocityY": 1.9872740286672685, - "timestamp": 0.4644349425937184 - }, - { - "x": 2.166370769756068, - "y": 6.293640927562095, - "heading": 0.3939319180386296, - "angularVelocity": 1.1819496828334453, - "velocityX": 0.4689565745450901, - "velocityY": 1.9183198366410883, - "timestamp": 0.51247925296241 - }, - { - "x": 2.20004037690368, - "y": 6.3792187798135584, - "heading": 0.4481157099984023, - "angularVelocity": 1.127787901292925, - "velocityX": 0.7008032145582243, - "velocityY": 1.7812276124839865, - "timestamp": 0.5605235633311016 - }, - { - "x": 2.241158412431322, - "y": 6.454264775706248, - "heading": 0.497428483818663, - "angularVelocity": 1.0264019494053482, - "velocityX": 0.8558356902638964, - "velocityY": 1.562016299469941, - "timestamp": 0.6085678736997933 - }, - { - "x": 2.283988140776994, - "y": 6.516572419092989, - "heading": 0.5400587851533989, - "angularVelocity": 0.8873121709436844, - "velocityX": 0.8914630685089917, - "velocityY": 1.296878712767307, - "timestamp": 0.6566121840684849 - }, - { - "x": 2.32381550199, - "y": 6.5663973747831905, - "heading": 0.5753207631425283, - "angularVelocity": 0.7339470109681908, - "velocityX": 0.8289714413084713, - "velocityY": 1.0370625638675175, - "timestamp": 0.7046564944371765 - }, - { - "x": 2.3578617471237973, - "y": 6.6047562023476845, - "heading": 0.6030896816888531, - "angularVelocity": 0.5779855790045973, - "velocityX": 0.7086426024752379, - "velocityY": 0.7984052069876424, - "timestamp": 0.7527008048058681 - }, - { - "x": 2.38454267716584, - "y": 6.632586125877766, - "heading": 0.6235365890913814, - "angularVelocity": 0.4255843667152008, - "velocityX": 0.5553400566538157, - "velocityY": 0.5792553440046356, - "timestamp": 0.8007451151745597 - }, - { - "x": 2.4028949714522336, - "y": 6.650619931398898, - "heading": 0.6369216126446875, - "angularVelocity": 0.2785974749265757, - "velocityX": 0.3819868397643466, - "velocityY": 0.3753577766595345, - "timestamp": 0.8487894255432513 - }, - { - "x": 2.4122893810272217, - "y": 6.659419059753418, - "heading": 0.6435012899724419, - "angularVelocity": 0.13695018780084564, - "velocityX": 0.19553636014120032, - "velocityY": 0.18314610589672853, - "timestamp": 0.8968337359119429 - }, - { - "x": 2.4122893810272217, - "y": 6.659419059753418, - "heading": 0.6435012899724419, - "angularVelocity": -1.217565429636075e-22, - "velocityX": -3.177543138441335e-19, - "velocityY": -2.2806254854922123e-19, - "timestamp": 0.9448780462806345 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/SpeakerLineSweep.traj b/src/main/deploy/choreo/SpeakerLineSweep.traj deleted file mode 100644 index fd39d52..0000000 --- a/src/main/deploy/choreo/SpeakerLineSweep.traj +++ /dev/null @@ -1,463 +0,0 @@ -{ - "samples": [ - { - "x": 1.3384926319122314, - "y": 4.117048263549805, - "heading": 5.0534082968991327e-20, - "angularVelocity": -4.6025172384168114e-20, - "velocityX": -3.7103990119463343e-19, - "velocityY": -2.821089137271851e-19, - "timestamp": 0 - }, - { - "x": 1.3653357299793498, - "y": 4.133349259334737, - "heading": -0.019009255341576676, - "angularVelocity": -0.2529021169326957, - "velocityX": 0.35712479022564303, - "velocityY": 0.2168710066776479, - "timestamp": 0.07516447696100347 - }, - { - "x": 1.4190230645876685, - "y": 4.165950829153562, - "heading": -0.05692830651267439, - "angularVelocity": -0.504481008904919, - "velocityX": 0.7142647268891719, - "velocityY": 0.43373640231328936, - "timestamp": 0.15032895392200693 - }, - { - "x": 1.4995568270554427, - "y": 4.214854288391961, - "heading": -0.11356410091898407, - "angularVelocity": -0.7534914988591354, - "velocityX": 1.0714338171947415, - "velocityY": 0.6506192980464744, - "timestamp": 0.22549343088301038 - }, - { - "x": 1.6069393550259177, - "y": 4.280063365399126, - "heading": -0.18864668681443278, - "angularVelocity": -0.9989105084094765, - "velocityX": 1.4286340078729833, - "velocityY": 0.8675517963225728, - "timestamp": 0.30065790784401386 - }, - { - "x": 1.7411719508325003, - "y": 4.36158463237266, - "heading": -0.28186205887156096, - "angularVelocity": -1.240151941794118, - "velocityX": 1.7858515249992974, - "velocityY": 1.0845717321471864, - "timestamp": 0.37582238480501734 - }, - { - "x": 1.8753985919559228, - "y": 4.44324068709112, - "heading": -0.3699075178015094, - "angularVelocity": -1.171370606032797, - "velocityX": 1.7857723029598294, - "velocityY": 1.0863649694632398, - "timestamp": 0.4509868617660208 - }, - { - "x": 1.982777264301858, - "y": 4.508571282122505, - "heading": -0.44023155047465523, - "angularVelocity": -0.9356019693934848, - "velocityX": 1.428582712038893, - "velocityY": 0.8691684911913804, - "timestamp": 0.5261513387270242 - }, - { - "x": 2.0633094933142337, - "y": 4.557571817735121, - "heading": -0.4929376794671588, - "angularVelocity": -0.7012106133572711, - "velocityX": 1.0714134158634099, - "velocityY": 0.6519108173670729, - "timestamp": 0.6013158156880277 - }, - { - "x": 2.1169967366564135, - "y": 4.590239479611671, - "heading": -0.5280752869185609, - "angularVelocity": -0.4674762450569844, - "velocityX": 0.7142635126701379, - "velocityY": 0.43461570142366257, - "timestamp": 0.6764802926490312 - }, - { - "x": 2.143840074539185, - "y": 4.606573104858398, - "heading": -0.545654759034219, - "angularVelocity": -0.23388005646309096, - "velocityX": 0.3571279807707339, - "velocityY": 0.217305114159198, - "timestamp": 0.7516447696100347 - }, - { - "x": 2.143840074539185, - "y": 4.606573104858398, - "heading": -0.545654759034219, - "angularVelocity": 1.5683301444144844e-20, - "velocityX": -3.0142842793093394e-19, - "velocityY": -5.3980366625836286e-20, - "timestamp": 0.8268092465710382 - }, - { - "x": 2.1859497246952704, - "y": 4.577622730310198, - "heading": -0.545654759034219, - "angularVelocity": -1.2223454048168632e-16, - "velocityX": 0.44131994440041794, - "velocityY": -0.30340735766328875, - "timestamp": 0.922226756453343 - }, - { - "x": 2.270169019699097, - "y": 4.519721984863281, - "heading": -0.545654759034219, - "angularVelocity": -4.2630447189396326e-16, - "velocityX": 0.8826398331680338, - "velocityY": -0.6068146770790392, - "timestamp": 1.0176442663356478 - }, - { - "x": 2.354388314702924, - "y": 4.4618212394163645, - "heading": -0.545654759034219, - "angularVelocity": -2.8775665677216486e-16, - "velocityX": 0.8826398331680338, - "velocityY": -0.6068146770790392, - "timestamp": 1.1130617762179527 - }, - { - "x": 2.396497964859009, - "y": 4.432870864868164, - "heading": -0.545654759034219, - "angularVelocity": -2.2425802645674346e-16, - "velocityX": 0.44131994440041783, - "velocityY": -0.30340735766328875, - "timestamp": 1.2084792861002576 - }, - { - "x": 2.396497964859009, - "y": 4.432870864868164, - "heading": -0.545654759034219, - "angularVelocity": -4.500777153356061e-21, - "velocityX": 6.959937778265549e-19, - "velocityY": -9.390851389278111e-19, - "timestamp": 1.3038967959825625 - }, - { - "x": 2.3892073841825243, - "y": 4.443517909183797, - "heading": -0.5399198559541022, - "angularVelocity": 0.11930046625875322, - "velocityX": -0.15166248877285388, - "velocityY": 0.22148540845205233, - "timestamp": 1.3519678828773805 - }, - { - "x": 2.3749151619024427, - "y": 4.465002653365564, - "heading": -0.5284131755570481, - "angularVelocity": 0.2393680097608593, - "velocityX": -0.29731431517980145, - "velocityY": 0.446936933811727, - "timestamp": 1.4000389697721984 - }, - { - "x": 2.3540076462609654, - "y": 4.497563533418931, - "heading": -0.5111034409840843, - "angularVelocity": 0.3600861909121844, - "velocityX": -0.43492912251441324, - "velocityY": 0.677348530200942, - "timestamp": 1.4481100566670164 - }, - { - "x": 2.327025085213186, - "y": 4.541504166789359, - "heading": -0.48796944493464006, - "angularVelocity": 0.4812455374695909, - "velocityX": -0.5613054081098445, - "velocityY": 0.9140761361725198, - "timestamp": 1.4961811435618344 - }, - { - "x": 2.2947682894540304, - "y": 4.597215511178205, - "heading": -0.45900953597015726, - "angularVelocity": 0.6024392381193364, - "velocityX": -0.6710228089856828, - "velocityY": 1.1589366496069795, - "timestamp": 1.5442522304566524 - }, - { - "x": 2.2585182251752487, - "y": 4.665190312064463, - "heading": -0.4242633451549343, - "angularVelocity": 0.7228085125524426, - "velocityX": -0.7540928782845764, - "velocityY": 1.4140475133210455, - "timestamp": 1.5923233173514704 - }, - { - "x": 2.2205222312578843, - "y": 4.7459443413027, - "heading": -0.38387247213865705, - "angularVelocity": 0.8402321566943997, - "velocityX": -0.7904126237149689, - "velocityY": 1.679887734074554, - "timestamp": 1.6403944042462884 - }, - { - "x": 2.184971991211838, - "y": 4.839372484112632, - "heading": -0.33834514086205514, - "angularVelocity": 0.9470834594653016, - "velocityX": -0.7395347669968644, - "velocityY": 1.9435413019546706, - "timestamp": 1.6884654911411063 - }, - { - "x": 2.158144285848682, - "y": 4.942362627890074, - "heading": -0.2893597447607032, - "angularVelocity": 1.0190199403756723, - "velocityX": -0.558084018816885, - "velocityY": 2.142455068735783, - "timestamp": 1.7365365780359243 - }, - { - "x": 2.143840074539185, - "y": 5.048724174499512, - "heading": -0.23976704076559605, - "angularVelocity": 1.0316534781836302, - "velocityX": -0.29756371726721886, - "velocityY": 2.2125887613517055, - "timestamp": 1.7846076649307423 - }, - { - "x": 2.150831153056428, - "y": 5.203120058815637, - "heading": -0.17022330405415592, - "angularVelocity": 0.9811154477697007, - "velocityX": 0.0986293727398952, - "velocityY": 2.1782002857159504, - "timestamp": 1.8554899824699562 - }, - { - "x": 2.177641559660979, - "y": 5.337627387175908, - "heading": -0.11192179088651392, - "angularVelocity": 0.8225113849499632, - "velocityX": 0.3782382903848818, - "velocityY": 1.8976147088567974, - "timestamp": 1.9263723000091701 - }, - { - "x": 2.2094472529458686, - "y": 5.444555699782394, - "heading": -0.06628298178054384, - "angularVelocity": 0.6438673380102371, - "velocityX": 0.44871124970334597, - "velocityY": 1.508532964477786, - "timestamp": 1.997254617548384 - }, - { - "x": 2.237923805238022, - "y": 5.523641889302388, - "heading": -0.032778902048380895, - "angularVelocity": 0.4726719003456341, - "velocityX": 0.4017440919083876, - "velocityY": 1.1157393305635117, - "timestamp": 2.068136935087598 - }, - { - "x": 2.2589370922392824, - "y": 5.575682196671565, - "heading": -0.010823932954501845, - "angularVelocity": 0.3097383078894531, - "velocityX": 0.2964531596986178, - "velocityY": 0.7341789768708907, - "timestamp": 2.139019252626812 - }, - { - "x": 2.270169258117676, - "y": 5.601413726806641, - "heading": 1.5872539686819988e-19, - "angularVelocity": 0.15270286483668877, - "velocityX": 0.1584621703738651, - "velocityY": 0.36301761889825573, - "timestamp": 2.2099015701660263 - }, - { - "x": 2.270169258117676, - "y": 5.601413726806641, - "heading": 2.0912492620225186e-20, - "angularVelocity": -6.859631204245618e-21, - "velocityX": -6.421349100095649e-19, - "velocityY": -5.26277378439996e-19, - "timestamp": 2.2807838877052404 - }, - { - "x": 2.258307261050276, - "y": 5.622913942657856, - "heading": 0.011289477414845548, - "angularVelocity": 0.17015589193737768, - "velocityX": -0.17878495319082285, - "velocityY": 0.3240529451078872, - "timestamp": 2.3471317366472 - }, - { - "x": 2.2359428908787105, - "y": 5.6665943436646735, - "heading": 0.0343229879295691, - "angularVelocity": 0.3471628829274178, - "velocityX": -0.3370775470223548, - "velocityY": 0.6583544410765856, - "timestamp": 2.41347958558916 - }, - { - "x": 2.2053270603401187, - "y": 5.733363241767063, - "heading": 0.06978539081037693, - "angularVelocity": 0.5344921266675867, - "velocityX": -0.461444206961015, - "velocityY": 1.0063460860773108, - "timestamp": 2.4798274345311198 - }, - { - "x": 2.1706747853331265, - "y": 5.824280847900589, - "heading": 0.1187288687566709, - "angularVelocity": 0.7376799508467724, - "velocityX": -0.522281815606492, - "velocityY": 1.3703173137241953, - "timestamp": 2.5461752834730795 - }, - { - "x": 2.1408548318247003, - "y": 5.939174457686258, - "heading": 0.18203167256068778, - "angularVelocity": 0.9541048400739172, - "velocityX": -0.44944868573678187, - "velocityY": 1.7316855273812526, - "timestamp": 2.6125231324150393 - }, - { - "x": 2.13002082306829, - "y": 6.069625216883796, - "heading": 0.25771635797715586, - "angularVelocity": 1.1407255340361668, - "velocityX": -0.1632910324777442, - "velocityY": 1.9661641074702068, - "timestamp": 2.678870981356999 - }, - { - "x": 2.143840074539185, - "y": 6.201476573944092, - "heading": 0.3371459606364029, - "angularVelocity": 1.197169221397527, - "velocityX": 0.2082848455717636, - "velocityY": 1.9872740286672685, - "timestamp": 2.745218830298959 - }, - { - "x": 2.166370769756068, - "y": 6.293640927562095, - "heading": 0.3939319180386296, - "angularVelocity": 1.1819496828334453, - "velocityX": 0.4689565745450901, - "velocityY": 1.9183198366410883, - "timestamp": 2.7932631406676505 - }, - { - "x": 2.20004037690368, - "y": 6.3792187798135584, - "heading": 0.4481157099984023, - "angularVelocity": 1.127787901292925, - "velocityX": 0.7008032145582243, - "velocityY": 1.7812276124839865, - "timestamp": 2.841307451036342 - }, - { - "x": 2.241158412431322, - "y": 6.454264775706248, - "heading": 0.497428483818663, - "angularVelocity": 1.0264019494053482, - "velocityX": 0.8558356902638964, - "velocityY": 1.562016299469941, - "timestamp": 2.8893517614050337 - }, - { - "x": 2.283988140776994, - "y": 6.516572419092989, - "heading": 0.5400587851533989, - "angularVelocity": 0.8873121709436844, - "velocityX": 0.8914630685089917, - "velocityY": 1.296878712767307, - "timestamp": 2.9373960717737253 - }, - { - "x": 2.32381550199, - "y": 6.5663973747831905, - "heading": 0.5753207631425283, - "angularVelocity": 0.7339470109681908, - "velocityX": 0.8289714413084713, - "velocityY": 1.0370625638675175, - "timestamp": 2.985440382142417 - }, - { - "x": 2.3578617471237973, - "y": 6.6047562023476845, - "heading": 0.6030896816888531, - "angularVelocity": 0.5779855790045973, - "velocityX": 0.7086426024752379, - "velocityY": 0.7984052069876424, - "timestamp": 3.0334846925111085 - }, - { - "x": 2.38454267716584, - "y": 6.632586125877766, - "heading": 0.6235365890913814, - "angularVelocity": 0.4255843667152008, - "velocityX": 0.5553400566538157, - "velocityY": 0.5792553440046356, - "timestamp": 3.0815290028798 - }, - { - "x": 2.4028949714522336, - "y": 6.650619931398898, - "heading": 0.6369216126446875, - "angularVelocity": 0.2785974749265757, - "velocityX": 0.3819868397643466, - "velocityY": 0.3753577766595345, - "timestamp": 3.1295733132484918 - }, - { - "x": 2.4122893810272217, - "y": 6.659419059753418, - "heading": 0.6435012899724419, - "angularVelocity": 0.13695018780084564, - "velocityX": 0.19553636014120032, - "velocityY": 0.18314610589672853, - "timestamp": 3.1776176236171834 - }, - { - "x": 2.4122893810272217, - "y": 6.659419059753418, - "heading": 0.6435012899724419, - "angularVelocity": -1.217565429636075e-22, - "velocityX": -3.177543138441335e-19, - "velocityY": -2.2806254854922123e-19, - "timestamp": 3.225661933985875 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/choreo/straight.1.traj b/src/main/deploy/choreo/straight.1.traj deleted file mode 100644 index d908095..0000000 --- a/src/main/deploy/choreo/straight.1.traj +++ /dev/null @@ -1,463 +0,0 @@ -{ - "samples": [ - { - "x": 0, - "y": 0, - "heading": 0, - "angularVelocity": 0, - "velocityX": 0, - "velocityY": 0, - "timestamp": 0 - }, - { - "x": 0.012461364814318285, - "y": -0.002128733796916202, - "heading": 0.013756561921662181, - "angularVelocity": 0.285804124999482, - "velocityX": 0.25889531754640904, - "velocityY": -0.04422623208102771, - "timestamp": 0.04813283195855594 - }, - { - "x": 0.03740309511527823, - "y": -0.006248987630995588, - "heading": 0.041324391840432455, - "angularVelocity": 0.572744814651819, - "velocityX": 0.5181853900147751, - "velocityY": -0.08560173308786548, - "timestamp": 0.09626566391711187 - }, - { - "x": 0.07484900201616503, - "y": -0.012186907924809389, - "heading": 0.08272652254732986, - "angularVelocity": 0.8601640298776957, - "velocityX": 0.7779701583552998, - "velocityY": -0.12336528004266527, - "timestamp": 0.1443984958756678 - }, - { - "x": 0.12482821818793152, - "y": -0.019717146955758342, - "heading": 0.13794905076625522, - "angularVelocity": 1.1472943928683421, - "velocityX": 1.038360182396921, - "velocityY": -0.15644703884102962, - "timestamp": 0.19253132783422375 - }, - { - "x": 0.18737504527415316, - "y": -0.028537671958847512, - "heading": 0.20693956274387973, - "angularVelocity": 1.433335816122928, - "velocityX": 1.2994628518861446, - "velocityY": -0.18325381333647586, - "timestamp": 0.2406641597927797 - }, - { - "x": 0.26252633323167285, - "y": -0.0382232347566564, - "heading": 0.2896170230327287, - "angularVelocity": 1.7176936599125763, - "velocityX": 1.5613311101708618, - "velocityY": -0.20122569987464067, - "timestamp": 0.2887969917513356 - }, - { - "x": 0.35031001440055287, - "y": -0.0481300070172943, - "heading": 0.38590367356434097, - "angularVelocity": 2.000436014538236, - "velocityX": 1.8237796862745346, - "velocityY": -0.20582151220954503, - "timestamp": 0.33692982370989155 - }, - { - "x": 0.450696748170316, - "y": -0.057172074563132974, - "heading": 0.4957889156973818, - "angularVelocity": 2.2829581734907265, - "velocityX": 2.0856186865588895, - "velocityY": -0.18785654568640825, - "timestamp": 0.3850626556684475 - }, - { - "x": 0.5633503852104077, - "y": -0.06319894582072934, - "heading": 0.6193672194715316, - "angularVelocity": 2.5674430268419504, - "velocityX": 2.3404739022439087, - "velocityY": -0.1252133110053802, - "timestamp": 0.43319548762700344 - }, - { - "x": 0.685830624894182, - "y": -0.06113391122452203, - "heading": 0.7549435996819158, - "angularVelocity": 2.816713139320792, - "velocityX": 2.544629823344573, - "velocityY": 0.0429028276995084, - "timestamp": 0.4813283195855594 - }, - { - "x": 0.8066597393617257, - "y": -0.04617163921030441, - "heading": 0.8898603595769036, - "angularVelocity": 2.8030089734000225, - "velocityX": 2.510326310564521, - "velocityY": 0.3108537645800824, - "timestamp": 0.5294611515441153 - }, - { - "x": 0.9189099141717783, - "y": -0.02160004381271017, - "heading": 1.0175269883060745, - "angularVelocity": 2.652381410657415, - "velocityX": 2.332091635636644, - "velocityY": 0.5104955266033642, - "timestamp": 0.5775939835026712 - }, - { - "x": 1.0207708941272908, - "y": 0.010458682297259464, - "heading": 1.1359499990683692, - "angularVelocity": 2.460337485736579, - "velocityX": 2.116247388959338, - "velocityY": 0.6660469539289383, - "timestamp": 0.6257268154612271 - }, - { - "x": 1.111569543420934, - "y": 0.04889918367069, - "heading": 1.2443295578062838, - "angularVelocity": 2.251676336668353, - "velocityX": 1.8864181806676996, - "velocityY": 0.7986336936610995, - "timestamp": 0.673859647419783 - }, - { - "x": 1.1909742021577774, - "y": 0.09306966785179949, - "heading": 1.342283029028095, - "angularVelocity": 2.0350656139691203, - "velocityX": 1.6496984595714943, - "velocityY": 0.9176788978288632, - "timestamp": 0.7219924793783389 - }, - { - "x": 1.2587913411434568, - "y": 0.14254575778272302, - "heading": 1.4296112637191771, - "angularVelocity": 1.8143174032700784, - "velocityX": 1.4089580069602496, - "velocityY": 1.027907312279572, - "timestamp": 0.7701253113368948 - }, - { - "x": 1.314895273605119, - "y": 0.19703124456664084, - "heading": 1.5062051322136352, - "angularVelocity": 1.591301932128325, - "velocityX": 1.1656063061066013, - "velocityY": 1.1319817381788748, - "timestamp": 0.8182581432954507 - }, - { - "x": 1.3591983318328855, - "y": 0.2563086152076721, - "heading": 1.572, - "angularVelocity": 1.3669436247386542, - "velocityX": 0.9204332349676234, - "velocityY": 1.2315371489479614, - "timestamp": 0.8663909752540065 - }, - { - "x": 1.398693061104153, - "y": 0.353516344242021, - "heading": 1.6453474136573116, - "angularVelocity": 1.0359581155088748, - "velocityX": 0.5578231496962517, - "velocityY": 1.3729609642927432, - "timestamp": 0.9371924995234338 - }, - { - "x": 1.411796339749547, - "y": 0.45841054945716486, - "heading": 1.6939212612034322, - "angularVelocity": 0.6860565227561809, - "velocityX": 0.1850705727115578, - "velocityY": 1.4815246747510802, - "timestamp": 1.007994023792861 - }, - { - "x": 1.397881489525668, - "y": 0.5678432265358275, - "heading": 1.7160211906964271, - "angularVelocity": 0.3121391766778358, - "velocityX": -0.196533201332329, - "velocityY": 1.545626004635564, - "timestamp": 1.0787955480622884 - }, - { - "x": 1.356679439913185, - "y": 0.6775734412172078, - "heading": 1.709679894673947, - "angularVelocity": -0.08956439974864017, - "velocityX": -0.5819373246216184, - "velocityY": 1.5498284226737011, - "timestamp": 1.1495970723317157 - }, - { - "x": 1.288745324711235, - "y": 0.7820755267448177, - "heading": 1.6740116063635861, - "angularVelocity": -0.503778536951113, - "velocityX": -0.9595007438460562, - "velocityY": 1.4759863803205586, - "timestamp": 1.220398596601143 - }, - { - "x": 1.1961920071832448, - "y": 0.8746365570289615, - "heading": 1.611254747728562, - "angularVelocity": -0.8863772253858547, - "velocityX": -1.3072221040861913, - "velocityY": 1.307331038974714, - "timestamp": 1.2912001208705703 - }, - { - "x": 1.083389795704412, - "y": 0.948533213635842, - "heading": 1.524163599950402, - "angularVelocity": -1.2300744747633494, - "velocityX": -1.5932172738199324, - "velocityY": 1.0437156172750617, - "timestamp": 1.3620016451399977 - }, - { - "x": 0.9560791672316398, - "y": 0.9988339442201157, - "heading": 1.415302796501782, - "angularVelocity": -1.537548867371306, - "velocityX": -1.7981340061028357, - "velocityY": 0.7104470010117288, - "timestamp": 1.432803169409425 - }, - { - "x": 0.8198890089988708, - "y": 1.022695779800415, - "heading": 1.2898193082830849, - "angularVelocity": -1.7723274959615798, - "velocityX": -1.9235483930333557, - "velocityY": 0.3370243201190965, - "timestamp": 1.5036046936788523 - }, - { - "x": 0.7790736707757948, - "y": 1.0273378867272385, - "heading": 1.2515602510236115, - "angularVelocity": -1.819279017917808, - "velocityX": -1.9408342431144037, - "velocityY": 0.2207395669377063, - "timestamp": 1.5246344845370252 - }, - { - "x": 0.7380044646421128, - "y": 1.0295195211607304, - "heading": 1.2123799670347075, - "angularVelocity": -1.8630848139736595, - "velocityX": -1.9529060660021396, - "velocityY": 0.10374018687133241, - "timestamp": 1.5456642753951981 - }, - { - "x": 0.6967966638008277, - "y": 1.0292302757995049, - "heading": 1.1723503095875711, - "angularVelocity": -1.9034738727123135, - "velocityX": -1.9594964647625315, - "velocityY": -0.013754076927169036, - "timestamp": 1.566694066253371 - }, - { - "x": 0.6555707618012633, - "y": 1.0264651245470997, - "heading": 1.1315478417710267, - "angularVelocity": -1.9402222348153986, - "velocityX": -1.9603572036258692, - "velocityY": -0.1314873396056913, - "timestamp": 1.587723857111544 - }, - { - "x": 0.614451882215819, - "y": 1.0212251556204266, - "heading": 1.0900527653992764, - "angularVelocity": -1.9731568731043334, - "velocityX": -1.9552681176315212, - "velocityY": -0.24916885583940843, - "timestamp": 1.6087536479697169 - }, - { - "x": 0.5735689980243861, - "y": 1.0135182359677948, - "heading": 1.047947840883984, - "angularVelocity": -2.0021561222007676, - "velocityX": -1.9440461613313953, - "velocityY": -0.36647628616985367, - "timestamp": 1.6297834388278898 - }, - { - "x": 0.533053970642776, - "y": 1.003359563192243, - "heading": 1.0053173349563786, - "angularVelocity": -2.027148354213788, - "velocityX": -1.9265539850038353, - "velocityY": -0.48306104630631314, - "timestamp": 1.6508132296860627 - }, - { - "x": 0.4930404325565279, - "y": 0.9907720627439722, - "heading": 0.9622460035998045, - "angularVelocity": -2.0481103044272726, - "velocityX": -1.9027073714666842, - "velocityY": -0.5985556648262538, - "timestamp": 1.6718430205442356 - }, - { - "x": 0.45366255172084463, - "y": 0.975786593934078, - "heading": 0.9188181149208268, - "angularVelocity": -2.0650651721579156, - "velocityX": -1.872480858285844, - "velocityY": -0.7125828740265546, - "timestamp": 1.6928728114024085 - }, - { - "x": 0.4150537287016667, - "y": 0.9584419388241254, - "heading": 0.8751165450534875, - "angularVelocity": -2.078079147912954, - "velocityX": -1.8359109360411605, - "velocityY": -0.8247659345224463, - "timestamp": 1.7139026022605814 - }, - { - "x": 0.3773452866607345, - "y": 0.9387845630555187, - "heading": 0.831222020292614, - "angularVelocity": -2.0872544599659952, - "velocityX": -1.7930963886061433, - "velocityY": -0.9347394798730188, - "timestamp": 1.7349323931187544 - }, - { - "x": 0.34066521726904847, - "y": 0.9168681556045045, - "heading": 0.7872125978765242, - "angularVelocity": -2.0927180261988396, - "velocityX": -1.744195633663711, - "velocityY": -1.0421600290188748, - "timestamp": 1.7559621839769273 - }, - { - "x": 0.30513704107762857, - "y": 0.8927529722899603, - "heading": 0.7431634524616072, - "angularVelocity": -2.0946069179664764, - "velocityX": -1.689421280079565, - "velocityY": -1.14671531814936, - "timestamp": 1.7769919748351002 - }, - { - "x": 0.27087882945074176, - "y": 0.8665050222413296, - "heading": 0.6991469687603771, - "angularVelocity": -2.093053801537168, - "velocityX": -1.6290324453499245, - "velocityY": -1.248131768197296, - "timestamp": 1.798021765693273 - }, - { - "x": 0.23800241947174072, - "y": 0.8381951451301575, - "heading": 0.6552330630664542, - "angularVelocity": -2.0881760541549332, - "velocityX": -1.5633255794469438, - "velocityY": -1.3461796792035183, - "timestamp": 1.819051556551446 - }, - { - "x": 0.13305224538517144, - "y": 0.6989004889295295, - "heading": 0.48787828383220494, - "angularVelocity": -2.0566563240619904, - "velocityX": -1.289753661258909, - "velocityY": -1.7118198649234242, - "timestamp": 1.9004238195184138 - }, - { - "x": 0.05614358166245768, - "y": 0.5353463812204284, - "heading": 0.33094243381226773, - "angularVelocity": -1.9286160209608998, - "velocityX": -0.9451459369384153, - "velocityY": -2.009949112211546, - "timestamp": 1.9817960824853815 - }, - { - "x": 0.015227456642039264, - "y": 0.3657189466124175, - "heading": 0.204927152384683, - "angularVelocity": -1.5486269747559986, - "velocityX": -0.5028264365343748, - "velocityY": -2.0845854400887145, - "timestamp": 2.063168345452349 - }, - { - "x": 0.0006891767765161591, - "y": 0.22077387650824845, - "heading": 0.11633490498693691, - "angularVelocity": -1.0887278314197715, - "velocityX": -0.17866382641250605, - "velocityY": -1.7812589304909483, - "timestamp": 2.144540608419317 - }, - { - "x": -0.002542868216226937, - "y": 0.11057083001523205, - "heading": 0.05574497013994884, - "angularVelocity": -0.7446018168572258, - "velocityX": -0.039719246766617713, - "velocityY": -1.354307259929005, - "timestamp": 2.2259128713862846 - }, - { - "x": -0.0015621928013223405, - "y": 0.036866126997437, - "heading": 0.017971061788070876, - "angularVelocity": -0.46421110799403414, - "velocityX": 0.012051716139474863, - "velocityY": -0.9057718235968786, - "timestamp": 2.3072851343532523 - }, - { - "x": -4.6578201989565843e-36, - "y": 0, - "heading": -1.0320017409223796e-35, - "angularVelocity": -0.2208499694222105, - "velocityX": 0.019198099504206964, - "velocityY": -0.4530552015298195, - "timestamp": 2.38865739732022 - }, - { - "x": 0, - "y": 0, - "heading": 9.149195606855488e-37, - "angularVelocity": 2.3165775416959576e-34, - "velocityX": 5.722984990582701e-35, - "velocityY": 0, - "timestamp": 2.470029660287188 - } - ] -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3NB-Sweep.auto b/src/main/deploy/pathplanner/autos/3NB-Sweep.auto deleted file mode 100644 index 3640082..0000000 --- a/src/main/deploy/pathplanner/autos/3NB-Sweep.auto +++ /dev/null @@ -1,203 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7265444397926331, - "y": 4.389017105102539 - }, - "rotation": -60.255120226647975 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Score Speaker Fixed" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "NoTeamBottom.1" - } - }, - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.6 - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Retract Intake" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - } - ] - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "NoTeamBottom.2" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.6 - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "NoTeamBottom.3" - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.6 - } - } - ] - } - } - ] - } - }, - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Retract Intake" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - } - ] - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3NB.auto b/src/main/deploy/pathplanner/autos/3NB.auto deleted file mode 100644 index 72611c9..0000000 --- a/src/main/deploy/pathplanner/autos/3NB.auto +++ /dev/null @@ -1,133 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.71, - "y": 4.38 - }, - "rotation": -59.03624661580474 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Score Speaker Fixed" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "3NB.1" - } - }, - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Retract Intake" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - } - ] - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "3NB.2" - } - }, - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "3NB.3" - } - }, - { - "type": "named", - "data": { - "name": "Retract Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/3NM.auto b/src/main/deploy/pathplanner/autos/3NM.auto deleted file mode 100644 index b7a0ad1..0000000 --- a/src/main/deploy/pathplanner/autos/3NM.auto +++ /dev/null @@ -1,139 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 1.3615734577178955, - "y": 5.537254333496094 - }, - "rotation": -2.414706623707546e-48 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Score Speaker Fixed" - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Mid 3N.1" - } - }, - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Retract Intake" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - } - ] - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Mid 3N.2" - } - }, - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - } - ] - } - }, - { - "type": "wait", - "data": { - "waitTime": 0.6 - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "Mid 3N.3" - } - }, - { - "type": "named", - "data": { - "name": "Retract Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/4NT.auto b/src/main/deploy/pathplanner/autos/4NT.auto deleted file mode 100644 index 30b7af2..0000000 --- a/src/main/deploy/pathplanner/autos/4NT.auto +++ /dev/null @@ -1,184 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7644821405410767, - "y": 6.707253456115723 - }, - "rotation": 59.534458682967 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "4NT.1" - } - }, - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "sequential", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Retract Intake" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - } - ] - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "4NT.2" - } - }, - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "4NT.3" - } - }, - { - "type": "named", - "data": { - "name": "Retract Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "4NT.4" - } - }, - { - "type": "named", - "data": { - "name": "Deploy Intake" - } - } - ] - } - }, - { - "type": "race", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "4NT.5" - } - }, - { - "type": "named", - "data": { - "name": "Retract Intake" - } - } - ] - } - }, - { - "type": "parallel", - "data": { - "commands": [ - { - "type": "named", - "data": { - "name": "Drivetrain Off" - } - }, - { - "type": "named", - "data": { - "name": "Score From Distance" - } - } - ] - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/New Auto.auto b/src/main/deploy/pathplanner/autos/New Auto.auto new file mode 100644 index 0000000..07d4e33 --- /dev/null +++ b/src/main/deploy/pathplanner/autos/New Auto.auto @@ -0,0 +1,18 @@ +{ + "version": 1.0, + "startingPose": { + "position": { + "x": 2, + "y": 2 + }, + "rotation": 0 + }, + "command": { + "type": "sequential", + "data": { + "commands": [] + } + }, + "folder": null, + "choreoAuto": false +} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/autos/NoScoring_3NBSweep.auto b/src/main/deploy/pathplanner/autos/NoScoring_3NBSweep.auto deleted file mode 100644 index dd67a4a..0000000 --- a/src/main/deploy/pathplanner/autos/NoScoring_3NBSweep.auto +++ /dev/null @@ -1,25 +0,0 @@ -{ - "version": 1.0, - "startingPose": { - "position": { - "x": 0.7265444397926332, - "y": 4.389017105102539 - }, - "rotation": -60.255120226647975 - }, - "command": { - "type": "sequential", - "data": { - "commands": [ - { - "type": "path", - "data": { - "pathName": "NoTeamBottom" - } - } - ] - } - }, - "folder": null, - "choreoAuto": true -} \ No newline at end of file diff --git a/src/main/java/org/robolancers321/Constants.java b/src/main/java/org/robolancers321/Constants.java index 2e2d4d5..550b756 100644 --- a/src/main/java/org/robolancers321/Constants.java +++ b/src/main/java/org/robolancers321/Constants.java @@ -39,7 +39,7 @@ public static final class DrivetrainConstants { public static final double kMaxTeleopRotationPercent = 1.0; public static final PathConstraints kAutoConstraints = - new PathConstraints(4.0, 3, 270 * Math.PI / 180, 360 * Math.PI / 180); + new PathConstraints(4.0, 3.0, 270 * Math.PI / 180, 360 * Math.PI / 180); public static final SwerveDriveKinematics kSwerveKinematics = new SwerveDriveKinematics( @@ -99,10 +99,10 @@ public static final class RetractorConstants { public static final double kGearRatio = 360.0; - public static final float kMinAngle = -15f; + public static final float kMinAngle = -18f; public static final float kMaxAngle = 155.0f; - public static final double kP = 0.0065; + public static final double kP = 0.008; //0.0065 public static final double kI = 0.000; public static final double kD = 0.0001; @@ -110,8 +110,8 @@ public static final class RetractorConstants { public static final double kG = 0.03; // 0.04? // 0.0155; public static final double kV = 0.000; - public static final double kMaxVelocityDeg = 180.0; - public static final double kMaxAccelerationDeg = 540.0; + public static final double kMaxVelocityDeg = 400.0; //180 + public static final double kMaxAccelerationDeg = 2000.0; //540 public static final double kToleranceDeg = 5.0; @@ -119,7 +119,7 @@ public enum RetractorSetpoint { kRetracted(155), kMating(154), kClearFromLauncher(70), - kIntake(-15), + kIntake(-18), kOuttake(20.0); public final double angle; @@ -151,7 +151,7 @@ public static final class FlywheelConstants { public static final int kMotorPort = 17; public static final boolean kInvertMotor = false; - public static final int kCurrentLimit = 60; + public static final int kCurrentLimit = 75; public static final double kRampUpRate = 10000000; // effectively infinite ramp up, keeping this for the infrastructure @@ -211,7 +211,7 @@ public static final class PivotConstants { public static final double kV = 0.0; // 0.35 public static final double kMaxVelocityDeg = 160; - public static final double kMaxAccelerationDeg = 2000; + public static final double kMaxAccelerationDeg = 1500; public static TrapezoidProfile.Constraints kProfileConstraints = new Constraints(kMaxVelocityDeg, kMaxAccelerationDeg); diff --git a/src/main/java/org/robolancers321/RobotContainer.java b/src/main/java/org/robolancers321/RobotContainer.java index 09e5e01..6b244f6 100644 --- a/src/main/java/org/robolancers321/RobotContainer.java +++ b/src/main/java/org/robolancers321/RobotContainer.java @@ -4,11 +4,13 @@ import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.simulation.AddressableLEDSim; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj.util.Color; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.button.Trigger; import org.robolancers321.Constants.FlywheelConstants; import org.robolancers321.commands.AutoPickupNote; @@ -20,8 +22,17 @@ import org.robolancers321.commands.ScoreSpeakerFixedAuto; import org.robolancers321.commands.ScoreSpeakerFixedTeleop; import org.robolancers321.commands.ScoreSpeakerFromDistance; -import org.robolancers321.commands.autonomous.Auto3NBSweep; -import org.robolancers321.commands.autonomous.Auto3NBSweepStraight; +import org.robolancers321.commands.autonomous.Auto3NBClose; +import org.robolancers321.commands.autonomous.Auto3NMClose; +import org.robolancers321.commands.autonomous.Auto3NTClose; +import org.robolancers321.commands.autonomous.Auto4NBSkip; +import org.robolancers321.commands.autonomous.Auto4NBSweep; +import org.robolancers321.commands.autonomous.Auto4NBSweepStraight; +import org.robolancers321.commands.autonomous.Auto4NMSweep; +import org.robolancers321.commands.autonomous.Auto4NMSweepFender; +import org.robolancers321.commands.autonomous.Auto4NMSweepFenderStraight; +import org.robolancers321.commands.autonomous.Auto4NTClose; +import org.robolancers321.commands.autonomous.Auto4NTSweep; import org.robolancers321.subsystems.LED.LED; import org.robolancers321.subsystems.LED.LED.Section; import org.robolancers321.subsystems.drivetrain.Drivetrain; @@ -31,6 +42,9 @@ import org.robolancers321.subsystems.launcher.Indexer; import org.robolancers321.subsystems.launcher.Pivot; +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; + public class RobotContainer { private Drivetrain drivetrain; private Retractor retractor; @@ -73,7 +87,12 @@ public RobotContainer() { private void configureLEDs() { // default, meteor red LED.registerSignal(1, () -> true, LED.meteorRain(0.02, LED.kDefaultMeteorColors)); - // LED.registerSignal(1, () -> true, LED.wave(Section.FULL, new Color(28, 169, 201), new Color(49,140,231))); + + // green water + // LED.registerSignal(1, () -> true, LED.wave(Section.FULL, new Color(109, 199, 201), new Color(49,164,176))); + + //blue water + // LED.registerSignal(1, () -> true, LED.wave(Section.FULL, new Color(85, 196, 232), new Color(3,147,202))); // sees note, blink orange LED.registerSignal( @@ -155,7 +174,8 @@ private void configureDriverController() { } /* - * Press A Button: score amp + * Hold A Button: rev for score amp + * Release A Button: eject for score amp * * Press Y Button: score speaker from distance * @@ -164,16 +184,11 @@ private void configureDriverController() { * */ private void configureManipulatorController() { - new Trigger(this.manipulatorController::getAButton) - .onTrue( - new ScoreAmp() - .finallyDo( - () -> { - CommandScheduler.getInstance() - .schedule( - new ParallelCommandGroup( - this.retractor.moveToRetracted(), this.pivot.moveToRetracted())); - })); + new Trigger(this.manipulatorController::getAButton).whileTrue(new ScoreAmp()); + new Trigger(this.manipulatorController::getAButton).onFalse(this.indexer.outtake().alongWith(new InstantCommand(() -> {}, this.flywheel)).andThen( + new ParallelCommandGroup(this.retractor.moveToRetracted(), this.pivot.moveToRetracted()) + )); + new Trigger(this.manipulatorController::getYButton) .onTrue( new ScoreSpeakerFromDistance() @@ -184,6 +199,7 @@ private void configureManipulatorController() { new ParallelCommandGroup( this.retractor.moveToRetracted(), this.pivot.moveToRetracted())); })); + new Trigger(this.manipulatorController::getXButton).whileTrue(new ScoreSpeakerFixedTeleop()); new Trigger(this.manipulatorController::getXButton) .onFalse( @@ -197,13 +213,34 @@ private void configureManipulatorController() { } private void configureAuto() { + /* + Sweep - pickup front 3 notes with curves + Sweep straight - pickup front 3 notes with straight line + Close - pickup front note first + Skip - pickup center note first then go back for front note + */ + this.autoChooser.setDefaultOption("Score And Sit", new ScoreSpeakerFixedAuto()); - this.autoChooser.addOption("3NB Sweep", new Auto3NBSweep()); - this.autoChooser.addOption("3NB Sweep Straight", new Auto3NBSweepStraight()); + + this.autoChooser.addOption("4NT Sweep", new Auto4NTSweep()); + this.autoChooser.addOption("4NT Close", new Auto4NTClose()); + this.autoChooser.addOption("3NT Close", new Auto3NTClose()); + + this.autoChooser.addOption("4NM Sweep", new Auto4NMSweep()); + this.autoChooser.addOption("3NM Close", new Auto3NMClose()); + this.autoChooser.addOption("4NM Sweep Fender", new Auto4NMSweepFender()); + this.autoChooser.addOption("4NM Sweep Fender Straight", new Auto4NMSweepFenderStraight()); + + this.autoChooser.addOption("4NB Sweep", new Auto4NBSweep()); + this.autoChooser.addOption("4NB Skip", new Auto4NBSkip()); + this.autoChooser.addOption("3NB Sweep Straight", new Auto4NBSweepStraight()); + this.autoChooser.addOption("3NB Close", new Auto3NBClose()); + + SmartDashboard.putData(autoChooser); } public Command getAutonomousCommand() { - // return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory("3NM")); + // return AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory("4NMSweep")); return this.autoChooser.getSelected(); } diff --git a/src/main/java/org/robolancers321/commands/AutoPickupNote.java b/src/main/java/org/robolancers321/commands/AutoPickupNote.java index 63b4da0..d71eeb8 100644 --- a/src/main/java/org/robolancers321/commands/AutoPickupNote.java +++ b/src/main/java/org/robolancers321/commands/AutoPickupNote.java @@ -37,7 +37,10 @@ public AutoPickupNote() { this.drivetrain .driveCommand(0.0, 2.5, 0.0, false) .until(this.sucker::noteDetected) - .withTimeout(1.0)), + .withTimeout(1.0), + this.drivetrain.stop(), + this.retractor.moveToMating() + ), new InstantCommand(), this.drivetrain::seesNote) ) diff --git a/src/main/java/org/robolancers321/commands/IntakeNote.java b/src/main/java/org/robolancers321/commands/IntakeNote.java index 80e0af2..3658bcf 100644 --- a/src/main/java/org/robolancers321/commands/IntakeNote.java +++ b/src/main/java/org/robolancers321/commands/IntakeNote.java @@ -2,6 +2,7 @@ package org.robolancers321.commands; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitUntilCommand; import org.robolancers321.subsystems.intake.Retractor; diff --git a/src/main/java/org/robolancers321/commands/PathAndAutoPickup b/src/main/java/org/robolancers321/commands/PathAndAutoPickup new file mode 100644 index 0000000..153d239 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/PathAndAutoPickup @@ -0,0 +1,32 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands; + +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class PathAndAutoPickup extends SequentialCommandGroup { + + Retractor retractor; + Sucker sucker; + + public PathAndAutoPickup(String pathName) { + + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + + this.addCommands( + new ParallelRaceGroup( + AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(pathName)), + new AutoPickupNote() + ) + ); + } +} \ No newline at end of file diff --git a/src/main/java/org/robolancers321/commands/PathAndIntake.java b/src/main/java/org/robolancers321/commands/PathAndIntake.java new file mode 100644 index 0000000..7143752 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/PathAndIntake.java @@ -0,0 +1,33 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands; + +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class PathAndIntake extends SequentialCommandGroup { + + Retractor retractor; + Sucker sucker; + + public PathAndIntake(String pathName) { + + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + + this.addCommands( + retractor.moveToIntake().withTimeout(0.3), + new ParallelRaceGroup( + AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(pathName)), + new IntakeNote() + ) + ); + } +} \ No newline at end of file diff --git a/src/main/java/org/robolancers321/commands/PathAndRetract.java b/src/main/java/org/robolancers321/commands/PathAndRetract.java new file mode 100644 index 0000000..3da62c8 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/PathAndRetract.java @@ -0,0 +1,26 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands; + +import com.pathplanner.lib.auto.AutoBuilder; +import com.pathplanner.lib.path.PathPlannerPath; +import edu.wpi.first.wpilibj2.command.ConditionalCommand; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; + +public class PathAndRetract extends SequentialCommandGroup { + private Retractor retractor; + + public PathAndRetract(String pathName) { + this.retractor = Retractor.getInstance(); + + this.addCommands( + new ParallelCommandGroup( + AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(pathName)), + this.retractor.moveToRetracted() + )); + } +} \ No newline at end of file diff --git a/src/main/java/org/robolancers321/commands/PathAndShoot.java b/src/main/java/org/robolancers321/commands/PathAndShoot.java index 3cd721e..65c337a 100644 --- a/src/main/java/org/robolancers321/commands/PathAndShoot.java +++ b/src/main/java/org/robolancers321/commands/PathAndShoot.java @@ -4,19 +4,26 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.path.PathPlannerPath; import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import org.robolancers321.subsystems.intake.Retractor; import org.robolancers321.subsystems.intake.Sucker; public class PathAndShoot extends SequentialCommandGroup { private Sucker sucker; + private Retractor retractor; public PathAndShoot(String pathName) { this.sucker = Sucker.getInstance(); + this.retractor = Retractor.getInstance(); this.addCommands( new ParallelRaceGroup( AutoBuilder.followPath(PathPlannerPath.fromChoreoTrajectory(pathName)), - new IntakeNote()), + new IntakeNote() + ), + retractor.moveToRetracted(), new ScoreSpeakerFromDistance().onlyIf(this.sucker::noteDetected)); } } diff --git a/src/main/java/org/robolancers321/commands/ScoreAmp.java b/src/main/java/org/robolancers321/commands/ScoreAmp.java index b6e905c..46f8a9c 100644 --- a/src/main/java/org/robolancers321/commands/ScoreAmp.java +++ b/src/main/java/org/robolancers321/commands/ScoreAmp.java @@ -1,6 +1,8 @@ /* (C) Robolancers 2024 */ package org.robolancers321.commands; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import org.robolancers321.subsystems.intake.Retractor; import org.robolancers321.subsystems.launcher.Flywheel; @@ -24,7 +26,7 @@ public ScoreAmp() { this.pivot.aimAtAmp(), new Shift(), this.flywheel.revAmp(), - this.indexer.outtake(), - this.flywheel.off()); + Commands.idle() + ); } } diff --git a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java index 0f977fd..e8a44b0 100644 --- a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java +++ b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedAuto.java @@ -27,7 +27,9 @@ public ScoreSpeakerFixedAuto() { this.addCommands( this.flywheel.revSpeaker(), new ParallelDeadlineGroup(this.indexer.acceptHandoff(), this.sucker.out()), - this.indexer.off(), - this.flywheel.off()); + this.indexer.off() // , + // stay revved during auto + // this.flywheel.off() + ); } } diff --git a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedTeleop.java b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedTeleop.java index 8d5c70a..b10e4be 100644 --- a/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedTeleop.java +++ b/src/main/java/org/robolancers321/commands/ScoreSpeakerFixedTeleop.java @@ -24,7 +24,7 @@ public ScoreSpeakerFixedTeleop() { this.addCommands( flywheel.revSpeaker(), - pivot.moveToAngle(PivotConstants.PivotSetpoint.kSpeaker.angle), + pivot.aimAtSpeakerFixed(), Commands.idle()); } } diff --git a/src/main/java/org/robolancers321/commands/ScoreSpeakerFromDistance.java b/src/main/java/org/robolancers321/commands/ScoreSpeakerFromDistance.java index 5ecc456..eb0f074 100644 --- a/src/main/java/org/robolancers321/commands/ScoreSpeakerFromDistance.java +++ b/src/main/java/org/robolancers321/commands/ScoreSpeakerFromDistance.java @@ -35,6 +35,7 @@ public ScoreSpeakerFromDistance() { this.flywheel.revSpeakerFromRPM( () -> AimTable.interpolateFlywheelRPM(this.drivetrain.getDistanceToSpeaker()))), this.indexer.outtake(), - this.indexer.off()); + this.indexer.off(), + this.flywheel.off()); } } diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto3NBClose.java b/src/main/java/org/robolancers321/commands/autonomous/Auto3NBClose.java new file mode 100644 index 0000000..45eb6f6 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto3NBClose.java @@ -0,0 +1,44 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.commands.PathAndIntake; +import org.robolancers321.commands.PathAndRetract; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.commands.ScoreSpeakerFromDistance; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto3NBClose extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto3NBClose() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndShoot("3NB-Close.1"), + new PathAndIntake("3NB-Close.2"), + new PathAndRetract("3NB-Close.3"), + new ScoreSpeakerFromDistance()); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto3NBSweep.java b/src/main/java/org/robolancers321/commands/autonomous/Auto3NBSweep.java deleted file mode 100644 index 3d08411..0000000 --- a/src/main/java/org/robolancers321/commands/autonomous/Auto3NBSweep.java +++ /dev/null @@ -1,25 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.autonomous; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import org.robolancers321.commands.PathAndShoot; -import org.robolancers321.commands.ScoreSpeakerFixedAuto; -import org.robolancers321.subsystems.drivetrain.Drivetrain; - -public class Auto3NBSweep extends SequentialCommandGroup { - private Drivetrain drivetrain; - - public Auto3NBSweep() { - this.drivetrain = Drivetrain.getInstance(); - - this.addCommands( - // TODO: test this - new InstantCommand( - () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), - new ScoreSpeakerFixedAuto(), - new PathAndShoot("NoTeamBottom.1"), - new PathAndShoot("NoTeamBottom.2"), - new PathAndShoot("NoTeamBottom.3")); - } -} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto3NBSweepStraight.java b/src/main/java/org/robolancers321/commands/autonomous/Auto3NBSweepStraight.java deleted file mode 100644 index f095b30..0000000 --- a/src/main/java/org/robolancers321/commands/autonomous/Auto3NBSweepStraight.java +++ /dev/null @@ -1,25 +0,0 @@ -/* (C) Robolancers 2024 */ -package org.robolancers321.commands.autonomous; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import org.robolancers321.commands.PathAndShoot; -import org.robolancers321.commands.ScoreSpeakerFixedAuto; -import org.robolancers321.subsystems.drivetrain.Drivetrain; - -public class Auto3NBSweepStraight extends SequentialCommandGroup { - private Drivetrain drivetrain; - - public Auto3NBSweepStraight() { - this.drivetrain = Drivetrain.getInstance(); - - this.addCommands( - // TODO: test this - new InstantCommand( - () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), - new ScoreSpeakerFixedAuto(), - new PathAndShoot("NoTeamBottomStraight.1"), - new PathAndShoot("NoTeamBottomStraight.2"), - new PathAndShoot("NoTeamBottomStraight.3")); - } -} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto3NMClose.java b/src/main/java/org/robolancers321/commands/autonomous/Auto3NMClose.java new file mode 100644 index 0000000..7300149 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto3NMClose.java @@ -0,0 +1,44 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.commands.PathAndIntake; +import org.robolancers321.commands.PathAndRetract; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.commands.ScoreSpeakerFromDistance; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto3NMClose extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto3NMClose() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndShoot("3NM-Close.1"), + new PathAndIntake("3NM-Close.2"), + new PathAndRetract("3NM-Close.3"), + new ScoreSpeakerFromDistance()); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto3NTClose.java b/src/main/java/org/robolancers321/commands/autonomous/Auto3NTClose.java new file mode 100644 index 0000000..635bac4 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto3NTClose.java @@ -0,0 +1,44 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.commands.PathAndIntake; +import org.robolancers321.commands.PathAndRetract; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.commands.ScoreSpeakerFromDistance; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto3NTClose extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto3NTClose() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndShoot("3NT-Close.1"), + new PathAndIntake("3NT-Close.2"), + new PathAndRetract("3NT-Close.3"), + new ScoreSpeakerFromDistance()); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto4NBSkip.java b/src/main/java/org/robolancers321/commands/autonomous/Auto4NBSkip.java new file mode 100644 index 0000000..01bac83 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto4NBSkip.java @@ -0,0 +1,47 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.commands.PathAndIntake; +import org.robolancers321.commands.PathAndRetract; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.commands.ScoreSpeakerFromDistance; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto4NBSkip extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto4NBSkip() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndIntake("4NB-Skip.1"), + new PathAndRetract("4NB-Skip.2"), + new ScoreSpeakerFromDistance(), + new PathAndIntake("4NB-Skip.3"), + new PathAndRetract("4NB-Skip.4"), + new ScoreSpeakerFromDistance(), + new PathAndShoot("4NB-Skip.5")); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto4NBSweep.java b/src/main/java/org/robolancers321/commands/autonomous/Auto4NBSweep.java new file mode 100644 index 0000000..2fa9c68 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto4NBSweep.java @@ -0,0 +1,40 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto4NBSweep extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto4NBSweep() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndShoot("4NB-Sweep.1"), + new PathAndShoot("4NB-Sweep.2"), + new PathAndShoot("4NB-Sweep.3")); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto4NBSweepStraight.java b/src/main/java/org/robolancers321/commands/autonomous/Auto4NBSweepStraight.java new file mode 100644 index 0000000..86a8193 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto4NBSweepStraight.java @@ -0,0 +1,40 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto4NBSweepStraight extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto4NBSweepStraight() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndShoot("4NB-SweepStraight.1"), + new PathAndShoot("4NB-SweepStraight.2"), + new PathAndShoot("4NB-SweepStraight.3")); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweep.java b/src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweep.java new file mode 100644 index 0000000..3e49686 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweep.java @@ -0,0 +1,40 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto4NMSweep extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto4NMSweep() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndShoot("4NM-Sweep.1"), + new PathAndShoot("4NM-Sweep.2"), + new PathAndShoot("4NM-Sweep.3")); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweepFender.java b/src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweepFender.java new file mode 100644 index 0000000..381959c --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweepFender.java @@ -0,0 +1,50 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import org.robolancers321.commands.PathAndIntake; +import org.robolancers321.commands.PathAndRetract; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto4NMSweepFender extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto4NMSweepFender() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndIntake("4NM-SweepFender.1"), + new PathAndRetract("4NM-SweepFender.2"), + new ScoreSpeakerFixedAuto(), + new PathAndIntake("4NM-SweepFender.3"), + new PathAndRetract("4NM-SweepFender.4"), + new ScoreSpeakerFixedAuto(), + new PathAndIntake("4NM-SweepFender.5"), + new PathAndRetract("4NM-SweepFender.6"), + new ScoreSpeakerFixedAuto() + ); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweepFenderStraight.java b/src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweepFenderStraight.java new file mode 100644 index 0000000..d6c7122 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto4NMSweepFenderStraight.java @@ -0,0 +1,50 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; + +import org.robolancers321.commands.PathAndIntake; +import org.robolancers321.commands.PathAndRetract; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto4NMSweepFenderStraight extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto4NMSweepFenderStraight() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndIntake("4NM-SweepFenderStraight.1"), + new PathAndRetract("4NM-SweepFenderStraight.2"), + new ScoreSpeakerFixedAuto(), + new PathAndIntake("4NM-SweepFenderStraight.3"), + new PathAndRetract("4NM-SweepFenderStraight.4"), + new ScoreSpeakerFixedAuto(), + new PathAndIntake("4NM-SweepFenderStraight.5"), + new PathAndRetract("4NM-SweepFenderStraight.6"), + new ScoreSpeakerFixedAuto() + ); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto4NTClose.java b/src/main/java/org/robolancers321/commands/autonomous/Auto4NTClose.java new file mode 100644 index 0000000..ed8beba --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto4NTClose.java @@ -0,0 +1,47 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.commands.PathAndIntake; +import org.robolancers321.commands.PathAndRetract; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.commands.ScoreSpeakerFromDistance; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto4NTClose extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto4NTClose() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndShoot("4NT-Close.1"), + new PathAndIntake("4NT-Close.2"), + new PathAndRetract("4NT-Close.3"), + new ScoreSpeakerFromDistance(), + new PathAndIntake("4NT-Close.4"), + new PathAndRetract("4NT-Close.5"), + new ScoreSpeakerFromDistance()); + } +} diff --git a/src/main/java/org/robolancers321/commands/autonomous/Auto4NTSweep.java b/src/main/java/org/robolancers321/commands/autonomous/Auto4NTSweep.java new file mode 100644 index 0000000..ebe00e0 --- /dev/null +++ b/src/main/java/org/robolancers321/commands/autonomous/Auto4NTSweep.java @@ -0,0 +1,40 @@ +/* (C) Robolancers 2024 */ +package org.robolancers321.commands.autonomous; + +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import org.robolancers321.commands.PathAndShoot; +import org.robolancers321.commands.ScoreSpeakerFixedAuto; +import org.robolancers321.subsystems.drivetrain.Drivetrain; +import org.robolancers321.subsystems.intake.Retractor; +import org.robolancers321.subsystems.intake.Sucker; +import org.robolancers321.subsystems.launcher.Flywheel; +import org.robolancers321.subsystems.launcher.Indexer; +import org.robolancers321.subsystems.launcher.Pivot; + +public class Auto4NTSweep extends SequentialCommandGroup { + private Drivetrain drivetrain; + private Retractor retractor; + private Sucker sucker; + private Pivot pivot; + private Indexer indexer; + private Flywheel flywheel; + + public Auto4NTSweep() { + this.drivetrain = Drivetrain.getInstance(); + this.retractor = Retractor.getInstance(); + this.sucker = Sucker.getInstance(); + this.pivot = Pivot.getInstance(); + this.indexer = Indexer.getInstance(); + this.flywheel = Flywheel.getInstance(); + + this.addCommands( + // TODO: test this + new InstantCommand( + () -> this.drivetrain.setYaw(this.drivetrain.getPose().getRotation().getDegrees())), + new ScoreSpeakerFixedAuto(), + new PathAndShoot("4NT-Sweep.1"), + new PathAndShoot("4NT-Sweep.2"), + new PathAndShoot("4NT-Sweep.3")); + } +} diff --git a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java index 8cbb8f7..a96c008 100644 --- a/src/main/java/org/robolancers321/subsystems/intake/Retractor.java +++ b/src/main/java/org/robolancers321/subsystems/intake/Retractor.java @@ -180,7 +180,8 @@ private void initTuning() { SmartDashboard.putNumber("retractor target position (deg)", this.getPositionDeg()); } - private void tune() { + private void + tune() { double tunedP = SmartDashboard.getNumber("retractor kp", RetractorConstants.kP); double tunedI = SmartDashboard.getNumber("retractor ki", RetractorConstants.kI); double tunedD = SmartDashboard.getNumber("retractor kd", RetractorConstants.kD); diff --git a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java index 5373b97..e775a99 100644 --- a/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java +++ b/src/main/java/org/robolancers321/subsystems/launcher/Indexer.java @@ -154,7 +154,7 @@ public Command acceptHandoff() { () -> { this.goalRPM = IndexerConstants.kHandoffRPM; }) - .alongWith((new WaitUntilCommand(this::exitBeamBroken).andThen(new WaitCommand(0.2))).withTimeout(2.0)); + .alongWith((new WaitUntilCommand(this::exitBeamBroken).andThen(new WaitCommand(0.1))).withTimeout(1.0)); } public Command outtake() {